Update tutorials for changes on moveit2 main (#785)

Author: sjahr

doc/examples/motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch.py

@@ -3,6 +3,7 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
 from ament_index_python.packages import get_package_share_directory
+from moveit_configs_utils import MoveItConfigsBuilder
 
 
 def load_file(package_name, file_path):
@@ -28,41 +29,29 @@ def load_yaml(package_name, file_path):
 
 
 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"
+    moveit_config = (
+        MoveItConfigsBuilder("moveit_resources_panda")
+        .robot_description(file_path="config/panda.urdf.xacro")
+        .planning_scene_monitor(
+            publish_robot_description=True, publish_robot_description_semantic=True
+        )
+        .planning_pipelines("ompl", ["ompl"])
+        .to_moveit_configs()
     )
 
-    # Planning Functionality
-    ompl_planning_pipeline_config = {
-        "planning_plugin": "ompl_interface/OMPLPlanner",
-        "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization 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,
-    }
-
     # MotionPlanningPipeline demo executable
     motion_planning_pipeline_demo = Node(
         name="motion_planning_pipeline_tutorial",
         package="moveit2_tutorials",
         executable="motion_planning_pipeline_tutorial",
         output="screen",
         parameters=[
-            robot_description,
-            robot_description_semantic,
-            kinematics_yaml,
-            ompl_planning_pipeline_config,
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.robot_description_kinematics,
+            moveit_config.planning_pipelines,
+            moveit_config.joint_limits,
         ],
     )
 

doc/examples/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp

@@ -108,7 +108,7 @@ 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, "", "planning_plugin", "request_adapters"));
+      new planning_pipeline::PlanningPipeline(robot_model, node, "ompl"));
 
   // Visualization
   // ^^^^^^^^^^^^^
@@ -138,6 +138,9 @@ int main(int argc, char** argv)
   // 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;
+  req.pipeline_id = "ompl";
+  req.planner_id = "RRTConnectkConfigDefault";
+  req.allowed_planning_time = 1.0;
   planning_interface::MotionPlanResponse res;
   geometry_msgs::msg::PoseStamped pose;
   pose.header.frame_id = "panda_link0";
@@ -148,8 +151,8 @@ int main(int argc, char** argv)
 
   // A tolerance of 0.01 m is specified in position
   // and 0.01 radians in orientation
-  std::vector<double> tolerance_pose(3, 0.01);
-  std::vector<double> tolerance_angle(3, 0.01);
+  std::vector<double> tolerance_pose(3, 0.1);
+  std::vector<double> tolerance_angle(3, 0.1);
 
   // We will create the request as a constraint using a helper
   // function available from the
@@ -165,16 +168,14 @@ int main(int argc, char** argv)
   {
     planning_scene_monitor::LockedPlanningSceneRO lscene(psm);
     /* Now, call the pipeline and check whether planning was successful. */
-    planning_pipeline->generatePlan(lscene, req, res);
+    /* Check that the planning was successful */
+    if (!planning_pipeline->generatePlan(lscene, req, res) || res.error_code.val != res.error_code.SUCCESS)
+    {
+      RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
+      rclcpp::shutdown();
+      return -1;
+    }
   }
-  /* Now, call the pipeline and check whether planning was successful. */
-  /* Check that the planning was successful */
-  if (res.error_code.val != res.error_code.SUCCESS)
-  {
-    RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
-    return 0;
-  }
-
   // Visualize the result
   // ^^^^^^^^^^^^^^^^^^^^
   rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>::SharedPtr display_publisher =
@@ -217,13 +218,12 @@ int main(int argc, char** argv)
   {
     planning_scene_monitor::LockedPlanningSceneRO lscene(psm);
     /* Now, call the pipeline and check whether planning was successful. */
-    planning_pipeline->generatePlan(lscene, req, res);
-  }
-  /* Check that the planning was successful */
-  if (res.error_code.val != res.error_code.SUCCESS)
-  {
-    RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
-    return 0;
+    if (!planning_pipeline->generatePlan(lscene, req, res) || res.error_code.val != res.error_code.SUCCESS)
+    {
+      RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
+      rclcpp::shutdown();
+      return -1;
+    }
   }
   /* Visualize the trajectory */
   RCLCPP_INFO(LOGGER, "Visualizing the trajectory");
@@ -264,12 +264,12 @@ int main(int argc, char** argv)
   {
     planning_scene_monitor::LockedPlanningSceneRO lscene(psm);
     /* Now, call the pipeline and check whether planning was successful. */
-    planning_pipeline->generatePlan(lscene, req, res);
-  }
-  if (res.error_code.val != res.error_code.SUCCESS)
-  {
-    RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
-    return 0;
+    if (!planning_pipeline->generatePlan(lscene, req, res) || res.error_code.val != res.error_code.SUCCESS)
+    {
+      RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
+      rclcpp::shutdown();
+      return -1;
+    }
   }
   /* Visualize the trajectory */
   RCLCPP_INFO(LOGGER, "Visualizing the trajectory");

doc/how_to_guides/parallel_planning/config/parallel_planning_moveit_cpp.yaml

@@ -10,7 +10,7 @@ planning_scene_monitor_options:
 
 # Define which planning pipelines are loaded by moveit_cpp. These loaded pipelines will be available for simple and parallel planning
 planning_pipelines:
-  pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp", "ompl_rrt_star"]
+  pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp", "ompl_2"]
 
 # Configure parameters of a multi-pipeline plan request. The actual request is created in the cpp code when calling for example:
 #   MultiPipelinePlanRequestParameters multi_pipeline_plan_request{
@@ -34,10 +34,11 @@ pilz_lin:
     max_acceleration_scaling_factor: 1.0
     planning_time: 0.8
 
-chomp:
+chomp_planner:
   plan_request_params:
     planning_attempts: 1
     planning_pipeline: chomp
+    pipeline_id: chomp
     max_velocity_scaling_factor: 1.0
     max_acceleration_scaling_factor: 1.0
     planning_time: 1.5
@@ -46,7 +47,7 @@ chomp:
 ompl_rrt_star:
   plan_request_params:
     planning_attempts: 1
-    planning_pipeline: ompl_rrt_star # Different OMPL pipeline name!
+    planning_pipeline: ompl_2 # Different OMPL pipeline name!
     planner_id: "RRTstarkConfigDefault"
     max_velocity_scaling_factor: 1.0
     max_acceleration_scaling_factor: 1.0

doc/how_to_guides/parallel_planning/launch/parallel_planning_example.launch.py

@@ -56,7 +56,7 @@ def launch_setup(context, *args, **kwargs):
 
     # Load additional OMPL pipeline
     ompl_planning_pipeline_config = {
-        "ompl_rrt_star": {
+        "ompl_2": {
             "planning_plugin": "ompl_interface/OMPLPlanner",
             "request_adapters": """\
                 default_planner_request_adapters/AddTimeOptimalParameterization \
@@ -71,7 +71,7 @@ def launch_setup(context, *args, **kwargs):
     ompl_planning_yaml = load_yaml(
         "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml"
     )
-    ompl_planning_pipeline_config["ompl_rrt_star"].update(ompl_planning_yaml)
+    ompl_planning_pipeline_config["ompl_2"].update(ompl_planning_yaml)
 
     # Warehouse config
     sqlite_database = os.path.join(
@@ -96,7 +96,11 @@ def launch_setup(context, *args, **kwargs):
         package="moveit2_tutorials",
         executable="parallel_planning_example",
         output="screen",
-        parameters=[moveit_config.to_dict(), warehouse_ros_config],
+        parameters=[
+            moveit_config.to_dict(),
+            warehouse_ros_config,
+            ompl_planning_pipeline_config,
+        ],
     )
 
     # RViz

doc/how_to_guides/parallel_planning/src/parallel_planning_main.cpp

@@ -225,7 +225,7 @@ class Demo
     // The MultiPipelinePlanRequestParameters choose a set of planning pipelines to be used for parallel planning. Here,
     // we use all available pipelines but it is also possible to use a subset of pipelines.
     moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters multi_pipeline_plan_request{
-      node_, { "ompl_rrtc", "pilz_lin", "chomp", "ompl_rrt_star" }
+      node_, { "ompl_rrtc", "pilz_lin", "chomp_planner", "ompl_rrt_star" }
     };
 
     auto plan_solution = planning_component_->plan(multi_pipeline_plan_request, &getShortestSolution);
@@ -293,7 +293,6 @@ int main(int argc, char** argv)
   RCLCPP_INFO(LOGGER, "Experiment 2 - Long motion with collisions");
   demo.setQueryGoal();
   demo.planAndPrint();
-
   rclcpp::shutdown();
   return 0;
 }

doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_mtc.cpp

@@ -101,17 +101,17 @@ mtc::Task MTCTaskNode::createTask()
 
   // Set up 3 separate pilz planners with different IDs
   auto pilz_ptp_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_, "pilz_industrial_motion_planner");
-  pilz_ptp_planner->setPlannerId("PTP");
+  pilz_ptp_planner->setPlannerId("pilz_industrial_motion_planner", "PTP");
   pilz_ptp_planner->setProperty("max_velocity_scaling_factor", 0.5);
   pilz_ptp_planner->setProperty("max_acceleration_scaling_factor", 0.5);
 
   auto pilz_lin_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_, "pilz_industrial_motion_planner");
-  pilz_lin_planner->setPlannerId("LIN");
+  pilz_lin_planner->setPlannerId("pilz_industrial_motion_planner", "LIN");
   pilz_lin_planner->setProperty("max_velocity_scaling_factor", 0.2);
   pilz_lin_planner->setProperty("max_acceleration_scaling_factor", 0.2);
 
   auto pilz_circ_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_, "pilz_industrial_motion_planner");
-  pilz_circ_planner->setPlannerId("CIRC");
+  pilz_circ_planner->setPlannerId("pilz_industrial_motion_planner", "CIRC");
   pilz_circ_planner->setProperty("max_velocity_scaling_factor", 0.3);
   pilz_circ_planner->setProperty("max_acceleration_scaling_factor", 0.3);