Skip to content

Commit 54576b8

Browse files
jwallace42RBT22
jwallace42
authored and
RBT22
committed
exceptions for compute path through poses (ros-navigation#3248)
* exceptions for compute path through poses * lint fix * code review * code review Co-authored-by: Joshua Wallace <josho.wallace.com>
1 parent 8d6ae3c commit 54576b8

File tree

9 files changed

+96
-33
lines changed

9 files changed

+96
-33
lines changed

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,10 @@ namespace nav2_behavior_tree
3232
class ComputePathThroughPosesAction
3333
: public BtActionNode<nav2_msgs::action::ComputePathThroughPoses>
3434
{
35+
using Action = nav2_msgs::action::ComputePathThroughPoses;
36+
using ActionResult = Action::Result;
37+
using ActionGoal = Action::Goal;
38+
3539
public:
3640
/**
3741
* @brief A constructor for nav2_behavior_tree::ComputePathThroughPosesAction
@@ -72,7 +76,6 @@ class ComputePathThroughPosesAction
7276
{
7377
return providedBasicPorts(
7478
{
75-
BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathThroughPoses node"),
7679
BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
7780
"goals",
7881
"Destinations to plan through"),
@@ -81,6 +84,9 @@ class ComputePathThroughPosesAction
8184
BT::InputPort<std::string>(
8285
"planner_id", "",
8386
"Mapped name to the planner plugin type to use"),
87+
BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathThroughPoses node"),
88+
BT::OutputPort<ActionResult::_error_code_type>(
89+
"compute_path_through_poses_error_code", "The compute path through poses error code"),
8490
});
8591
}
8692
};

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,10 @@ namespace nav2_behavior_tree
2929
*/
3030
class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePathToPose>
3131
{
32+
using Action = nav2_msgs::action::ComputePathToPose;
33+
using ActionResult = Action::Result;
34+
using ActionGoal = Action::Goal;
35+
3236
public:
3337
/**
3438
* @brief A constructor for nav2_behavior_tree::ComputePathToPoseAction
@@ -57,7 +61,7 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
5761
BT::NodeStatus on_aborted() override;
5862

5963
/**
60-
* @brief Function to perform some user-defined operation upon cancelation of the action
64+
* @brief Function to perform some user-defined operation upon cancellation of the action
6165
*/
6266
BT::NodeStatus on_cancelled() override;
6367

@@ -74,15 +78,15 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
7478
{
7579
return providedBasicPorts(
7680
{
77-
BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
78-
BT::OutputPort<nav2_msgs::action::ComputePathToPose::Result::_error_code_type>(
79-
"compute_path_to_pose_error_code", "The compute path to pose error code"),
8081
BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
8182
BT::InputPort<geometry_msgs::msg::PoseStamped>(
8283
"start", "Start pose of the path if overriding current robot pose"),
8384
BT::InputPort<std::string>(
8485
"planner_id", "",
8586
"Mapped name to the planner plugin type to use"),
87+
BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
88+
BT::OutputPort<ActionResult::_error_code_type>(
89+
"compute_path_to_pose_error_code", "The compute path to pose error code"),
8690
});
8791
}
8892
};

‎nav2_behavior_tree/nav2_tree_nodes.xml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -87,6 +87,8 @@
8787
<input_port name="server_timeout">Server timeout</input_port>
8888
<input_port name="planner_id">Mapped name to the planner plugin type to use</input_port>
8989
<output_port name="path">Path created by ComputePathToPose node</output_port>
90+
<output_port name="compute_path_through_poses_error_code">"The compute path through poses
91+
pose error code"</output_port>
9092
</Action>
9193

9294
<Action ID="RemovePassedGoals">

‎nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -41,20 +41,25 @@ void ComputePathThroughPosesAction::on_tick()
4141
BT::NodeStatus ComputePathThroughPosesAction::on_success()
4242
{
4343
setOutput("path", result_.result->path);
44+
// Set empty error code, action was successful
45+
setOutput("compute_path_through_poses_error_code", ActionGoal::NONE);
4446
return BT::NodeStatus::SUCCESS;
4547
}
4648

4749
BT::NodeStatus ComputePathThroughPosesAction::on_aborted()
4850
{
4951
nav_msgs::msg::Path empty_path;
5052
setOutput("path", empty_path);
53+
setOutput("compute_path_through_poses_error_code", result_.result->error_code);
5154
return BT::NodeStatus::FAILURE;
5255
}
5356

5457
BT::NodeStatus ComputePathThroughPosesAction::on_cancelled()
5558
{
5659
nav_msgs::msg::Path empty_path;
5760
setOutput("path", empty_path);
61+
// Set empty error code, action was cancelled
62+
setOutput("compute_path_through_poses_error_code", ActionGoal::NONE);
5863
return BT::NodeStatus::SUCCESS;
5964
}
6065

‎nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp‎

Lines changed: 3 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ ComputePathToPoseAction::ComputePathToPoseAction(
2424
const std::string & xml_tag_name,
2525
const std::string & action_name,
2626
const BT::NodeConfiguration & conf)
27-
: BtActionNode<nav2_msgs::action::ComputePathToPose>(xml_tag_name, action_name, conf)
27+
: BtActionNode<Action>(xml_tag_name, action_name, conf)
2828
{
2929
}
3030

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

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

‎nav2_core/include/nav2_core/planner_exceptions.hpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -99,6 +99,13 @@ class PlannerTFError : public PlannerException
9999
: PlannerException(description) {}
100100
};
101101

102+
class NoViapointsGiven : public PlannerException
103+
{
104+
public:
105+
explicit NoViapointsGiven(const std::string & description)
106+
: PlannerException(description) {}
107+
};
108+
102109
} // namespace nav2_core
103110

104111
#endif // NAV2_CORE__PLANNER_EXCEPTIONS_HPP_

‎nav2_msgs/action/ComputePathThroughPoses.action‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,16 @@
1+
# Error codes
2+
# Note: The expected priority order of the errors should match the message order
3+
int16 NONE=0
4+
int16 UNKNOWN=1
5+
int16 TF_ERROR=2
6+
int16 START_OUTSIDE_MAP=3
7+
int16 GOAL_OUTSIDE_MAP=4
8+
int16 START_OCCUPIED=5
9+
int16 GOAL_OCCUPIED=6
10+
int16 TIMEOUT=7
11+
int16 NO_VALID_PATH=8
12+
int16 NO_VIAPOINTS_GIVEN=9
13+
114
#goal definition
215
geometry_msgs/PoseStamped[] goals
316
geometry_msgs/PoseStamped start
@@ -7,5 +20,6 @@ bool use_start # If false, use current robot pose as path start, if true, use st
720
#result definition
821
nav_msgs/Path path
922
builtin_interfaces/Duration planning_time
23+
int16 error_code
1024
---
1125
#feedback definition

‎nav2_planner/include/nav2_planner/planner_server.hpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -107,7 +107,9 @@ class PlannerServer : public nav2_util::LifecycleNode
107107
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
108108

109109
using ActionToPose = nav2_msgs::action::ComputePathToPose;
110+
using ActionToPoseGoal = ActionToPose::Goal;
110111
using ActionThroughPoses = nav2_msgs::action::ComputePathThroughPoses;
112+
using ActionThroughPosesGoal = ActionThroughPoses::Goal;
111113
using ActionServerToPose = nav2_util::SimpleActionServer<ActionToPose>;
112114
using ActionServerThroughPoses = nav2_util::SimpleActionServer<ActionThroughPoses>;
113115

‎nav2_planner/src/planner_server.cpp‎

Lines changed: 48 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -364,11 +364,13 @@ void PlannerServer::computePlanThroughPoses()
364364

365365
auto start_time = steady_clock_.now();
366366

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

