Geting error subscriber/publiser failed: need more than 1 value
I'm trying to use a custom message on my arduino with ROS. The message was created in catkinws and added to the rosserial library according to the tutorial http://wiki.ros.org/rosserial/Tutorials/Adding%20Other%20Messages. The message is responsive in the C code for the arduino (The arduino Ide compiles the code w/o error). The error appears when I run rosrun rosserialpython serial_node.py /dev/ttyUSB0 to see the subscriber and publisher on the arduino. The error says "Creation of subscriber failed: need more than 1 value to unpack" and "Creation of publisher failed: need more than 1 value to unpack".
The C code for the arduino where kinematic_service/JointMsg is my custom msg.
include <ros.h>
include <std_msgs/Empty.h>
include <std_msgs/Float32MultiArray.h>
include <std_msgs/Bool.h>
include <AS5048A.h>
include <kinematic_service/JointMsg.h>
ros::NodeHandle nh;
kinematic_service::JointMsg arr;
std_msgs:: Bool stop_pub_joints;
std_msgs:: Bool finished_move;
ros::Publisher Joint_State_pub("Joint_State_uc", &arr);
ros::Publisher stop_pub_pub("publish_joint", &stop_pub_joints);
ros::Publisher finished_pub("finished", &finished_move);
// define pins backwards are on + 1 of the forward
const int motor1_forward = 22;
const int motor2_forward = 24;
const int motor3_forward = 26;
const int motor4_forward = 28;
bool message_recieved = false;
void stop_publish() {
stop_pub_joints.data = false;
stop_pub_pub.publish(&stop_pub_joints);
}
bool in_range(float current, float target){
// set tolerance
float tol =1.5;
if((target + tol) < current && (target - tol) < current){
return true;
}
else {
return false;
}
}
float get_angle(int sensor) {
AS5048A angleSensor(sensor);
angleSensor.init();
delay(50);
float val = angleSensor.getRawRotation();
Serial.print("Got rotation for sensor :");
Serial.print(sensor);
Serial.print('\n');
Serial.print("angle is :");
float angle = val*360/16384;
Serial.println(val, DEC);
Serial.print('\n');
return angle;
}
void move_actuator(int select_motor,float goal,float current,bool orientation){
// select_motor = forward for that motor
// select_motor+1= backwards for that motor
if (in_range(goal,current)!=true){
if (orientation == true){
if (goal < current){
digitalWrite(select_motor,HIGH); //
digitalWrite(select_motor+1,LOW);
}
else{
digitalWrite(select_motor,LOW); //
digitalWrite(select_motor+1,HIGH);
}
}
else{
if (goal < current){
digitalWrite(select_motor,HIGH); //
digitalWrite(select_motor+1,LOW);
}
else{
digitalWrite(select_motor,LOW); //
digitalWrite(select_motor+1,HIGH);
}
}
}
else{
digitalWrite(select_motor,LOW); //
digitalWrite(select_motor+1,LOW);
}
}
// subscriber callback move to desired position
void messageCB(const kinematic_service::JointMsg& joint_msg) {
//stop_publish(); // stops the publishing of desired position
message_recieved = true;
float goal1 = joint_msg.omega;
float goal2 = joint_msg.alpha;
float goal3 = joint_msg.beta;
float goal4 = joint_msg.gamma;
// add conversion from hex to degree
//float sensor1 = 1.00;
//float sensor2 = 3.00;
//float sensor3 = 3.00;
//float sensor4 = 7.00;
float sensor1 = get_angle(22);
float sensor2 = get_angle(22);
float sensor3 = get_angle(22);
float sensor4 = get_angle(22);
// set the value in the array
arr.omega = sensor1;
arr.alpha = sensor2;
arr.beta = sensor3;
arr.gamma = sensor4;
while(finished_move.data == false){
// if in range of joint angles, publish "finished" (true) and stop publishing joints
if (in_range(goal1,sensor1) == true && in_range(goal2,sensor2) && in_range(goal2,sensor3) && in_range(goal4,sensor4)){
stop_pub_joints.data = false;
stop_pub_pub.publish(&stop_pub_joints);
finished_move.data = true;
Joint_State_pub.publish(&arr);
finished_pub.publish(&finished_move);
}
// if not in range publish "not finished" (false) and the joint states
else{
stop_pub_joints.data = false;
stop_pub_pub.publish(&stop_pub_joints);
finished_move.data = false;
Joint_State_pub.publish(&arr);
finished_pub.publish(&finished_move);
// actuators function
move_actuator(motor1_forward,goal1,sensor1,true);
move_actuator(motor2_forward,goal2,sensor2,true);
move_actuator(motor3_forward,goal3,sensor3,true);
move_actuator(motor4_forward,goal4,sensor4,true);
}
}
}
ros::Subscriber<kinematic_service::JointMsg>joint_ik_sub("joint_IK",&messageCB);
//ros::Subscriber<std_msgs::Bool> sub_bool("UC_transmit_angles",&messageCB_bool);
void setup() {
// put your setup code here, to run once:
nh.loginfo("in callback function");
nh.getHardware()->setBaud(57600);
nh.initNode();
nh.subscribe(joint_ik_sub);
nh.advertise(Joint_State_pub);
nh.advertise(stop_pub_pub);
nh.advertise(finished_pub);
pinMode(LED_BUILTIN, OUTPUT);
//arr.data = (float*)malloc(sizeof(float)*4);
//arr.data_length = 4;
}
void loop() {
// put your main code here, to run repeatedly:
nh.spinOnce();
digitalWrite(LED_BUILTIN, HIGH);
delay(100);
digitalWrite(LED_BUILTIN, LOW);
delay(100);
//finished_move.data = true;
//finished_pub.publish(&finished_move);
}
I'm using Ubuntu 18.04, ROS melodic and kernel 5.4.0-52-generic
EDITED:
And my custom message
float32 omega
float32 alpha
float32 beta
float32 gamma
Asked by ptkth on 2020-11-06 08:20:39 UTC
Comments
Please, post your message definition as well.
Asked by tryan on 2020-11-06 15:02:34 UTC
Edited the post with message definition and also in this comment.
Asked by ptkth on 2020-11-08 09:20:12 UTC