outputting tracker feedback on ComputeAndTrack BT node

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp

@@ -99,12 +99,31 @@ class ComputeAndTrackRouteAction : public BtActionNode<nav2_msgs::action::Comput
           "Whether to use the start pose or the robot's current pose"),
         BT::InputPort<bool>(
           "use_poses", false, "Whether to use poses or IDs for start and goal"),
-        BT::OutputPort<builtin_interfaces::msg::Duration>("execution_duration",
+        BT::OutputPort<builtin_interfaces::msg::Duration>(
+          "execution_duration",
           "Time taken to compute and track route"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "The compute route error code"),
         BT::OutputPort<std::string>(
           "error_msg", "The compute route error msg"),
+        BT::OutputPort<uint16_t>(
+          "last_node_id",
+          "ID of the previous node"),
+        BT::OutputPort<uint16_t>(
+          "next_node_id",
+          "ID of the next node"),
+        BT::OutputPort<uint16_t>(
+          "current_edge_id",
+          "ID of current edge"),
+        BT::OutputPort<nav2_msgs::msg::Route>(
+          "route",
+          "List of RouteNodes to go from start to end"),
+        BT::OutputPort<nav_msgs::msg::Path>(
+          "path",
+          "Path created by ComputeAndTrackRoute node"),
+        BT::OutputPort<bool>(
+          "rerouted",
+          "Whether the plan has rerouted"),
       });
   }
 };

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -135,6 +135,12 @@
       <output_port name="execution_time">Duration to compute route</output_port>
       <output_port name="error_code_id">"Compute route to pose error code"</output_port>
       <output_port name="error_msg">"Compute route to pose error message"</output_port>
+      <output_port name="last_node_id">"ID of the previous node"</output_port>
+      <output_port name="next_node_id">"ID of the next node"</output_port>
+      <output_port name="current_edge_id">"e"</output_port>
+      <output_port name="route">"List of RouteNodes to go from start to end"</output_port>
+      <output_port name="path">"Path created by ComputeAndTrackRoute node"</output_port>
+      <output_port name="rerouted">"Whether the plan has rerouted"</output_port>
     </Action>
 
     <Action ID="RemovePassedGoals">

nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp

@@ -82,7 +82,7 @@ void ComputeAndTrackRouteAction::on_timeout()
 }
 
 void ComputeAndTrackRouteAction::on_wait_for_result(
-  std::shared_ptr<const Action::Feedback>/*feedback*/)
+  std::shared_ptr<const Action::Feedback> feedback)
 {
   // Check for request updates to the goal
   bool use_poses = false, use_start = false;
@@ -129,6 +129,15 @@ void ComputeAndTrackRouteAction::on_wait_for_result(
   if (goal_updated_) {
     on_tick();
   }
+
+  if (feedback) {
+    setOutput("last_node_id", feedback->last_node_id);
+    setOutput("next_node_id", feedback->next_node_id);
+    setOutput("current_edge_id", feedback->current_edge_id);
+    setOutput("route", feedback->route);
+    setOutput("path", feedback->path);
+    setOutput("rerouted", feedback->rerouted);
+  }
 }
 
 }  // namespace nav2_behavior_tree