[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

@@ -80,6 +80,9 @@ 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"),
+        BT::InputPort<bool>(
+          "force_use_current_pose", "Use current robot pose, even if `start` is provided "
+          "(effectively ignoring it)"),
         BT::InputPort<std::string>(
           "planner_id", "",
           "Mapped name to the planner plugin type to use"),

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -77,6 +77,7 @@
     <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="force_use_current_pose">Use current robot pose, even if `start` is provided (effectively ignoring it)</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

@@ -33,7 +33,13 @@ void ComputePathToPoseAction::on_tick()
   getInput("goal", goal_.goal);
   getInput("planner_id", goal_.planner_id);
   if (getInput("start", goal_.start)) {
-    goal_.use_start = true;
+    bool force_use_current_pose = false;
+    getInput("force_use_current_pose", force_use_current_pose);
+    if (force_use_current_pose) {
+      goal_.use_start = false;
+    } else {
+      goal_.use_start = true;
+    }
   }
 }