Improve handling of hybrid planner result, feedback #500

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_)
     {
@@ -163,7 +164,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:

moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp

@@ -48,6 +48,7 @@ HybridPlanningManager::HybridPlanningManager(const rclcpp::NodeOptions& options)
 {
   initialized_ = false;
   // Initialize hybrid planning component after after construction
+  // TODO(sjahr) Remove once life cycle component nodes are available
   timer_ = this->create_wall_timer(1ms, [this]() {
     if (initialized_)
     {
@@ -77,7 +78,7 @@ bool HybridPlanningManager::initialize()
   }
   catch (pluginlib::PluginlibException& ex)
   {
-    RCLCPP_FATAL(LOGGER, "Exception while creating planner logic plugin loader %s", ex.what());
+    RCLCPP_FATAL(LOGGER, "Exception while creating planner logic plugin loader '%s'", ex.what());
   }
   // TODO(sjahr) Refactor parameter declaration and use repository wide solution
   std::string logic_plugin_name = "";
@@ -101,7 +102,7 @@ bool HybridPlanningManager::initialize()
   }
   catch (pluginlib::PluginlibException& ex)
   {
-    RCLCPP_ERROR(LOGGER, "Exception while loading planner logic '%s': %s", logic_plugin_name.c_str(), ex.what());
+    RCLCPP_ERROR(LOGGER, "Exception while loading planner logic '%s': '%s'", logic_plugin_name.c_str(), ex.what());
   }
 
   // Initialize local planning action client
@@ -142,14 +143,23 @@ bool HybridPlanningManager::initialize()
   // Initialize global solution subscriber
   global_solution_sub_ = create_subscription<moveit_msgs::msg::MotionPlanResponse>(
       "global_trajectory", 1, [this](const moveit_msgs::msg::MotionPlanResponse::SharedPtr msg) {
-        planner_logic_instance_->react(moveit_hybrid_planning::BasicHybridPlanningEvent::GLOBAL_SOLUTION_AVAILABLE);
+        // Reaction result from the latest event
+        ReactionResult reaction_result =
+            planner_logic_instance_->react(moveit_hybrid_planning::BasicHybridPlanningEvent::GLOBAL_SOLUTION_AVAILABLE);
+        if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
+        {
+          auto result = std::make_shared<moveit_msgs::action::HybridPlanning::Result>();
+          result->error_code.val = reaction_result.error_code.val;
+          result->error_message = reaction_result.error_message;
+          hybrid_planning_goal_handle_->abort(result);
+          RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to  '%s'", reaction_result.event.c_str());
+        }
       });
   return true;
 }
 
 bool HybridPlanningManager::planGlobalTrajectory()
 {
-  RCLCPP_INFO(LOGGER, "START PLANNING");
   auto global_goal_options = rclcpp_action::Client<moveit_msgs::action::GlobalPlanner>::SendGoalOptions();
 
   // Add goal response callback
@@ -170,17 +180,28 @@ bool HybridPlanningManager::planGlobalTrajectory()
       };
   // Add result callback
   global_goal_options.result_callback =
-      [this](const rclcpp_action::ClientGoalHandle<moveit_msgs::action::GlobalPlanner>::WrappedResult& result) {
-        planner_logic_instance_->react(
+      [this](const rclcpp_action::ClientGoalHandle<moveit_msgs::action::GlobalPlanner>::WrappedResult& global_result) {
+        // Reaction result from the latest event
+        ReactionResult reaction_result = planner_logic_instance_->react(
             moveit_hybrid_planning::BasicHybridPlanningEvent::GLOBAL_PLANNING_ACTION_FINISHED);
+        auto result = std::make_shared<moveit_msgs::action::HybridPlanning::Result>();
+        if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
+        {
+          auto result = std::make_shared<moveit_msgs::action::HybridPlanning::Result>();
+          result->error_code.val = reaction_result.error_code.val;
+          result->error_message = reaction_result.error_message;
+          hybrid_planning_goal_handle_->abort(result);
+          RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to  '%s'", reaction_result.event.c_str());
+        }
       };
+
   // Forward global trajectory goal from hybrid planning request TODO(sjahr) pass goal as function argument
   auto global_goal_msg = moveit_msgs::action::GlobalPlanner::Goal();
   global_goal_msg.request = latest_hybrid_planning_goal_;
   // Send global planning goal and wait until it's accepted
   auto goal_handle_future = global_planner_action_client_->async_send_goal(global_goal_msg, global_goal_options);
   return true;  // return always success TODO(sjahr) add more error checking
-}
+};
 
 bool HybridPlanningManager::runLocalPlanner()
 {
@@ -210,13 +231,32 @@ bool HybridPlanningManager::runLocalPlanner()
   local_goal_options.feedback_callback =
       [this](rclcpp_action::ClientGoalHandle<moveit_msgs::action::LocalPlanner>::SharedPtr /*unused*/,
              const std::shared_ptr<const moveit_msgs::action::LocalPlanner::Feedback> local_planner_feedback) {
-        planner_logic_instance_->react(local_planner_feedback->feedback);
+        // Reaction result from the latest event
+        ReactionResult reaction_result = planner_logic_instance_->react(local_planner_feedback->feedback);
+        if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
+        {
+          auto result = std::make_shared<moveit_msgs::action::HybridPlanning::Result>();
+          result->error_code.val = reaction_result.error_code.val;
+          result->error_message = reaction_result.error_message;
+          hybrid_planning_goal_handle_->abort(result);
+          RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to  '%s'", reaction_result.event.c_str());
+        }
       };
 
   // Add result callback to print the result
   local_goal_options.result_callback =
       [this](const rclcpp_action::ClientGoalHandle<moveit_msgs::action::LocalPlanner>::WrappedResult& result) {
-        planner_logic_instance_->react(moveit_hybrid_planning::BasicHybridPlanningEvent::LOCAL_PLANNING_ACTION_FINISHED);
+        // Reaction result from the latest event
+        ReactionResult reaction_result = planner_logic_instance_->react(
+            moveit_hybrid_planning::BasicHybridPlanningEvent::LOCAL_PLANNING_ACTION_FINISHED);
+        if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
+        {
+          auto result = std::make_shared<moveit_msgs::action::HybridPlanning::Result>();
+          result->error_code.val = reaction_result.error_code.val;
+          result->error_message = reaction_result.error_message;
+          hybrid_planning_goal_handle_->abort(result);
+          RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to  '%s'", reaction_result.event.c_str());
+        }
       };
 
   // Send global planning goal
@@ -232,7 +272,17 @@ void HybridPlanningManager::hybridPlanningRequestCallback(
   latest_hybrid_planning_goal_ = (hybrid_planning_goal_handle_->get_goal())->request;
 
   // React on incoming planning request
-  planner_logic_instance_->react(moveit_hybrid_planning::BasicHybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED);
+  // Reaction result from the latest event
+  ReactionResult reaction_result = planner_logic_instance_->react(
+      moveit_hybrid_planning::BasicHybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED);
+  if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
+  {
+    auto result = std::make_shared<moveit_msgs::action::HybridPlanning::Result>();
+    result->error_code.val = reaction_result.error_code.val;
+    result->error_message = reaction_result.error_message;
+    hybrid_planning_goal_handle_->abort(result);
+    RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to  '%s'", reaction_result.event.c_str());
+  }
 }
 
 void HybridPlanningManager::sendHybridPlanningResponse(bool success)

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

@@ -48,6 +48,6 @@ class ReplanInvalidatedTrajectory : public SinglePlanExecution  // Inherit from
 public:
   ReplanInvalidatedTrajectory(){};
   ~ReplanInvalidatedTrajectory() override{};
-  bool react(const std::string& event) override;
+  ReactionResult react(const std::string& event) override;
 };
 }  // namespace moveit_hybrid_planning

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

@@ -49,7 +49,7 @@ class SinglePlanExecution : public PlannerLogicInterface
   ~SinglePlanExecution() override{};
   bool
   initialize(const std::shared_ptr<moveit_hybrid_planning::HybridPlanningManager>& hybrid_planning_manager) override;
-  bool react(BasicHybridPlanningEvent event) override;
-  bool react(const std::string& event) override;
+  ReactionResult react(const BasicHybridPlanningEvent& event) override;
+  ReactionResult react(const std::string& event) override;
 };
 }  // namespace moveit_hybrid_planning

moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp

@@ -41,20 +41,21 @@ namespace moveit_hybrid_planning
 {
 const rclcpp::Logger LOGGER = rclcpp::get_logger("hybrid_planning_manager");
 
-bool ReplanInvalidatedTrajectory::react(const std::string& event)
+ReactionResult ReplanInvalidatedTrajectory::react(const std::string& event)
 {
   if (event == "collision_ahead")
   {
     if (!hybrid_planning_manager_->planGlobalTrajectory())  // Start global planning
     {
       hybrid_planning_manager_->sendHybridPlanningResponse(false);
     }
+    return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
   }
-  else if (!event.empty())
+  else
   {
-    RCLCPP_WARN(LOGGER, "Received unkown event: %s", event.c_str());
+    return ReactionResult(event, "'ReplanInvalidatedTrajectory' plugin cannot handle this event.",
+                          moveit_msgs::msg::MoveItErrorCodes::FAILURE);
   }
-  return true;
 };
 }  // namespace moveit_hybrid_planning
 

moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp

@@ -49,7 +49,7 @@ bool SinglePlanExecution::initialize(
   return true;
 }
 
-bool SinglePlanExecution::react(BasicHybridPlanningEvent event)
+ReactionResult SinglePlanExecution::react(const BasicHybridPlanningEvent& event)
 {
   switch (event)
   {
@@ -71,16 +71,16 @@ bool SinglePlanExecution::react(BasicHybridPlanningEvent event)
       hybrid_planning_manager_->sendHybridPlanningResponse(true);
       break;
     default:
-      // Do nothing
       break;
+      // Do nothing
   }
-  return true;
+  return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
 }
