arduino sketch to subscribe to joint state msg
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
Have you run
to check the topic's subscribers and publishers? You can also run
to see what the node is subscribing to.
What about
?
added this to the question. thank you
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. Takingfake_controller_joint_states
as input is also not very nice.Look into
FollowJointTrajectory
.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.
From the documentation:
Controlling joints is done with
FollowJointTrajectory
actions. Or at least, that is how it was designed.I don't know what this means.