Ask Your Question
1

Having trouble publishing JointState msg from Arduino

asked 2020-03-04 02:57:35 -0500

Benjamin C gravatar image

updated 2020-03-11 06:50:38 -0500

Hi everyone, I am currently creating my own robotic arm actuated with Dynamixel motors. The point of my code is to read the motor's position with an Arduino Mega and fulfill a sensor_msgs/JointState msg to be published over ROS on a /joint_state topic in order to get a feedback. Rosserial is usually working quite well, but due to some mistakes, I supposed in my arduino's code, the /joint_state topic is empty when I do a rostopic echo /joint_states (nothing is being published).


Here's my arduino code:

#include <DynamixelSerial.h> //rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200
#include <ros.h>
#include <sensor_msgs/JointState.h>

ros::NodeHandle  nh;

sensor_msgs::JointState robot_state;
ros::Publisher pub("joint_states", &robot_state);

int posJoint;

char robot_id = "arm";
char *joint_name[6] = {"joint_01", "joint_02", "joint_03", "joint_04", "joint_05", "joint_06"};
float pos[6];
float vel[6];
float eff[6];

void setup() {
  // put your setup code here, to run once:
  nh.getHardware()->setBaud(115200);
  nh.initNode();
  nh.advertise(pub);
  delay(1000);

  Dynamixel.setSerial(&Serial1); // &Serial - Arduino UNO/NANO/MICRO, &Serial1, &Serial2, &Serial3 - Arduino Mega
  Dynamixel.begin(1000000,2);  // Inicialize the servo at 1 Mbps and Pin Control 2
  delay(1000);
  Dynamixel.moveSpeed(1,512,512); //Home position
  Dynamixel.moveSpeed(2,512,512);
  Dynamixel.moveSpeed(3,512,512);
  Dynamixel.moveSpeed(4,512,512);
  Dynamixel.moveSpeed(5,512,512);
  Dynamixel.moveSpeed(6,512,512);

}

void loop() {
  // put your main code here, to run repeatedly:
  for(int id = 1; id <= 6; id++){ // Fulfill the arrays whith motor readings
    posJoint = posToRadians(Dynamixel.readPosition(id));
    pos[id] = posJoint;
    vel[id] = 0.5; // Value only for testing
    eff[id] = 0.1; // Value only for testing
  }

  // Fulfill the sensor_msg/JointState msg
  robot_state.name_length = 6;
  robot_state.velocity_length = 6;
  robot_state.position_length = 6;
  robot_state.effort_length = 6;

  robot_state.header.stamp = nh.now();
  robot_state.header.frame_id = robot_id;
  robot_state.name = joint_name;
  robot_state.position = pos;
  robot_state.velocity = vel;
  robot_state.effort = eff;

  pub.publish( &robot_state);
  nh.spinOnce();
  delay(10);

}

double posToRadians(int posInt) // Dynamixel position into radians
{
  float position_degrees = map(posInt, 0, 1023, -150, 150);
  float position_radians = (position_degrees * 3.14)/180;

  return position_radians;
}

Specs: - ROS melodic (up to date) - Ubuntu mate 18.04 - Arduino IDE 1.8.8

Feel free to ask me for more informations

Have a nice day

edit retag flag offensive close merge delete

Comments

rosserial on Arduinos requires some more bookkeeping when dealing with arrays.

Have you see wiki/rosserial/Overview/Limitations - Arrays and made sure to do everything it mentions?

gvdhoorn gravatar image gvdhoorn  ( 2020-03-04 03:01:29 -0500 )edit

No, I hadn't seen it. I might have to add Header header; and replace sensor_msgs::JointState robot_state; by sensor_msgs::JointState[] robot_state; I'll check if it works later. Thank you !

Benjamin C gravatar image Benjamin C  ( 2020-03-04 05:27:32 -0500 )edit

No, that's not what that page tells you. The message shown there is just an example.

Arrays in messages have an extra field when used with rosserial: <array_field_name>_length, as lengths of arrays / lists cannot be determined from a pointer alone.

You don't appear to be setting that field, so most likely rosserial assumes the message it receives contains zero-length arrays.

gvdhoorn gravatar image gvdhoorn  ( 2020-03-04 06:02:59 -0500 )edit

Isn't it what I have already done there ?

robot_state.name_length = 6;
  robot_state.velocity_length = 6;
  robot_state.position_length = 6;
  robot_state.effort_length = 6

Thank you for your help

Benjamin C gravatar image Benjamin C  ( 2020-03-04 11:07:22 -0500 )edit

You're correct. That would be those fields. I'd missed them when reading your question.

I'd also assumed that with "empty", you mean there are messages on /joint_states, but they don't contain any information. In fact, re-reading, I now assume you mean there no messages published at all. Correct?

gvdhoorn gravatar image gvdhoorn  ( 2020-03-04 11:34:30 -0500 )edit

Yes, there is nothing at all published on /joint_states.

Benjamin C gravatar image Benjamin C  ( 2020-03-04 16:22:05 -0500 )edit

Then I would suggest to take a step back: can you publish any message from your Arduino code to the ROS side?

Start with a simple std_msgs/Int32 or something similar.

gvdhoorn gravatar image gvdhoorn  ( 2020-03-05 02:06:19 -0500 )edit

Ufortunately yes, simple messages work fine and also all the Arduino exemple codes working as well.

Benjamin C gravatar image Benjamin C  ( 2020-03-05 06:16:25 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-03-11 16:05:40 -0500

Benjamin C gravatar image

updated 2020-03-11 16:11:18 -0500

I've solved my problem, that was the nh.spinOnce(); that wasn't called quickly enough. The fullfilement of my array pos was slow because of the Dynamixel.readPosition(id)) statement. This leads to a bad sync between the Arduino and ROS. So if you have the same error as me:

[INFO] [1583446774.076031]: Requesting topics...
[INFO] [1583446774.226598]: Setup publisher on joint_states [sensor_msgs/JointState]
[ERROR] [1583446789.223287]: Lost sync with device, restarting...

Just make sure that spinOnce(); is called at a sufficiently high frequency.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2020-03-04 02:57:35 -0500

Seen: 70 times

Last updated: Mar 11