Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Displaying joint paths for the entire robot in rviz

ROS Setup

In order to create a ROS node that sends display paths to rviz, we will create a package to contain our code. We'll use the handy roscreate-pkg command where we want to create the package directory with a dependency on the motion_planning_msgs and roscpp packages as shown below:

roscreate-pkg rviz_display_path_tutorial motion_planning_msgs planning_environment

After this is done we'll need to roscd to the package we created, since we'll be using it as our workspace

roscd rviz_display_path_tutorial

Also, make sure to set an environment variable called ROBOT to sim. E.g., in a bash shell,

 export ROBOT=sim

Example code

Now, create a directory called src in this package and open a file src/rviz_path_display.cpp to put your code in. Here's the code, we will walk through the important pieces of it.

   1 #include <motion_planning_msgs/DisplayPath.h>
   2 #include <planning_environment/monitors/joint_state_monitor.h>
   3 #include <boost/thread.hpp>
   4 
   5 void spinThread()
   6 {
   7   ros::spin();
   8 }
   9 
  10 int main(int argc, char** argv)
  11 {
  12   ros::init(argc, argv, "display_path_publisher");
  13   boost::thread spin_thread(&spinThread);
  14   ros::NodeHandle root_handle;
  15   planning_environment::JointStateMonitor joint_state_monitor;
  16   ros::Publisher display_path_publisher = root_handle.advertise<motion_planning_msgs::DisplayPath>("joint_path_display", 1);
  17   while(display_path_publisher.getNumSubscribers() < 1 && root_handle.ok())
  18   {
  19     ROS_INFO("Waiting for subscriber");
  20     ros::Duration(0.1).sleep();
  21   }
  22   motion_planning_msgs::DisplayPath display_path;
  23   unsigned int num_points = 100;
  24 
  25   display_path.model_id = "pr2";
  26   display_path.path.header.frame_id = "base_footprint";
  27   display_path.path.header.stamp = ros::Time::now();
  28   display_path.path.joint_names.push_back("r_shoulder_pan_joint");
  29   display_path.path.points.resize(num_points);
  30 
  31   for(unsigned int i=0; i < num_points; i++)
  32   {    
  33     display_path.path.points[i].positions.push_back(-(M_PI*i/4.0)/num_points);
  34   }
  35   display_path.robot_state.joint_state =  joint_state_monitor.getJointStateRealJoints();
  36   display_path.robot_state.joint_state.header.stamp = ros::Time::now();
  37   display_path.robot_state.joint_state.header.frame_id = "base_footprint";
  38   ROS_INFO("Publishing path for display");
  39   display_path_publisher.publish(display_path);
  40   joint_state_monitor.stop();
  41   ros::shutdown();
  42   spin_thread.join();
  43   return(0);
  44 }

Detailed Explanation

This tutorials assumes you have completed the ROS Tutorials. So, we won't go into the details of the ROS setup here. Note that the code does ros::spin() in a different thread. This is important to ensure that the joint_state_monitor that we will discuss in the next section has its callbacks serviced periodically.

The path that we will specify to the visualizer may only contain a subset of all the joints on the robot. To specify positions for other joints on the robot, we can use the robot_state field inside the DisplayPath message. We will fill up the robot_state with the current robot state (note that we can fill it with any values we want). This step is made easier by using the joint state monitor that provides easy access to the current robot state of the robot.

Joint state monitor

The joint state monitor listens for the current state of the robot on the joint_states topic. It can return the joint states for all the joints on the robot or only the real joints. Real joints are actual physical joints. There can also be virtual joints which are created to represent a certain motion of the robot. E.g., the r_gripper_joint on the PR2 is a virtual joint.

The rviz display only needs real joint information. So, that's what we will supply it. To do this, we first create a joint state monitor and then call the getJointStateRealJoints method on it to fill up the joint state information inside the robot state.

   1   display_path.robot_state.joint_state = joint_state_monitor.getJointStateRealJoints();

Calling the visualizer plugin

The visualizer listens on a topic so we will first create a ros::Publisher. We will wait on the publisher until a subscriber connects.

   1   ros::Publisher display_path_publisher = root_handle.advertise<motion_planning_msgs::DisplayPath>("joint_path_display", 1);
   2   while(display_path_publisher.getNumSubscribers() < 1 && root_handle.ok())
   3   {
   4     ROS_INFO("Waiting for subscriber");
   5     ros::Duration(0.1).sleep();
   6   }

We have already figured out how to populate the DisplayPath message with the current robot_state, now we have to

Now, after we create and populate the DisplayPath message, we will publish it on a topic to rviz.

   1   display_path_publisher.publish(display_path);

Robot Setup

You will need a robot to be up and running for this tutorial. The robot can be a simulated robot in gazebo. We will use a PR2 model simulated in gazebo for this example. To launch this model

roscd pr2_gazebo
roslaunch pr2_empty_world.launch