Address review and cleanup

doc/how_to_guides/parallel_planning/config/parallel_planning_config.rviz

@@ -9,7 +9,7 @@ Panels:
         - /PlanningScene1
         - /Trajectory1
       Splitter Ratio: 0.5
-    Tree Height: 679
+    Tree Height: 671
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -157,6 +157,8 @@ Visualization Manager:
       Enabled: true
       Name: MarkerArray
       Namespaces:
+        Path: true
+        Sphere: true
         Text: true
       Topic:
         Depth: 5
@@ -411,25 +413,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 5.1277079582214355
+      Distance: 3.8018128871917725
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.2651444971561432
-        Y: 0.5764509439468384
-        Z: 0.707146406173706
+        X: 0.4451259672641754
+        Y: -0.10787936300039291
+        Z: 0.5332368612289429
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.27039825916290283
+      Pitch: 0.16039825975894928
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 6.033580303192139
+      Yaw: 4.0435791015625
     Saved: ~
 Window Geometry:
   Displays:
@@ -441,7 +443,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   RvizVisualToolsGui:
     collapsed: false
   Selection:

doc/how_to_guides/parallel_planning/config/parallel_planning_moveit_cpp.yaml

@@ -1,3 +1,4 @@
+# Default configurations. More information can be found in the moveit_cpp and planning scene monitor tutorials
 planning_scene_monitor_options:
   name: "planning_scene_monitor"
   robot_description: "robot_description"
@@ -7,18 +8,22 @@ planning_scene_monitor_options:
   monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
   wait_for_initial_state_timeout: 10.0
 
+# Define which planning pipelines are loaded by moveit_cpp. These loaded pipelines will be available for simple and parallel planning
 planning_pipelines:
-  #namespace: "moveit_cpp"  # optional, default is ~
   pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp"]
 
-ompl_rrtc:
-  plan_request_params:
-    planning_attempts: 1
-    planning_pipeline: ompl
-    planner_id: "RRTConnectkConfigDefault"
-    max_velocity_scaling_factor: 1.0
-    max_acceleration_scaling_factor: 1.0
-    planning_time: 0.5
+# 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{
+#         node, { "ompl_rrtc", "pilz_lin" }
+#       };
+ompl_rrtc:  # Namespace for individual plan request
+  plan_request_params:  # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp
+    planning_attempts: 1  # Number of attempts the planning pipeline tries to solve a given motion planning problem
+    planning_pipeline: ompl  # Name of the pipeline that is being used
+    planner_id: "RRTConnectkConfigDefault"  # Name of the specific planner to be used by the pipeline
+    max_velocity_scaling_factor: 1.0  # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
+    max_acceleration_scaling_factor: 1.0  # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
+    planning_time: 0.5  # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned
 
 pilz_lin:
   plan_request_params:

doc/how_to_guides/parallel_planning/parallel_planning_tutorial.rst

@@ -1,5 +1,5 @@
-How to run multiple planning pipelines in parallel with MoveItCpp?
-==================================================================
+Running Multiple Planning Pipelines in Parallel with MoveItCpp
+==============================================================
 
 MoveItCpp offers an API that allows:
 
@@ -9,7 +9,7 @@ MoveItCpp offers an API that allows:
 
 Using multiple pipelines can be beneficial for several reasons, including:
 
-- It is unsure which planner will produce the best solution for a given planning problem
+- The planner that will produce the best solution is not known a priori
 - There is a chance that the preferred planner might fail and a backup solution should be available
 
 A general introduction to moveit_cpp can be found in the :doc:`/doc/examples/moveit_cpp/moveitcpp_tutorial`.
@@ -27,25 +27,27 @@ of the planning component's :code:`plan(...)` function is used:
       SolutionCallbackFunction solution_selection_callback,
       StoppingCriterionFunction stopping_criterion_callback)
 
-This function tries to plan a trajectory from the a start state, to a goal state that satisfy a set of constraints. Based on the configuration
+This function tries to plan a trajectory from a start state to a goal state that satisfies a set of constraints. Based on the configuration
 provided by the :code:`parameters`, multiple threads are launched and each tries to solve the planning problem with a different planning pipeline. Once
-all pipelines found a solution (Reminder: No solution is also a possible result), the :code:`solution_selection_callback` is called to determine which
+all pipelines have been terminated (Reminder: No solution is also a possible result), the :code:`solution_selection_callback` is called to determine which
 solution is returned as :code:`MotionPlanResponse`. By default, all pipelines use their time budget defined by the :code:`planning_time` field of the :code:`PlanRequestParameters`, but it is possible to terminate the parallel planning earlier by using the :code:`stopping_criterion_callback`. This function
 is iteratively called during the parallel planning process and, if the stopping criterion is met, terminates pipelines that have not found a solution yet.
 
