Add motion planning pipeline tutorial

CMakeLists.txt

@@ -54,7 +54,7 @@ include_directories(${THIS_PACKAGE_INCLUDE_DIRS})
 # add_subdirectory(doc/interactivity)
 # add_subdirectory(doc/kinematics)
 # add_subdirectory(doc/motion_planning_api)
-# add_subdirectory(doc/motion_planning_pipeline)
+add_subdirectory(doc/motion_planning_pipeline)
 add_subdirectory(doc/move_group_interface)
 # add_subdirectory(doc/move_group_python_interface)
 # add_subdirectory(doc/perception_pipeline)

doc/motion_planning_pipeline/CMakeLists.txt

@@ -1,8 +1,13 @@
 add_executable(motion_planning_pipeline_tutorial
     src/motion_planning_pipeline_tutorial.cpp)
-target_link_libraries(motion_planning_pipeline_tutorial
-    ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-install(TARGETS motion_planning_pipeline_tutorial DESTINATION
-    ${CATKIN_PACKAGE_BIN_DESTINATION})
+target_include_directories(motion_planning_pipeline_tutorial
+    PUBLIC include)
+ament_target_dependencies(motion_planning_pipeline_tutorial
+    ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
 
-install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(TARGETS motion_planning_pipeline_tutorial
+    DESTINATION lib/${PROJECT_NAME}
+)
+install(DIRECTORY launch
+    DESTINATION share/${PROJECT_NAME}
+)

doc/motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch.py

@@ -0,0 +1,66 @@
+import os
+import yaml
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+
+def load_file(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, "r") as file:
+            return file.read()
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def load_yaml(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, "r") as file:
+            return yaml.safe_load(file)
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def generate_launch_description():
+    # planning_context
+    robot_description_config = load_file(
+        "moveit_resources_panda_description", "urdf/panda.urdf"
+    )
+    robot_description = {"robot_description": robot_description_config}
+
+    robot_description_semantic_config = load_file(
+        "moveit_resources_panda_moveit_config", "config/panda.srdf"
+    )
+    robot_description_semantic = {
+        "robot_description_semantic": robot_description_semantic_config
+    }
+
+    kinematics_yaml = load_yaml(
+        "moveit_resources_panda_moveit_config", "config/kinematics.yaml"
+    )
+
+    motion_planning_pipeline_tutorial = Node(
+        name="motion_planning_pipeline_tutorial",
+        package="moveit2_tutorials",
+        executable="motion_planning_pipeline_tutorial",
+        prefix="xterm -e",
+        output="screen",
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            kinematics_yaml,
+            {"planning_plugin": "ompl_interface/OMPLPlanner"},
+            {
+                "planning_adapters": "default_planner_request_adapters/AddTimeParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints"
+            },
+            {"start_state_max_bounds_error": 0.1},
+        ],
+    )
+
+    return LaunchDescription([motion_planning_pipeline_tutorial])

doc/motion_planning_pipeline/motion_planning_pipeline_tutorial.rst

@@ -1,8 +1,3 @@
-:moveit1:
-
-..
-   Once updated for MoveIt 2, remove all lines above title (including this comment and :moveit1: tag)
-
 Motion Planning Pipeline
 ==================================
 
@@ -22,15 +17,17 @@ Running the Code
 ----------------
 Open two shells. In the first shell start RViz and wait for everything to finish loading: ::
 
-  roslaunch panda_moveit_config demo.launch
+  ros2 launch moveit_resources_panda_moveit_config demo.launch.py
 
 In the second shell, run the launch file: ::
 
- roslaunch moveit_tutorials motion_planning_pipeline_tutorial.launch
+  ros2 launch moveit2_tutorials motion_planning_pipeline_tutorial.launch.py
+
+**Note:** Because **MoveitVisualTools** has not been ported to ROS2 this tutoral has made use of xterm and a simple prompter to help the user progress through each demo step.
+To install xterm please run the following command: ::
 
-**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 <../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#rviz-visual-tools>`_.
+  sudo apt-get install -y xterm
 
-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
 ---------------
@@ -48,4 +45,4 @@ The entire code can be seen :codedir:`here in the MoveIt GitHub project<motion_p
 
 The Launch File
 ---------------
-The entire launch file is :codedir:`here <motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch>` on GitHub. All the code in this tutorial can be compiled and run from the moveit_tutorials package that you have as part of your MoveIt setup.
+The entire launch file is :codedir:`here <motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch.py>` on GitHub. All the code in this tutorial can be compiled and run from the moveit_tutorials package that you have as part of your MoveIt setup.

doc/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp

@@ -34,8 +34,8 @@
 
 /* Author: Sachin Chitta, Mike Lautman*/
 
-#include <pluginlib/class_loader.h>
-#include <ros/ros.h>
+#include <pluginlib/class_loader.hpp>
+#include <rclcpp/rclcpp.hpp>
 
 // MoveIt
 #include <moveit/robot_model_loader/robot_model_loader.h>
@@ -44,16 +44,40 @@
 #include <moveit/planning_interface/planning_interface.h>
 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 #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 <moveit_msgs/msg/display_trajectory.hpp>
+#include <moveit_msgs/msg/planning_scene.hpp>
+/* #include <moveit_visual_tools/moveit_visual_tools.h>  This has not been ported to ros2 yet */
+#include <rviz_visual_tools/rviz_visual_tools.hpp>
+/* this is a standin for moveit_visual_tools visual_tools.prompt */
+#include <moveit/macros/console_colors.h>
+
+void prompt(const std::string& message)
+{
+  printf(MOVEIT_CONSOLE_COLOR_GREEN "\n%s" MOVEIT_CONSOLE_COLOR_RESET, message.c_str());
+  fflush(stdout);
+  while (std::cin.get() != '\n' && rclcpp::ok())
+    ;
+}
+
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("motion_planning_pipeline_tutorial");
 
 int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "move_group_tutorial");
-  ros::AsyncSpinner spinner(1);
-  spinner.start();
-  ros::NodeHandle node_handle("~");
+  rclcpp::init(argc, argv);
+  rclcpp::NodeOptions node_options;
+  RCLCPP_INFO(LOGGER, "Initialize node");
+
+  // This enables loading undeclared parameters
+  // best practice would be to declare parameters in the corresponding classes
+  // and provide descriptions about expected use
+  node_options.automatically_declare_parameters_from_overrides(true);
+  rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("motion_planning_pipeline", "", node_options);
+
+  // We spin up a SingleThreadedExecutor for the current state monitor to get information
+  // about the robot's state.
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node);
+  std::thread([&executor]() { executor.spin(); }).detach();
 
   // BEGIN_TUTORIAL
   // Start
@@ -67,14 +91,14 @@ int main(int argc, char** argv)
   // .. _RobotModelLoader:
   //     http://docs.ros.org/noetic/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html
   robot_model_loader::RobotModelLoaderPtr robot_model_loader(
-      new robot_model_loader::RobotModelLoader("robot_description"));
+      new robot_model_loader::RobotModelLoader(node, "robot_description"));
 
   // Using the RobotModelLoader, we can construct a planing scene monitor that
   // will create a planning scene, monitors planning scene diffs, and apply the diffs to it's
   // internal planning scene. We then call startSceneMonitor, startWorldGeometryMonitor and
   // startStateMonitor to fully initialize the planning scene monitor
   planning_scene_monitor::PlanningSceneMonitorPtr psm(
-      new planning_scene_monitor::PlanningSceneMonitor(robot_model_loader));
+      new planning_scene_monitor::PlanningSceneMonitor(node, robot_model_loader));
 
   /* listen for planning scene messages on topic /XXX and apply them to the internal planning scene
                        the internal planning scene accordingly */
@@ -102,19 +126,20 @@ int main(int argc, char** argv)
   // We can now setup the PlanningPipeline object, which will use the ROS parameter server
   // to determine the set of request adapters and the planning plugin to use
   planning_pipeline::PlanningPipelinePtr planning_pipeline(
-      new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters"));
+      new planning_pipeline::PlanningPipeline(robot_model, node, "planning_plugin", "request_adapters"));
 
   // Visualization
   // ^^^^^^^^^^^^^
   // The package MoveItVisualTools provides many capabilities 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");
+  /* moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); */
+  rviz_visual_tools::RvizVisualTools visual_tools("panda_link0", "moveit_cpp_tutorial", node);
   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();
+  // visual_tools.loadRemoteControl();
 
   /* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/
   Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
@@ -125,15 +150,16 @@ int main(int argc, char** argv)
   visual_tools.trigger();
 
   /* We can also use visual_tools to wait for user input */
-  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
+  prompt("Press 'Enter' to start the demo");
+  /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); */
 
   // Pose Goal
   // ^^^^^^^^^
   // We will now create a motion plan request for the right arm of the Panda
   // specifying the desired pose of the end-effector as input.
   planning_interface::MotionPlanRequest req;
   planning_interface::MotionPlanResponse res;
-  geometry_msgs::PoseStamped pose;
+  geometry_msgs::msg::PoseStamped pose;
   pose.header.frame_id = "panda_link0";
   pose.pose.position.x = 0.3;
   pose.pose.position.y = 0.0;
@@ -153,7 +179,7 @@ int main(int argc, char** argv)
   // .. _kinematic_constraints:
   //     http://docs.ros.org/noetic/api/moveit_core/html/cpp/namespacekinematic__constraints.html#a88becba14be9ced36fefc7980271e132
   req.group_name = "panda_arm";
-  moveit_msgs::Constraints pose_goal =
+  moveit_msgs::msg::Constraints pose_goal =
       kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);
   req.goal_constraints.push_back(pose_goal);
 
@@ -168,29 +194,29 @@ int main(int argc, char** argv)
   /* Check that the planning was successful */
   if (res.error_code_.val != res.error_code_.SUCCESS)
   {
-    ROS_ERROR("Could not compute plan successfully");
+    RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
     return 0;
   }
 
   // Visualize the result
   // ^^^^^^^^^^^^^^^^^^^^
-  ros::Publisher display_publisher =
-      node_handle.advertise<moveit_msgs::DisplayTrajectory>("/display_planned_path", 1, true);
-  moveit_msgs::DisplayTrajectory display_trajectory;
+  auto display_publisher = node->create_publisher<moveit_msgs::msg::DisplayTrajectory>("/display_planned_path", 1);
+  moveit_msgs::msg::DisplayTrajectory display_trajectory;
 
   /* Visualize the trajectory */
-  ROS_INFO("Visualizing the trajectory");
-  moveit_msgs::MotionPlanResponse response;
+  RCLCPP_INFO(LOGGER, "Visualizing the trajectory");
+  moveit_msgs::msg::MotionPlanResponse response;
   res.getMessage(response);
 
   display_trajectory.trajectory_start = response.trajectory_start;
   display_trajectory.trajectory.push_back(response.trajectory);
-  display_publisher.publish(display_trajectory);
-  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
+  display_publisher->publish(display_trajectory);
+  // visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
   visual_tools.trigger();
 
   /* Wait for user input */
-  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
+  prompt("Press 'Enter' to continue the demo");
+  /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */
 
   // Joint Space Goals
   // ^^^^^^^^^^^^^^^^^
@@ -203,7 +229,8 @@ int main(int argc, char** argv)
   moveit::core::RobotState goal_state(*robot_state);
   std::vector<double> joint_values = { -1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0 };
   goal_state.setJointGroupPositions(joint_model_group, joint_values);
-  moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
+  moveit_msgs::msg::Constraints joint_goal =
+      kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
 
   req.goal_constraints.clear();
   req.goal_constraints.push_back(joint_goal);
@@ -218,21 +245,22 @@ int main(int argc, char** argv)
   /* Check that the planning was successful */
   if (res.error_code_.val != res.error_code_.SUCCESS)
   {
-    ROS_ERROR("Could not compute plan successfully");
+    RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
     return 0;
   }
   /* Visualize the trajectory */
-  ROS_INFO("Visualizing the trajectory");
+  RCLCPP_INFO(LOGGER, "Visualizing the trajectory");
   res.getMessage(response);
   display_trajectory.trajectory_start = response.trajectory_start;
   display_trajectory.trajectory.push_back(response.trajectory);
   // Now you should see two planned trajectories in series
-  display_publisher.publish(display_trajectory);
-  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
+  display_publisher->publish(display_trajectory);
+  // visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
   visual_tools.trigger();
 
   /* Wait for user input */
-  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
+  prompt("Press 'Enter' to continue the demo");
+  /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */
 
   // Using a Planning Request Adapter
   // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -264,22 +292,24 @@ int main(int argc, char** argv)
   }
   if (res.error_code_.val != res.error_code_.SUCCESS)
   {
-    ROS_ERROR("Could not compute plan successfully");
+    RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
     return 0;
   }
   /* Visualize the trajectory */
-  ROS_INFO("Visualizing the trajectory");
+  RCLCPP_INFO(LOGGER, "Visualizing the trajectory");
   res.getMessage(response);
   display_trajectory.trajectory_start = response.trajectory_start;
   display_trajectory.trajectory.push_back(response.trajectory);
   /* Now you should see three planned trajectories in series*/
-  display_publisher.publish(display_trajectory);
-  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
+  display_publisher->publish(display_trajectory);
+  // visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
   visual_tools.trigger();
 
   /* Wait for user input */
-  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo");
+  prompt("Press 'Enter' to finish the demo");
+  /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo"); */
 
-  ROS_INFO("Done");
+  RCLCPP_INFO(LOGGER, "Done");
+  rclcpp::shutdown();
   return 0;
 }

doc/planning_scene_ros_api/planning_scene_ros_api_tutorial.rst

@@ -20,7 +20,7 @@ Running the code
 ----------------
 Open two shells. In the first shell start RViz and wait for everything to finish loading: ::
 
-  ros2 launch moveit2_tutorials demo.launch.py
+  ros2 launch moveit_resources_panda_moveit_config demo.launch.py
 
 In the second shell, run the launch file for this demo: ::