Add servo solver plugin

moveit_ros/hybrid_planning/CMakeLists.txt

@@ -7,10 +7,13 @@ moveit_package()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(control_msgs REQUIRED)
+find_package(generate_parameter_library REQUIRED)
 find_package(moveit_core REQUIRED)
 find_package(moveit_msgs REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
+find_package(moveit_servo REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
@@ -29,15 +32,23 @@ set(LIBRARIES
   forward_trajectory_plugin
   motion_planning_pipeline_plugin
   replan_invalidated_trajectory_plugin
+  servo_local_solver_plugin
   simple_sampler_plugin
   single_plan_execution_plugin
+  # Parameters
+  local_planner_parameters
+  hp_manager_parameters
+  forward_trajectory_parameters
+  servo_solver_parameters
 )
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
+  control_msgs
   moveit_core
   moveit_msgs
   moveit_ros_planning
   moveit_ros_planning_interface
+  moveit_servo
   pluginlib
   rclcpp
   rclcpp_action
@@ -75,7 +86,7 @@ install(TARGETS ${LIBRARIES}
   RUNTIME DESTINATION bin
   INCLUDES DESTINATION include/moveit_hybrid_planning)
 
-install(TARGETS cancel_action hybrid_planning_demo_node
+install(TARGETS cancel_action
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/moveit_hybrid_planning
@@ -86,11 +97,7 @@ install(DIRECTORY ${THIS_PACKAGE_INCLUDE_DIRS} DESTINATION include/moveit_hybrid
 install(DIRECTORY test/launch DESTINATION share/moveit_hybrid_planning)
 install(DIRECTORY test/config DESTINATION share/moveit_hybrid_planning)
 
-pluginlib_export_plugin_description_file(moveit_hybrid_planning single_plan_execution_plugin.xml)
-pluginlib_export_plugin_description_file(moveit_hybrid_planning moveit_planning_pipeline_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)
+pluginlib_export_plugin_description_file(moveit_hybrid_planning plugins.xml)
 
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 ament_export_targets(moveit_hybrid_planningTargets HAS_LIBRARY_TARGET)

moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp

@@ -62,17 +62,9 @@ GlobalPlannerComponent::GlobalPlannerComponent(const rclcpp::NodeOptions& option
 bool GlobalPlannerComponent::initializeGlobalPlanner()
 {
   // Initialize global planning request action server
-  std::string global_planning_action_name = "";
-  node_->declare_parameter("global_planning_action_name", "");
-  node_->get_parameter<std::string>("global_planning_action_name", global_planning_action_name);
-  if (global_planning_action_name.empty())
-  {
-    RCLCPP_ERROR(node_->get_logger(), "global_planning_action_name was not defined");
-    return false;
-  }
   cb_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   global_planning_request_server_ = rclcpp_action::create_server<moveit_msgs::action::GlobalPlanner>(
-      node_, global_planning_action_name,
+      node_, "global_planning_action",
       // Goal callback
       [this](const rclcpp_action::GoalUUID& /*unused*/,
              const std::shared_ptr<const moveit_msgs::action::GlobalPlanner::Goal>& /*unused*/) {

moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/CMakeLists.txt

@@ -1,4 +1,6 @@
 # Hybrid planning manager component
+generate_parameter_library(hp_manager_parameters res/hp_manager_parameters.yaml)
 add_library(moveit_hybrid_planning_manager SHARED src/hybrid_planning_manager.cpp)
 set_target_properties(moveit_hybrid_planning_manager PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+target_link_libraries(moveit_hybrid_planning_manager hp_manager_parameters)
 ament_target_dependencies(moveit_hybrid_planning_manager ${THIS_PACKAGE_INCLUDE_DEPENDS})

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

@@ -50,13 +50,27 @@ enum class HybridPlanningEvent
   GLOBAL_PLANNING_ACTION_SUCCESSFUL,
   GLOBAL_PLANNING_ACTION_ABORTED,
   GLOBAL_PLANNING_ACTION_CANCELED,
+  GLOBAL_PLANNING_ACTION_REJECTED,
   // Indicates that the global planner found a solution (This solution is not necessarily the last or best solution)
   GLOBAL_SOLUTION_AVAILABLE,
   // Result of the local planning action
   LOCAL_PLANNING_ACTION_SUCCESSFUL,
   LOCAL_PLANNING_ACTION_ABORTED,
   LOCAL_PLANNING_ACTION_CANCELED,
+  LOCAL_PLANNING_ACTION_REJECTED,
   // Undefined event to allow empty reaction events to indicate failure
   UNDEFINED
 };
+
+/**
+ * Enum class HybridPlanningAction - This class defines the basic actions that the HP manager can perform
+ */
+enum class HybridPlanningAction
+{
+  DO_NOTHING,
+  RETURN_HP_SUCCESS,
+  RETURN_HP_FAILURE,
+  SEND_GLOBAL_SOLVER_REQUEST,
+  SEND_LOCAL_SOLVER_REQUEST
+};
 }  // namespace moveit::hybrid_planning

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

@@ -54,14 +54,14 @@ namespace moveit::hybrid_planning
 /**
  * Class HybridPlanningManager - ROS 2 component node that implements the hybrid planning manager.
  */
-class HybridPlanningManager : public rclcpp::Node
+class HybridPlanningManager
 {
 public:
   /** \brief Constructor */
   HybridPlanningManager(const rclcpp::NodeOptions& options);
 
   /** \brief Destructor */
-  ~HybridPlanningManager() override
+  ~HybridPlanningManager()
   {
     // Join the thread used for long-running callbacks
     if (long_callback_thread_.joinable())
@@ -70,21 +70,19 @@ class HybridPlanningManager : public rclcpp::Node
     }
   }
 
-  /**
-   * Allows creation of a smart pointer that references to instances of this object
-   * @return shared pointer of the HybridPlanningManager instance that called the function
-   */
-  std::shared_ptr<HybridPlanningManager> shared_from_this()
-  {
-    return std::static_pointer_cast<HybridPlanningManager>(Node::shared_from_this());
-  }
-
   /**
    * Load and initialized planner logic plugin and ROS 2 action and topic interfaces
    * @return Initialization successful yes/no
    */
   bool initialize();
 
+  // This function is required to make this class a valid NodeClass
+  // see https://docs.ros2.org/latest/api/rclcpp_components/register__node__macro_8hpp.html
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface()  // NOLINT
+  {
+    return node_->get_node_base_interface();  // NOLINT
+  }
+
   /**
    * Cancel any active action goals, including global and local planners
    */
@@ -115,19 +113,22 @@ class HybridPlanningManager : public rclcpp::Node
    */
   void sendHybridPlanningResponse(bool success);
 
+  /**
+   * @brief Process the action result and do an action call if necessary
+   *
+   * @param result Result to an event
+   */
+  void processReactionResult(const ReactionResult& result);
+
 private:
+  std::shared_ptr<rclcpp::Node> node_;
+
   // Planner logic plugin loader
   std::unique_ptr<pluginlib::ClassLoader<PlannerLogicInterface>> planner_logic_plugin_loader_;
 
   // Planner logic instance to implement reactive behavior
   std::shared_ptr<PlannerLogicInterface> planner_logic_instance_;
 
-  // Timer to trigger events periodically
-  rclcpp::TimerBase::SharedPtr timer_;
-
-  // Flag that indicates whether the manager is initialized
-  bool initialized_;
-
   // Flag that indicates hybrid planning has been canceled
   std::atomic<bool> stop_hybrid_planning_;
 

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

@@ -48,8 +48,9 @@ namespace moveit::hybrid_planning
 // Describes the outcome of a reaction to an event in the hybrid planning architecture
 struct ReactionResult
 {
-  ReactionResult(const HybridPlanningEvent& planning_event, const std::string& error_msg, int error_code)
-    : error_message(error_msg), error_code(error_code)
+  ReactionResult(const HybridPlanningEvent& planning_event, const std::string& error_msg, const int error_code,
+                 const HybridPlanningAction& action = HybridPlanningAction::DO_NOTHING)
+    : error_message(error_msg), error_code(error_code), action(action)
   {
     switch (planning_event)
     {
@@ -79,10 +80,18 @@ struct ReactionResult
         break;
       case HybridPlanningEvent::UNDEFINED:
         event = "Undefined event";
+        break;
+      case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_REJECTED:
+        event = "Global planning action rejected";
+        break;
+      case HybridPlanningEvent::LOCAL_PLANNING_ACTION_REJECTED:
+        event = "Local planning action rejected";
+        break;
     }
   };
-  ReactionResult(const std::string& event, const std::string& error_msg, int error_code)
-    : event(event), error_message(error_msg), error_code(error_code){};
+  ReactionResult(const std::string& event, const std::string& error_msg, const int error_code,
+                 const HybridPlanningAction& action = HybridPlanningAction::DO_NOTHING)
+    : event(event), error_message(error_msg), error_code(error_code), action(action){};
 
   // Event that triggered the reaction
   std::string event;
@@ -92,9 +101,10 @@ struct ReactionResult
 
   // Error code
   MoveItErrorCode error_code;
-};
 
-class HybridPlanningManager;  // Forward declaration
+  // Action to that needs to be performed by the HP manager
+  HybridPlanningAction action;
+};
 
 /**
  * Class PlannerLogicInterface - Base class for a planner logic. The logic defines how to react to different events that
@@ -113,10 +123,12 @@ class PlannerLogicInterface
 
   /**
    * Initialize the planner logic
-   * @param hybrid_planning_manager The hybrid planning manager instance to initialize this logic with.
    * @return true if initialization was successful
    */
-  virtual bool initialize(const std::shared_ptr<HybridPlanningManager>& hybrid_planning_manager) = 0;
+  virtual bool initialize()
+  {
+    return true;
+  };
 
   /**
    * React to event defined in HybridPlanningEvent enum
@@ -131,9 +143,5 @@ class PlannerLogicInterface
    * @return Reaction result that summarizes the outcome of the reaction
    */
   virtual ReactionResult react(const std::string& event) = 0;
-
-protected:
-  // The hybrid planning manager instance that runs this logic plugin
-  std::shared_ptr<HybridPlanningManager> hybrid_planning_manager_ = nullptr;
 };
 }  // namespace moveit::hybrid_planning

moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/res/hp_manager_parameters.yaml

@@ -0,0 +1,6 @@
+hp_manager_parameters:
+  planner_logic_plugin_name: {
+    type: string,
+    description: "Name of the planner logic plugin",
+    default_value: "moveit::hybrid_planning/ReplanInvalidatedTrajectory",
+  }