RobotStateDisplay/C++ API

In this section, we will walk you through the C++ API for visualizing the kinematic state (pose) of the robot.

The Rviz RobotState Display
The RobotState display is an Rviz plugin which displays the robot. The kinematic state of the displayed robot can be set with ROS messages.

Start Rviz (if you have trouble see Moveit Rviz Quickstart)

If you see the robot in the display then disable any Displays which are showing the robot (e.g. unclick MotionPlanning::SceneRobot::ShowSceneRobot, MotionPlanning::PlanningRequest::QueryStartState, MotionPlanning::PlanningRequest::QueryGoalState, MotionPlanning::PlannedPath::ShowRobotVisual, etc). In the Displays panel click Add. In the popup double click RobotState under moveit_ros_visualization.

You will see the robot in its default state. (If not, make sure RobotDescription is set to robot_description.

Select RoboticStateTopic in the RobotState display and set its value to /tutorial_robot_state. This tells the display to listen for DisplayRobotState messages on the /tutorial_robot_state ROS topic.

Code
Start by initializing ROS, and creating a KinematicModel, KinematicState, and JointStateGroup for the right_arm as described in the Kinematics tutorial.

Now create a ros node and announce that we will publish DisplayRobotState messages on the tutorial_robot_state topic.

Indicate we want our loop to run at 1Hz and begin the loop.

In each loop we first generate a random state for the joints in the right_arm group:

Then generate a DisplayRobotState message initialized with our KinematicState (with random arm position):

And publish that message:

Be sure to call ros::spinOnce within the loop to allow ROS to send the message:

The entire code
The entire code can be seen here in the moveit_pr2 github project.

The code linked above also contains a second loop which positions the robot arm in specific locations using inverse kinematics.

Compiling the code
Follow the instructions for compiling code from source.

The launch file
To run the code, you will need a launch file that does two things:
 * Upload the PR2 URDF and SRDF onto the param server:
 * Puts the kinematics_solver configuration generated by the MoveIt! Setup Assistant onto the ROS parameter server in the namespace of the node that instantiates the classes in this tutorial:

The entire launch file is here on github. All the code in this tutorial can be compiled and run from the pr2_moveit_tutorials package that you have as part of your MoveIt! setup.

Running the code

 * Roslaunch the launch file to run the code directly from pr2_moveit_tutorials:

Expected Output
When you run the code you should see the robot in the Rviz RobotState Display moving its arm to 5 random locations, and then to 20 specific locations around a circle in front of the robot.

To see the messages being sent from the state_display_tutorial node to the Rviz display, run

Links

 * Kinamatic State overview
 * Tutorial demonstrating use of Kinematic State in C++
 * Starting and using Rviz
 * Back to Kinematics
 * Back to Visualization