adding moveit visual tools prompt as breakpoints (moveit#55)

* adding moveit visual tools prompt as breakpoints

* minor fixup

doc/motion_planning_api/motion_planning_api_tutorial.rst

@@ -17,6 +17,11 @@ Roslaunch the launch file to run the code directly from moveit_tutorials::
 
  roslaunch moveit_tutorials motion_planning_api_tutorial.launch
 
+**Note:** This tutorial uses the **RvizVisualToolsGui** panel to step through the demo. To add this panel to RViz, follow the instructions in the `Visualization Tutorial <../visualization/visualization_tutorial.html#rviz-visual-tools>`_.
+
+After a short moment, the RViz window should appear and look similar to the one at the top of this page. To progress through each demo step either press the **Next** button in the **RvizVisualToolsGui** panel at the bottom of the screen or select **Key Tool** in the **Tools** panel at the top of the screen and then press **N** on your keyboard while RViz is focused.
+
+
 Expected Output
 ---------------
 In RViz, we should be able to see four trajectories being replayed eventually:

doc/motion_planning_api/src/motion_planning_api_tutorial.cpp

@@ -44,12 +44,13 @@
 #include <moveit/kinematic_constraints/utils.h>
 #include <moveit_msgs/DisplayTrajectory.h>
 #include <moveit_msgs/PlanningScene.h>
+#include <moveit_visual_tools/moveit_visual_tools.h>
 
 #include <boost/scoped_ptr.hpp>
 
 int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "move_group_tutorial");
+  ros::init(argc, argv, "motion_planning_tutorial");
   ros::AsyncSpinner spinner(1);
   spinner.start();
   ros::NodeHandle node_handle("~");
@@ -68,8 +69,13 @@ int main(int argc, char** argv)
   //
   // .. _RobotModelLoader:
   //     http://docs.ros.org/indigo/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html
+  const std::string PLANNING_GROUP = "panda_arm";
   robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
   robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
+  /* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/
+  robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model));
+  const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);
+
 
   // Using the :moveit_core:`RobotModel`, we can construct a
   // :planning_scene:`PlanningScene` that maintains the state of
@@ -113,9 +119,32 @@ int main(int argc, char** argv)
                                                          << "Available plugins: " << ss.str());
   }
 
-  /* Sleep a little to allow time to startup rviz, etc. */
-  ros::WallDuration sleep_time(15.0);
-  sleep_time.sleep();
+  // Visualization
+  // ^^^^^^^^^^^^^
+  // The package MoveItVisualTools provides many capabilties for visualizing objects, robots,
+  // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script
+  namespace rvt = rviz_visual_tools;
+  moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
+  visual_tools.deleteAllMarkers();
+
+  /* Remote control is an introspection tool that allows users to step through a high level script
+     via buttons and keyboard shortcuts in RViz */
+  visual_tools.loadRemoteControl();
+
+  /* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/
+  Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
+  text_pose.translation().z() = 1.75;
+  visual_tools.publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE);
+
+  /* Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations */
+  visual_tools.trigger();
+
+  /* Sleep a little to allow time to startup rviz, etc..
+     This ensures that visual_tools.prompt() isn't lost in a sea of logs*/
+  ros::Duration(10).sleep();
+
+  /* We can also use visual_tools to wait for user input */
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
 
   // Pose Goal
   // ^^^^^^^^^
@@ -174,15 +203,14 @@ int main(int argc, char** argv)
   display_trajectory.trajectory.push_back(response.trajectory);
   display_publisher.publish(display_trajectory);
 
-  sleep_time.sleep();
+  /* We can also use visual_tools to wait for user input */
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
 
   // Joint Space Goals
   // ^^^^^^^^^^^^^^^^^
   /* First, set the state in the planning scene to the final state of the last plan */
-  robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst();
   planning_scene->setCurrentState(response.trajectory_start);
-  const robot_state::JointModelGroup* joint_model_group = robot_state.getJointModelGroup("panda_arm");
-  robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
+  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
 
   // Now, setup a joint space goal
   robot_state::RobotState goal_state(robot_model);
@@ -212,9 +240,12 @@ int main(int argc, char** argv)
   /* Now you should see two planned trajectories in series*/
   display_publisher.publish(display_trajectory);
 
+  /* Wait for user input */
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
+
   /* We will add more goals. But first, set the state in the planning
      scene to the final state of the last plan */
-  robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
+  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
 
   /* Now, we go back to the first goal*/
   req.goal_constraints.clear();
@@ -225,6 +256,10 @@ int main(int argc, char** argv)
   display_trajectory.trajectory.push_back(response.trajectory);
   display_publisher.publish(display_trajectory);
 
