Ask Your Question
0

arduino sketch to subscribe to joint state msg

asked 2018-06-29 16:37:33 -0500

chris annin gravatar image

updated 2018-06-29 17:20:43 -0500

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
edit retag flag offensive close merge delete

Comments

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.

jayess gravatar imagejayess ( 2018-06-29 16:56:19 -0500 )edit

What about

rosnode info /serial_node

?

jayess gravatar imagejayess ( 2018-06-29 17:16:05 -0500 )edit

added this to the question. thank you

chris annin gravatar imagechris annin ( 2018-06-29 17:21:37 -0500 )edit

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.

gvdhoorn gravatar imagegvdhoorn ( 2018-06-30 01:24:11 -0500 )edit

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.

chris annin gravatar imagechris annin ( 2018-06-30 11:54:00 -0500 )edit

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.

gvdhoorn gravatar imagegvdhoorn ( 2018-06-30 13:11:27 -0500 )edit

Is this fopaux

I don't know what this means.

gvdhoorn gravatar imagegvdhoorn ( 2018-06-30 13:11:43 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-06-30 02:07:26 -0500

chris annin gravatar image

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);
}
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

Stats

Asked: 2018-06-29 16:37:33 -0500

Seen: 448 times

Last updated: Jun 30 '18