## page was renamed from motion_planning_rviz_plugin/Tutorials/DisplayingJointTrajectoriesInRviz
## page was renamed from motion_planning_rviz_plugin/Tutorials/DisplayingJointPathsInRviz
## page was renamed from motion_planning_rviz_plugin/Tutorials/DisplayingJointPathsinrviz
## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links 
## note.0= 
## descriptive title for the tutorial
## title = Displaying joint paths for the entire robot in rviz
## multi-line description to be displayed in search 
## description = This tutorial will teach you how to display a robot model in rviz and visualize joint paths for any set of joints on the robot.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for 
## level= BeginnerCategory
## keywords =
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

== 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. 

{{{
#!cplusplus block=simple_arm_goals
#include <motion_planning_msgs/DisplayTrajectory.h>
#include <planning_environment/monitors/joint_state_monitor.h>
#include <boost/thread.hpp>

void spinThread()
{
  ros::spin();
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "display_trajectory_publisher");
  boost::thread spin_thread(&spinThread);
  ros::NodeHandle root_handle;
  planning_environment::JointStateMonitor joint_state_monitor;
  ros::Publisher display_trajectory_publisher = root_handle.advertise<motion_planning_msgs::DisplayTrajectory>("joint_path_display", 1);
  while(display_trajectory_publisher.getNumSubscribers() < 1 && root_handle.ok())
  {
    ROS_INFO("Waiting for subscriber");
    ros::Duration(0.1).sleep();
  }
  motion_planning_msgs::DisplayTrajectory display_trajectory;
  unsigned int num_points = 100;

  display_trajectory.model_id = "pr2";
  display_trajectory.trajectory.joint_trajectory.header.frame_id = "base_footprint";
  display_trajectory.trajectory.joint_trajectory.header.stamp = ros::Time::now();
  display_trajectory.trajectory.joint_trajectory.joint_names.push_back("r_shoulder_pan_joint");
  display_trajectory.trajectory.joint_trajectory.points.resize(num_points);

  for(unsigned int i=0; i < num_points; i++)
  {    
    display_trajectory.trajectory.joint_trajectory.points[i].positions.push_back(-(M_PI*i/4.0)/num_points);
  }
  display_trajectory.robot_state.joint_state =  joint_state_monitor.getJointStateRealJoints();
  ROS_INFO("Publishing path for display");
  display_trajectory_publisher.publish(display_trajectory);
  joint_state_monitor.stop();
  ros::shutdown();
  spin_thread.join();
  return(0);
}
}}}

== 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.
{{{
#!cplusplus block=publish
  ros::Publisher display_trajectory_publisher = root_handle.advertise<motion_planning_msgs::DisplayTrajectory>("joint_path_display", 1);
  while(display_trajectory_publisher.getNumSubscribers() < 1 && root_handle.ok())
  {
    ROS_INFO("Waiting for subscriber");
    ros::Duration(0.1).sleep();
  }
}}}

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. 
{{{
#!cplusplus block=path
  display_trajectory.trajectory.joint_trajectory.header.frame_id = "base_footprint";
  display_trajectory.trajectory.joint_trajectory.header.stamp = ros::Time::now();
  display_trajectory.trajectory.joint_trajectory.joint_names.push_back("r_shoulder_pan_joint");
  display_trajectory.trajectory.joint_trajectory.points.resize(num_points);

  for(unsigned int i=0; i < num_points; i++)
  {    
    display_trajectory.trajectory.joint_trajectory.points[i].positions.push_back(-(M_PI*i/4.0)/num_points);
  }
}}}

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.

{{{
#!cplusplus block=display_path
  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.
{{{
#!cplusplus block=display_path
  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.

{{attachment:rviz_initial_setup.png}}

## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE