arduino sketch to subscribe to joint state msg

Question

I'm trying to use rosserial on arduino to subscribe to move_group/fake_controller_joint_states and then publish the first joints position on a topic chatter just to test verify that im receiving the position but it seems that the call back is not working as I don't seem to receive the values being sent on fake_controller_joint_states. Im using an arduino mega, kinetic and ubuntu 16.04. I greatly appreciate any direction or input. thank you.

#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
#include <ros.h>
#include <std_msgs/Float64.h>
#include <sensor_msgs/JointState.h>
#include <stdlib.h>

ros::NodeHandle nh;
std_msgs::Float64 mydata;

float angle[6] = {0,0,0,0,0,0};

void cmd_cb(const sensor_msgs::JointState& cmd_arm)
{
  angle[0] = cmd_arm.position[0];
  angle[1] = cmd_arm.position[1];
  angle[2] = cmd_arm.position[2];
  angle[3] = cmd_arm.position[3];
  angle[4] = cmd_arm.position[4];
  angle[5] = cmd_arm.position[5];
}

ros::Subscriber<sensor_msgs::JointState> sub("/move_group/fake_controller_joint_states", cmd_cb);
ros::Publisher chatter("chatter", &mydata);

void setup()
{
  nh.initNode();
  nh.subscribe(sub);
  nh.advertise(chatter);
}

void loop()
{
  mydata.data = angle[1];
  chatter.publish( &mydata );
  nh.spinOnce();
  delay(1);
}

rostopic info /move_group/fake_controller_joint_states returns:

Type: sensor_msgs/JointState

Publishers: 
 * /move_group (http://chris-Latitude-3540:40629/)

Subscribers: 
 * /joint_state_publisher (http://chris-Latitude-3540:41687/)
 * /rostopic_5632_1530298559582 (http://chris-Latitude-3540:32869/)
 * /serial_node (http://chris-Latitude-3540:44431/)

rosnode info /serial_node returns:

Node [/serial_node]
Publications: 
 * /chatter [std_msgs/Float64]
 * /diagnostics [diagnostic_msgs/DiagnosticArray]
 * /rosout [rosgraph_msgs/Log]

Subscriptions: 
 * /move_group/fake_controller_joint_states [sensor_msgs/JointState]

Services: 
 * /serial_node/get_loggers
 * /serial_node/set_logger_level

contacting node http://chris-Latitude-3540:44431/ ...
Pid: 10725
Connections:
 * topic: /chatter
    * to: /rostopic_9634_1530306645550
    * direction: outbound
    * transport: TCPROS
 * topic: /rosout
    * to: /rosout
    * direction: outbound
    * transport: TCPROS
 * topic: /move_group/fake_controller_joint_states
    * to: /move_group (http://chris-Latitude-3540:40629/)
    * direction: inbound
    * transport: TCPROS

---

Originally posted by chris annin on ROS Answers with karma: 58 on 2018-06-29

Post score: 0

---

Original comments

Comment by jayess on 2018-06-29:
Have you run

rostopic info /move_group/fake_controller_joint_states

to check the topic's subscribers and publishers? You can also run

rosnode info <node-name>

to see what the node is subscribing to.

Comment by jayess on 2018-06-29:
What about

rosnode info /serial_node

?

Comment by chris annin on 2018-06-29:
added this to the question. thank you

Comment by gvdhoorn on 2018-06-30:
Not an answer, but: please note that JointState msgs are not meant to be used for commanding pose goals, but for reporting current joint states of your robot. Taking fake_controller_joint_states as input is also not very nice.

Look into FollowJointTrajectory.

Comment by chris annin on 2018-06-30:
i appreciate your feedback, what do you mean by "Taking fake_controller_joint_states as input is also not very nice" Is this fopaux or against protocol? This seems to provides exactly what is needed for an open loop robot.

Comment by gvdhoorn on 2018-06-30:
From the documentation:

This is a message that holds data to describe the state of a set of [..] joints. [..] The header specifies the time at which the joint states were recorded.

Controlling joints is done with FollowJointTrajectory actions. Or at least, that is how it was designed.

Comment by gvdhoorn on 2018-06-30:\

Is this fopaux

I don't know what this means.

Answer 1

I was able to get it working by changing the baud rate to 115200

I added this line to the void_setup

nh.getHardware()->setBaud(115200);

I added the baud rate parameter to the rosserial launch:

rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200

This is the sketch that works:

#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
#include <ros.h>
#include <std_msgs/Float64.h>
#include <sensor_msgs/JointState.h>
#include <stdlib.h>

ros::NodeHandle nh;

float angle[6] = {0,0,0,0,0,0};

void cmd_cb(const sensor_msgs::JointState& cmd_arm)
{
  angle[0] = cmd_arm.position[0];
  angle[1] = cmd_arm.position[1];
  angle[2] = cmd_arm.position[2];
  angle[3] = cmd_arm.position[3];
  angle[4] = cmd_arm.position[4];
  angle[5] = cmd_arm.position[5];
 }

std_msgs::Float64 mydata;
ros::Subscriber<sensor_msgs::JointState> sub("/move_group/fake_controller_joint_states", cmd_cb);
ros::Publisher chatter("chatter", &mydata);

void setup()
{
  nh.getHardware()->setBaud(115200);
  nh.initNode();
  nh.subscribe(sub);
  nh.advertise(chatter);
}

void loop()
{
  mydata.data = angle[1];
  chatter.publish( &mydata );
  nh.spinOnce();
  delay(1);
}

---

Originally posted by chris annin with karma: 58 on 2018-06-30

This answer was ACCEPTED on the original site

Post score: 1