Skip to content

Commit 5bb0346

Browse files
doisygGuillaume Doisy
andauthored
[nav2_behavior_tree] Add force_use_current_pose to ComputePathToPoseAction (#4925)
* Add force_use_current_pose Signed-off-by: Guillaume Doisy <[email protected]> * xml update Signed-off-by: Guillaume Doisy <[email protected]> * rename to use_start Signed-off-by: Guillaume Doisy <[email protected]> * lint Signed-off-by: Guillaume Doisy <[email protected]> * descriptions Signed-off-by: Guillaume Doisy <[email protected]> * simplify logic Signed-off-by: Guillaume Doisy <[email protected]> --------- Signed-off-by: Guillaume Doisy <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]>
1 parent aed5d97 commit 5bb0346

File tree

3 files changed

+26
-4
lines changed

3 files changed

+26
-4
lines changed

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

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -85,7 +85,11 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
8585
{
8686
BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
8787
BT::InputPort<geometry_msgs::msg::PoseStamped>(
88-
"start", "Start pose of the path if overriding current robot pose"),
88+
"start",
89+
"Used as the planner start pose instead of the current robot pose, if use_start is"
90+
" not false (i.e. not provided or set to true)"),
91+
BT::InputPort<bool>(
92+
"use_start", "For using or not using (i.e. ignoring) the provided start pose"),
8993
BT::InputPort<std::string>(
9094
"planner_id", "",
9195
"Mapped name to the planner plugin type to use"),

nav2_behavior_tree/nav2_tree_nodes.xml

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -78,7 +78,8 @@
7878

7979
<Action ID="ComputePathToPose">
8080
<input_port name="goal">Destination to plan to</input_port>
81-
<input_port name="start">Start pose of the path if overriding current robot pose</input_port>
81+
<input_port name="start">Use as the planner start pose instead of the current robot pose, if use_start is not false (i.e. not provided or set to true)</input_port>
82+
<input_port name="use_start">For using or not using (i.e. ignoring) the provided start pose</input_port>
8283
<input_port name="planner_id">Mapped name to the planner plugin type to use</input_port>
8384
<input_port name="server_name">Server name</input_port>
8485
<input_port name="server_timeout">Server timeout</input_port>

nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp

Lines changed: 19 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -32,8 +32,25 @@ void ComputePathToPoseAction::on_tick()
3232
{
3333
getInput("goal", goal_.goal);
3434
getInput("planner_id", goal_.planner_id);
35-
if (getInput("start", goal_.start)) {
36-
goal_.use_start = true;
35+
36+
// if "use_start" is provided try to enforce it (true or false), but we cannot enforce true if
37+
// start is not provided
38+
goal_.use_start = false;
39+
if (getInput("use_start", goal_.use_start)) {
40+
if (goal_.use_start && !getInput("start", goal_.start)) {
41+
// in case we don't have a "start" pose
42+
goal_.use_start = false;
43+
RCLCPP_ERROR(
44+
node_->get_logger(),
45+
"use_start is set to true but no start pose was provided, falling back to default "
46+
"behavior, i.e. using the current robot pose");
47+
}
48+
} else {
49+
// else if "use_start" is not provided, but "start" is, then use it in order to not change
50+
// the legacy behavior
51+
if (getInput("start", goal_.start)) {
52+
goal_.use_start = true;
53+
}
3754
}
3855
}
3956

0 commit comments

Comments
 (0)