Add feedback in navigation2 actions (#1736)

* adding navigate to pose feedback and remove random crawl from master

* adding controller feedback

* recovery feedback actions

* Update nav2_controller/src/nav2_controller.cpp

Co-Authored-By: Carl Delsey <1828778+cdelsey@users.noreply.github.com>

* Add feedback in wait action and make general improvements

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Fix rebase errors

* Make feedback reset across different goals

Co-authored-by: stevemacenski <stevenmacenski@gmail.com>
Co-authored-by: Carl Delsey <1828778+cdelsey@users.noreply.github.com>

nav2_behavior_tree/CMakeLists.txt

@@ -98,9 +98,6 @@ list(APPEND plugin_libs nav2_recovery_node_bt_node)
 add_library(nav2_navigate_to_pose_action_bt_node SHARED plugins/action/navigate_to_pose_action.cpp)
 list(APPEND plugin_libs nav2_navigate_to_pose_action_bt_node)
 
-add_library(nav2_random_crawl_action_bt_node SHARED plugins/action/random_crawl_action.cpp)
-list(APPEND plugin_libs nav2_random_crawl_action_bt_node)
-
 add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp)
 list(APPEND plugin_libs nav2_pipeline_sequence_bt_node)
 

nav2_behavior_tree/README.md

@@ -75,7 +75,6 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a
 | NavigateToPose | Action | Invokes the NavigateToPose ROS2 action server, which is implemented by the bt_navigator module. |
 | RateController | Decorator | A node that throttles the tick rate for its child. The tick rate can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `RateController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. |
 | DistanceController | Decorator | A node that controls the tick rate for its child based on the distance traveled. The distance to be traveled before replanning can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `DistanceController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. |
-| RandomCrawl | Action | This BT action invokes the RandomCrawl ROS2 action server, which is implemented by the nav2_experimental/nav2_rl/nav2_turtlebot3_rl (in the nav2_experimental branch) experimental module. The RandomCrawl action server will direct the robot to randomly navigate its environment without hitting any obstacles. |
 | RecoveryNode | Control | The RecoveryNode is a control flow node with two children.  It returns SUCCESS if and only if the first child returns SUCCESS. The second child will be executed only if the first child returns FAILURE. If the second child SUCCEEDS, then the first child will be executed again. The user can specify how many times the recovery actions should be taken before returning FAILURE. In nav2, the RecoveryNode is included in Behavior Trees to implement recovery actions upon failures.
 | Spin | Action | Invokes the Spin ROS2 action server, which is implemented by the nav2_recoveries module. This action is using in nav2 Behavior Trees as a recovery behavior. |
 | PipelineSequence | Control | Ticks the first child till it succeeds, then ticks the first and second children till the second one succeeds. It then ticks the first, second, and third children until the third succeeds, and so on, and so on. If at any time a child returns RUNNING, that doesn't change the behavior. If at any time a child returns FAILURE, that stops all children and returns FAILURE overall.|

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -179,7 +179,7 @@ class BtActionNode : public BT::ActionNodeBase
     if (should_cancel_goal()) {
       auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
       if (rclcpp::spin_until_future_complete(node_, future_cancel) !=
-        rclcpp::executor::FutureReturnCode::SUCCESS)
+        rclcpp::FutureReturnCode::SUCCESS)
       {
         RCLCPP_ERROR(
           node_->get_logger(),
@@ -202,13 +202,8 @@ class BtActionNode : public BT::ActionNodeBase
     auto status = goal_handle_->get_status();
 
     // Check if the goal is still executing
-    if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
-      status == action_msgs::msg::GoalStatus::STATUS_EXECUTING)
-    {
-      return true;
-    }
-
-    return false;
+    return status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
+           status == action_msgs::msg::GoalStatus::STATUS_EXECUTING;
   }
 
 
@@ -238,6 +233,14 @@ class BtActionNode : public BT::ActionNodeBase
     }
   }
 
+  void increment_recovery_count()
+  {
+    int recovery_count = 0;
+    config().blackboard->get<int>("number_recoveries", recovery_count);  // NOLINT
+    recovery_count += 1;
+    config().blackboard->set<int>("number_recoveries", recovery_count);  // NOLINT
+  }
+
   std::string action_name_;
   typename std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_;
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -120,6 +120,14 @@ class BtServiceNode : public BT::SyncActionNode
   }
 
 protected:
