[nav2_behavior_tree] Add force_use_current_pose to ComputePathToPoseAction

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

@@ -85,7 +85,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",
+          "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)"),
+        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

@@ -78,7 +78,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,37 @@
 {
   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
+  bool bb_use_start;
+  if (getInput("use_start", bb_use_start)) {
+    if (bb_use_start) {
+      // check that we have a "start" pose
+      geometry_msgs::msg::PoseStamped bb_start;
+      if (getInput("start", bb_start)) {
+        goal_.start = bb_start;
+        goal_.use_start = true;
+      } else {
+        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 {
+      goal_.use_start = false;
+    }
+  } else {
+    // else if "use_start" is not provided, but "start" is, then use it in order to not change
+    // the legacy behavior
+    geometry_msgs::msg::PoseStamped bb_start;
+    if (getInput("start", bb_start)) {
+      goal_.start = bb_start;
+      goal_.use_start = true;
+    } else {
+      goal_.use_start = false;
+    }
   }
 }