Feedback in navigation2 actions

nav2_behavior_tree/CMakeLists.txt

@@ -92,9 +92,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

@@ -73,7 +73,6 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a
 | TransformAvailable | Condition | Checks if a TF transform is available. Returns failure if it cannot be found. Once found, it will always return success. Useful for initial condition checks. |
 | 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. |
-| 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_service_node.hpp

@@ -36,6 +36,8 @@ class BtServiceNode : public BT::CoroActionNode
   {
     node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
 
+    request_ = std::make_shared<typename ServiceT::Request>();
+
     // Get the required items from the blackboard
     server_timeout_ =
       config().blackboard->get<std::chrono::milliseconds>("server_timeout");
@@ -104,7 +106,6 @@ class BtServiceNode : public BT::CoroActionNode
   // Fill in service request with information if necessary
   virtual void on_tick()
   {
-    request_ = std::make_shared<typename ServiceT::Request>();
   }
 
   // An opportunity to do something after

nav2_behavior_tree/plugins/action/back_up_action.cpp

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

nav2_behavior_tree/plugins/action/clear_costmap_service.cpp

@@ -34,6 +34,14 @@ class ClearEntireCostmapService : public BtServiceNode<nav2_msgs::srv::ClearEnti
   : BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>(service_node_name, conf)
   {
   }
+
+  void on_tick() override
+  {
+    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
+  }
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/plugins/action/follow_path_action.cpp

@@ -52,7 +52,7 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
     // Check if the goal has been updated
     if (config().blackboard->get<bool>("path_updated")) {
       // Reset the flag in the blackboard
-      config().blackboard->set("path_updated", false);
+      config().blackboard->set("path_updated", false);  // NOLINT
 
       // Grab the new goal and set the flag so that we send the new goal to
       // the action server on the next loop iteration

nav2_behavior_tree/plugins/action/spin_action.cpp

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

nav2_behavior_tree/plugins/action/wait_action.cpp

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

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_;
@@ -123,14 +125,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

@@ -28,7 +28,8 @@ 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");
 
@@ -53,6 +54,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()
@@ -96,6 +99,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_);
@@ -109,6 +114,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;
@@ -216,6 +222,7 @@ BtNavigator::navigateToPose()
   BT::Tree tree = bt_->buildTreeFromText(xml_string_, blackboard_);
 
   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()) {
@@ -224,6 +231,30 @@ BtNavigator::navigateToPose()
         initializeGoalPose();
       }
       topic_logger.flush();
+
+      // action server feedback (pose, duration of task,
+      // number of recoveries, and distance remaining to goal)
+      geometry_msgs::msg::TransformStamped transform;
+      geometry_msgs::msg::PoseStamped current_pose;
+      transform = tf_->lookupTransform(robot_frame_, global_frame_, tf2::TimePointZero);
+      current_pose.header = transform.header;
+      current_pose.pose.position.x = transform.transform.translation.x;
+      current_pose.pose.position.y = transform.transform.translation.y;
+      current_pose.pose.position.z = transform.transform.translation.z;
+      current_pose.pose.orientation = transform.transform.rotation;
+      feedback_msg->current_pose = current_pose;
+
+      geometry_msgs::msg::Point & c_pose = current_pose.pose.position;
+      geometry_msgs::msg::PoseStamped goal_pose;
+      blackboard_->get("goal", goal_pose);
+      feedback_msg->l2_distance_remaining = sqrt(
+        (c_pose.x - goal_pose.pose.position.x) * (c_pose.x - goal_pose.pose.position.x) +
+        (c_pose.y - goal_pose.pose.position.y) * (c_pose.y - goal_pose.pose.position.y));
+      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
@@ -254,6 +285,8 @@ void
 BtNavigator::initializeGoalPose()
 {
   auto goal = action_server_->get_current_goal();
+  start_time_ = now();
+  blackboard_->set<int>("number_recoveries", 0);  // NOLINT
 
   RCLCPP_INFO(
     get_logger(), "Begin navigating from current location to (%.2f, %.2f)",

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

nav2_controller/src/nav2_controller.cpp

@@ -302,6 +302,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path)
   RCLCPP_DEBUG(
     get_logger(), "Path end point is (%.2f, %.2f)",
     end_pose.pose.position.x, end_pose.pose.position.y);
+  end_pose_ = end_pose.pose;
 }
 
 void ControllerServer::computeAndPublishVelocity()
@@ -321,6 +322,13 @@ void ControllerServer::computeAndPublishVelocity()
     pose,
     nav_2d_utils::twist2Dto3D(twist));
 
+  std::shared_ptr<Action::Feedback> feedback = std::make_shared<Action::Feedback>();
+  feedback->speed = std::hypot(cmd_vel_2d.twist.linear.x, cmd_vel_2d.twist.linear.y);
+  feedback->distance_to_goal = sqrt(
+    (end_pose_.position.x - pose.pose.position.x) * (end_pose_.position.x - pose.pose.position.x) +
+    (end_pose_.position.y - pose.pose.position.y) * (end_pose_.position.y - pose.pose.position.y));
+  action_server_->publish_feedback(feedback);
+
   RCLCPP_DEBUG(get_logger(), "Publishing velocity at time %.2f", now().seconds());
   publishVelocity(cmd_vel_2d);
 }

nav2_msgs/CMakeLists.txt

@@ -31,7 +31,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "action/Wait.action"
   "action/Spin.action"
   "action/DummyRecovery.action"
-  "action/RandomCrawl.action"
   "action/FollowWaypoints.action"
   DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs
 )

nav2_msgs/action/BackUp.action

@@ -5,4 +5,4 @@ float32 speed
 #result definition
 std_msgs/Empty result
 ---
-#feedback
+float32 distance_traveled

nav2_msgs/action/FollowPath.action

@@ -6,3 +6,5 @@ string controller_id
 std_msgs/Empty result
 ---
 #feedback
+float32 distance_to_goal
+float32 speed

nav2_msgs/action/NavigateToPose.action

@@ -4,4 +4,7 @@ geometry_msgs/PoseStamped pose
 #result definition
 std_msgs/Empty result
 ---
-#feedback
+geometry_msgs/PoseStamped current_pose
+builtin_interfaces/Duration navigation_time
+int16 number_of_recoveries
+float32 l2_distance_remaining

nav2_msgs/action/Spin.action

@@ -4,4 +4,4 @@ float32 target_yaw
 #result definition
 std_msgs/Empty result
 ---
-#feedback
+float32 angular_distance_traveled

nav2_recoveries/plugins/back_up.cpp

@@ -76,6 +76,11 @@ Status BackUp::onCycleUpdate()
     stopRobot();
     return Status::SUCCEEDED;
   }
+
+  auto feedback = std::make_shared<BackUpAction::Feedback>();
+  feedback->distance_traveled = distance;
+  action_server_->publish_feedback(feedback);
+
   // TODO(mhpanah): cmd_vel value should be passed as a parameter
   geometry_msgs::msg::Twist cmd_vel;
   cmd_vel.linear.y = 0.0;