+  /* Wait for user input */
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
+
+
   // Adding Path Constraints
   // ^^^^^^^^^^^^^^^^^^^^^^^
   // Let's add a new pose goal again. This time we will also add a path constraint to the motion.
@@ -237,7 +272,7 @@ int main(int argc, char** argv)
   moveit_msgs::Constraints pose_goal_2 =
       kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);
   /* First, set the state in the planning scene to the final state of the last plan */
-  robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
+  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
   /* Now, let's try to move to this new pose goal*/
   req.goal_constraints.clear();
   req.goal_constraints.push_back(pose_goal_2);
@@ -271,8 +306,8 @@ int main(int argc, char** argv)
   display_publisher.publish(display_trajectory);
 
   // END_TUTORIAL
-  sleep_time.sleep();
-  ROS_INFO("Done");
+  /* Wait for user input */
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to exit the demo");
   planner_instance.reset();
 
   return 0;

doc/motion_planning_pipeline/motion_planning_pipeline_tutorial.rst

@@ -19,6 +19,10 @@ Roslaunch the launch file to run the code directly from moveit_tutorials: ::
 
  roslaunch moveit_tutorials motion_planning_pipeline_tutorial.launch
 
+**Note:** This tutorial uses the **RvizVisualToolsGui** panel to step through the demo. To add this panel to RViz, follow the instructions in the `Visualization Tutorial <../visualization/visualization_tutorial.html#rviz-visual-tools>`_.
+
+After a short moment, the RViz window should appear and look similar to the one at the top of this page. To progress through each demo step either press the **Next** button in the **RvizVisualToolsGui** panel at the bottom of the screen or select **Key Tool** in the **Tools** panel at the top of the screen and then press **N** on your keyboard while RViz is focused.
+
 Expected Output
 ---------------
 In RViz, we should be able to see three trajectories being replayed eventually:

doc/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp

@@ -32,7 +32,7 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
-/* Author: Sachin Chitta */
+/* Author: Sachin Chitta, Mike Lautman*/
 
 #include <pluginlib/class_loader.h>
 #include <ros/ros.h>
@@ -44,6 +44,7 @@
 #include <moveit/kinematic_constraints/utils.h>
 #include <moveit_msgs/DisplayTrajectory.h>
 #include <moveit_msgs/PlanningScene.h>
+#include <moveit_visual_tools/moveit_visual_tools.h>
 
 int main(int argc, char** argv)
 {
@@ -78,9 +79,32 @@ int main(int argc, char** argv)
   planning_pipeline::PlanningPipelinePtr planning_pipeline(
       new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters"));
 
-  /* Sleep a little to allow time to startup rviz, etc. */
-  ros::WallDuration sleep_time(15.0);
-  sleep_time.sleep();
+  // Visualization
+  // ^^^^^^^^^^^^^
+  // The package MoveItVisualTools provides many capabilties for visualizing objects, robots,
+  // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script
+  namespace rvt = rviz_visual_tools;
+  moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
+  visual_tools.deleteAllMarkers();
+
+  /* Remote control is an introspection tool that allows users to step through a high level script
+     via buttons and keyboard shortcuts in RViz */
+  visual_tools.loadRemoteControl();
+
+  /* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/
+  Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
+  text_pose.translation().z() = 1.75;
+  visual_tools.publishText(text_pose, "Motion Planning Pipeline Demo", rvt::WHITE, rvt::XLARGE);
+
+  /* Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations */
+  visual_tools.trigger();
+
+  /* Sleep a little to allow time to startup rviz, etc..
+     This ensures that visual_tools.prompt() isn't lost in a sea of logs*/
+  ros::Duration(10).sleep();
+
+  /* We can also use visual_tools to wait for user input */
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
 
   // Pose Goal
   // ^^^^^^^^^
@@ -136,7 +160,9 @@ int main(int argc, char** argv)
   display_trajectory.trajectory.push_back(response.trajectory);
   display_publisher.publish(display_trajectory);
 
-  sleep_time.sleep();
+  /* Wait for user input */
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
+
 
   // Joint Space Goals
   // ^^^^^^^^^^^^^^^^^
@@ -170,7 +196,10 @@ int main(int argc, char** argv)
   display_trajectory.trajectory.push_back(response.trajectory);
   // Now you should see two planned trajectories in series
   display_publisher.publish(display_trajectory);
-  sleep_time.sleep();
+
+  /* Wait for user input */
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
+
 
   // Using a Planning Request Adapter
   // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -207,7 +236,9 @@ int main(int argc, char** argv)
   /* Now you should see three planned trajectories in series*/
   display_publisher.publish(display_trajectory);
 
-  sleep_time.sleep();
+  /* Wait for user input */
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo");
+
   ROS_INFO("Done");
   return 0;
 }

doc/move_group_interface/move_group_interface_tutorial.rst

@@ -22,7 +22,9 @@ In the second shell, run the launch file: ::
 
   roslaunch moveit_tutorials move_group_interface_tutorial.launch
 
-After a short moment, the RViz window should appear and look similar to the one at the top of this page. To progress through each demo step either press the **Next** button in the **RvizVisualToolsGui** pannel at the bottom of the screen or select **Key Tool** in the **Tools** pannel at the top of the screen and then press **N** on your keyboard while RViz is focused.
+**Note:** This tutorial uses the **RvizVisualToolsGui** panel to step through the demo. To add this panel to RViz, follow the instructions in the `Visualization Tutorial <../visualization/visualization_tutorial.html#rviz-visual-tools>`_.
+
+After a short moment, the RViz window should appear and look similar to the one at the top of this page. To progress through each demo step either press the **Next** button in the **RvizVisualToolsGui** panel at the bottom of the screen or select **Key Tool** in the **Tools** panel at the top of the screen and then press **N** on your keyboard while RViz is focused.
 
 Expected Output
 ---------------

doc/move_group_interface/src/move_group_interface_tutorial.cpp

@@ -136,7 +136,7 @@ int main(int argc, char** argv)
   visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
   visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
   visual_tools.trigger();
-  visual_tools.prompt("next step");
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
 
   // Moving to a pose goal
   // ^^^^^^^^^^^^^^^^^^^^^
@@ -178,7 +178,7 @@ int main(int argc, char** argv)
   visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
   visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
   visual_tools.trigger();
-  visual_tools.prompt("next step");
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
 
   // Planning with Path Constraints
   // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -281,7 +281,7 @@ int main(int argc, char** argv)
   for (std::size_t i = 0; i < waypoints.size(); ++i)
     visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
   visual_tools.trigger();
-  visual_tools.prompt("next step");
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
 
   // Adding/Removing Objects and Attaching/Detaching Objects
   // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -323,8 +323,8 @@ int main(int argc, char** argv)
   visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
   visual_tools.trigger();
 
-  // Sleep to allow MoveGroup to recieve and process the collision object message
-  ros::Duration(1.0).sleep();
+  // Wait for MoveGroup to recieve and process the collision object message
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
 
   // Now when we plan a trajectory it will avoid the obstacle
   move_group.setStartState(*move_group.getCurrentState());
@@ -353,8 +353,8 @@ int main(int argc, char** argv)
   visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE);
   visual_tools.trigger();
 