-bool SinglePlanExecution::react(const std::string& event)
+
+ReactionResult SinglePlanExecution::react(const std::string& event)
 {
-  auto& clock = *hybrid_planning_manager_->get_clock();
-  RCLCPP_INFO_THROTTLE(LOGGER, clock, 1000, event.c_str());
-  return true;
+  return ReactionResult(event, "'Single-Plan-Execution' plugin cannot handle events given as string.",
+                        moveit_msgs::msg::MoveItErrorCodes::FAILURE);
 };
 }  // namespace moveit_hybrid_planning
 

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

@@ -54,9 +54,10 @@ class ForwardTrajectory : public LocalConstraintSolverInterface
                   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;
+  moveit_msgs::action::LocalPlanner::Feedback
+  solve(const robot_trajectory::RobotTrajectory& local_trajectory,
+        const std::shared_ptr<const moveit_msgs::action::LocalPlanner::Goal> local_goal,
+        trajectory_msgs::msg::JointTrajectory& local_solution) override;
 
 private:
   rclcpp::Node::SharedPtr node_handle_;

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp

@@ -60,12 +60,13 @@ bool ForwardTrajectory::initialize(const rclcpp::Node::SharedPtr& node,
 
 bool ForwardTrajectory::reset()
 {
+  path_invalidation_event_send_ = false;
   return true;
 };
 
 moveit_msgs::action::LocalPlanner::Feedback
 ForwardTrajectory::solve(const robot_trajectory::RobotTrajectory& local_trajectory,
-                         const std::vector<moveit_msgs::msg::Constraints>& local_constraints,
+                         const std::shared_ptr<const moveit_msgs::action::LocalPlanner::Goal> local_goal,
                          trajectory_msgs::msg::JointTrajectory& local_solution)
 {
   // Create controller command trajectory

moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.h

@@ -70,13 +70,13 @@ class LocalConstraintSolverInterface
   /**
    * Solve local planning problem for the current loop run
    * @param local_trajectory The local trajectory to pursue
-   * @param local_constraints Local goal constraints
+   * @param local_goal Local goal constraints
    * @param local_solution solution plan in joint space
    * @return Feedback event from the current solver call i.e. "Collision detected"
    */
   virtual moveit_msgs::action::LocalPlanner::Feedback
   solve(const robot_trajectory::RobotTrajectory& local_trajectory,
-        const std::vector<moveit_msgs::msg::Constraints>& local_constraints,
+        const std::shared_ptr<const moveit_msgs::action::LocalPlanner::Goal> local_goal,
         trajectory_msgs::msg::JointTrajectory& local_solution) = 0;
 
   /**