Ask Your Question

Having trouble publishing JointState msg from Arduino

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

Benjamin C gravatar image

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

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 _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:

  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
  Dynamixel.moveSpeed(1,512,512); //Home position


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 =;
  robot_state.header.frame_id = robot_id; = joint_name;
  robot_state.position = pos;
  robot_state.velocity = vel;
  robot_state.effort = eff;

  pub.publish( &robot_state);


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


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 -0600 )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 -0600 )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 -0600 )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 -0600 )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 -0600 )edit

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

Benjamin C gravatar image Benjamin C  ( 2020-03-04 16:22:05 -0600 )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 -0600 )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 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

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

Benjamin C gravatar image

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

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



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

Seen: 506 times

Last updated: Mar 11 '20