diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 5c87558a2ed..af5f3ac2405 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/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 plugin_lib_names_; + // Odometry smoother object + std::unique_ptr odom_smoother_; + // A client that we'll use to send a command message to our own task server rclcpp_action::Client::SharedPtr self_client_; diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index fcb761322c6..bb8776ad5a6 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #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(client_node_, 0.3); + action_server_ = std::make_unique( 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 = + [¤t_pose, ¤t_path]() { + size_t closest_pose_idx = 0; + double curr_min_dist = std::numeric_limits::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("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); diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index 792bee20ad1..a7b778f4d6a 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/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 diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 52bd5ebb417..0458461fb06 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/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 diff --git a/nav2_util/test/test_geometry_utils.cpp b/nav2_util/test/test_geometry_utils.cpp index 9c379575bbd..52a2d4b457d 100644 --- a/nav2_util/test/test_geometry_utils.cpp +++ b/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); +}