-  /* Sleep to allow MoveGroup to recieve and process the attached collision object message */
-  ros::Duration(1.0).sleep();
+  /* Wait for MoveGroup to recieve and process the attached collision object message */
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object attaches to the robot");
 
   // Now, let's detach the collision object from the robot.
   ROS_INFO_NAMED("tutorial", "Detach the object from the robot");
@@ -364,8 +364,8 @@ int main(int argc, char** argv)
   visual_tools.publishText(text_pose, "Object dettached from robot", rvt::WHITE, rvt::XLARGE);
   visual_tools.trigger();
 
-  /* Sleep to allow MoveGroup to recieve and process the detach collision object message */
-  ros::Duration(1.0).sleep();
+  /* Wait for MoveGroup to recieve and process the attached collision object message */
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object detaches to the robot");
 
   // Now, let's remove the collision object from the world.
   ROS_INFO_NAMED("tutorial", "Remove the object from the world");
@@ -377,8 +377,8 @@ int main(int argc, char** argv)
   visual_tools.publishText(text_pose, "Object removed", rvt::WHITE, rvt::XLARGE);
   visual_tools.trigger();
 
-  /* Sleep to give RViz time to show the object is no longer there.*/
-  ros::Duration(1.0).sleep();
+  /* Wait for MoveGroup to recieve and process the attached collision object message */
+  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears");
 
   // END_TUTORIAL
 

doc/planning_scene_ros_api/planning_scene_ros_api_tutorial.rst

@@ -21,6 +21,10 @@ In the second shell, run the launch file for this demo: ::
 
  roslaunch moveit_tutorials planning_scene_ros_api_tutorial.launch
 
+**Note:** This tutorial uses the **RvizVisualToolsGui** panel to step through the demo. To add this panel to RViz, follow the instructions in the `Visualization Tutorial <../visualization/visualization_tutorial.html#rviz-visual-tools>`_.
+
+After a short moment, the RViz window should appear and look similar to the one at the top of this page. To progress through each demo step either press the **Next** button in the **RvizVisualToolsGui** panel at the bottom of the screen or select **Key Tool** in the **Tools** panel at the top of the screen and then press **N** on your keyboard while RViz is focused.
+
 Expected Output
 ---------------
 In RViz, you should be able to see the following: