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_trajectory_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_trajectory_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_display_trajectory.cpp to put your code in. Here's the code, we will walk through the important pieces of it.

   1 #include <motion_planning_msgs/DisplayTrajectory.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_trajectory_publisher");
  13   boost::thread spin_thread(&spinThread);
  14   ros::NodeHandle root_handle;
  15   planning_environment::JointStateMonitor joint_state_monitor;
  16   ros::Publisher display_trajectory_publisher = root_handle.advertise<motion_planning_msgs::DisplayTrajectory>("joint_path_display", 1);
  17   while(display_trajectory_publisher.getNumSubscribers() < 1 && root_handle.ok())
  18   {
  19     ROS_INFO("Waiting for subscriber");
  20     ros::Duration(0.1).sleep();
  21   }
  22   motion_planning_msgs::DisplayTrajectory display_trajectory;
  23   unsigned int num_points = 100;
  24 
  25   display_trajectory.model_id = "pr2";
  26   display_trajectory.trajectory.joint_trajectory.header.frame_id = "base_footprint";
  27   display_trajectory.trajectory.joint_trajectory.header.stamp = ros::Time::now();
  28   display_trajectory.trajectory.joint_trajectory.joint_names.push_back("r_shoulder_pan_joint");
  29   display_trajectory.trajectory.joint_trajectory.points.resize(num_points);
  30 
  31   for(unsigned int i=0; i < num_points; i++)
  32   {    
  33     display_trajectory.trajectory.joint_trajectory.points[i].positions.push_back(-(M_PI*i/4.0)/num_points);
  34   }
  35   display_trajectory.robot_state.joint_state =  joint_state_monitor.getJointStateRealJoints();
  36   ROS_INFO("Publishing path for display");
  37   display_trajectory_publisher.publish(display_trajectory);
  38   joint_state_monitor.stop();
  39   ros::shutdown();
  40   spin_thread.join();
  41   return(0);
  42 }

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 visualizer listens on a topic using a message of type motion_planning_msgs/DisplayTrajectory, so we will first create a ros::Publisher. We will wait on the publisher until a subscriber connects.

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

We will assign a model_id to the display message. This id is needed to tell rviz which robot this trajectory is for.

  display_trajectory.model_id = "pr2";

Next we will fill up the trajectory field inside the DisplayTrajectory message with a dummy trajectory.

   1   display_trajectory.trajectory.joint_trajectory.header.frame_id = "base_footprint";
   2   display_trajectory.trajectory.joint_trajectory.header.stamp = ros::Time::now();
   3   display_trajectory.trajectory.joint_trajectory.joint_names.push_back("r_shoulder_pan_joint");
   4   display_trajectory.trajectory.joint_trajectory.points.resize(num_points);
   5 
   6   for(unsigned int i=0; i < num_points; i++)
   7   {    
   8     display_trajectory.trajectory.joint_trajectory.points[i].positions.push_back(-(M_PI*i/4.0)/num_points);
   9   }

The trajectory 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 DisplayTrajectory 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.

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.

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_trajectory.robot_state.joint_state = joint_state_monitor.getJointStateRealJoints();

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

   1   display_trajectory_publisher.publish(display_trajectory);

Building the node

Now that we have a package and a source file, we'll want to build and then try things out. The first step will be to add our src/rviz_display_trajectory.cpp file to our CMakeLists.txt file to get it to build. Open up CMakeLists.txt in your editor of choice and add the following line to the bottom of the file.

rosbuild_add_executable(rviz_display_trajectory src/rviz_display_trajectory.cpp)

Once this is done, we can build our executable by typing make.

make

Simulator 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

Rviz Setup

Make sure the package motion_planning_rviz_plugin is compiled.

rosmake motion_planning_rviz_plugin

Now, startup rviz. From the left-hand side menu, click the ADD button. In the pop-menu, under Display Type, choose Planning under the Motion Planning field.

You should see a planning field on the left hand side of the menu. Click on the empty field next to the "topic name" and fill in the name of the topic we are publishing on (joint_path_display). Click on the empty field next to the robot_description field and fill in the name of ros parameter from which the display can get the raw urdf (usually, this is robot_description).

Now, run your executable

./bin/joint_trajectory_display

and watch the robot display move the arm towards the final position. You can use the State display time to speed up or slow down the rate at which joint trajectory positions are displayed.