Robotics StackExchange | Archived questions

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.

float32 omega
float32 alpha
float32 beta
float32 gamma

Asked by ptkth on 2020-11-08 09:20:12 UTC

Answers