Skip to content

Commit

Permalink
Enable reaction to planner failure in the planner logic (#3)
Browse files Browse the repository at this point in the history 
* Add unsuccessful action Hybrid Planning events and handle them in logic

* Replace std::once with simple bool variable

* Remove unneeded variable and update comments

Don't const& for built-in types

Co-authored-by: Tyler Weaver <tylerjw@gmail.com>
  • Loading branch information
@sjahr @tylerjw
2 people authored and AndyZe committed Dec 3, 2021
1 parent 0c3bef1 commit e8c5d1b
Show file tree
Hide file tree
Showing 7 changed files with 105 additions and 52 deletions.
2 changes: 0 additions & 2 deletions ...planner/global_planner_component/include/moveit/global_planner/global_planner_component.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,8 +76,6 @@ class GlobalPlannerComponent
// Global planner instance
std::shared_ptr<GlobalPlannerInterface> global_planner_instance_;

moveit_msgs::msg::MotionPlanResponse last_global_solution_;

// Global planning request action server
rclcpp_action::Server<moveit_msgs::action::GlobalPlanner>::SharedPtr global_planning_request_server_;

Expand Down
15 changes: 6 additions & 9 deletions .../hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<moveit_msgs::action::GlobalPlanner::Result>();
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<moveit_msgs::action::GlobalPlanner::Result>();
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<moveit_msgs::action::GlobalPlanner::Result>();
result->response = planning_solution;
goal_handle->abort(result);
}

// Reset the global planner
global_planner_instance_->reset();
};
Expand Down
15 changes: 10 additions & 5 deletions ...lanning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
26 changes: 20 additions & 6 deletions ...anning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,26 +48,40 @@ 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)
{
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
Expand Down
59 changes: 40 additions & 19 deletions ...hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -179,26 +179,31 @@ bool HybridPlanningManager::sendGlobalPlannerAction()
global_goal_options.result_callback =
[this](const rclcpp_action::ClientGoalHandle<moveit_msgs::action::GlobalPlanner>::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<moveit_msgs::action::HybridPlanner::Result>();
if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
{
auto result = std::make_shared<moveit_msgs::action::HybridPlanner::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());
}
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<moveit_msgs::action::HybridPlanner::Result>();
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());
}
};

Expand Down Expand Up @@ -254,16 +259,31 @@ bool HybridPlanningManager::sendLocalPlannerAction()

// Add result callback to print the result
local_goal_options.result_callback =
[this](const rclcpp_action::ClientGoalHandle<moveit_msgs::action::LocalPlanner>::WrappedResult& action_result) {
[this](const rclcpp_action::ClientGoalHandle<moveit_msgs::action::LocalPlanner>::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<moveit_msgs::action::HybridPlanner::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());
}
Expand Down Expand Up @@ -299,11 +319,12 @@ void HybridPlanningManager::sendHybridPlanningResponse(bool success)
auto result = std::make_shared<moveit_msgs::action::HybridPlanner::Result>();
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);
}
}
Expand Down
3 changes: 3 additions & 0 deletions ...anager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,5 +50,8 @@ class SinglePlanExecution : public PlannerLogicInterface
bool initialize(const std::shared_ptr<HybridPlanningManager>& 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
37 changes: 26 additions & 11 deletions ...brid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<HybridPlanningManager>& hybrid_planning_manager)
{
hybrid_planning_manager_ = hybrid_planning_manager;
Expand All @@ -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)
Expand Down

0 comments on commit e8c5d1b

Please sign in to comment.