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
#else
#include
#endif
#include
#include
#include
#include
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 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
↧