+  void increment_recovery_count()
+  {
+    int recovery_count = 0;
+    config().blackboard->get<int>("number_recoveries", recovery_count);  // NOLINT
+    recovery_count += 1;
+    config().blackboard->set<int>("number_recoveries", recovery_count);  // NOLINT
+  }
+
   std::string service_name_, service_node_name_;
   typename std::shared_ptr<rclcpp::Client<ServiceT>> service_client_;
   std::shared_ptr<typename ServiceT::Request> request_;

nav2_behavior_tree/plugins/action/back_up_action.cpp

@@ -51,6 +51,11 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
     goal_.speed = speed;
   }
 
+  void on_tick() override
+  {
+    increment_recovery_count();
+  }
+
   static BT::PortsList providedPorts()
   {
     return providedBasicPorts(

nav2_behavior_tree/plugins/action/clear_costmap_service.cpp

@@ -34,6 +34,11 @@ class ClearEntireCostmapService : public BtServiceNode<nav2_msgs::srv::ClearEnti
   : BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>(service_node_name, conf)
   {
   }
+
+  void on_tick() override
+  {
+    increment_recovery_count();
+  }
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/plugins/action/spin_action.cpp

@@ -42,6 +42,11 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
     goal_.target_yaw = dist;
   }
 
+  void on_tick() override
+  {
+    increment_recovery_count();
+  }
+
   static BT::PortsList providedPorts()
   {
     return providedBasicPorts(

nav2_behavior_tree/plugins/action/wait_action.cpp

@@ -46,6 +46,11 @@ class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
     goal_.time.sec = duration;
   }
 
+  void on_tick() override
+  {
+    increment_recovery_count();
+  }
+
   // Any BT node that accepts parameters must provide a requiredNodeParameters method
   static BT::PortsList providedPorts()
   {

nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp

@@ -80,11 +80,7 @@ class GoalReachedCondition : public BT::ConditionNode
     double dx = goal.pose.position.x - current_pose.pose.position.x;
     double dy = goal.pose.position.y - current_pose.pose.position.y;
 
-    if ( (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_) ) {
-      return true;
-    } else {
-      return false;
-    }
+    return (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_);
   }
 
   static BT::PortsList providedPorts()

nav2_behavior_tree/plugins/decorator/distance_controller.cpp

@@ -68,8 +68,8 @@ inline BT::NodeStatus DistanceController::tick()
   }
 
   // Get euclidean distance
-  auto travelled = nav2_util::geometry_utils::euclideanDistance(
-    start_pose_.pose.position, current_pose.pose.position);
+  auto travelled = nav2_util::geometry_utils::euclidean_distance(
+    start_pose_.pose, current_pose.pose);
 
   // The child gets ticked the first time through and every time the threshold
   // distance is crossed. In addition, once the child begins to run, it is

nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml

@@ -49,6 +49,8 @@ amcl_rclcpp_node:
 bt_navigator:
   ros__parameters:
     use_sim_time: True
+    global_frame: map
+    robot_base_frame: base_link
     bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node

nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml

@@ -49,6 +49,8 @@ amcl_rclcpp_node:
 bt_navigator:
   ros__parameters:
     use_sim_time: True
+    global_frame: map
+    robot_base_frame: base_link
     bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node

nav2_bringup/bringup/params/nav2_params.yaml

@@ -49,6 +49,8 @@ amcl_rclcpp_node:
 bt_navigator:
   ros__parameters:
     use_sim_time: True
+    global_frame: map
+    robot_base_frame: base_link
     bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node

nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp

@@ -88,7 +88,9 @@ class BtNavigator : public nav2_util::LifecycleNode
    */
   nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override;
 
-  using ActionServer = nav2_util::SimpleActionServer<nav2_msgs::action::NavigateToPose>;
+  using Action = nav2_msgs::action::NavigateToPose;
+
+  using ActionServer = nav2_util::SimpleActionServer<Action>;
 
   // Our action server implements the NavigateToPose action
   std::unique_ptr<ActionServer> action_server_;
@@ -125,14 +127,19 @@ class BtNavigator : public nav2_util::LifecycleNode
   std::vector<std::string> plugin_lib_names_;
 
   // A client that we'll use to send a command message to our own task server
-  rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SharedPtr self_client_;
+  rclcpp_action::Client<Action>::SharedPtr self_client_;
 
   // A regular, non-spinning ROS node that we can use for calls to the action client
   rclcpp::Node::SharedPtr client_node_;
 
   // Spinning transform that can be used by the BT nodes
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  // Metrics for feedback
+  rclcpp::Time start_time_;
+  std::string robot_frame_;
+  std::string global_frame_;
 };
 
 }  // namespace nav2_bt_navigator

