Refactor local planner plugins

moveit_ros/hybrid_planning/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(moveit_msgs REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_core REQUIRED)
 find_package(std_srvs REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(trajectory_msgs REQUIRED)
 find_package(control_toolbox REQUIRED)
@@ -26,9 +27,8 @@ set(LIBRARIES
   single_plan_execution_plugin
   trajectory_operator_interface
   local_constraint_solver_interface
-  next_waypoint_sampler_plugin
+  simple_sampler_plugin
   single_plan_execution_plugin
-  decelerate_before_collision_plugin
   replan_invalidated_trajectory_plugin
   forward_trajectory_plugin
 )
@@ -40,6 +40,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   moveit_core
   moveit_ros_planning
   moveit_msgs
+  std_msgs
   std_srvs
   tf2_ros
   trajectory_msgs
@@ -76,8 +77,7 @@ install(DIRECTORY test/launch DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME})
 
 pluginlib_export_plugin_description_file(moveit_hybrid_planning single_plan_execution_plugin.xml)
-pluginlib_export_plugin_description_file(moveit_hybrid_planning next_waypoint_sampler_plugin.xml)
-pluginlib_export_plugin_description_file(moveit_hybrid_planning decelerate_before_collision_plugin.xml)
+pluginlib_export_plugin_description_file(moveit_hybrid_planning simple_sampler_plugin.xml)
 pluginlib_export_plugin_description_file(moveit_hybrid_planning replan_invalidated_trajectory_plugin.xml)
 pluginlib_export_plugin_description_file(moveit_hybrid_planning forward_trajectory_plugin.xml)
 

moveit_ros/hybrid_planning/README.md

@@ -1,29 +1,23 @@
 # Hybrid Planning
-Implementation of MoveIt 2's new hybrid planning architecture. You find more information in the [project's issue](https://github.com/ros-planning/moveit2/issues/300) and on the [MoveIt 2 roadmap](https://moveit.ros.org/documentation/contributing/roadmap/).
+Implementation of MoveIt 2's new hybrid planning architecture. You find more information in the project's issues[#300](https://github.com/ros-planning/moveit2/issues/300), [#433](https://github.com/ros-planning/moveit2/issues/433)  and on the [MoveIt 2 roadmap](https://moveit.ros.org/documentation/contributing/roadmap/).
 
 ## Getting started
 1. Build moveit2 from source and make sure you use [this fork](https://github.com/sjahr/moveit_msgs) of moveit_msgs for the hybrid planning specific action definitions
 2. To start the demo run:
 ```
 ros2 launch moveit_hybrid_planning hybrid_planning.launch.py
 ```
-You can exchange the planner logic plugin in the hybrid_planning_manager.yaml
-
-## Components
-- hybrid_planning_manager: Control unit of the architecture. The manager coordinates planners and implements reactive behavior defined by the planner logic plugin.
-  - Implementation as ROS 2 component node
-  - Planner logic plugin interface
-- global_planner: ROS 2 component node that receives global motion planning requests and computes a solution to the global problem
-- local_planner: Solves local constraints and can be used to execute the global solution. The is realized with the constraint solver plugin. The local planner also handles trajectory blending and matching with the trajectory operator plugin.
-  - Implementation as ROS 2 component node
-  - Constraint solver plugin interface
-  - Trajectory operator plugin interface
-- planner_logic_plugins: Planner logic plugin implementation
-  - replan_invalidated_trajectory: Replan the global solution if the local planner triggers a collision detection event
-  - single_plan_execution: Call the global planner once and execute the this global solution
-- trajectory_operator_plugins: Trajectory operator plugin implementations
-  - next_waypoint_sample: Forwards the next three waypoints of the global trajectory to the local solver based on the current robot state. When the global solution is updated, the internal (old) reference trajectory is dumped in favor of the update
-- constraint_solver_plugins: Constraint solver plugin implementations
-  - handle_imminent_collision: Follows local trajectory and stops in front of collision objects
-- test:
-  - hybrid_planning_test_node: Simple test demo
+You can exchange the planner logic plugin in the hybrid_planning_manager.yaml. Currently available demo plugins are:
+- planner logic plugins:
+  - replan_invalidated_trajectory: Runs the global planner once and starts executing the global solution
+   with the local planner. In case the local planner detects a collision the global planner is rerun to update the
+   invalidated global trajectory.
+  - single_plan_execution: Run the global planner once and starts executing the global solution
+    with the local planner
+- trajectory operator plugins:
+  - simple_sampler: Samples the next global trajectory waypoint as local goal constraint
+   based on the current robot state. When the waypoint is reached the index that marks the current local goal constraint
+   is updated to the next global trajectory waypoint. Global trajectory updates simply replace the reference trajectory.
+- local solver plugins:
+  - forward_trajectory: Forwards the next waypoint of the sampled local trajectory.
+   Additionally, it is possible to enable collision checking, which lets the robot stop in front of a collision object.

moveit_ros/hybrid_planning/forward_trajectory_plugin.xml

@@ -1,7 +1,7 @@
 <library path="forward_trajectory_plugin">
   <class name="moveit_hybrid_planning/ForwardTrajectory" type="moveit_hybrid_planning::ForwardTrajectory" base_class_type="moveit_hybrid_planning::LocalConstraintSolverInterface">
     <description>
-    Simple trajectory operator plugin that appends new global trajectories to the local planner's reference trajectory and extracts the next trajectory point based on the current robot state and an index.
+    Simple local solver plugin that forwards the next waypoint of the sampled local trajectory.
     </description>
   </class>
 </library>

moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h

@@ -33,6 +33,8 @@
  *********************************************************************/
 
 /* Author: Sebastian Jahr
+   Description: This planner logic plugin runs the global planner once and starts executing the global solution
+    with the local planner.
  */
 
 #include <moveit/hybrid_planning_manager/planner_logic_interface.h>

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt

@@ -1,10 +1,3 @@
-add_library(decelerate_before_collision_plugin SHARED
-  src/decelerate_before_collision.cpp
-)
-set_target_properties(decelerate_before_collision_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-ament_target_dependencies(decelerate_before_collision_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS})
-target_link_libraries(decelerate_before_collision_plugin local_constraint_solver_interface)
-
 add_library(forward_trajectory_plugin SHARED
   src/forward_trajectory.cpp
 )

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h

@@ -34,6 +34,7 @@
 
 /* Author: Sebastian Jahr
    Description: Simple local solver plugin that forwards the next waypoint of the sampled local trajectory.
+   Additionally, it is possible to enable collision checking, which lets the robot stop in front of a collision object.
  */
 
 #pragma once
@@ -51,9 +52,16 @@ class ForwardTrajectory : public LocalConstraintSolverInterface
   bool initialize(const rclcpp::Node::SharedPtr& node,
                   const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
                   const std::string& group_name) override;
+  bool reset() override;
 
   moveit_msgs::action::LocalPlanner::Feedback solve(const robot_trajectory::RobotTrajectory& local_trajectory,
                                                     const std::vector<moveit_msgs::msg::Constraints>& local_constraints,
                                                     trajectory_msgs::msg::JointTrajectory& local_solution) override;
+
+private:
+  rclcpp::Node::SharedPtr node_handle_;
+  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
+  bool path_invalidation_event_send_;  // Send path invalidation event only once
+  bool stop_before_collision_;
 };
 }  // namespace moveit_hybrid_planning