Refactor local planner plugins (moveit#447)

* Add reset method for trajectory operator and local constraint sampler
* Refactor next_waypoint_sampler into simple_sampler
* Include collision checking to forward_trajectory and remove unneeded plugin
* Fix formatting and plugin description
* Update README and add missing planner logic plugin description

Add TODO to use lifecycle components nodes to trigger initialization

Return a reaction result instead of bool on react()

Set invalidation flag to false on reset() in ForwardTrajectory local solver

Return local planner feedback from trajectory operator function calls

Fix segfault caused by passing through the global trajectory

Update comment, unify logging and add missing config parameters

Update to rolling

moveit2.repos

@@ -1,4 +1,9 @@
 repositories:
+# TODO(sjahr): Remove once hybrid planning action definitions are finalized and merged
+  moveit_msgs:
+    type: git
+    url: https://github.com/sjahr/moveit_msgs
+    version: hybrid_planning
   geometric_shapes:
     type: git
     url: https://github.com/ros-planning/geometric_shapes

moveit_ros/hybrid_planning/CMakeLists.txt

@@ -27,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
 )
@@ -78,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/global_planner/src/global_planner_component.cpp

@@ -80,6 +80,7 @@ GlobalPlannerComponent::GlobalPlannerComponent(const rclcpp::NodeOptions& option
   global_trajectory_pub_ = this->create_publisher<moveit_msgs::msg::MotionPlanResponse>("global_trajectory", 1);
 
   // Initialize global planner after construction
+  // TODO(sjahr) Remove once life cycle component nodes are available
   timer_ = this->create_wall_timer(1ms, [this]() {
     if (initialized_)
     {
@@ -90,9 +91,8 @@ GlobalPlannerComponent::GlobalPlannerComponent(const rclcpp::NodeOptions& option
       initialized_ = this->init();
       if (!initialized_)
       {
-        const std::string error = "Failed to initialize global planner";
         timer_->cancel();
-        throw std::runtime_error(error);
+        throw std::runtime_error("Failed to initialize global planner");
       }
     }
   });
@@ -110,18 +110,16 @@ bool GlobalPlannerComponent::init()
   RCLCPP_DEBUG(LOGGER, "Configuring Planning Scene Monitor");
   if (!planning_scene_monitor_->getPlanningScene())
   {
-    const std::string error = "Unable to configure planning scene monitor";
-    RCLCPP_FATAL(LOGGER, error);
-    throw std::runtime_error(error);
+    RCLCPP_ERROR(LOGGER, "Unable to configure planning scene monitor");
+    return false;
   }
 
   robot_model_ = planning_scene_monitor_->getRobotModel();
   if (!robot_model_)
   {
-    const std::string error = "Unable to construct robot model. Please make sure all needed information is on the "
-                              "parameter server.";
-    RCLCPP_FATAL(LOGGER, error);
-    throw std::runtime_error(error);
+    RCLCPP_ERROR(LOGGER, "Unable to construct robot model. Please make sure all needed information is on the "
+                         "parameter server.");
+    return false;
   }
 
   // Start state and scene monitors
@@ -163,7 +161,6 @@ void GlobalPlannerComponent::globalPlanningRequestCallback(
   const auto goal = goal_handle->get_goal();
 
   // Plan global trajectory
-  RCLCPP_INFO(LOGGER, "Start global planning");
   moveit_msgs::msg::MotionPlanResponse planning_solution = plan(goal->request);
 
   // Publish global planning solution to the local planner

moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h

@@ -45,9 +45,10 @@
 #include <moveit_msgs/action/global_planner.hpp>
 #include <moveit_msgs/action/hybrid_planning.hpp>
 
-#include <pluginlib/class_loader.hpp>
 #include <moveit/hybrid_planning_manager/planner_logic_interface.h>
 
+#include <pluginlib/class_loader.hpp>
+
 namespace moveit_hybrid_planning
 {
 /**

moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h

@@ -39,11 +39,76 @@
 #pragma once
 
 #include <rclcpp/rclcpp.hpp>
-
+#include <moveit_msgs/msg/move_it_error_codes.hpp>
 #include <moveit/hybrid_planning_manager/hybrid_planning_events.h>
 
 namespace moveit_hybrid_planning
 {
+// TODO(sjahr): Move this into utility package
+class MoveItErrorCode : public moveit_msgs::msg::MoveItErrorCodes
+{
+public:
+  MoveItErrorCode()
+  {
+    val = 0;
+  }
+  MoveItErrorCode(int code)
+  {
+    val = code;
+  }
+  MoveItErrorCode(const moveit_msgs::msg::MoveItErrorCodes& code)
+  {
+    val = code.val;
+  }
+  explicit operator bool() const
+  {
+    return val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
+  }
+  bool operator==(const int code) const
+  {
+    return val == code;
+  }
+  bool operator!=(const int code) const
+  {
+    return val != code;
+  }
+};
+
+// Describes the outcome of a reaction to an event in the hybrid planning architecture
+struct ReactionResult
+{
+  ReactionResult(const BasicHybridPlanningEvent& planning_event, const std::string& error_msg, const int& error_code)
+    : error_message(error_msg), error_code(error_code)
+  {
+    switch (planning_event)
+    {
+      case moveit_hybrid_planning::BasicHybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED:
+        event = "Hybrid planning request received";
+        break;
+      case moveit_hybrid_planning::BasicHybridPlanningEvent::GLOBAL_PLANNING_ACTION_FINISHED:
+        event = "Global planning action finished";
+        break;
+      case moveit_hybrid_planning::BasicHybridPlanningEvent::GLOBAL_SOLUTION_AVAILABLE:
+        event = "Global solution available";
+        break;
+      case moveit_hybrid_planning::BasicHybridPlanningEvent::LOCAL_PLANNING_ACTION_FINISHED:
+        event = "Local planning action finished";
+        break;
+    }
+  };
+  ReactionResult(const std::string& event, const std::string& error_msg, const int& error_code)
+    : event(event), error_message(error_msg), error_code(error_code){};
+
+  // Event that triggered the reaction
+  std::string event;
+
+  // Additional error description
+  std::string error_message;
+
+  // Error code
+  MoveItErrorCode error_code;
+};
+
 class HybridPlanningManager;  // Forward declaration
 
 /**
@@ -65,16 +130,16 @@ class PlannerLogicInterface
   /**
    * React to event defined in BasicHybridPlanningEvent enum
    * @param event Basic hybrid planning event
-   * @return true if reaction was successful
+   * @return Reaction result that summarizes the outcome of the reaction
    */
-  virtual bool react(BasicHybridPlanningEvent event) = 0;
+  virtual ReactionResult react(const BasicHybridPlanningEvent& event) = 0;
 
   /**
    * React to custom event
    * @param event Encoded as string
-   * @return true if reaction was successful
+   * @return Reaction result that summarizes the outcome of the reaction
    */
-  virtual bool react(const std::string& event) = 0;
+  virtual ReactionResult react(const std::string& event) = 0;
   virtual ~PlannerLogicInterface(){};
 
 protected: