Improve NavigationToPose action feedback

nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp

@@ -24,6 +24,7 @@
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_msgs/action/navigate_to_pose.hpp"
 #include "nav_msgs/msg/path.hpp"
+#include "nav2_util/odometry_utils.hpp"
 #include "nav2_util/simple_action_server.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "tf2_ros/transform_listener.h"
@@ -130,6 +131,9 @@ class BtNavigator : public nav2_util::LifecycleNode
   // Libraries to pull plugins (BT Nodes) from
   std::vector<std::string> plugin_lib_names_;
 
+  // Odometry smoother object
+  std::unique_ptr<nav2_util::OdomSmoother> odom_smoother_;
+
   // A client that we'll use to send a command message to our own task server
   rclcpp_action::Client<Action>::SharedPtr self_client_;
 

nav2_bt_navigator/src/bt_navigator.cpp

@@ -21,6 +21,7 @@
 #include <utility>
 #include <vector>
 #include <set>
+#include <limits>
 
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
@@ -103,6 +104,9 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
     rclcpp::SystemDefaultsQoS(),
     std::bind(&BtNavigator::onGoalPoseReceived, this, std::placeholders::_1));
 
+  // Odometry smoother object for getting current speed
+  odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(client_node_, 0.3);
+
   action_server_ = std::make_unique<ActionServer>(
     get_node_base_interface(),
     get_node_clock_interface(),
@@ -269,17 +273,56 @@ BtNavigator::navigateToPose()
 
       // action server feedback (pose, duration of task,
       // number of recoveries, and distance remaining to goal)
+      geometry_msgs::msg::PoseStamped current_pose;
       nav2_util::getCurrentPose(
-        feedback_msg->current_pose, *tf_, global_frame_, robot_frame_, transform_tolerance_);
+        current_pose, *tf_, global_frame_, robot_frame_, transform_tolerance_);
 
       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);
+      // Get current path points
+      nav_msgs::msg::Path current_path;
+      blackboard_->get("path", current_path);
+
+      // Find the closest pose to current pose on global path
+      auto find_closest_pose_idx =
+        [&current_pose, &current_path]() {
+          size_t closest_pose_idx = 0;
+          double curr_min_dist = std::numeric_limits<double>::max();
+          for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) {
+            double curr_dist = nav2_util::geometry_utils::euclidean_distance(
+              current_pose, current_path.poses[curr_idx]);
+            if (curr_dist < curr_min_dist) {
+              curr_min_dist = curr_dist;
+              closest_pose_idx = curr_idx;
+            }
+          }
+          return closest_pose_idx;
+        };
+
+      // Calculate distance on the path
+      double distance_remaining =
+        nav2_util::geometry_utils::calculate_path_length(current_path, find_closest_pose_idx());
+
+      // Default value for time remaining
+      rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds(0.0);
+
+      // Get current speed
+      geometry_msgs::msg::Twist current_odom = odom_smoother_->getTwist();
+      double current_linear_speed = hypot(current_odom.linear.x, current_odom.linear.y);
+
+      // Calculate estimated time taken to goal if speed is higher than 1cm/s
+      // and at least 10cm to go
+      if ((std::abs(current_linear_speed) > 0.01) && (distance_remaining > 0.1)) {
+        estimated_time_remaining =
+          rclcpp::Duration::from_seconds(distance_remaining / std::abs(current_linear_speed));
+      }
 
       int recovery_count = 0;
+      feedback_msg->current_pose = current_pose;
       blackboard_->get<int>("number_recoveries", recovery_count);
+      feedback_msg->distance_remaining = distance_remaining;
+      feedback_msg->estimated_time_remaining = estimated_time_remaining;
       feedback_msg->number_of_recoveries = recovery_count;
       feedback_msg->navigation_time = now() - start_time_;
       action_server_->publish_feedback(feedback_msg);

nav2_msgs/action/NavigateToPose.action

@@ -7,5 +7,6 @@ std_msgs/Empty result
 ---
 geometry_msgs/PoseStamped current_pose
 builtin_interfaces/Duration navigation_time
+builtin_interfaces/Duration estimated_time_remaining
 int16 number_of_recoveries
 float32 distance_remaining

nav2_util/include/nav2_util/geometry_utils.hpp

@@ -22,6 +22,7 @@
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "nav_msgs/msg/path.hpp"
 
 namespace nav2_util
 {
@@ -62,6 +63,18 @@ inline double euclidean_distance(
   return euclidean_distance(pos1.pose, pos2.pose);
 }
 
+inline double calculate_path_length(const nav_msgs::msg::Path & path, size_t start_index = 0)
+{
+  if (start_index >= path.poses.size() - 1) {
+    return 0.0;
+  }
+  double path_length = 0.0;
+  for (size_t idx = start_index; idx < path.poses.size() - 1; ++idx) {
+    path_length += euclidean_distance(path.poses[idx].pose, path.poses[idx + 1].pose);
+  }
+  return path_length;
+}
+
 }  // namespace geometry_utils
 }  // namespace nav2_util
 

nav2_util/test/test_geometry_utils.cpp

@@ -16,9 +16,11 @@
 #include "nav2_util/geometry_utils.hpp"
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/pose.hpp"
+#include "nav_msgs/msg/path.hpp"
 #include "gtest/gtest.h"
 
 using nav2_util::geometry_utils::euclidean_distance;
+using nav2_util::geometry_utils::calculate_path_length;
 
 TEST(GeometryUtils, euclidean_distance_point)
 {
@@ -49,3 +51,50 @@ TEST(GeometryUtils, euclidean_distance_pose)
 
   ASSERT_NEAR(euclidean_distance(pose1, pose2), 10.24695, 1e-5);
 }
+
+TEST(GeometryUtils, calculate_path_length)
+{
+  nav_msgs::msg::Path straight_line_path;
+  size_t nb_path_points = 10;
+  float distance_between_poses = 2.0;
+  float current_x_loc = 0.0;
+
+  for (size_t i = 0; i < nb_path_points; ++i) {
+    geometry_msgs::msg::PoseStamped pose_stamped_msg;
+    pose_stamped_msg.pose.position.x = current_x_loc;
+
+    straight_line_path.poses.push_back(pose_stamped_msg);
+
+    current_x_loc += distance_between_poses;
+  }
+
+  ASSERT_NEAR(
+    calculate_path_length(straight_line_path),
+    (nb_path_points - 1) * distance_between_poses, 1e-5);
+
+  ASSERT_NEAR(
+    calculate_path_length(straight_line_path, straight_line_path.poses.size()),
+    0.0, 1e-5);
+
+  nav_msgs::msg::Path circle_path;
+  float polar_distance = 2.0;
+  uint32_t current_polar_angle_deg = 0;
+  constexpr float pi = 3.14159265358979;
+
+  while (current_polar_angle_deg != 360) {
+    float x_loc = polar_distance * std::cos(current_polar_angle_deg * (pi / 180.0));
+    float y_loc = polar_distance * std::sin(current_polar_angle_deg * (pi / 180.0));
+
+    geometry_msgs::msg::PoseStamped pose_stamped_msg;
+    pose_stamped_msg.pose.position.x = x_loc;
+    pose_stamped_msg.pose.position.y = y_loc;
+
+    circle_path.poses.push_back(pose_stamped_msg);
+
+    current_polar_angle_deg += 1;
+  }
+
+  ASSERT_NEAR(
+    calculate_path_length(circle_path),
+    2 * pi * polar_distance, 1e-1);
+}