Write FollowPath Feedback to blackboard

Author: adivardi

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp

@@ -78,15 +78,18 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
    */
   static BT::PortsList providedPorts()
   {
-    return providedBasicPorts(
-      {
+    return providedBasicPorts({
         BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
         BT::InputPort<std::string>("controller_id", ""),
         BT::InputPort<std::string>("goal_checker_id", ""),
         BT::InputPort<std::string>("progress_checker_id", ""),
-        BT::OutputPort<ActionResult::_error_code_type>(
-          "error_code_id", "The follow path error code"),
-      });
+        BT::OutputPort<double>("distance_to_goal", "Current distance_to_goal from feedback"),
+        BT::OutputPort<uint64_t>(
+            "closest_path_pose_idx",
+            "Current index of the pose on global path closest to current robot pose from feedback"),
+        BT::OutputPort<double>("distance_to_path", "Current distance_to_path from feedback"),
+        BT::OutputPort<ActionResult::_error_code_type>("error_code_id", "The follow path error code"),
+    });
   }
 };
 

nav2_behavior_tree/plugins/action/follow_path_action.cpp

@@ -56,7 +56,7 @@ BT::NodeStatus FollowPathAction::on_cancelled()
 }
 
 void FollowPathAction::on_wait_for_result(
-  std::shared_ptr<const Action::Feedback>/*feedback*/)
+  std::shared_ptr<const Action::Feedback> feedback)
 {
   // Grab the new path
   nav_msgs::msg::Path new_path;
@@ -92,6 +92,16 @@ void FollowPathAction::on_wait_for_result(
     goal_.progress_checker_id = new_progress_checker_id;
     goal_updated_ = true;
   }
+
+  if (!goal_updated_ && feedback) // feedback may be nullptr if no new feedback was received
+  {
+    setOutput("distance_to_goal", static_cast<double>(feedback->distance_to_goal));
+    setOutput("closest_path_pose_idx", feedback->closest_path_pose_idx);
+    setOutput("distance_to_path", static_cast<double>(feedback->distance_to_path));
+    // std::cout << "feedback:" << "distance_to_goal: " << feedback->distance_to_goal
+    //           << "closest_path_pose_idx: " << feedback->closest_path_pose_idx
+    //           << "distance_to_path: " << feedback->distance_to_path << std::endl;
+  }
 }
 
 }  // namespace nav2_behavior_tree