From 7872c73362985a95cdacf2df3ef15526fcd7f260 Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Mon, 21 Mar 2022 11:18:40 -0600 Subject: [PATCH] Bound closest pose by costmap and turnaround point (#2812) * created integrated distance util * chng transform begin , end and always limit lp pt * chg trnsfm begin end and use euclidean distance * linting fix * linting fix * limit trnsfm to cusp * remove print * linting fix * lint fix * hypot * use direction change as upper bound for closest pose * bound closest waypoint by direction change * use max_transform_dist for closest_pose_upper_bound * remove first_element_beyond optimization todo * added path_utils for test path generation * initial test for path_utils * added full failing test for path_utils * fixed path_utils segfault * properly initialize straight transform * fixed right turn infinite loop * fixed path_utils transforms for half-turns * all path_utils tests passing * added half turn test for path_utils * remove unused dependencies from test_path_utils * move path_utils classes into path_building_blocks namespace * use hypot for euclidean_distance * rename findDirectionChange to findCusp * use integrated distance for lookahead distance * Revert "use integrated distance for lookahead distance" This reverts commit 6e4c6cd3a795faf29f977df7316b5d58d9c1f135. * parameterize transformation_begin bound * change default to a regular constant * use std::hypot for x, y, z * initial failing test for transformGlobalPlan * refactor transform global plan into separate test fixture * make TransformGlobalPlan test fixture, and fix transforms * no_pruning_on_large_costmap test passing * added another transformGlobalPlan test * added a test for all poses outside of costmap * added good shortcut test for transformGlobalPlan * added more tests for rpp costmap pruning * cpplint * remove unused rclcpp::init and rclcpp::shutdown * change default max_distance_between_iterations to max_costmap_extent * rename max_distance_between_iterations to max_robot_pose_search_dist * increase default dwb prune distance * add initial docs for max_robot_pose_search_dist to README * add note about when to set max_robot_pose_search_dist * rename first_element_beyond to first_after_integrated_distance * renamed findCusp to findVelocitySignChange * move path_utils to RPP tests * only check velocity sign change when reversing is enabled * do not check cusp for dwb transformed plan end Co-authored-by: sathak93 Co-authored-by: Adam Aposhian --- .../dwb_core/src/dwb_local_planner.cpp | 52 ++- .../README.md | 2 + .../regulated_pure_pursuit_controller.hpp | 16 +- .../src/regulated_pure_pursuit_controller.cpp | 44 ++- .../test/CMakeLists.txt | 5 + .../test/path_utils/path_utils.cpp | 88 +++++ .../test/path_utils/path_utils.hpp | 111 ++++++ .../test/path_utils/test_path_utils.cpp | 128 ++++++ .../test/test_regulated_pp.cpp | 372 +++++++++++++++++- .../include/nav2_util/geometry_utils.hpp | 40 +- 10 files changed, 807 insertions(+), 51 deletions(-) create mode 100644 nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp create mode 100644 nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp create mode 100644 nav2_regulated_pure_pursuit_controller/test/path_utils/test_path_utils.cpp diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index ab7baed046..91dd768b2e 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -46,6 +46,7 @@ #include "nav_2d_utils/conversions.hpp" #include "nav_2d_utils/parameters.hpp" #include "nav_2d_utils/tf_help.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" #include "pluginlib/class_list_macros.hpp" @@ -53,6 +54,7 @@ #include "geometry_msgs/msg/twist_stamped.hpp" using nav2_util::declare_parameter_if_not_declared; +using nav2_util::geometry_utils::euclidean_distance; namespace dwb_core { @@ -87,7 +89,7 @@ void DWBLocalPlanner::configure( rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, dwb_plugin_name_ + ".prune_distance", - rclcpp::ParameterValue(1.0)); + rclcpp::ParameterValue(2.0)); declare_parameter_if_not_declared( node, dwb_plugin_name_ + ".debug_trajectory_details", rclcpp::ParameterValue(false)); @@ -434,17 +436,6 @@ DWBLocalPlanner::scoreTrajectory( return score; } -double -getSquareDistance( - const geometry_msgs::msg::Pose2D & pose_a, - const geometry_msgs::msg::Pose2D & pose_b) -{ - double x_diff = pose_a.x - pose_b.x; - double y_diff = pose_a.y - pose_b.y; - - return x_diff * x_diff + y_diff * y_diff; -} - nav_2d_msgs::msg::Path2D DWBLocalPlanner::transformGlobalPlan( const nav_2d_msgs::msg::Pose2DStamped & pose) @@ -467,46 +458,51 @@ DWBLocalPlanner::transformGlobalPlan( nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); double dist_threshold = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) * costmap->getResolution() / 2.0; - double sq_dist_threshold = dist_threshold * dist_threshold; + // If prune_plan is enabled (it is by default) then we want to restrict the // plan to distances within that range as well. - double sq_prune_dist = prune_distance_ * prune_distance_; + double prune_dist = prune_distance_; // Set the maximum distance we'll include points before getting to the part // of the path where the robot is located (the start of the plan). Basically, // these are the points the robot has already passed. - double sq_transform_start_threshold; + double transform_start_threshold; if (prune_plan_) { - sq_transform_start_threshold = std::min(sq_dist_threshold, sq_prune_dist); + transform_start_threshold = std::min(dist_threshold, prune_dist); } else { - sq_transform_start_threshold = sq_dist_threshold; + transform_start_threshold = dist_threshold; } // Set the maximum distance we'll include points after the part of the part of // the plan near the robot (the end of the plan). This determines the amount // of the plan passed on to the critics - double sq_transform_end_threshold; + double transform_end_threshold; if (shorten_transformed_plan_) { - sq_transform_end_threshold = std::min(sq_dist_threshold, sq_prune_dist); + transform_end_threshold = std::min(dist_threshold, prune_dist); } else { - sq_transform_end_threshold = sq_dist_threshold; + transform_end_threshold = dist_threshold; } - // Find the first pose in the plan that's less than sq_transform_start_threshold + // Find the first pose in the global plan that's further than prune distance + // from the robot using integrated distance + auto prune_point = nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_.poses.begin(), global_plan_.poses.end(), prune_dist); + + // Find the first pose in the plan (upto prune_point) that's less than transform_start_threshold // from the robot. auto transformation_begin = std::find_if( - begin(global_plan_.poses), end(global_plan_.poses), + begin(global_plan_.poses), prune_point, [&](const auto & global_plan_pose) { - return getSquareDistance(robot_pose.pose, global_plan_pose) < sq_transform_start_threshold; + return euclidean_distance(robot_pose.pose, global_plan_pose) < transform_start_threshold; }); - // Find the first pose in the end of the plan that's further than sq_transform_end_threshold - // from the robot + // Find the first pose in the end of the plan that's further than transform_end_threshold + // from the robot using integrated distance auto transformation_end = std::find_if( - transformation_begin, end(global_plan_.poses), - [&](const auto & global_plan_pose) { - return getSquareDistance(robot_pose.pose, global_plan_pose) > sq_transform_end_threshold; + transformation_begin, global_plan_.poses.end(), + [&](const auto & pose) { + return euclidean_distance(pose, robot_pose.pose) > transform_end_threshold; }); // Transform the near part of the global plan into the robot's frame of reference. diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index d621a4bc2e..e285e30e96 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -75,6 +75,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. | | `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | | `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | +| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | Example fully-described XML with default parameter values: @@ -121,6 +122,7 @@ controller_server: use_rotate_to_heading: true rotate_to_heading_min_angle: 0.785 max_angular_accel: 3.2 + max_robot_pose_search_dist: 10.0 cost_scaling_dist: 0.3 cost_scaling_gain: 1.0 inflation_cost_scaling_factor: 3.0 diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index b69bd0e63c..157ead6519 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -112,11 +112,14 @@ class RegulatedPurePursuitController : public nav2_core::Controller protected: /** - * @brief Transforms global plan into same frame as pose, clips far away poses and possibly prunes passed poses + * @brief Transforms global plan into same frame as pose and clips poses ineligible for lookaheadPoint + * Points ineligible to be selected as a lookahead point if they are any of the following: + * - Outside the local_costmap (collision avoidance cannot be assured) * @param pose pose to transform * @return Path in new frame */ - nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose); + nav_msgs::msg::Path transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose); /** * @brief Transform a pose to another frame. @@ -232,7 +235,13 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param pose Pose input to determine the cusp position * @return robot distance from the cusp */ - double findDirectionChange(const geometry_msgs::msg::PoseStamped & pose); + double findVelocitySignChange(const geometry_msgs::msg::PoseStamped & pose); + + /** + * Get the greatest extent of the costmap in meters from the center. + * @return max of distance from center in meters to edge of costmap + */ + double getCostmapMaxExtent() const; /** * @brief Callback executed when a parameter change is detected @@ -272,6 +281,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller double rotate_to_heading_min_angle_; double goal_dist_tol_; bool allow_reversing_; + double max_robot_pose_search_dist_; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_path_pub_; diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 2c3e7c63fb..cd5532fc3c 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -105,6 +105,9 @@ void RegulatedPurePursuitController::configure( node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); declare_parameter_if_not_declared( node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_robot_pose_search_dist", + rclcpp::ParameterValue(getCostmapMaxExtent())); node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); base_desired_linear_vel_ = desired_linear_vel_; @@ -147,6 +150,9 @@ void RegulatedPurePursuitController::configure( node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_); node->get_parameter(plugin_name_ + ".allow_reversing", allow_reversing_); node->get_parameter("controller_frequency", control_frequency); + node->get_parameter( + plugin_name_ + ".max_robot_pose_search_dist", + max_robot_pose_search_dist_); transform_tolerance_ = tf2::durationFromSec(transform_tolerance); control_duration_ = 1.0 / control_frequency; @@ -232,7 +238,8 @@ std::unique_ptr RegulatedPurePursuitController return carrot_msg; } -double RegulatedPurePursuitController::getLookAheadDistance(const geometry_msgs::msg::Twist & speed) +double RegulatedPurePursuitController::getLookAheadDistance( + const geometry_msgs::msg::Twist & speed) { // If using velocity-scaled look ahead distances, find and clamp the dist // Else, use the static look ahead distance @@ -270,11 +277,11 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Check for reverse driving if (allow_reversing_) { // Cusp check - double dist_to_direction_change = findDirectionChange(pose); + double dist_to_cusp = findVelocitySignChange(pose); // if the lookahead distance is further than the cusp, use the cusp distance instead - if (dist_to_direction_change < lookahead_dist) { - lookahead_dist = dist_to_direction_change; + if (dist_to_cusp < lookahead_dist) { + lookahead_dist = dist_to_cusp; } } @@ -602,23 +609,27 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( } // We'll discard points on the plan that are outside the local costmap - nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); - const double max_costmap_dim = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()); - const double max_transform_dist = max_costmap_dim * costmap->getResolution() / 2.0; + double max_costmap_extent = getCostmapMaxExtent(); + + auto closest_pose_upper_bound = + nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist_); // First find the closest pose on the path to the robot + // bounded by when the path turns around (if it does) so we don't get a pose from a later + // portion of the path auto transformation_begin = nav2_util::geometry_utils::min_by( - global_plan_.poses.begin(), global_plan_.poses.end(), + global_plan_.poses.begin(), closest_pose_upper_bound, [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { return euclidean_distance(robot_pose, ps); }); - // Find points definitely outside of the costmap so we won't transform them. + // Find points up to max_transform_dist so we only transform them. auto transformation_end = std::find_if( - transformation_begin, end(global_plan_.poses), - [&](const auto & global_plan_pose) { - return euclidean_distance(robot_pose, global_plan_pose) > max_transform_dist; + transformation_begin, global_plan_.poses.end(), + [&](const auto & pose) { + return euclidean_distance(pose, robot_pose) > max_costmap_extent; }); // Lambda to transform a PoseStamped from global frame to local @@ -652,7 +663,7 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( return transformed_plan; } -double RegulatedPurePursuitController::findDirectionChange( +double RegulatedPurePursuitController::findVelocitySignChange( const geometry_msgs::msg::PoseStamped & pose) { // Iterating through the global path to determine the position of the cusp @@ -700,6 +711,13 @@ bool RegulatedPurePursuitController::transformPose( return false; } +double RegulatedPurePursuitController::getCostmapMaxExtent() const +{ + const double max_costmap_dim_meters = std::max( + costmap_->getSizeInMetersX(), costmap_->getSizeInMetersX()); + return max_costmap_dim_meters / 2.0; +} + rcl_interfaces::msg::SetParametersResult RegulatedPurePursuitController::dynamicParametersCallback( diff --git a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt index aee6297fcb..9d299636ff 100644 --- a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt @@ -1,6 +1,7 @@ # tests for regulated PP ament_add_gtest(test_regulated_pp test_regulated_pp.cpp + path_utils/path_utils.cpp ) ament_target_dependencies(test_regulated_pp ${dependencies} @@ -8,3 +9,7 @@ ament_target_dependencies(test_regulated_pp target_link_libraries(test_regulated_pp ${library_name} ) + +# Path utils test +ament_add_gtest(test_path_utils path_utils/test_path_utils.cpp path_utils/path_utils.cpp) +ament_target_dependencies(test_path_utils nav_msgs geometry_msgs) diff --git a/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp b/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp new file mode 100644 index 0000000000..4b516ec1f3 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp @@ -0,0 +1,88 @@ +// Copyright (c) 2022 Adam Aposhian +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "path_utils.hpp" + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace path_utils +{ + +void append_transform_to_path( + nav_msgs::msg::Path & path, + tf2::Transform & relative_transform) +{ + // Add a new empty pose + path.poses.emplace_back(); + // Get the previous, last pose (after the emplace_back so the reference isn't invalidated) + auto & previous_pose = *(path.poses.end() - 2); + auto & new_pose = path.poses.back(); + + // get map_transform of previous_pose + tf2::Transform map_transform; + tf2::fromMsg(previous_pose.pose, map_transform); + + tf2::Transform full_transform; + full_transform.mult(map_transform, relative_transform); + + tf2::toMsg(full_transform, new_pose.pose); + new_pose.header.frame_id = previous_pose.header.frame_id; +} + +void Straight::append(nav_msgs::msg::Path & path, double spacing) const +{ + auto num_points = std::floor(length_ / spacing); + path.poses.reserve(path.poses.size() + num_points); + tf2::Transform translation(tf2::Quaternion::getIdentity(), tf2::Vector3(spacing, 0.0, 0.0)); + for (size_t i = 1; i <= num_points; ++i) { + append_transform_to_path(path, translation); + } +} + +double chord_length(double radius, double radians) +{ + return 2 * radius * sin(radians / 2); +} + +void Arc::append(nav_msgs::msg::Path & path, double spacing) const +{ + double length = radius_ * std::abs(radians_); + size_t num_points = std::floor(length / spacing); + double radians_per_step = radians_ / num_points; + tf2::Transform transform( + tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), radians_per_step), + tf2::Vector3(chord_length(radius_, std::abs(radians_per_step)), 0.0, 0.0)); + path.poses.reserve(path.poses.size() + num_points); + for (size_t i = 0; i < num_points; ++i) { + append_transform_to_path(path, transform); + } +} + +nav_msgs::msg::Path generate_path( + geometry_msgs::msg::PoseStamped start, + double spacing, + std::initializer_list> segments) +{ + nav_msgs::msg::Path path; + path.header = start.header; + path.poses.push_back(start); + for (const auto & segment : segments) { + segment->append(path, spacing); + } + return path; +} + +} // namespace path_utils diff --git a/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp b/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp new file mode 100644 index 0000000000..c079e21614 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.hpp @@ -0,0 +1,111 @@ +// Copyright (c) 2022 FireFly Automatix +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Adam Aposhian + +#ifndef PATH_UTILS__PATH_UTILS_HPP_ +#define PATH_UTILS__PATH_UTILS_HPP_ + +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" + +namespace path_utils +{ + +/** + * Build human-readable test paths + */ +class PathSegment +{ +public: + virtual void append(nav_msgs::msg::Path & path, double spacing) const = 0; + virtual ~PathSegment() {} +}; + +class Arc : public PathSegment +{ +public: + explicit Arc(double radius, double radians) + : radius_(radius), radians_(radians) {} + void append(nav_msgs::msg::Path & path, double spacing) const override; + +private: + double radius_; + double radians_; +}; + +class Straight : public PathSegment +{ +public: + explicit Straight(double length) + : length_(length) {} + void append(nav_msgs::msg::Path & path, double spacing) const override; + +private: + double length_; +}; + +class LeftTurn : public Arc +{ +public: + explicit LeftTurn(double radius) + : Arc(radius, M_PI_2) {} +}; + +class RightTurn : public Arc +{ +public: + explicit RightTurn(double radius) + : Arc(radius, -M_PI_2) {} +}; + +class LeftTurnAround : public Arc +{ +public: + explicit LeftTurnAround(double radius) + : Arc(radius, M_PI) {} +}; + +class RightTurnAround : public Arc +{ +public: + explicit RightTurnAround(double radius) + : Arc(radius, -M_PI) {} +}; + +class LeftCircle : public Arc +{ +public: + explicit LeftCircle(double radius) + : Arc(radius, 2.0 * M_PI) {} +}; + +class RightCircle : public Arc +{ +public: + explicit RightCircle(double radius) + : Arc(radius, -2.0 * M_PI) {} +}; + +nav_msgs::msg::Path generate_path( + geometry_msgs::msg::PoseStamped start, + double spacing, + std::initializer_list> segments); + +} // namespace path_utils + +#endif // PATH_UTILS__PATH_UTILS_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/test/path_utils/test_path_utils.cpp b/nav2_regulated_pure_pursuit_controller/test/path_utils/test_path_utils.cpp new file mode 100644 index 0000000000..795555521a --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/test/path_utils/test_path_utils.cpp @@ -0,0 +1,128 @@ +// Copyright (c) 2022 Adam Aposhian +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "path_utils.hpp" +#include "gtest/gtest.h" + +using namespace path_utils; // NOLINT + +TEST(PathUtils, test_generate_straight) +{ + geometry_msgs::msg::PoseStamped start; + start.header.frame_id = "test_frame"; + + constexpr double path_length = 2.0; + constexpr double spacing = 1.0; + + auto path = generate_path( + start, spacing, { + std::make_unique(path_length) + }); + EXPECT_EQ(path.poses.size(), 3u); + for (const auto & pose : path.poses) { + EXPECT_EQ(pose.header.frame_id, start.header.frame_id); + } + EXPECT_DOUBLE_EQ(path.poses[0].pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(path.poses[0].pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(path.poses[0].pose.position.z, 0.0); + + EXPECT_NEAR(path.poses[1].pose.position.x, 1.0, 0.1); + EXPECT_NEAR(path.poses[1].pose.position.y, 0.0, 0.1); + EXPECT_NEAR(path.poses[1].pose.position.z, 0.0, 0.1); + + EXPECT_NEAR(path.poses[2].pose.position.x, 2.0, 0.1); + EXPECT_NEAR(path.poses[2].pose.position.y, 0.0, 0.1); + EXPECT_NEAR(path.poses[2].pose.position.z, 0.0, 0.1); +} + +TEST(PathUtils, test_half_turn) +{ + // Start at a more interesting place, turned the other way + geometry_msgs::msg::PoseStamped start; + start.header.frame_id = "map"; + start.pose.position.x = 1.0; + start.pose.position.y = -1.0; + start.pose.orientation.x = 0.0; + start.pose.orientation.y = 0.0; + start.pose.orientation.z = 1.0; + start.pose.orientation.w = 0.0; + + constexpr double spacing = 0.1; + constexpr double radius = 2.0; + + auto path = generate_path( + start, spacing, { + std::make_unique(radius), + }); + constexpr double expected_path_length = M_PI * radius; + EXPECT_NEAR(path.poses.size(), 1 + static_cast(expected_path_length / spacing), 10); + for (const auto & pose : path.poses) { + EXPECT_EQ(pose.header.frame_id, start.header.frame_id); + } + + // Check the last pose + auto & last_pose = path.poses.back(); + auto & last_position = last_pose.pose.position; + EXPECT_NEAR(last_position.x, 1.0, 0.2); + EXPECT_NEAR(last_position.y, 3.0, 0.2); + EXPECT_DOUBLE_EQ(last_position.z, 0.0); + + // Should be facing forward now + auto & last_orientation = last_pose.pose.orientation; + EXPECT_NEAR(last_orientation.x, 0.0, 0.1); + EXPECT_NEAR(last_orientation.y, 0.0, 0.1); + EXPECT_NEAR(last_orientation.z, 0.0, 0.1); + EXPECT_NEAR(last_orientation.w, 1.0, 0.1); +} + +TEST(PathUtils, test_generate_all) +{ + geometry_msgs::msg::PoseStamped start; + start.header.frame_id = "map"; + + constexpr double spacing = 0.1; + + auto path = generate_path( + start, spacing, { + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0), + std::make_unique(1.0, 2 * M_PI), // another circle + }); + constexpr double expected_path_length = 1.0 + 2.0 * (M_PI_2 + M_PI_2) + 2.0 * (M_PI) +3.0 * + (2.0 * M_PI); + EXPECT_NEAR(path.poses.size(), 1 + static_cast(expected_path_length / spacing), 50); + for (const auto & pose : path.poses) { + EXPECT_EQ(pose.header.frame_id, start.header.frame_id); + } + + // Check the last pose + auto & last_pose = path.poses.back(); + auto & last_position = last_pose.pose.position; + EXPECT_NEAR(last_position.x, 3.0, 0.5); + EXPECT_NEAR(last_position.y, 6.0, 0.5); + EXPECT_DOUBLE_EQ(last_position.z, 0.0); + + auto & last_orientation = last_pose.pose.orientation; + EXPECT_NEAR(last_orientation.x, 0.0, 0.1); + EXPECT_NEAR(last_orientation.y, 0.0, 0.1); + EXPECT_NEAR(last_orientation.z, 0.0, 0.1); + EXPECT_NEAR(last_orientation.w, 1.0, 0.1); +} diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 9139097890..b8d7ecdad7 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -22,8 +22,10 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_util/lifecycle_node.hpp" +#include "path_utils/path_utils.hpp" #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" +#include "nav2_core/exceptions.hpp" class RclCppFixture { @@ -92,10 +94,16 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure linear_vel, sign); } - double findDirectionChangeWrapper( + double findVelocitySignChangeWrapper( const geometry_msgs::msg::PoseStamped & pose) { - return findDirectionChange(pose); + return findVelocitySignChange(pose); + } + + nav_msgs::msg::Path transformGlobalPlanWrapper( + const geometry_msgs::msg::PoseStamped & pose) + { + return transformGlobalPlan(pose); } }; @@ -108,6 +116,7 @@ TEST(RegulatedPurePursuitTest, basicAPI) // instantiate auto ctrl = std::make_shared(); + costmap->on_configure(rclcpp_lifecycle::State()); ctrl->configure(node, name, tf, costmap); ctrl->activate(); ctrl->deactivate(); @@ -150,7 +159,7 @@ TEST(RegulatedPurePursuitTest, createCarrotMsg) EXPECT_EQ(rtn->point.z, 0.01); } -TEST(RegulatedPurePursuitTest, findDirectionChange) +TEST(RegulatedPurePursuitTest, findVelocitySignChange) { auto ctrl = std::make_shared(); geometry_msgs::msg::PoseStamped pose; @@ -166,13 +175,13 @@ TEST(RegulatedPurePursuitTest, findDirectionChange) path.poses[2].pose.position.x = -1.0; path.poses[2].pose.position.y = -1.0; ctrl->setPlan(path); - auto rtn = ctrl->findDirectionChangeWrapper(pose); + auto rtn = ctrl->findVelocitySignChangeWrapper(pose); EXPECT_EQ(rtn, sqrt(5.0)); path.poses[2].pose.position.x = 3.0; path.poses[2].pose.position.y = 3.0; ctrl->setPlan(path); - rtn = ctrl->findDirectionChangeWrapper(pose); + rtn = ctrl->findVelocitySignChangeWrapper(pose); EXPECT_EQ(rtn, std::numeric_limits::max()); } @@ -452,3 +461,356 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) EXPECT_EQ(node->get_parameter("test.allow_reversing").as_bool(), false); EXPECT_EQ(node->get_parameter("test.use_rotate_to_heading").as_bool(), false); } + +class TransformGlobalPlanTest : public ::testing::Test +{ +protected: + void SetUp() override + { + ctrl_ = std::make_shared(); + node_ = std::make_shared("testRPP"); + tf_buffer_ = std::make_shared(node_->get_clock()); + costmap_ = std::make_shared("fake_costmap"); + } + + void configure_costmap(uint16_t width, double resolution) + { + constexpr char costmap_frame[] = "test_costmap_frame"; + constexpr char robot_frame[] = "test_robot_frame"; + + auto results = costmap_->set_parameters( + { + rclcpp::Parameter("global_frame", costmap_frame), + rclcpp::Parameter("robot_base_frame", robot_frame), + rclcpp::Parameter("width", width), + rclcpp::Parameter("height", width), + rclcpp::Parameter("resolution", resolution) + }); + for (const auto & result : results) { + EXPECT_TRUE(result.successful) << result.reason; + } + + rclcpp_lifecycle::State state; + costmap_->on_configure(state); + } + + void configure_controller(double max_robot_pose_search_dist) + { + std::string plugin_name = "test_rpp"; + nav2_util::declare_parameter_if_not_declared( + node_, plugin_name + ".max_robot_pose_search_dist", + rclcpp::ParameterValue(max_robot_pose_search_dist)); + ctrl_->configure(node_, plugin_name, tf_buffer_, costmap_); + } + + void setup_transforms(geometry_msgs::msg::Point & robot_position) + { + transform_time_ = node_->get_clock()->now(); + // Note: transforms go parent to child + + // We will have a separate path and costmap frame for completeness, + // but we will leave them cooincident for convenience. + geometry_msgs::msg::TransformStamped path_to_costmap; + path_to_costmap.header.frame_id = PATH_FRAME; + path_to_costmap.header.stamp = transform_time_; + path_to_costmap.child_frame_id = COSTMAP_FRAME; + path_to_costmap.transform.translation.x = 0.0; + path_to_costmap.transform.translation.y = 0.0; + path_to_costmap.transform.translation.z = 0.0; + + geometry_msgs::msg::TransformStamped costmap_to_robot; + costmap_to_robot.header.frame_id = COSTMAP_FRAME; + costmap_to_robot.header.stamp = transform_time_; + costmap_to_robot.child_frame_id = ROBOT_FRAME; + costmap_to_robot.transform.translation.x = robot_position.x; + costmap_to_robot.transform.translation.y = robot_position.y; + costmap_to_robot.transform.translation.z = robot_position.z; + + tf2_msgs::msg::TFMessage tf_message; + tf_message.transforms = { + path_to_costmap, + costmap_to_robot + }; + for (const auto transform : tf_message.transforms) { + tf_buffer_->setTransform(transform, "test", false); + } + tf_buffer_->setUsingDedicatedThread(true); // lying to let it do transforms + } + + static constexpr char PATH_FRAME[] = "test_path_frame"; + static constexpr char COSTMAP_FRAME[] = "test_costmap_frame"; + static constexpr char ROBOT_FRAME[] = "test_robot_frame"; + + std::shared_ptr ctrl_; + std::shared_ptr node_; + std::shared_ptr costmap_; + std::shared_ptr tf_buffer_; + rclcpp::Time transform_time_; +}; + +// This tests that not only should nothing get pruned on a costmap +// that contains the entire global_plan, and also that it doesn't skip to the end of the path +// which is closer to the robot pose than the start. +TEST_F(TransformGlobalPlanTest, no_pruning_on_large_costmap) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + robot_pose.pose.position.x = -0.1; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + // A really big costmap + // the max_costmap_extent should be 50m + configure_costmap(100u, 0.1); + configure_controller(5.0); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + constexpr double spacing = 0.1; + constexpr double circle_radius = 1.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(circle_radius) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + + auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); + EXPECT_EQ(transformed_plan.poses.size(), global_plan.poses.size()); +} + +// This plan shouldn't get pruned because of the costmap, +// but should be half pruned because it is halfway around the circle +TEST_F(TransformGlobalPlanTest, transform_start_selection) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + robot_pose.pose.position.x = 0.0; + robot_pose.pose.position.y = 4.0; // on the other side of the circle + robot_pose.pose.position.z = 0.0; + // Could set orientation going the other way, but RPP doesn't care + constexpr double spacing = 0.1; + constexpr double circle_radius = 2.0; // diameter 4 + + // A really big costmap + // the max_costmap_extent should be 50m + configure_costmap(100u, 0.1); + // This should just be at least half the circumference: pi*r ~= 6 + constexpr double max_robot_pose_search_dist = 10.0; + configure_controller(max_robot_pose_search_dist); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(circle_radius) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); + EXPECT_NEAR(transformed_plan.poses.size(), global_plan.poses.size() / 2, 1); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.x, 0.0, 0.5); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.y, 0.0, 0.5); +} + +// This should throw an exception when all poses are outside of the costmap +TEST_F(TransformGlobalPlanTest, all_poses_outside_of_costmap) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + // far away from the path + robot_pose.pose.position.x = 1000.0; + robot_pose.pose.position.y = 1000.0; + robot_pose.pose.position.z = 0.0; + // Could set orientation going the other way, but RPP doesn't care + constexpr double spacing = 0.1; + constexpr double circle_radius = 2.0; // diameter 4 + + // A "normal" costmap + // the max_costmap_extent should be 50m + configure_costmap(10u, 0.1); + // This should just be at least half the circumference: pi*r ~= 6 + constexpr double max_robot_pose_search_dist = 10.0; + configure_controller(max_robot_pose_search_dist); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(circle_radius) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + EXPECT_THROW(ctrl_->transformGlobalPlanWrapper(robot_pose), nav2_core::PlannerException); +} + +// Should shortcut the circle if the circle is shorter than max_robot_pose_search_dist +TEST_F(TransformGlobalPlanTest, good_circle_shortcut) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + // far away from the path + robot_pose.pose.position.x = -0.1; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + // Could set orientation going the other way, but RPP doesn't care + constexpr double spacing = 0.1; + constexpr double circle_radius = 2.0; // diameter 4 + + // A "normal" costmap + // the max_costmap_extent should be 50m + configure_costmap(100u, 0.1); + // This should just be at least the circumference: 2*pi*r ~= 12 + constexpr double max_robot_pose_search_dist = 15.0; + configure_controller(max_robot_pose_search_dist); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(circle_radius) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); + EXPECT_NEAR(transformed_plan.poses.size(), 1, 1); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.x, 0.0, 0.5); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.y, 0.0, 0.5); +} + +// Simple costmap pruning on a straight line +TEST_F(TransformGlobalPlanTest, costmap_pruning) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + // far away from the path + robot_pose.pose.position.x = -0.1; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + // Could set orientation going the other way, but RPP doesn't care + constexpr double spacing = 1.0; + + // A "normal" costmap + // the max_costmap_extent should be 50m + configure_costmap(20u, 0.5); + constexpr double max_robot_pose_search_dist = 10.0; + configure_controller(max_robot_pose_search_dist); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + constexpr double path_length = 100.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(path_length) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); + EXPECT_NEAR(transformed_plan.poses.size(), 10u, 1); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.x, 0.0, 0.5); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.y, 0.0, 0.5); +} + +// Should prune out later portions of the path that come back into the costmap +TEST_F(TransformGlobalPlanTest, prune_after_leaving_costmap) +{ + geometry_msgs::msg::PoseStamped robot_pose; + robot_pose.header.frame_id = COSTMAP_FRAME; + robot_pose.header.stamp = transform_time_; + // far away from the path + robot_pose.pose.position.x = -0.1; + robot_pose.pose.position.y = 0.0; + robot_pose.pose.position.z = 0.0; + // Could set orientation going the other way, but RPP doesn't care + constexpr double spacing = 1.0; + + // A "normal" costmap + // the max_costmap_extent should be 50m + configure_costmap(20u, 0.5); + constexpr double max_robot_pose_search_dist = 10.0; + configure_controller(max_robot_pose_search_dist); + setup_transforms(robot_pose.pose.position); + + // Set up test path; + + geometry_msgs::msg::PoseStamped start_of_path; + start_of_path.header.frame_id = PATH_FRAME; + start_of_path.header.stamp = transform_time_; + start_of_path.pose.position.x = 0.0; + start_of_path.pose.position.y = 0.0; + start_of_path.pose.position.z = 0.0; + + constexpr double path_length = 100.0; + + auto global_plan = path_utils::generate_path( + start_of_path, spacing, { + std::make_unique(path_length), + std::make_unique(1.0), + std::make_unique(path_length) + }); + + ctrl_->setPlan(global_plan); + + // Transform the plan + auto transformed_plan = ctrl_->transformGlobalPlanWrapper(robot_pose); + // This should be essentially the same as the regular straight path + EXPECT_NEAR(transformed_plan.poses.size(), 10u, 1); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.x, 0.0, 0.5); + EXPECT_NEAR(transformed_plan.poses[0].pose.position.y, 0.0, 0.5); +} diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 17e654fc28..a06c6c3431 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -19,6 +19,7 @@ #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -54,7 +55,7 @@ inline double euclidean_distance( double dx = pos1.x - pos2.x; double dy = pos1.y - pos2.y; double dz = pos1.z - pos2.z; - return std::sqrt(dx * dx + dy * dy + dz * dz); + return std::hypot(dx, dy, dz); } /** @@ -70,7 +71,7 @@ inline double euclidean_distance( double dx = pos1.position.x - pos2.position.x; double dy = pos1.position.y - pos2.position.y; double dz = pos1.position.z - pos2.position.z; - return std::sqrt(dx * dx + dy * dy + dz * dz); + return std::hypot(dx, dy, dz); } /** @@ -86,6 +87,22 @@ inline double euclidean_distance( return euclidean_distance(pos1.pose, pos2.pose); } +/** + * @brief Get the L2 distance between 2 geometry_msgs::Pose2D + * @param pos1 First pose + * @param pos1 Second pose + * @return double L2 distance + */ +inline double euclidean_distance( + const geometry_msgs::msg::Pose2D & pos1, + const geometry_msgs::msg::Pose2D & pos2) +{ + double dx = pos1.x - pos2.x; + double dy = pos1.y - pos2.y; + + return std::hypot(dx, dy); +} + /** * Find element in iterator with the minimum calculated value */ @@ -107,6 +124,25 @@ inline Iter min_by(Iter begin, Iter end, Getter getCompareVal) return lowest_it; } +/** + * Find first element in iterator that is greater integrated distance than comparevalue + */ +template +inline Iter first_after_integrated_distance(Iter begin, Iter end, Getter getCompareVal) +{ + if (begin == end) { + return end; + } + Getter dist = 0.0; + for (Iter it = begin; it != end - 1; it++) { + dist += euclidean_distance(*it, *(it + 1)); + if (dist > getCompareVal) { + return it + 1; + } + } + return end; +} + /** * @brief Calculate the length of the provided path, starting at the provided index * @param path Path containing the poses that are planned