Improve NavigationToPose action feedback (#2246)

* first commit

* Added empty line at end

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* linting

* linting

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* Variable name fix

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* changed shared_ptr to weak_ptr

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* get node handle from blackboard

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* changed initialization of odom smoother object

* changed initialization of odom smoother object

* changed initialization of odom smoother object

* changed initialization of odom smoother object

* addressed comment re std::hypot

* added overloaded constructor

* added overloaded constructor

* added overloaded constructor

* added try catch and template to blackboard get path

Signed-off-by: Deepak Talwar <deepaktalwardt@farmwise.io>

* lint

Signed-off-by: Deepak ...

nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp

@@ -23,6 +23,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_behavior_tree/bt_action_server.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "tf2_ros/buffer.h"
@@ -122,6 +123,9 @@ protected:
   // A client that we'll use to send a command message to our own task server
   rclcpp_action::Client<Action>::SharedPtr self_client_;
 
+  // Odometry smoother object
+  std::unique_ptr<nav2_util::OdomSmoother> odom_smoother_;
+
   // Metrics for feedback
   rclcpp::Time start_time_;
   std::string robot_frame_;

nav2_bt_navigator/src/bt_navigator.cpp

@@ -18,6 +18,7 @@
 #include <string>
 #include <utility>
 #include <set>
+#include <limits>
 #include <vector>
 
 #include "nav2_util/geometry_utils.hpp"
@@ -118,6 +119,9 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   blackboard->set<bool>("initial_pose_received", false);  // NOLINT
   blackboard->set<int>("number_recoveries", 0);  // NOLINT
 
+  // Odometry smoother object for getting current speed
+  odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(shared_from_this(), 0.3);
+
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -208,23 +212,65 @@ void
 BtNavigator::onLoop()
 {
   // action server feedback (pose, duration of task,
-  // number of recoveries, and distance remaining to goal)
+  // time remaining, number of recoveries, and
+  // distance remaining to goal)
   auto feedback_msg = std::make_shared<Action::Feedback>();
 
+  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_);
 
   auto blackboard = bt_action_server_->getBlackboard();
 
-  geometry_msgs::msg::PoseStamped goal_pose;
-  blackboard->get(goal_blackboard_id_, goal_pose);
-
-  feedback_msg->distance_remaining = nav2_util::geometry_utils::euclidean_distance(
-    feedback_msg->current_pose.pose, goal_pose.pose);
+  try {
+    // Get current path points
+    nav_msgs::msg::Path current_path;
+    blackboard->get<nav_msgs::msg::Path>("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 = std::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));
+    }
+
+    feedback_msg->distance_remaining = distance_remaining;
+    feedback_msg->estimated_time_remaining = estimated_time_remaining;
+  } catch (...) {
+    // Ignore
+  }
 
   int recovery_count = 0;
   blackboard->get<int>("number_recoveries", recovery_count);
   feedback_msg->number_of_recoveries = recovery_count;
+  feedback_msg->current_pose = current_pose;
   feedback_msg->navigation_time = now() - start_time_;
 
   bt_action_server_->publishFeedback(feedback_msg);

nav2_controller/include/nav2_controller/nav2_controller.hpp

@@ -237,6 +237,9 @@ protected:
   // Last time the controller generated a valid command
   rclcpp::Time last_valid_cmd_time_;
 
+  // Current path container
+  nav_msgs::msg::Path current_path_;
+
 private:
   /**
     * @brief Callback for speed limiting messages

nav2_controller/src/nav2_controller.cpp

@@ -17,6 +17,7 @@
 #include <memory>
 #include <string>
 #include <utility>
+#include <limits>
 
 #include "nav2_core/exceptions.hpp"
 #include "nav_2d_utils/conversions.hpp"
@@ -373,6 +374,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path)
     get_logger(), "Path end point is (%.2f, %.2f)",
     end_pose.pose.position.x, end_pose.pose.position.y);
   end_pose_ = end_pose.pose;
+  current_path_ = path;
 }
 
 void ControllerServer::computeAndPublishVelocity()
@@ -421,7 +423,26 @@ void ControllerServer::computeAndPublishVelocity()
 
   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 = nav2_util::geometry_utils::euclidean_distance(end_pose_, pose.pose);
+
+  // Find the closest pose to current pose on global path
+  nav_msgs::msg::Path & current_path = current_path_;
+  auto find_closest_pose_idx =
+    [&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(
+          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;
+    };
+
+  feedback->distance_to_goal =
+    nav2_util::geometry_utils::calculate_path_length(current_path_, find_closest_pose_idx());
   action_server_->publish_feedback(feedback);
 
   RCLCPP_DEBUG(get_logger(), "Publishing velocity at time %.2f", now().seconds());

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
 {
@@ -44,7 +45,7 @@ inline geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle)
  * @brief Get the L2 distance between 2 geometry_msgs::Points
  * @param pos1 First point
  * @param pos1 Second point
- * @param double L2 distance
+ * @return double L2 distance
  */
 inline double euclidean_distance(
   const geometry_msgs::msg::Point & pos1,
@@ -60,7 +61,7 @@ inline double euclidean_distance(
  * @brief Get the L2 distance between 2 geometry_msgs::Poses
  * @param pos1 First pose
  * @param pos1 Second pose
- * @param double L2 distance
+ * @return double L2 distance
  */
 inline double euclidean_distance(
   const geometry_msgs::msg::Pose & pos1,
@@ -76,7 +77,7 @@ inline double euclidean_distance(
  * @brief Get the L2 distance between 2 geometry_msgs::PoseStamped
  * @param pos1 First pose
  * @param pos1 Second pose
- * @param double L2 distance
+ * @return double L2 distance
  */
 inline double euclidean_distance(
   const geometry_msgs::msg::PoseStamped & pos1,
@@ -85,6 +86,26 @@ inline double euclidean_distance(
   return euclidean_distance(pos1.pose, pos2.pose);
 }
 
+/**
+ * @brief Calculate the length of the provided path, starting at the provided index
+ * @param path Path containing the poses that are planned
+ * @param start_index Optional argument specifying the starting index for
+ * the calculation of path length. Provide this if you want to calculate length of a
+ * subset of the path.
+ * @return double Path length
+ */
+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/include/nav2_util/odometry_utils.hpp

@@ -52,6 +52,18 @@ public:
     double filter_duration = 0.3,
     const std::string & odom_topic = "odom");
 
+  /**
+   * @brief Overloadded Constructor for nav_util::LifecycleNode parent
+   * that subscribes to an Odometry topic
+   * @param parent NodeHandle for creating subscriber
+   * @param filter_duration Duration for odom history (seconds)
+   * @param odom_topic Topic on which odometry should be received
+   */
+  explicit OdomSmoother(
+    const nav2_util::LifecycleNode::WeakPtr & parent,
+    double filter_duration = 0.3,
+    const std::string & odom_topic = "odom");
+
   /**
    * @brief Get twist msg from smoother
    * @return twist Twist msg

nav2_util/src/odometry_utils.cpp

@@ -43,6 +43,26 @@ OdomSmoother::OdomSmoother(
   odom_cumulate_.twist.twist.angular.z = 0;
 }
 
+OdomSmoother::OdomSmoother(
+  const nav2_util::LifecycleNode::WeakPtr & parent,
+  double filter_duration,
+  const std::string & odom_topic)
+: odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration))
+{
+  auto node = parent.lock();
+  odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(
+    odom_topic,
+    rclcpp::SystemDefaultsQoS(),
+    std::bind(&OdomSmoother::odomCallback, this, std::placeholders::_1));
+
+  odom_cumulate_.twist.twist.linear.x = 0;
+  odom_cumulate_.twist.twist.linear.y = 0;
+  odom_cumulate_.twist.twist.linear.z = 0;
+  odom_cumulate_.twist.twist.angular.x = 0;
+  odom_cumulate_.twist.twist.angular.y = 0;
+  odom_cumulate_.twist.twist.angular.z = 0;
+}
+
 void OdomSmoother::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
 {
   std::lock_guard<std::mutex> lock(odom_mutex_);

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);
+}