Skip to content

readrobotstate

Salvo Virga edited this page Jun 19, 2016 · 15 revisions

Read the robot state (C++)

Both ROSMonitor and ROSSmartServo publish the robot state on respective ROS topics.
As for any topic in ROS, you can print out its stream from command line using rostopic echo, here is an example for the JointPosition topic :
rostopic echo /iiwa/state/JointPosition
The same applies to any other topic used by ROSMonitor and ROSSmartServo.
You can use the syntax presented here to do what you need (e.g. dump the output into a file with in a CSV format).

NOTE : remember to source your ROS workspace with source devel/setup.bash for any new terminal window, or add that to your .bashrc

C++

From code you can simply create a Subscriber within a ROS Node to the topic you want containing the information you need.
In our auxiliary repository for educational purpose, there is a very stupid example of a ROS Node that just prints on screen the current joint position, every 10 seconds.
HERE you have the full code. Remember that is just an example.

Here we just cover the most important parts.
First we define a callback function for the ROS Subscriber. It will be called asynchronously every time a new message arrives on the given ROS Topic :

void jointPositionCallback(const iiwa_msgs::JointPosition& jp)
{
  if (!isRobotConnected)
    ~isRobotConnected;
  current_joint_position = jp;
}

In the main function, we define a subscriber to the ROS Topic of the JointPosition sent by the robot, using the callback function just defined. Then (within a loop in the full code) we print out the obtained joint position :

ros::Subscriber sub = nh.subscribe("/iiwa/state/JointPosition", 1, jointPositionCallback);

[...]

ROS_INFO("Current Joint Position is : [ %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f]", 
	   current_joint_position.position.a1, 
	   current_joint_position.position.a2, 
	   current_joint_position.position.a3, 
	   current_joint_position.position.a4, 
	   current_joint_position.position.a5, 
	   current_joint_position.position.a6, 
	   current_joint_position.position.a7);

You can create any subscriber you need to any topic you want, and by now you should know which topics are used to stream all the robot information.

The example above is quite trivial and not well written, but it should give you a clear idea on how things work.
I also used on purpose an async spinner and ROS parameters, just to trigger some curiosity about those topics on users new to ROS. You might want to do in depth about those reading the ROS documentation.

Run the ROS Node

This ROS Node can be compiled as any other ROS Node, just properly add its source file to the CMakeLists file, like HERE.

Compile and source the workspace, then you can call (in our case):
rosrun iiwa_tool_examples read_robot_state

However, this needs a roscore running and the robot description to be loaded beforhand.
Something smarter, is to create a launch file that loads the robot description and runs the ROS node at once.
Plus you could add some ROS parameters used by this (and other) ROS node.
Launch files are very useful and you might want to have a look at their ROS documentation if you are new to ROS.
An example is available HERE.
There we define some parameters like toolName, joint_position_topic, ros_rate, that are used at runtime by the ROS Node.

<!-- Parameters to pass to the ROS node -->
<arg name="joint_position_topic" default="/iiwa/state/JointPosition"/>
<arg name="ros_rate" default="1000"/>

Then, we load the robot description and start the robot state publisher :

<param name="/iiwa/publishJointStates" type="boolean" value="true" />

<include ns="iiwa" file="$(find iiwa_tool_moveit)/launch/planning_context.launch">
  <arg name="load_robot_description" value="true"/>
</include>

<node ns="iiwa" name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
  respawn="false" output="screen" />

We then start the ROS Node we just wrote and compiled, with the parameters just defined :

<node ns="iiwa" name="ReadRobotState" pkg="iiwa_tool_examples" type="read_robot_state" respawn="false" output="screen">
  <param name="joint_position_topic" value="$(arg joint_position_topic)"/>
  <param name="ros_rate" value="$(arg ros_rate)"/>
</node>

And finally, we run the RViZ visualization tool with some configuration, to show what we want to see :

<node ns="iiwa" name="$(anon rviz)" pkg="rviz" type="rviz" respawn="true" 
  args="-d $(find iiwa_tool_examples)/launch/iiwa_tool.rviz" output="screen" />

We can just run this launch file and have everything running at once :
roslaunch iiwa_tool_examples iiwa_tool_read.launch

Clone this wiki locally