nav2_bt_navigator/src/bt_navigator.cpp

@@ -21,14 +21,17 @@
 #include <utility>
 #include <vector>
 
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
 #include "nav2_behavior_tree/bt_conversions.hpp"
 #include "nav2_bt_navigator/ros_topic_logger.hpp"
 
 namespace nav2_bt_navigator
 {
 
 BtNavigator::BtNavigator()
-: nav2_util::LifecycleNode("bt_navigator", "", true)
+: nav2_util::LifecycleNode("bt_navigator", "", true),
+  start_time_(0)
 {
   RCLCPP_INFO(get_logger(), "Creating");
 
@@ -55,6 +58,8 @@ BtNavigator::BtNavigator()
   // Declare this node's parameters
   declare_parameter("bt_xml_filename");
   declare_parameter("plugin_lib_names", plugin_libs);
+  declare_parameter("global_frame", std::string("map"));
+  declare_parameter("robot_base_frame", std::string("base_link"));
 }
 
 BtNavigator::~BtNavigator()
@@ -98,6 +103,8 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
 
   // Get the libraries to pull plugins from
   plugin_lib_names_ = get_parameter("plugin_lib_names").as_string_array();
+  global_frame_ = get_parameter("global_frame").as_string();
+  robot_frame_ = get_parameter("robot_base_frame").as_string();
 
   // Create the class that registers our custom nodes and executes the BT
   bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_);
@@ -111,6 +118,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   blackboard_->set<std::chrono::milliseconds>("server_timeout", std::chrono::milliseconds(10));  // NOLINT
   blackboard_->set<bool>("path_updated", false);  // NOLINT
   blackboard_->set<bool>("initial_pose_received", false);  // NOLINT
+  blackboard_->set<int>("number_recoveries", 0);  // NOLINT
 
   // Get the BT filename to use from the node parameter
   std::string bt_xml_filename;
@@ -217,6 +225,7 @@ BtNavigator::navigateToPose()
     };
 
   RosTopicLogger topic_logger(client_node_, tree_);
+  std::shared_ptr<Action::Feedback> feedback_msg = std::make_shared<Action::Feedback>();
 
   auto on_loop = [&]() {
       if (action_server_->is_preempt_requested()) {
@@ -225,6 +234,22 @@ BtNavigator::navigateToPose()
         initializeGoalPose();
       }
       topic_logger.flush();
+
+      // action server feedback (pose, duration of task,
+      // number of recoveries, and distance remaining to goal)
+      nav2_util::getCurrentPose(feedback_msg->current_pose, *tf_);
+
+      geometry_msgs::msg::PoseStamped goal_pose;
+      blackboard_->get("goal", goal_pose);
+
+      feedback_msg->distance_remaining = nav2_util::geometry_utils::euclidean_distance(
+        feedback_msg->current_pose.pose, goal_pose.pose);
+
+      int recovery_count = 0;
+      blackboard_->get<int>("number_recoveries", recovery_count);
+      feedback_msg->number_of_recoveries = recovery_count;
+      feedback_msg->navigation_time = now() - start_time_;
+      action_server_->publish_feedback(feedback_msg);
     };
 
   // Execute the BT that was previously created in the configure step
@@ -263,6 +288,10 @@ BtNavigator::initializeGoalPose()
     get_logger(), "Begin navigating from current location to (%.2f, %.2f)",
     goal->pose.pose.position.x, goal->pose.pose.position.y);
 
+  // Reset state for new action feedback
+  start_time_ = now();
+  blackboard_->set<int>("number_recoveries", 0);  // NOLINT
+
   // Update the goal pose on the blackboard
   blackboard_->set("goal", goal->pose);
 }

nav2_controller/include/nav2_controller/nav2_controller.hpp

@@ -107,7 +107,8 @@ class ControllerServer : public nav2_util::LifecycleNode
    */
   nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override;
 
-  using ActionServer = nav2_util::SimpleActionServer<nav2_msgs::action::FollowPath>;
+  using Action = nav2_msgs::action::FollowPath;
+  using ActionServer = nav2_util::SimpleActionServer<Action>;
 
   // Our action server implements the FollowPath action
   std::unique_ptr<ActionServer> action_server_;
@@ -215,6 +216,7 @@ class ControllerServer : public nav2_util::LifecycleNode
 
   // Whether we've published the single controller warning yet
   bool single_controller_warning_given_{false};
+  geometry_msgs::msg::Pose end_pose_;
 };
 
 }  // namespace nav2_controller