diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_component.h b/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_component.h index f7f0c9272d..ac86ee1a93 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_component.h +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_component/include/moveit/global_planner/global_planner_component.h @@ -76,8 +76,6 @@ class GlobalPlannerComponent // Global planner instance std::shared_ptr global_planner_instance_; - moveit_msgs::msg::MotionPlanResponse last_global_solution_; - // Global planning request action server rclcpp_action::Server::SharedPtr global_planning_request_server_; diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp b/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp index f1160332f9..e4e7ae6826 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp @@ -119,25 +119,22 @@ void GlobalPlannerComponent::globalPlanningRequestCallback( { // Plan global trajectory moveit_msgs::msg::MotionPlanResponse planning_solution = global_planner_instance_->plan(goal_handle); + + // Send action response + auto result = std::make_shared(); + result->response = planning_solution; + if (planning_solution.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { // Publish global planning solution to the local planner global_trajectory_pub_->publish(planning_solution); - - // Send action response - auto result = std::make_shared(); - result->response = planning_solution; goal_handle->succeed(result); - - // Save newest planning solution - last_global_solution_ = planning_solution; // TODO(sjahr) Add Service to expose this } else { - auto result = std::make_shared(); - result->response = planning_solution; goal_handle->abort(result); } + // Reset the global planner global_planner_instance_->reset(); }; diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.h index 1ab6c58ccd..4deeebdee1 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.h @@ -46,12 +46,17 @@ enum class HybridPlanningEvent { // Occurs when the hybrid planning manager receives a planning request HYBRID_PLANNING_REQUEST_RECEIVED, - // Indicates that the global planning action finished (Does not indicate planning success! The action response - // contains this information) - GLOBAL_PLANNING_ACTION_FINISHED, + // Result of the global planning action + GLOBAL_PLANNING_ACTION_SUCCESSFUL, + GLOBAL_PLANNING_ACTION_ABORTED, + GLOBAL_PLANNING_ACTION_CANCELED, // Indicates that the global planner found a solution (This solution is not necessarily the last or best solution) GLOBAL_SOLUTION_AVAILABLE, - // Indicates that the local planning action finished (Does not indicate planning success! The action response contains this information) - LOCAL_PLANNING_ACTION_FINISHED, + // Result of the local planning action + LOCAL_PLANNING_ACTION_SUCCESSFUL, + LOCAL_PLANNING_ACTION_ABORTED, + LOCAL_PLANNING_ACTION_CANCELED, + // Undefined event to allow empty reaction events to indicate failure + UNDEFINED }; } // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h index 7cc9455695..d42a3eeae8 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h @@ -48,7 +48,7 @@ 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, const int& error_code) + ReactionResult(const HybridPlanningEvent& planning_event, const std::string& error_msg, int error_code) : error_message(error_msg), error_code(error_code) { switch (planning_event) @@ -56,18 +56,32 @@ struct ReactionResult case HybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED: event = "Hybrid planning request received"; break; - case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_FINISHED: - event = "Global planning action finished"; + case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_SUCCESSFUL: + event = "Global planning action successful"; + break; + case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_ABORTED: + event = "Global planning action aborted"; + break; + case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_CANCELED: + event = "Global planning action canceled"; break; case HybridPlanningEvent::GLOBAL_SOLUTION_AVAILABLE: event = "Global solution available"; break; - case HybridPlanningEvent::LOCAL_PLANNING_ACTION_FINISHED: - event = "Local planning action finished"; + case HybridPlanningEvent::LOCAL_PLANNING_ACTION_SUCCESSFUL: + event = "Local planning action successful"; + break; + case HybridPlanningEvent::LOCAL_PLANNING_ACTION_ABORTED: + event = "Local planning action aborted"; + break; + case HybridPlanningEvent::LOCAL_PLANNING_ACTION_CANCELED: + event = "Local planning action canceled"; break; + case HybridPlanningEvent::UNDEFINED: + event = "Undefined event"; } }; - ReactionResult(const std::string& event, const std::string& error_msg, const int& error_code) + ReactionResult(const std::string& event, const std::string& error_msg, int error_code) : event(event), error_message(error_msg), error_code(error_code){}; // Event that triggered the reaction diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp index d73b17caa9..a7575cd087 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp @@ -179,26 +179,31 @@ bool HybridPlanningManager::sendGlobalPlannerAction() global_goal_options.result_callback = [this](const rclcpp_action::ClientGoalHandle::WrappedResult& global_result) { // Reaction result from the latest event - if (global_result.code == rclcpp_action::ResultCode::SUCCEEDED) + ReactionResult reaction_result = + ReactionResult(HybridPlanningEvent::UNDEFINED, "", moveit_msgs::msg::MoveItErrorCodes::FAILURE); + switch (global_result.code) { - ReactionResult reaction_result = - planner_logic_instance_->react(HybridPlanningEvent::GLOBAL_PLANNING_ACTION_FINISHED); - auto result = std::make_shared(); - if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) - { - auto result = std::make_shared(); - 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()); - } + case rclcpp_action::ResultCode::SUCCEEDED: + reaction_result = planner_logic_instance_->react(HybridPlanningEvent::GLOBAL_PLANNING_ACTION_SUCCESSFUL); + break; + case rclcpp_action::ResultCode::CANCELED: + reaction_result = planner_logic_instance_->react(HybridPlanningEvent::GLOBAL_PLANNING_ACTION_CANCELED); + break; + case rclcpp_action::ResultCode::ABORTED: + reaction_result = planner_logic_instance_->react(HybridPlanningEvent::GLOBAL_PLANNING_ACTION_ABORTED); + break; + default: + break; } - else + // Abort hybrid planning if reaction fails + if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { auto result = std::make_shared(); - result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; - result->error_message = "Global planner failed to find a solution"; + 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()); } }; @@ -254,16 +259,31 @@ bool HybridPlanningManager::sendLocalPlannerAction() // Add result callback to print the result local_goal_options.result_callback = - [this](const rclcpp_action::ClientGoalHandle::WrappedResult& action_result) { + [this](const rclcpp_action::ClientGoalHandle::WrappedResult& local_result) { // Reaction result from the latest event ReactionResult reaction_result = - planner_logic_instance_->react(HybridPlanningEvent::LOCAL_PLANNING_ACTION_FINISHED); - // TODO: This always thinks the action was successful! + ReactionResult(HybridPlanningEvent::UNDEFINED, "", moveit_msgs::msg::MoveItErrorCodes::FAILURE); + switch (local_result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + reaction_result = planner_logic_instance_->react(HybridPlanningEvent::LOCAL_PLANNING_ACTION_SUCCESSFUL); + break; + case rclcpp_action::ResultCode::CANCELED: + reaction_result = planner_logic_instance_->react(HybridPlanningEvent::LOCAL_PLANNING_ACTION_CANCELED); + break; + case rclcpp_action::ResultCode::ABORTED: + reaction_result = planner_logic_instance_->react(HybridPlanningEvent::LOCAL_PLANNING_ACTION_ABORTED); + break; + default: + break; + } + // Abort hybrid planning if reaction fails if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { auto result = std::make_shared(); 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()); } @@ -299,11 +319,12 @@ void HybridPlanningManager::sendHybridPlanningResponse(bool success) auto result = std::make_shared(); if (success) { - result->error_code.val = 1; + result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; hybrid_planning_goal_handle_->succeed(result); } else { + result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; hybrid_planning_goal_handle_->abort(result); } } diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h index 12a4401f81..339631ef9d 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h @@ -50,5 +50,8 @@ class SinglePlanExecution : public PlannerLogicInterface bool initialize(const std::shared_ptr& hybrid_planning_manager) override; ReactionResult react(const HybridPlanningEvent& event) override; ReactionResult react(const std::string& event) override; + +private: + bool local_planner_started_ = false; }; } // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp index f46057d3d1..f0306aa3b3 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp @@ -41,8 +41,6 @@ const rclcpp::Logger LOGGER = rclcpp::get_logger("hybrid_planning_manager"); namespace moveit::hybrid_planning { -std::once_flag LOCAL_PLANNER_STARTED; - bool SinglePlanExecution::initialize(const std::shared_ptr& hybrid_planning_manager) { hybrid_planning_manager_ = hybrid_planning_manager; @@ -54,27 +52,44 @@ ReactionResult SinglePlanExecution::react(const HybridPlanningEvent& event) switch (event) { case HybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED: + // Handle new hybrid planning request if (!hybrid_planning_manager_->sendGlobalPlannerAction()) // Start global planning { hybrid_planning_manager_->sendHybridPlanningResponse(false); } - break; + // Reset local planner started flag + local_planner_started_ = false; + return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); case HybridPlanningEvent::GLOBAL_SOLUTION_AVAILABLE: - std::call_once(LOCAL_PLANNER_STARTED, [this]() { // ensure the local planner is not started twice + // Do nothing since we wait for the global planning action to finish + return ReactionResult(event, "Do nothing", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_SUCCESSFUL: + // Activate local planner once global solution is available + if (!local_planner_started_) + { // ensure the local planner is not started twice if (!hybrid_planning_manager_->sendLocalPlannerAction()) // Start local planning { hybrid_planning_manager_->sendHybridPlanningResponse(false); } - }); - break; - case HybridPlanningEvent::LOCAL_PLANNING_ACTION_FINISHED: + local_planner_started_ = true; + } + return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_ABORTED: + // Abort hybrid planning if no global solution is found + return ReactionResult(event, "Global planner failed to find a solution", + moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED); + case HybridPlanningEvent::LOCAL_PLANNING_ACTION_SUCCESSFUL: + // Finish hybrid planning action successfully because local planning action succeeded hybrid_planning_manager_->sendHybridPlanningResponse(true); - break; + return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + case HybridPlanningEvent::LOCAL_PLANNING_ACTION_ABORTED: + // Local planning failed so abort hybrid planning + return ReactionResult(event, "Local planner failed to find a solution", + moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED); default: - break; - // Do nothing + // Unknown event, abort hybrid planning + return ReactionResult(event, "Unknown event", moveit_msgs::msg::MoveItErrorCodes::FAILURE); } - return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); } ReactionResult SinglePlanExecution::react(const std::string& event)