From ff271799fb3ac3ab62b087572cb54126a2d0d527 Mon Sep 17 00:00:00 2001 From: Deepak Talwar Date: Sat, 13 Feb 2021 14:32:27 -0800 Subject: [PATCH 01/10] first commit --- nav2_bt_navigator/src/bt_navigator.cpp | 14 ++++++++++++++ nav2_msgs/action/NavigateToPose.action | 1 + 2 files changed, 15 insertions(+) diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index fcb761322c6..a308c6d181d 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -275,9 +275,23 @@ BtNavigator::navigateToPose() geometry_msgs::msg::PoseStamped goal_pose; blackboard_->get("goal", goal_pose); + // Get current path points + nav_msgs::msg::Path current_path; + blackboard_->get("path", current_path); + + // Calculate distance on the path + + // Get current speed + + // Calculate estimated time taken to goal + + // TODO (Deepak): Update this to add path distance computation feedback_msg->distance_remaining = nav2_util::geometry_utils::euclidean_distance( feedback_msg->current_pose.pose, goal_pose.pose); + // feedback_msg->estimated_navigation_time_remaining = nav2_util::geometry_utils:: + // estimate_navigation_time_remaining(distance, current_velocity); + int recovery_count = 0; blackboard_->get("number_recoveries", recovery_count); feedback_msg->number_of_recoveries = recovery_count; diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index 792bee20ad1..c0dcb7267a3 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_navigation_time_remaining int16 number_of_recoveries float32 distance_remaining From 03847c2d2571d77dd55b8112c82d5908b53e68e5 Mon Sep 17 00:00:00 2001 From: Deepak Talwar Date: Thu, 18 Feb 2021 00:25:20 -0800 Subject: [PATCH 02/10] Added estimated remaining time and accurate distance to feedback message for NavigateToPose action --- .../bringup/launch/tb3_simulation_launch.py | 15 ++++--- .../nav2_bt_navigator/bt_navigator.hpp | 6 +++ nav2_bt_navigator/src/bt_navigator.cpp | 44 ++++++++++++++----- .../include/nav2_util/geometry_utils.hpp | 12 +++++ 4 files changed, 61 insertions(+), 16 deletions(-) diff --git a/nav2_bringup/bringup/launch/tb3_simulation_launch.py b/nav2_bringup/bringup/launch/tb3_simulation_launch.py index ce843a8adbe..2db2c145610 100644 --- a/nav2_bringup/bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/bringup/launch/tb3_simulation_launch.py @@ -76,7 +76,8 @@ def generate_launch_description(): declare_map_yaml_cmd = DeclareLaunchArgument( 'map', - default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), + default_value=os.path.join( + bringup_dir, 'maps', 'turtlebot3_world.yaml'), description='Full path to map file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( @@ -102,7 +103,8 @@ def generate_launch_description(): declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', - default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), + default_value=os.path.join( + bringup_dir, 'rviz', 'nav2_default_view.rviz'), description='Full path to the RVIZ config file to use') declare_use_simulator_cmd = DeclareLaunchArgument( @@ -141,7 +143,8 @@ def generate_launch_description(): cwd=[launch_dir], output='screen') start_gazebo_client_cmd = ExecuteProcess( - condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless])), + condition=IfCondition(PythonExpression( + [use_simulator, ' and not ', headless])), cmd=['gzclient'], cwd=[launch_dir], output='screen') @@ -159,14 +162,16 @@ def generate_launch_description(): arguments=[urdf]) rviz_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')), + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'rviz_launch.py')), condition=IfCondition(use_rviz), launch_arguments={'namespace': '', 'use_namespace': 'False', 'rviz_config': rviz_config_file}.items()) bringup_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'bringup_launch.py')), launch_arguments={'namespace': namespace, 'use_namespace': use_namespace, 'slam': slam, 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..1437ed7327b 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_; @@ -142,6 +146,8 @@ class BtNavigator : public nav2_util::LifecycleNode // Metrics for feedback rclcpp::Time start_time_; + double current_linear_speed_; + rclcpp::Duration estimated_navigation_time_remaining_; std::string robot_frame_; std::string global_frame_; double transform_tolerance_; diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index a308c6d181d..681cd8ea323 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" @@ -32,7 +33,9 @@ namespace nav2_bt_navigator BtNavigator::BtNavigator() : nav2_util::LifecycleNode("bt_navigator", "", false), - start_time_(0) + start_time_(0), + current_linear_speed_(0.0), + estimated_navigation_time_remaining_(rclcpp::Duration::from_seconds(0.0)) { RCLCPP_INFO(get_logger(), "Creating"); @@ -102,6 +105,9 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) "goal_pose", 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(), @@ -280,20 +286,35 @@ BtNavigator::navigateToPose() blackboard_->get("path", current_path); // Calculate distance on the path + double distance_remaining = nav2_util::geometry_utils::calculate_distance_to_goal(current_path); + + if (distance_remaining <= transform_tolerance_) + { + estimated_navigation_time_remaining_ = rclcpp::Duration::from_seconds(0.0); + } + else + { + // Get current speed + current_linear_speed_ = odom_smoother_->getTwist().linear.x; + + // Calculate estimated time taken to goal if speed is higher than 1mm/s + if (std::abs(current_linear_speed_) > 0.05) + { + estimated_navigation_time_remaining_ = + rclcpp::Duration::from_seconds(distance_remaining / std::abs(current_linear_speed_)); + } + } - // Get current speed - - // Calculate estimated time taken to goal - - // TODO (Deepak): Update this to add path distance computation - feedback_msg->distance_remaining = nav2_util::geometry_utils::euclidean_distance( - feedback_msg->current_pose.pose, goal_pose.pose); - - // feedback_msg->estimated_navigation_time_remaining = nav2_util::geometry_utils:: - // estimate_navigation_time_remaining(distance, current_velocity); + std::cout << "=============== Current stats ================" << std::endl + << "Distance remaining: " << feedback_msg->distance_remaining << std::endl + << "Speed: " << current_linear_speed_ << std::endl + << "Time remaining: " << estimated_navigation_time_remaining_.seconds() << "." << + estimated_navigation_time_remaining_.nanoseconds() << std::endl; int recovery_count = 0; blackboard_->get("number_recoveries", recovery_count); + feedback_msg->distance_remaining = distance_remaining; + feedback_msg->estimated_navigation_time_remaining = estimated_navigation_time_remaining_; feedback_msg->number_of_recoveries = recovery_count; feedback_msg->navigation_time = now() - start_time_; action_server_->publish_feedback(feedback_msg); @@ -348,4 +369,5 @@ BtNavigator::onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr self_client_->async_send_goal(goal); } + } // namespace nav2_bt_navigator diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 52bd5ebb417..58d6813c09b 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,17 @@ inline double euclidean_distance( return euclidean_distance(pos1.pose, pos2.pose); } +inline double calculate_distance_to_goal(const nav_msgs::msg::Path& path) +{ + double path_length = 0.0; + for (size_t idx = 0; 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 From dac19bf2451efd097036911333b364bbcc8c82bd Mon Sep 17 00:00:00 2001 From: Deepak Talwar Date: Thu, 18 Feb 2021 00:30:11 -0800 Subject: [PATCH 03/10] Removed prints --- .../bringup/launch/tb3_simulation_launch.py | 15 +++++---------- nav2_bt_navigator/src/bt_navigator.cpp | 8 -------- nav2_util/include/nav2_util/geometry_utils.hpp | 1 - 3 files changed, 5 insertions(+), 19 deletions(-) diff --git a/nav2_bringup/bringup/launch/tb3_simulation_launch.py b/nav2_bringup/bringup/launch/tb3_simulation_launch.py index 2db2c145610..ce843a8adbe 100644 --- a/nav2_bringup/bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/bringup/launch/tb3_simulation_launch.py @@ -76,8 +76,7 @@ def generate_launch_description(): declare_map_yaml_cmd = DeclareLaunchArgument( 'map', - default_value=os.path.join( - bringup_dir, 'maps', 'turtlebot3_world.yaml'), + default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), description='Full path to map file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( @@ -103,8 +102,7 @@ def generate_launch_description(): declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', - default_value=os.path.join( - bringup_dir, 'rviz', 'nav2_default_view.rviz'), + default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), description='Full path to the RVIZ config file to use') declare_use_simulator_cmd = DeclareLaunchArgument( @@ -143,8 +141,7 @@ def generate_launch_description(): cwd=[launch_dir], output='screen') start_gazebo_client_cmd = ExecuteProcess( - condition=IfCondition(PythonExpression( - [use_simulator, ' and not ', headless])), + condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless])), cmd=['gzclient'], cwd=[launch_dir], output='screen') @@ -162,16 +159,14 @@ def generate_launch_description(): arguments=[urdf]) rviz_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'rviz_launch.py')), + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')), condition=IfCondition(use_rviz), launch_arguments={'namespace': '', 'use_namespace': 'False', 'rviz_config': rviz_config_file}.items()) bringup_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'bringup_launch.py')), + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), launch_arguments={'namespace': namespace, 'use_namespace': use_namespace, 'slam': slam, diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 681cd8ea323..48e90376074 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -305,12 +304,6 @@ BtNavigator::navigateToPose() } } - std::cout << "=============== Current stats ================" << std::endl - << "Distance remaining: " << feedback_msg->distance_remaining << std::endl - << "Speed: " << current_linear_speed_ << std::endl - << "Time remaining: " << estimated_navigation_time_remaining_.seconds() << "." << - estimated_navigation_time_remaining_.nanoseconds() << std::endl; - int recovery_count = 0; blackboard_->get("number_recoveries", recovery_count); feedback_msg->distance_remaining = distance_remaining; @@ -369,5 +362,4 @@ BtNavigator::onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr self_client_->async_send_goal(goal); } - } // namespace nav2_bt_navigator diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 58d6813c09b..08c76a81cf6 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -73,7 +73,6 @@ inline double calculate_distance_to_goal(const nav_msgs::msg::Path& path) return path_length; } - } // namespace geometry_utils } // namespace nav2_util From c7f71ff5b04d3ca2cc03d02e3e92546fefd2b00c Mon Sep 17 00:00:00 2001 From: Deepak Talwar Date: Thu, 18 Feb 2021 00:32:55 -0800 Subject: [PATCH 04/10] Removed inline keyword --- nav2_util/include/nav2_util/geometry_utils.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 08c76a81cf6..ee7e10b179d 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -63,7 +63,7 @@ inline double euclidean_distance( return euclidean_distance(pos1.pose, pos2.pose); } -inline double calculate_distance_to_goal(const nav_msgs::msg::Path& path) +double calculate_distance_to_goal(const nav_msgs::msg::Path& path) { double path_length = 0.0; for (size_t idx = 0; idx < path.poses.size() - 1; ++idx) From 01fcbb655fee6962073c2bd363cdf4abf8684bdc Mon Sep 17 00:00:00 2001 From: Deepak Talwar Date: Thu, 18 Feb 2021 14:27:04 -0800 Subject: [PATCH 05/10] WIP-Added testing --- nav2_bt_navigator/src/bt_navigator.cpp | 17 ++++++-------- .../include/nav2_util/geometry_utils.hpp | 5 ++--- nav2_util/test/test_geometry_utils.cpp | 22 +++++++++++++++++++ 3 files changed, 31 insertions(+), 13 deletions(-) diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 48e90376074..f735d2795e8 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -22,7 +22,7 @@ #include #include -#include "nav2_util/geometry_utils.hpp" +// #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" @@ -104,7 +104,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) "goal_pose", 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); @@ -285,20 +285,17 @@ BtNavigator::navigateToPose() blackboard_->get("path", current_path); // Calculate distance on the path - double distance_remaining = nav2_util::geometry_utils::calculate_distance_to_goal(current_path); + double distance_remaining = + nav2_util::geometry_utils::calculate_distance_to_goal(current_path); - if (distance_remaining <= transform_tolerance_) - { + if (distance_remaining <= transform_tolerance_) { estimated_navigation_time_remaining_ = rclcpp::Duration::from_seconds(0.0); - } - else - { + } else { // Get current speed current_linear_speed_ = odom_smoother_->getTwist().linear.x; // Calculate estimated time taken to goal if speed is higher than 1mm/s - if (std::abs(current_linear_speed_) > 0.05) - { + if (std::abs(current_linear_speed_) > 0.05) { estimated_navigation_time_remaining_ = rclcpp::Duration::from_seconds(distance_remaining / std::abs(current_linear_speed_)); } diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index ee7e10b179d..c5c08dbbbdc 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -63,11 +63,10 @@ inline double euclidean_distance( return euclidean_distance(pos1.pose, pos2.pose); } -double calculate_distance_to_goal(const nav_msgs::msg::Path& path) +double calculate_distance_to_goal(const nav_msgs::msg::Path & path) { double path_length = 0.0; - for (size_t idx = 0; idx < path.poses.size() - 1; ++idx) - { + for (size_t idx = 0; idx < path.poses.size() - 1; ++idx) { path_length += euclidean_distance(path.poses[idx].pose, path.poses[idx + 1].pose); } return path_length; diff --git a/nav2_util/test/test_geometry_utils.cpp b/nav2_util/test/test_geometry_utils.cpp index 9c379575bbd..8b80abfa6c3 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_distance_to_goal; TEST(GeometryUtils, euclidean_distance_point) { @@ -49,3 +51,23 @@ TEST(GeometryUtils, euclidean_distance_pose) ASSERT_NEAR(euclidean_distance(pose1, pose2), 10.24695, 1e-5); } + +TEST(GeometryUtils, calculate_distance_to_goal) +{ + 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_distance_to_goal(straight_line_path), + (nb_path_points - 1) * distance_between_poses, 1e-5); +} From f032440b17fa1cbeef25e26489092f2a11106141 Mon Sep 17 00:00:00 2001 From: Deepak Talwar Date: Thu, 18 Feb 2021 16:29:10 -0800 Subject: [PATCH 06/10] Renamed function and added back inline keyword --- nav2_bt_navigator/src/bt_navigator.cpp | 4 ++-- nav2_util/include/nav2_util/geometry_utils.hpp | 2 +- nav2_util/test/test_geometry_utils.cpp | 7 ++++--- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index f735d2795e8..33f8474f777 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -22,7 +22,7 @@ #include #include -// #include "nav2_util/geometry_utils.hpp" +#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" @@ -286,7 +286,7 @@ BtNavigator::navigateToPose() // Calculate distance on the path double distance_remaining = - nav2_util::geometry_utils::calculate_distance_to_goal(current_path); + nav2_util::geometry_utils::calculate_path_length(current_path); if (distance_remaining <= transform_tolerance_) { estimated_navigation_time_remaining_ = rclcpp::Duration::from_seconds(0.0); diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index c5c08dbbbdc..0fed6bb344a 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -63,7 +63,7 @@ inline double euclidean_distance( return euclidean_distance(pos1.pose, pos2.pose); } -double calculate_distance_to_goal(const nav_msgs::msg::Path & path) +inline double calculate_path_length(const nav_msgs::msg::Path & path) { double path_length = 0.0; for (size_t idx = 0; idx < path.poses.size() - 1; ++idx) { diff --git a/nav2_util/test/test_geometry_utils.cpp b/nav2_util/test/test_geometry_utils.cpp index 8b80abfa6c3..6538051e1fb 100644 --- a/nav2_util/test/test_geometry_utils.cpp +++ b/nav2_util/test/test_geometry_utils.cpp @@ -20,7 +20,7 @@ #include "gtest/gtest.h" using nav2_util::geometry_utils::euclidean_distance; -using nav2_util::geometry_utils::calculate_distance_to_goal; +using nav2_util::geometry_utils::calculate_path_length; TEST(GeometryUtils, euclidean_distance_point) { @@ -52,7 +52,7 @@ TEST(GeometryUtils, euclidean_distance_pose) ASSERT_NEAR(euclidean_distance(pose1, pose2), 10.24695, 1e-5); } -TEST(GeometryUtils, calculate_distance_to_goal) +TEST(GeometryUtils, calculate_path_length) { nav_msgs::msg::Path straight_line_path; size_t nb_path_points = 10; @@ -68,6 +68,7 @@ TEST(GeometryUtils, calculate_distance_to_goal) current_x_loc += distance_between_poses; } - ASSERT_NEAR(calculate_distance_to_goal(straight_line_path), + ASSERT_NEAR( + calculate_path_length(straight_line_path), (nb_path_points - 1) * distance_between_poses, 1e-5); } From 6208937b110e08ea431ba1b96eb250e0b17253ae Mon Sep 17 00:00:00 2001 From: Deepak Talwar Date: Fri, 19 Feb 2021 18:04:07 -0800 Subject: [PATCH 07/10] Added circle path length unit test and addressed comments --- .../nav2_bt_navigator/bt_navigator.hpp | 2 +- nav2_bt_navigator/src/bt_navigator.cpp | 12 +++++----- nav2_msgs/action/NavigateToPose.action | 2 +- nav2_util/test/test_geometry_utils.cpp | 22 +++++++++++++++++++ 4 files changed, 30 insertions(+), 8 deletions(-) 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 1437ed7327b..2925020c643 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -147,7 +147,7 @@ class BtNavigator : public nav2_util::LifecycleNode // Metrics for feedback rclcpp::Time start_time_; double current_linear_speed_; - rclcpp::Duration estimated_navigation_time_remaining_; + rclcpp::Duration estimated_time_remaining_; std::string robot_frame_; std::string global_frame_; double transform_tolerance_; diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 33f8474f777..752e20ba0af 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -34,7 +34,7 @@ BtNavigator::BtNavigator() : nav2_util::LifecycleNode("bt_navigator", "", false), start_time_(0), current_linear_speed_(0.0), - estimated_navigation_time_remaining_(rclcpp::Duration::from_seconds(0.0)) + estimated_time_remaining_(rclcpp::Duration::from_seconds(0.0)) { RCLCPP_INFO(get_logger(), "Creating"); @@ -289,14 +289,14 @@ BtNavigator::navigateToPose() nav2_util::geometry_utils::calculate_path_length(current_path); if (distance_remaining <= transform_tolerance_) { - estimated_navigation_time_remaining_ = rclcpp::Duration::from_seconds(0.0); + estimated_time_remaining_ = rclcpp::Duration::from_seconds(0.0); } else { // Get current speed current_linear_speed_ = odom_smoother_->getTwist().linear.x; - // Calculate estimated time taken to goal if speed is higher than 1mm/s - if (std::abs(current_linear_speed_) > 0.05) { - estimated_navigation_time_remaining_ = + // Calculate estimated time taken to goal if speed is higher than 5mm/s + if (std::abs(current_linear_speed_) > 0.005) { + estimated_time_remaining_ = rclcpp::Duration::from_seconds(distance_remaining / std::abs(current_linear_speed_)); } } @@ -304,7 +304,7 @@ BtNavigator::navigateToPose() int recovery_count = 0; blackboard_->get("number_recoveries", recovery_count); feedback_msg->distance_remaining = distance_remaining; - feedback_msg->estimated_navigation_time_remaining = estimated_navigation_time_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 c0dcb7267a3..a7b778f4d6a 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -7,6 +7,6 @@ std_msgs/Empty result --- geometry_msgs/PoseStamped current_pose builtin_interfaces/Duration navigation_time -builtin_interfaces/Duration estimated_navigation_time_remaining +builtin_interfaces/Duration estimated_time_remaining int16 number_of_recoveries float32 distance_remaining diff --git a/nav2_util/test/test_geometry_utils.cpp b/nav2_util/test/test_geometry_utils.cpp index 6538051e1fb..b039371a255 100644 --- a/nav2_util/test/test_geometry_utils.cpp +++ b/nav2_util/test/test_geometry_utils.cpp @@ -71,4 +71,26 @@ TEST(GeometryUtils, calculate_path_length) ASSERT_NEAR( calculate_path_length(straight_line_path), (nb_path_points - 1) * distance_between_poses, 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); } From a9707cb8c93050c60500222dda75c24fb1bb0b38 Mon Sep 17 00:00:00 2001 From: Deepak Talwar Date: Sun, 28 Feb 2021 22:15:55 -0800 Subject: [PATCH 08/10] Addressed comments - added calculation of remaining path length --- nav2_bt_navigator/src/bt_navigator.cpp | 31 ++++++++++++++++--- .../include/nav2_util/geometry_utils.hpp | 7 +++-- nav2_util/test/test_geometry_utils.cpp | 8 +++-- 3 files changed, 37 insertions(+), 9 deletions(-) diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 752e20ba0af..e6f864b142a 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -274,8 +274,9 @@ 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); @@ -284,24 +285,44 @@ BtNavigator::navigateToPose() 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 (auto curr_it = current_path.poses.begin(); curr_it != current_path.poses.end(); ++curr_it) { + double curr_dist = nav2_util::geometry_utils::euclidean_distance(current_pose, *curr_it); + if (curr_dist < curr_min_dist) { + curr_min_dist = curr_dist; + closest_pose_idx = curr_it - current_path.poses.begin(); + } + } + return closest_pose_idx; + }; + + size_t closest_pose_idx = find_closest_pose_idx(); + // Calculate distance on the path double distance_remaining = - nav2_util::geometry_utils::calculate_path_length(current_path); + nav2_util::geometry_utils::calculate_path_length(current_path, closest_pose_idx); if (distance_remaining <= transform_tolerance_) { estimated_time_remaining_ = rclcpp::Duration::from_seconds(0.0); } else { // Get current speed - current_linear_speed_ = odom_smoother_->getTwist().linear.x; + geometry_msgs::msg::Twist current_odom = odom_smoother_->getTwist(); + current_linear_speed_ = hypot(current_odom.linear.x, current_odom.linear.y); - // Calculate estimated time taken to goal if speed is higher than 5mm/s - if (std::abs(current_linear_speed_) > 0.005) { + // 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_; diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 0fed6bb344a..286a6449ea6 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -63,10 +63,13 @@ inline double euclidean_distance( return euclidean_distance(pos1.pose, pos2.pose); } -inline double calculate_path_length(const nav_msgs::msg::Path & path) +inline double calculate_path_length(const nav_msgs::msg::Path & path, size_t start_index) { + if (start_index >= path.poses.size() - 1) { + return 0.0; + } double path_length = 0.0; - for (size_t idx = 0; idx < path.poses.size() - 1; ++idx) { + 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; diff --git a/nav2_util/test/test_geometry_utils.cpp b/nav2_util/test/test_geometry_utils.cpp index b039371a255..5e757bd5d00 100644 --- a/nav2_util/test/test_geometry_utils.cpp +++ b/nav2_util/test/test_geometry_utils.cpp @@ -69,9 +69,13 @@ TEST(GeometryUtils, calculate_path_length) } ASSERT_NEAR( - calculate_path_length(straight_line_path), + calculate_path_length(straight_line_path, 0), (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; @@ -91,6 +95,6 @@ TEST(GeometryUtils, calculate_path_length) } ASSERT_NEAR( - calculate_path_length(circle_path), + calculate_path_length(circle_path, 0), 2 * pi * polar_distance, 1e-1); } From db2cbc53c91faa84da3024de2367c79ecb2e212a Mon Sep 17 00:00:00 2001 From: Deepak Talwar Date: Sat, 6 Mar 2021 14:43:06 -0800 Subject: [PATCH 09/10] Addressed PR comments --- .../nav2_bt_navigator/bt_navigator.hpp | 2 - nav2_bt_navigator/src/bt_navigator.cpp | 44 +++++++++---------- .../include/nav2_util/geometry_utils.hpp | 2 +- nav2_util/test/test_geometry_utils.cpp | 4 +- 4 files changed, 23 insertions(+), 29 deletions(-) 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 2925020c643..af5f3ac2405 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -146,8 +146,6 @@ class BtNavigator : public nav2_util::LifecycleNode // Metrics for feedback rclcpp::Time start_time_; - double current_linear_speed_; - rclcpp::Duration estimated_time_remaining_; std::string robot_frame_; std::string global_frame_; double transform_tolerance_; diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index e6f864b142a..7d4c3ec994e 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -32,9 +32,7 @@ namespace nav2_bt_navigator BtNavigator::BtNavigator() : nav2_util::LifecycleNode("bt_navigator", "", false), - start_time_(0), - current_linear_speed_(0.0), - estimated_time_remaining_(rclcpp::Duration::from_seconds(0.0)) + start_time_(0) { RCLCPP_INFO(get_logger(), "Creating"); @@ -290,42 +288,40 @@ BtNavigator::navigateToPose() [¤t_pose, ¤t_path]() { size_t closest_pose_idx = 0; double curr_min_dist = std::numeric_limits::max(); - for (auto curr_it = current_path.poses.begin(); curr_it != current_path.poses.end(); ++curr_it) { - double curr_dist = nav2_util::geometry_utils::euclidean_distance(current_pose, *curr_it); + 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_it - current_path.poses.begin(); + closest_pose_idx = curr_idx; } } return closest_pose_idx; }; - size_t closest_pose_idx = find_closest_pose_idx(); - // Calculate distance on the path double distance_remaining = - nav2_util::geometry_utils::calculate_path_length(current_path, closest_pose_idx); - - if (distance_remaining <= transform_tolerance_) { - estimated_time_remaining_ = rclcpp::Duration::from_seconds(0.0); - } else { - // Get current speed - geometry_msgs::msg::Twist current_odom = odom_smoother_->getTwist(); - 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_)); - } + 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->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_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 286a6449ea6..0458461fb06 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -63,7 +63,7 @@ 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) +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; diff --git a/nav2_util/test/test_geometry_utils.cpp b/nav2_util/test/test_geometry_utils.cpp index 5e757bd5d00..52a2d4b457d 100644 --- a/nav2_util/test/test_geometry_utils.cpp +++ b/nav2_util/test/test_geometry_utils.cpp @@ -69,7 +69,7 @@ TEST(GeometryUtils, calculate_path_length) } ASSERT_NEAR( - calculate_path_length(straight_line_path, 0), + calculate_path_length(straight_line_path), (nb_path_points - 1) * distance_between_poses, 1e-5); ASSERT_NEAR( @@ -95,6 +95,6 @@ TEST(GeometryUtils, calculate_path_length) } ASSERT_NEAR( - calculate_path_length(circle_path, 0), + calculate_path_length(circle_path), 2 * pi * polar_distance, 1e-1); } From ec76858f22e07bba04b49854a4313edbefb16dcd Mon Sep 17 00:00:00 2001 From: Deepak Talwar Date: Sat, 6 Mar 2021 14:50:40 -0800 Subject: [PATCH 10/10] lint --- nav2_bt_navigator/src/bt_navigator.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 7d4c3ec994e..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" @@ -284,7 +285,7 @@ BtNavigator::navigateToPose() blackboard_->get("path", current_path); // Find the closest pose to current pose on global path - auto find_closest_pose_idx = + auto find_closest_pose_idx = [¤t_pose, ¤t_path]() { size_t closest_pose_idx = 0; double curr_min_dist = std::numeric_limits::max(); @@ -310,7 +311,7 @@ BtNavigator::navigateToPose() 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 + // 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 =