InteractiveRobot/InteractivityTutorial

This section walks you through some C++ code which allows you to interact with the robot and its environment in rviz while your code is running. This code will be reused in later visualization tutorials.

Classes
The code for this tutorial is mainly in the InteractiveRobot class which we will walk through below. The InteractiveRobot class maintains a RobotModel, a RobotState, and information about "the world" (in this case "the world" is a single yellow cube).

The InteractiveRobot class uses the IMarker class which maintains an interactive marker. This tutorial does not cover the implementation of the IMarker class (imarker.cpp), but most of the code is copied from the basic_controls tutorial and you can read more there about interactive markers if you are interested.

Code Description
We will walk through the code in the order that it is run in the program, starting with the main function in interactivity_tutorial.cpp: After the standard ros initialization we create an instance of the InteractiveRobot class. The constructor (implemented in interactive_robot.cpp) does all the setup for interacting with the robot so lets go through it: Many of the members are constructed with default values (0) and are initialized below. The main thing that is initialized above is the robot_model_loader::RDFLoader rdf_loader_ member which is initialized with the name of the parameter containing the URDF and SRDF (robot_description which is by default "robot_description"). The rdf_loader_ is used to initialize the RobotModel below.

Here we also create various publishers:
 * The robot_state_publisher_ publishes DisplayRobotState messages to tell Rviz how to draw the PR2 robot (see the RobotStateDisplay tutorial for details).
 * The world_state_publisher_ publishes Marker messages to tell Rviz how to draw the world (the "world" in this case is a single yellow cube).
 * The interactive_marker_server_ is used by the IMarker class to create interactive markers.

In the body of the InteractiveRobot constructor we first initialize the robot: The members are created as described in the Kinematics tutorial. The is initialized from the default pose of the last link in the "right_arm" group.
 * robot_model::RobotModelPtr robot_model_
 * robot_state::RobotStatePtr robot_state_
 * robot_state::JointStateGroup* group_</tt>
 * desired_group_end_link_pose_</tt>

Next we create an interactive marker for controlling the robot arm and another for controlling the yellow "world geometry" cube: The movedRobotMarkerCallback</tt> and movedWorldMarkerCallback</tt> callbacks will be called when the markers are moved in Rviz. We will look at them closely below.

Finally: we set up a timer to update the robot/world state and publish it to Rviz. The timer calls the InteractiveRobot::updateCallback</tt> method. Using the timer makes this code run more interactively. Messages from the interactive markers come very quickly. If we update the robot and world (run IK, do collision checks, etc) every time we get a message we quickly fall behind and the user is left waiting while the robot slowly catches up. Or, worse, we miss messages which makes it seem like the robot is ignoring the marker movements. By using the timer we allow many messages to arrive quickly and run the updateCallback</tt> at a rate that allows us to keep up.

Now back to the rest of the main</tt> function: First we set a userCallback</tt> function which allows us to add our own code when the robot runs. We'll see where this gets called below.

Then we enter the ros::spin</tt> loop until the user hits CTRL-C. The rest of the code runs in callback functions which we will look at next: These callbacks are called by the IMarker class when the interactive markers are moved. They just pass the pose of the marker on to the next set of functions: These simply record the pose and call scheduleUpdate</tt> to trigger the timer: The setCallbackTimer</tt> calculates when the callback should run and sets the timer to call the updateCallback</tt> function. If it is time to run that function right now setCallbackTimer</tt> returns true. The goal of the setCallbackTimer</tt> function is to call updateCallback</tt> when needed, but wait long enough between calls to collect messages so we do not fall behind and lose interactivity.

The updateCallback</tt> function just measures how long it takes itself to run (this time, <tt>average_callback_duration_</tt>, is used in <tt>setCallbackTimer</tt> to decide how long to wait between callbacks). The interesting work is done in <tt>updateAll</tt>: This code publishes the world state to Rviz and then uses IK to calculate a new RobotState that satisfies the <tt>desired_group_end_link_pose_</tt> from the interactive marker. If the IK solver succeeds then we publish the new RobotState to Rviz and then call the <tt>user_callback_</tt> function which was set with <tt>robot.setUserCallback(userCallback)</tt> in <tt>main</tt> above.

The callback code is very simple and just prints the pose of the robot arm. In future tutorials we will use this callback to do more interesting work such as checking collisions.

The entire code
The entire code can be seen here in the moveit_pr2 github project.
 * interactivity_tutorial.cpp - the main function
 * interactive_robot.h - InteractiveRobot class
 * interactive_robot.cpp - InteractiveRobot class implementation
 * imarker.h - IMarker class
 * imarker.cpp - IMarker class implementation
 * pose_string.h - helper for pretty-printing poses
 * pose_string.cpp - helper for pretty-printing poses

Compiling the code
Follow the instructions for compiling code from source. If you install the moveit_pr2 package from github then you can simply type <tt>catkin_make</tt> from the top level directory.

Launch file
A launch file is located here. It loads the URDF and SRDF parameters for the PR2 robot, launches Rviz, and runs the <tt>interactivity_tutorial</tt> program described above. If moveit_pr2 is in your <tt>ROS_PACKAGE_PATH</tt> then launch it by typing: roslaunch pr2_moveit_tutorials interactivity_tutorial.launch

Rviz setup
When rviz starts up you will have to add some displays to see the objects your code is publishing. This is done in the "Displays" panel in rviz.
 * Under GlobalOptions set FixedFrame to <tt>/base_footprint</tt>
 * Click Add and (under moveit_ros_visualization) add a RobotState display.
 * Set the RobotState::RobotDescription to <tt>robot_description</tt>
 * Set the RobotState::RobotStateTopic to <tt>interactive_robot_state</tt>
 * Click Add and (under rviz) add a Marker display.
 * Set the Marker::MarkerTopic to <tt>interactive_robot_markers</tt>
 * Click Add and (under rviz) add a InteractiveMarkers display.
 * Set the Marker::UpdateTopic to <tt>interactive_robot_imarkers/update</tt>

You should now see the PR2 robot with 2 interactive markers which you can drag around.



Rviz Troubleshooting
If you want to start with a fresh rviz (remove all extra displays and set the UI back to defaults) then quit Rviz, remove (or rename) the file <tt>~/.rviz/default.rviz</tt> and then restart. You will then need to follow the above Rviz setup instructions to add displays.

Interacting
In Rviz you will see 2 sets of Red/Green/Blue interactive marker arrows. Drag these around with the mouse.

One of these is attached to the robot arm. Moving or rotating this marker causes the callback in your program to update the RobotState with a new pose based on the marker position. (If you move the marker too far then the IK solver will not be able to find a solution and the arm will not move until the marker is placed back within range.)

The other marker is attached to the yellow cube (the "world geometry"). This allows you to move the world geometry around.

Each time the robot arm or world geometry moves the callback is called. You can see the <tt>userCallback</tt> function printing messages about the current arm pose in the terminal window. The callback only gets called if the IK solver is able to find a solution for the marker's pose.

Next steps
This code allows you to easily interact with the robot and run code (in the userCallback) whenever the world geometry changes. In future tutorials we will use this interactive robot framework to help visualize collision detection and other planning scene tasks.