372+
geometry_msgs::msg::PoseStamped curr_start, curr_goal;
373+
372374
try {
373375
if (isServerInactive(action_server_poses_) || isCancelRequested(action_server_poses_)) {
374376
return;
@@ -378,11 +380,8 @@ void PlannerServer::computePlanThroughPoses()
378380

379381
getPreemptedGoalIfRequested(action_server_poses_, goal);
380382

381-
if (goal->goals.size() == 0) {
382-
RCLCPP_WARN(
383-
get_logger(),
384-
"Compute path through poses requested a plan with no viapoint poses, returning.");
385-
action_server_poses_->terminate_current();
383+
if (goal->goals.empty()) {
384+
throw nav2_core::NoViapointsGiven("No viapoints given");
386385
}
387386

388387
// Use start pose if provided otherwise use current robot pose
@@ -392,8 +391,6 @@ void PlannerServer::computePlanThroughPoses()
392391
}
393392

394393
// Get consecutive paths through these points
395-
std::vector<geometry_msgs::msg::PoseStamped>::iterator goal_iter;
396-
geometry_msgs::msg::PoseStamped curr_start, curr_goal;
397394
for (unsigned int i = 0; i != goal->goals.size(); i++) {
398395
// Get starting point
399396
if (i == 0) {
@@ -436,13 +433,42 @@ void PlannerServer::computePlanThroughPoses()
436433
}
437434

438435
action_server_poses_->succeeded_current(result);
436+
} catch (nav2_core::StartOccupied & ex) {
437+
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
438+
result->error_code = ActionThroughPosesGoal::START_OCCUPIED;
439+
action_server_poses_->terminate_current(result);
440+
} catch (nav2_core::GoalOccupied & ex) {
441+
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
442+
result->error_code = ActionThroughPosesGoal::GOAL_OCCUPIED;
443+
action_server_poses_->terminate_current(result);
444+
} catch (nav2_core::NoValidPathCouldBeFound & ex) {
445+
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
446+
result->error_code = ActionThroughPosesGoal::NO_VALID_PATH;
447+
action_server_poses_->terminate_current(result);
448+
} catch (nav2_core::PlannerTimedOut & ex) {
449+
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
450+
result->error_code = ActionThroughPosesGoal::TIMEOUT;
451+
action_server_poses_->terminate_current(result);
452+
} catch (nav2_core::StartOutsideMapBounds & ex) {
453+
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
454+
result->error_code = ActionThroughPosesGoal::START_OUTSIDE_MAP;
455+
action_server_poses_->terminate_current(result);
456+
} catch (nav2_core::GoalOutsideMapBounds & ex) {
457+
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
458+
result->error_code = ActionThroughPosesGoal::GOAL_OUTSIDE_MAP;
459+
action_server_poses_->terminate_current(result);
460+
} catch (nav2_core::PlannerTFError & ex) {
461+
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
462+
result->error_code = ActionThroughPosesGoal::TF_ERROR;
463+
action_server_poses_->terminate_current(result);
464+
} catch (nav2_core::NoViapointsGiven & ex) {
465+
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
466+
result->error_code = ActionThroughPosesGoal::NO_VIAPOINTS_GIVEN;
467+
action_server_poses_->terminate_current(result);
439468
} catch (std::exception & ex) {
440-
RCLCPP_WARN(
441-
get_logger(),
442-
"%s plugin failed to plan through %zu points with final goal (%.2f, %.2f): \"%s\"",
443-
goal->planner_id.c_str(), goal->goals.size(), goal->goals.back().pose.position.x,
444-
goal->goals.back().pose.position.y, ex.what());
445-
action_server_poses_->terminate_current();
469+
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
470+
result->error_code = ActionThroughPosesGoal::UNKNOWN;
471+
action_server_poses_->terminate_current(result);
446472
}
447473
}
448474

@@ -497,39 +523,38 @@ PlannerServer::computePlan()
497523
"Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz",
498524
1 / max_planner_duration_, 1 / cycle_duration.seconds());
499525
}
500-
501526
action_server_pose_->succeeded_current(result);
502527
} catch (nav2_core::StartOccupied & ex) {
503528
exceptionWarning(start, goal->goal, goal->planner_id, ex);
504-
result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OCCUPIED;
529+
result->error_code = ActionToPoseGoal::START_OCCUPIED;
505530
action_server_pose_->terminate_current(result);
506531
} catch (nav2_core::GoalOccupied & ex) {
507532
exceptionWarning(start, goal->goal, goal->planner_id, ex);
508-
result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OCCUPIED;
533+
result->error_code = ActionToPoseGoal::GOAL_OCCUPIED;
509534
action_server_pose_->terminate_current(result);
510535
} catch (nav2_core::NoValidPathCouldBeFound & ex) {
511536
exceptionWarning(start, goal->goal, goal->planner_id, ex);
512-
result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NO_VALID_PATH;
537+
result->error_code = ActionToPoseGoal::NO_VALID_PATH;
513538
action_server_pose_->terminate_current(result);
514539
} catch (nav2_core::PlannerTimedOut & ex) {
515540
exceptionWarning(start, goal->goal, goal->planner_id, ex);
516-
result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TIMEOUT;
541+
result->error_code = ActionToPoseGoal::TIMEOUT;
517542
action_server_pose_->terminate_current(result);
518543
} catch (nav2_core::StartOutsideMapBounds & ex) {
519544
exceptionWarning(start, goal->goal, goal->planner_id, ex);
520-
result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OUTSIDE_MAP;
545+
result->error_code = ActionToPoseGoal::START_OUTSIDE_MAP;
521546
action_server_pose_->terminate_current(result);
522547
} catch (nav2_core::GoalOutsideMapBounds & ex) {
523548
exceptionWarning(start, goal->goal, goal->planner_id, ex);
524-
result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OUTSIDE_MAP;
549+
result->error_code = ActionToPoseGoal::GOAL_OUTSIDE_MAP;
525550
action_server_pose_->terminate_current(result);
526551
} catch (nav2_core::PlannerTFError & ex) {
527552
exceptionWarning(start, goal->goal, goal->planner_id, ex);
528-
result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TF_ERROR;
553+
result->error_code = ActionToPoseGoal::TF_ERROR;
529554
action_server_pose_->terminate_current(result);
530555
} catch (std::exception & ex) {
531556
exceptionWarning(start, goal->goal, goal->planner_id, ex);
532-
result->error_code = nav2_msgs::action::ComputePathToPose::Goal::UNKNOWN;
557+
result->error_code = ActionToPoseGoal::UNKNOWN;
533558
action_server_pose_->terminate_current(result);
534559
}
535560
}

0 commit comments

Comments
 (0)