Ability to get a path from the planner from any 2 poses (start_pose and goal): add use_start_pose and start_pose to ComputePathToPose action

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

@@ -42,6 +42,8 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
       {
         BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
         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<std::string>("planner_id", ""),
       });
   }

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -18,6 +18,7 @@
 
     <Action ID="ComputePathToPose">
       <input_port name="goal">Destination to plan to</input_port>
+      <input_port name="start_pose">Start pose of the path if overriding current robot pose</input_port>
       <output_port name="path">Path created by ComputePathToPose node</output_port>
       <input_port name="planner_id"/>
     </Action>

nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp

@@ -32,6 +32,9 @@ void ComputePathToPoseAction::on_tick()
 {
   getInput("goal", goal_.pose);
   getInput("planner_id", goal_.planner_id);
+  if (getInput("start", goal_.start)) {
+    goal_.use_start_pose = true;
+  }
 }
 
 BT::NodeStatus ComputePathToPoseAction::on_success()

nav2_msgs/action/ComputePathToPose.action

@@ -1,6 +1,8 @@
 #goal definition
 geometry_msgs/PoseStamped pose
+geometry_msgs/PoseStamped start
 string planner_id
+bool use_start_pose
 ---
 #result definition
 nav_msgs/Path path

nav2_planner/src/planner_server.cpp

@@ -232,8 +232,11 @@ PlannerServer::computePlan()
       r.sleep();
     }
 
+    // Use start_pose if provided otherwise use current robot pose
     geometry_msgs::msg::PoseStamped start;
-    if (!costmap_ros_->getRobotPose(start)) {
+    if (goal->use_start_pose) {
+      start = goal->start;
+    } else if (!costmap_ros_->getRobotPose(start)) {
       action_server_->terminate_current();
       return;
     }