-Demo
-----
+Example
+-------
 
 The following demo serves as an example of how you can configure and use MoveItCpp's parallel planning interface. First of all, let's
 run the demo: ::
 
   ros2 launch moveit2_tutorials parallel_planning_example.launch.py
 
-A complex kitchen scene is loaded and two planning problems solved. The first one is a small motion of the EE towards the ground. This problem is likely to be solved by all three
+A complex kitchen scene is loaded and two planning problems are solved. The first one is a small motion of the end effector towards the ground. This problem is likely to be solved by all three
 planners, but with significant differences in the planning time. The second problem is a much harder and most likely only the :code:`RRTConnect` planner will succeed. This demo
 suggests that a well-configured parallel planning setup is versatile, and can be used in a broad variety of motion planning problems.
 
 What code is necessary to use parallel planning?
+------------------------------------------------
+
 First, you need to initialize :code:`moveit_cpp` and a planning component that will solve your planning problems. Next, you need to set start state and goal constraints:
 
 .. code-block:: c++
@@ -159,4 +161,4 @@ Once :code:`MultiPipelinePlanRequestParameters` and optionally :code:`SolutionCa
 Tips
 ----
 
-- When you want to use multiple pipelines with the same planner parallel it is recommended to initialize multiple planning pipelines in moveit_cpp rather than using the same one in multiple parallel planning requests
+- When you want to use the same pipeline with the different planners (e.g. PILZ planner with PTP and LIN) in parallel, it is recommended to initialize multiple planning pipelines in moveit_cpp rather than using the same one in multiple parallel planning requests

doc/how_to_guides/parallel_planning/src/parallel_planning_main.cpp

@@ -86,8 +86,6 @@ class Demo
     text_pose.translation().z() = 1.75;
     visual_tools_.publishText(text_pose, "Parallel Planning Tutorial", rvt::WHITE, rvt::XLARGE);
     visual_tools_.trigger();
-
-    visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
   }
 
   bool loadPlanningSceneAndQuery()
@@ -184,6 +182,8 @@ class Demo
     }
 
     planning_query_request_ = static_cast<moveit_msgs::msg::MotionPlanRequest>(*planning_query);
+    visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
+    visual_tools_.trigger();
     return true;
   }
 
@@ -196,11 +196,11 @@ class Demo
     auto robot_goal_state = planning_component_->getStartState();
     robot_goal_state->setJointPositions("panda_joint1", &panda_joint1);
     robot_goal_state->setJointPositions("panda_joint2", &panda_joint2);
+    robot_goal_state->setJointPositions("panda_joint3", &panda_joint3);
     robot_goal_state->setJointPositions("panda_joint4", &panda_joint4);
     robot_goal_state->setJointPositions("panda_joint5", &panda_joint5);
     robot_goal_state->setJointPositions("panda_joint6", &panda_joint6);
     robot_goal_state->setJointPositions("panda_joint7", &panda_joint7);
-    robot_goal_state->setJointPositions("panda_joint3", &panda_joint3);
 
     // Set goal state
     planning_component_->setGoal(*robot_goal_state);
@@ -224,6 +224,8 @@ class Demo
     // Set start state as current state
     planning_component_->setStartStateToCurrentState();
 
+    // 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" }
     };
@@ -274,9 +276,6 @@ int main(int argc, char** argv)
 
   parallel_planning_example::Demo demo(node);
 
-  /* Otherwise robot with zeros joint_states */
-  rclcpp::sleep_for(std::chrono::seconds(1));
-
   if (!demo.loadPlanningSceneAndQuery())
   {
     rclcpp::shutdown();
@@ -285,15 +284,13 @@ int main(int argc, char** argv)
 
   RCLCPP_INFO(LOGGER, "Starting MoveIt Tutorials...");
 
-  // // Experiment 1 - Short free-space motion
-  // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+  // Experiment 1 - Short free-space motion, PILZ is expected to create the fastest and shortest solution
   RCLCPP_INFO(LOGGER, "Experiment 1 - Short free-space motion");
 
   demo.setJointGoal(0.0, -0.8144019900299497, 0.0, -2.6488387075338133, 0.0, 1.8344367175038623, 0.7849999829891612);
   demo.planAndPrint();
 
-  // // Experiment 2 - Long motion with collisions
-  // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+  // Experiment 2 - Long motion with collisions, CHOMP and PILZ are likely to fail here due to the difficulty of the planning problem
   RCLCPP_INFO(LOGGER, "Experiment 2 - Long motion with collisions");
   demo.setQueryGoal();
   demo.planAndPrint();