Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

exceptions for compute path through poses #3248

Merged
Show file tree
Hide file tree
Changes from 2 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,10 @@ namespace nav2_behavior_tree
class ComputePathThroughPosesAction
: public BtActionNode<nav2_msgs::action::ComputePathThroughPoses>
{
using Action = nav2_msgs::action::ComputePathThroughPoses;
using ActionResult = Action::Result;
using ActionGoal = Action::Goal;

public:
/**
* @brief A constructor for nav2_behavior_tree::ComputePathThroughPosesAction
Expand Down Expand Up @@ -72,7 +76,6 @@ class ComputePathThroughPosesAction
{
return providedBasicPorts(
{
BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathThroughPoses node"),
BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
"goals",
"Destinations to plan through"),
Expand All @@ -81,6 +84,9 @@ class ComputePathThroughPosesAction
BT::InputPort<std::string>(
"planner_id", "",
"Mapped name to the planner plugin type to use"),
BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathThroughPoses node"),
BT::OutputPort<ActionResult::_error_code_type>(
"compute_path_through_poses_error_code", "The compute path through poses error code"),
});
}
};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,10 @@ namespace nav2_behavior_tree
*/
class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePathToPose>
{
using Action = nav2_msgs::action::ComputePathToPose;
using ActionResult = Action::Result;
using ActionGoal = Action::Goal;

public:
/**
* @brief A constructor for nav2_behavior_tree::ComputePathToPoseAction
Expand Down Expand Up @@ -57,7 +61,7 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
BT::NodeStatus on_aborted() override;

/**
* @brief Function to perform some user-defined operation upon cancelation of the action
* @brief Function to perform some user-defined operation upon cancellation of the action
*/
BT::NodeStatus on_cancelled() override;

Expand All @@ -74,15 +78,15 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
{
return providedBasicPorts(
{
BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
BT::OutputPort<nav2_msgs::action::ComputePathToPose::Result::_error_code_type>(
"compute_path_to_pose_error_code", "The compute path to pose error code"),
BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
BT::InputPort<geometry_msgs::msg::PoseStamped>(
"start", "Start pose of the path if overriding current robot pose"),
BT::InputPort<std::string>(
"planner_id", "",
"Mapped name to the planner plugin type to use"),
BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
BT::OutputPort<ActionResult::_error_code_type>(
"compute_path_to_pose_error_code", "The compute path to pose error code"),
});
}
};
Expand Down
2 changes: 2 additions & 0 deletions nav2_behavior_tree/nav2_tree_nodes.xml
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,8 @@
<input_port name="server_timeout">Server timeout</input_port>
<input_port name="planner_id">Mapped name to the planner plugin type to use</input_port>
<output_port name="path">Path created by ComputePathToPose node</output_port>
<output_port name="compute_path_through_poses_error_code">"The compute path through poses
pose error code"</output_port>
</Action>

<Action ID="RemovePassedGoals">
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,20 +41,25 @@ void ComputePathThroughPosesAction::on_tick()
BT::NodeStatus ComputePathThroughPosesAction::on_success()
{
setOutput("path", result_.result->path);
// Set empty error code, action was successful
setOutput("compute_path_through_poses_error_code", ActionGoal::NONE);
return BT::NodeStatus::SUCCESS;
}

BT::NodeStatus ComputePathThroughPosesAction::on_aborted()
{
nav_msgs::msg::Path empty_path;
setOutput("path", empty_path);
setOutput("compute_path_through_poses_error_code", result_.result->error_code);
return BT::NodeStatus::FAILURE;
}

BT::NodeStatus ComputePathThroughPosesAction::on_cancelled()
{
nav_msgs::msg::Path empty_path;
setOutput("path", empty_path);
// Set empty error code, action was cancelled
setOutput("compute_path_through_poses_error_code", ActionGoal::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ ComputePathToPoseAction::ComputePathToPoseAction(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf)
: BtActionNode<nav2_msgs::action::ComputePathToPose>(xml_tag_name, action_name, conf)
: BtActionNode<Action>(xml_tag_name, action_name, conf)
{
}

Expand All @@ -41,8 +41,7 @@ BT::NodeStatus ComputePathToPoseAction::on_success()
{
setOutput("path", result_.result->path);
// Set empty error code, action was successful
result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE;
setOutput("compute_path_to_pose_error_code", result_.result->error_code);
setOutput("compute_path_to_pose_error_code", ActionGoal::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand All @@ -59,8 +58,7 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled()
nav_msgs::msg::Path empty_path;
setOutput("path", empty_path);
// Set empty error code, action was cancelled
result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE;
setOutput("compute_path_to_pose_error_code", result_.result->error_code);
setOutput("compute_path_to_pose_error_code", ActionGoal::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand Down
7 changes: 7 additions & 0 deletions nav2_core/include/nav2_core/planner_exceptions.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,13 @@ class PlannerTFError : public PlannerException
: PlannerException(description) {}
};

class NoWaypointsGiven : public PlannerException
{
public:
explicit NoWaypointsGiven(const std::string & description)
: PlannerException(description) {}
};

} // namespace nav2_core

#endif // NAV2_CORE__PLANNER_EXCEPTIONS_HPP_
14 changes: 14 additions & 0 deletions nav2_msgs/action/ComputePathThroughPoses.action
Original file line number Diff line number Diff line change
@@ -1,3 +1,16 @@
# Error codes
# Note: The expected priority order of the errors should match the message order
int16 NONE=0
int16 UNKNOWN=1
int16 TF_ERROR=2
int16 START_OUTSIDE_MAP=3
int16 GOAL_OUTSIDE_MAP=4
int16 START_OCCUPIED=5
int16 GOAL_OCCUPIED=6
int16 TIMEOUT=7
int16 NO_VALID_PATH=8
int16 NO_WAYPOINTS_GIVEN=9
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved

#goal definition
geometry_msgs/PoseStamped[] goals
geometry_msgs/PoseStamped start
Expand All @@ -7,5 +20,6 @@ bool use_start # If false, use current robot pose as path start, if true, use st
#result definition
nav_msgs/Path path
builtin_interfaces/Duration planning_time
int16 error_code
---
#feedback definition
2 changes: 2 additions & 0 deletions nav2_planner/include/nav2_planner/planner_server.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,9 @@ class PlannerServer : public nav2_util::LifecycleNode
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;

using ActionToPose = nav2_msgs::action::ComputePathToPose;
using ActionToPoseGoal = ActionToPose::Goal;
using ActionThroughPoses = nav2_msgs::action::ComputePathThroughPoses;
using ActionThroughPosesGoal = ActionThroughPoses::Goal;
using ActionServerToPose = nav2_util::SimpleActionServer<ActionToPose>;
using ActionServerThroughPoses = nav2_util::SimpleActionServer<ActionThroughPoses>;

Expand Down
69 changes: 49 additions & 20 deletions nav2_planner/src/planner_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -336,11 +336,13 @@ void PlannerServer::computePlanThroughPoses()

auto start_time = steady_clock_.now();

// Initialize the ComputePathToPose goal and result
// Initialize the ComputePathThroughPoses goal and result
auto goal = action_server_poses_->get_current_goal();
auto result = std::make_shared<ActionThroughPoses::Result>();
nav_msgs::msg::Path concat_path;

geometry_msgs::msg::PoseStamped curr_start, curr_goal;
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved

try {
if (isServerInactive(action_server_poses_) || isCancelRequested(action_server_poses_)) {
return;
Expand All @@ -350,11 +352,12 @@ void PlannerServer::computePlanThroughPoses()

getPreemptedGoalIfRequested(action_server_poses_, goal);

if (goal->goals.size() == 0) {
if (goal->goals.empty()) {
RCLCPP_WARN(
get_logger(),
"Compute path through poses requested a plan with no viapoint poses, returning.");
"Compute path through poses requested a plan with no waypoint poses, returning.");
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
action_server_poses_->terminate_current();
return;
}

// Use start pose if provided otherwise use current robot pose
Expand All @@ -364,8 +367,6 @@ void PlannerServer::computePlanThroughPoses()
}

// Get consecutive paths through these points
std::vector<geometry_msgs::msg::PoseStamped>::iterator goal_iter;
geometry_msgs::msg::PoseStamped curr_start, curr_goal;
for (unsigned int i = 0; i != goal->goals.size(); i++) {
// Get starting point
if (i == 0) {
Expand Down Expand Up @@ -408,13 +409,42 @@ void PlannerServer::computePlanThroughPoses()
}

action_server_poses_->succeeded_current(result);
} catch (nav2_core::StartOccupied & ex) {
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
result->error_code = ActionThroughPosesGoal::START_OCCUPIED;
action_server_poses_->terminate_current(result);
} catch (nav2_core::GoalOccupied & ex) {
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
result->error_code = ActionThroughPosesGoal::GOAL_OCCUPIED;
action_server_poses_->terminate_current(result);
} catch (nav2_core::NoValidPathCouldBeFound & ex) {
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
result->error_code = ActionThroughPosesGoal::NO_VALID_PATH;
action_server_poses_->terminate_current(result);
} catch (nav2_core::PlannerTimedOut & ex) {
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
result->error_code = ActionThroughPosesGoal::TIMEOUT;
action_server_poses_->terminate_current(result);
} catch (nav2_core::StartOutsideMapBounds & ex) {
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
result->error_code = ActionThroughPosesGoal::START_OUTSIDE_MAP;
action_server_poses_->terminate_current(result);
} catch (nav2_core::GoalOutsideMapBounds & ex) {
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
result->error_code = ActionThroughPosesGoal::GOAL_OUTSIDE_MAP;
action_server_poses_->terminate_current(result);
} catch (nav2_core::PlannerTFError & ex) {
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
result->error_code = ActionThroughPosesGoal::TF_ERROR;
action_server_poses_->terminate_current(result);
} catch (nav2_core::NoWaypointsGiven & ex) {
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
result->error_code = ActionThroughPosesGoal::NO_WAYPOINTS_GIVEN;
action_server_poses_->terminate_current(result);
} catch (std::exception & ex) {
RCLCPP_WARN(
get_logger(),
"%s plugin failed to plan through %zu points with final goal (%.2f, %.2f): \"%s\"",
goal->planner_id.c_str(), goal->goals.size(), goal->goals.back().pose.position.x,
goal->goals.back().pose.position.y, ex.what());
action_server_poses_->terminate_current();
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
result->error_code = ActionThroughPosesGoal::UNKNOWN;
action_server_poses_->terminate_current(result);
}
}

Expand Down Expand Up @@ -469,39 +499,38 @@ PlannerServer::computePlan()
"Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz",
1 / max_planner_duration_, 1 / cycle_duration.seconds());
}

action_server_pose_->succeeded_current(result);
} catch (nav2_core::StartOccupied & ex) {
exceptionWarning(start, goal->goal, goal->planner_id, ex);
result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OCCUPIED;
result->error_code = ActionToPoseGoal::START_OCCUPIED;
action_server_pose_->terminate_current(result);
} catch (nav2_core::GoalOccupied & ex) {
exceptionWarning(start, goal->goal, goal->planner_id, ex);
result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OCCUPIED;
result->error_code = ActionToPoseGoal::GOAL_OCCUPIED;
action_server_pose_->terminate_current(result);
} catch (nav2_core::NoValidPathCouldBeFound & ex) {
exceptionWarning(start, goal->goal, goal->planner_id, ex);
result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NO_VALID_PATH;
result->error_code = ActionToPoseGoal::NO_VALID_PATH;
action_server_pose_->terminate_current(result);
} catch (nav2_core::PlannerTimedOut & ex) {
exceptionWarning(start, goal->goal, goal->planner_id, ex);
result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TIMEOUT;
result->error_code = ActionToPoseGoal::TIMEOUT;
action_server_pose_->terminate_current(result);
} catch (nav2_core::StartOutsideMapBounds & ex) {
exceptionWarning(start, goal->goal, goal->planner_id, ex);
result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OUTSIDE_MAP;
result->error_code = ActionToPoseGoal::START_OUTSIDE_MAP;
action_server_pose_->terminate_current(result);
} catch (nav2_core::GoalOutsideMapBounds & ex) {
exceptionWarning(start, goal->goal, goal->planner_id, ex);
result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OUTSIDE_MAP;
result->error_code = ActionToPoseGoal::GOAL_OUTSIDE_MAP;
action_server_pose_->terminate_current(result);
} catch (nav2_core::PlannerTFError & ex) {
exceptionWarning(start, goal->goal, goal->planner_id, ex);
result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TF_ERROR;
result->error_code = ActionToPoseGoal::TF_ERROR;
action_server_pose_->terminate_current(result);
} catch (std::exception & ex) {
exceptionWarning(start, goal->goal, goal->planner_id, ex);
result->error_code = nav2_msgs::action::ComputePathToPose::Goal::UNKNOWN;
result->error_code = ActionToPoseGoal::UNKNOWN;
action_server_pose_->terminate_current(result);
}
}
Expand Down