outputting tracker feedback on ComputeAndTrack BT node

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

@@ -99,14 +99,36 @@ 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"),
       });
   }
+
+private:
+  Action::Feedback feedback_;
 };
 
 }  // namespace nav2_behavior_tree

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">"ID of the current edge"</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

@@ -26,6 +26,14 @@
   const BT::NodeConfiguration & conf)
 : BtActionNode<Action>(xml_tag_name, action_name, conf)
 {
+  nav_msgs::msg::Path empty_path;
+  nav2_msgs::msg::Route empty_route;
+  feedback_.last_node_id = 0;
+  feedback_.next_node_id = 0;
+  feedback_.current_edge_id = 0;
+  feedback_.route = empty_route;
+  feedback_.path = empty_path;
+  feedback_.rerouted = false;
 }
 
 void ComputeAndTrackRouteAction::on_tick()
@@ -52,6 +60,12 @@
 
 BT::NodeStatus ComputeAndTrackRouteAction::on_success()
 {
+  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);
   setOutput("execution_duration", result_.result->execution_duration);
   setOutput("error_code_id", ActionResult::NONE);
   setOutput("error_msg", "");
@@ -60,6 +74,20 @@
 
 BT::NodeStatus ComputeAndTrackRouteAction::on_aborted()
 {
+  nav_msgs::msg::Path empty_path;
+  nav2_msgs::msg::Route empty_route;
+  feedback_.last_node_id = 0;
+  feedback_.next_node_id = 0;
+  feedback_.current_edge_id = 0;
+  feedback_.route = empty_route;
+  feedback_.path = empty_path;
+  feedback_.rerouted = false;
+  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);
   setOutput("execution_duration", builtin_interfaces::msg::Duration());
   setOutput("error_code_id", result_.result->error_code);
   setOutput("error_msg", result_.result->error_msg);
@@ -68,6 +96,20 @@
 
 BT::NodeStatus ComputeAndTrackRouteAction::on_cancelled()
 {
+  nav_msgs::msg::Path empty_path;
+  nav2_msgs::msg::Route empty_route;
+  feedback_.last_node_id = 0;
+  feedback_.next_node_id = 0;
+  feedback_.current_edge_id = 0;
+  feedback_.route = empty_route;
+  feedback_.path = empty_path;
+  feedback_.rerouted = false;
+  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);
   // Set empty error code, action was cancelled
   setOutput("execution_duration", builtin_interfaces::msg::Duration());
   setOutput("error_code_id", ActionResult::NONE);
@@ -82,7 +124,7 @@
 }
 
 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 +171,22 @@
   if (goal_updated_) {
     on_tick();
   }
+
+  if (feedback) {
+    feedback_.last_node_id = feedback->last_node_id;
+    feedback_.next_node_id = feedback->next_node_id;
+    feedback_.current_edge_id = feedback->current_edge_id;
+    feedback_.route = feedback->route;
+    feedback_.path = feedback->path;
+    feedback_.rerouted = feedback->rerouted;
+  }
+
+  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