[nav2_behavior_tree] Add force_use_current_pose to ComputePathToPoseAction (ros-navigation#4925)

* Add force_use_current_pose

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

* xml update

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

* rename to use_start

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

* lint

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

* descriptions

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

* simplify logic

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

---------

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

Author: 2 people

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

@@ -79,7 +79,11 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
       {
         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"),
+          "start",
+          "Used 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)"),
+        BT::InputPort<bool>(
+          "use_start", "For using or not using (i.e. ignoring) the provided start pose"),
         BT::InputPort<std::string>(
           "planner_id", "",
           "Mapped name to the planner plugin type to use"),

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -76,7 +76,8 @@
 
     <Action ID="ComputePathToPose">
       <input_port name="goal">Destination to plan to</input_port>
-      <input_port name="start">Start pose of the path if overriding current robot pose</input_port>
+      <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>
+      <input_port name="use_start">For using or not using (i.e. ignoring) the provided start pose</input_port>
       <input_port name="planner_id">Mapped name to the planner plugin type to use</input_port>
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>

nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp

@@ -32,8 +32,25 @@ void ComputePathToPoseAction::on_tick()
 {
   getInput("goal", goal_.goal);
   getInput("planner_id", goal_.planner_id);
-  if (getInput("start", goal_.start)) {
-    goal_.use_start = true;
+
+  // if "use_start" is provided try to enforce it (true or false), but we cannot enforce true if
+  // start is not provided
+  goal_.use_start = false;
+  if (getInput("use_start", goal_.use_start)) {
+    if (goal_.use_start && !getInput("start", goal_.start)) {
+      // in case we don't have a "start" pose
+      goal_.use_start = false;
+      RCLCPP_ERROR(
+          node_->get_logger(),
+          "use_start is set to true but no start pose was provided, falling back to default "
+          "behavior, i.e. using the current robot pose");
+    }
+  } else {
+    // else if "use_start" is not provided, but "start" is, then use it in order to not change
+    // the legacy behavior
+    if (getInput("start", goal_.start)) {
+      goal_.use_start = true;
+    }
   }
 }