From ef0d4724d0a13a9c81d0ce7bc53b011f46ac0e06 Mon Sep 17 00:00:00 2001 From: alexanderjyuen <103065090+alexanderjyuen@users.noreply.github.com> Date: Wed, 11 Jun 2025 19:53:43 -0700 Subject: [PATCH] Route server corner smoothing (#5226) * added edge length method Signed-off-by: Alexander Yuen * Added corner arc class Signed-off-by: Alexander Yuen * replaced double vectors with Coordinates, added methods to return start and end coordinates Signed-off-by: Alexander Yuen * using Coordinates, fixed direction of tangents Signed-off-by: Alexander Yuen * added corner arc in header, added logger in protected variable Signed-off-by: Alexander Yuen * first pass of corner smoothing algorithm Signed-off-by: Alexander Yuen * reassigning next edge to have a different start, if a corner occurs before it Signed-off-by: Alexander Yuen * using unique pointer instead of raw pointers for new edges and nodes Signed-off-by: Alexander Yuen * added smoothing parameter Signed-off-by: Alexander Yuen * made angle of interpolation a parameter Signed-off-by: Alexander Yuen * const for return methods, added flag for smoothing corners Signed-off-by: Alexander Yuen * moved getEdgeLength() into the Directional Edge struct Signed-off-by: Alexander Yuen * using float instead of double Signed-off-by: Alexander Yuen * smoothing radius is float, couple methods moved to protected Signed-off-by: Alexander Yuen * removed signed_angle_ as a member variable Signed-off-by: Alexander Yuen * removed unnecessary member variables Signed-off-by: Alexander Yuen * removed angle of interpolation and inferring it from path density and radius instead Signed-off-by: Alexander Yuen * consolidated corner arc into one header function Signed-off-by: Alexander Yuen * readded newline Signed-off-by: Alexander Yuen * changed corner arc to corner smoothing Signed-off-by: Alexander Yuen * replaced the use of edges with coordinates to generate smoothing arc, removed storage of nodes and edges Signed-off-by: Alexander Yuen * linting Signed-off-by: Alexander Yuen * fixing cpplint Signed-off-by: Alexander Yuen * linting for headers Signed-off-by: Alexander Yuen * cpplinting Signed-off-by: Alexander Yuen * Update nav2_route/src/path_converter.cpp Signed-off-by: Steve Macenski * Update nav2_route/src/path_converter.cpp Signed-off-by: Steve Macenski * Update nav2_route/src/path_converter.cpp Signed-off-by: Steve Macenski * Update nav2_route/src/path_converter.cpp Signed-off-by: Steve Macenski * Update nav2_route/include/nav2_route/corner_smoothing.hpp Signed-off-by: Steve Macenski * fixed divide by zeros and accessing empty route.edges Signed-off-by: Alexander Yuen * uncrustify linting Signed-off-by: Alexander Yuen * cpp linting Signed-off-by: Alexander Yuen * path converter linting Signed-off-by: Alexander Yuen * changed all doubles to floats Signed-off-by: Alexander Yuen * added check for edges that are colinear to avoid divide by 0, fixed final edge interpolation Signed-off-by: Alexander Yuen * linting Signed-off-by: Alexander Yuen * Update nav2_route/include/nav2_route/corner_smoothing.hpp Signed-off-by: Steve Macenski * added doxygen for corner arc class Signed-off-by: Alexander Yuen * added warning message if corner can't be smoothed Signed-off-by: Alexander Yuen * added smooth_corners to the nav2 params file Signed-off-by: Alexander Yuen * added smoothing flag and radius parameter to README.md' Signed-off-by: Alexander Yuen * typo in README Signed-off-by: Alexander Yuen * added testing for corner smoothing Signed-off-by: Alexander Yuen * Update nav2_route/include/nav2_route/corner_smoothing.hpp Signed-off-by: Steve Macenski --------- Signed-off-by: Alexander Yuen Signed-off-by: Steve Macenski Co-authored-by: Steve Macenski (cherry picked from commit 792bcac93e44258a97f3d38cdf6a3a8f5714d4b6) --- nav2_bringup/params/nav2_params.yaml | 1 + nav2_route/README.md | 2 + .../include/nav2_route/corner_smoothing.hpp | 176 ++++++++++++++++++ .../include/nav2_route/path_converter.hpp | 4 + nav2_route/include/nav2_route/types.hpp | 9 + nav2_route/src/path_converter.cpp | 52 +++++- nav2_route/test/CMakeLists.txt | 8 + nav2_route/test/test_corner_smoothing.cpp | 115 ++++++++++++ 8 files changed, 361 insertions(+), 6 deletions(-) create mode 100644 nav2_route/include/nav2_route/corner_smoothing.hpp create mode 100644 nav2_route/test/test_corner_smoothing.cpp diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index f1d7fb2169b..cfbcdc858bf 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -447,6 +447,7 @@ route_server: # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson boundary_radius_to_achieve_node: 1.0 radius_to_achieve_node: 2.0 + smooth_corners: true operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"] ReroutingService: plugin: "nav2_route::ReroutingService" diff --git a/nav2_route/README.md b/nav2_route/README.md index a434276bff8..c0eb510b1db 100644 --- a/nav2_route/README.md +++ b/nav2_route/README.md @@ -114,6 +114,8 @@ route_server: path_density: 0.05 # Density of points for generating the dense nav_msgs/Path from route (m) max_iterations: 0 # Maximum number of search iterations, if 0, uses maximum possible max_planning_time: 2.0 # Maximum planning time (seconds) + smooth_corners: true # Whether to smooth corners formed by adjacent edges or not + smoothing_radius: 1.0 # Radius of corner to fit into the corner costmap_topic: 'global_costmap/costmap_raw' # Costmap topic when enable_nn_search is enabled. May also be used by the collision monitor operation and/or the costmap edge scorer if using the same topic to share resources. graph_file_loader: "GeoJsonGraphFileLoader" # Name of default file loader diff --git a/nav2_route/include/nav2_route/corner_smoothing.hpp b/nav2_route/include/nav2_route/corner_smoothing.hpp new file mode 100644 index 00000000000..f14d2f43a41 --- /dev/null +++ b/nav2_route/include/nav2_route/corner_smoothing.hpp @@ -0,0 +1,176 @@ +// Copyright (c) 2025, Polymath Robotics +// +// 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. + +#ifndef NAV2_ROUTE__CORNER_SMOOTHING_HPP_ +#define NAV2_ROUTE__CORNER_SMOOTHING_HPP_ + +#include +#include +#include + +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" + +namespace nav2_route +{ +/** + * @class nav2_route::CornerArc + * @brief A class used to smooth corners defined by the edges and nodes + * of the route graph. Used with path converter to output a corner smoothed plan + */ +class CornerArc +{ +public: + /** + * @brief A constructor for nav2_route::CornerArc + * @param start start coordinate of corner to be smoothed + * @param corner corner coordinate of corner to be smoothed + * @param end end coordinate of corner to be smoothed + * @param minimum_radius smoothing radius to fit to the corner + */ + CornerArc( + const Coordinates & start, const Coordinates & corner, const Coordinates & end, + float minimum_radius) + { + start_edge_length_ = hypotf(corner.x - start.x, corner.y - start.y); + end_edge_length_ = hypotf(end.x - corner.x, end.y - corner.y); + + // invalid scenario would cause equations to blow up + if (start_edge_length_ == 0.0 || end_edge_length_ == 0.0) {return;} + + float angle = getAngleBetweenEdges(start, corner, end); + + // cannot smooth a 0 (back and forth) or 180 (straight line) angle + if (std::abs(angle) < 1E-6 || std::abs(angle - M_PI) < 1E-6) {return;} + + float tangent_length = minimum_radius / (std::tan(std::fabs(angle) / 2)); + + if (tangent_length < start_edge_length_ && tangent_length < end_edge_length_) { + std::vector start_edge_unit_tangent = + {(start.x - corner.x) / start_edge_length_, (start.y - corner.y) / start_edge_length_}; + std::vector end_edge_unit_tangent = + {(end.x - corner.x) / end_edge_length_, (end.y - corner.y) / end_edge_length_}; + + float bisector_x = start_edge_unit_tangent[0] + end_edge_unit_tangent[0]; + float bisector_y = start_edge_unit_tangent[1] + end_edge_unit_tangent[1]; + float bisector_magnitude = std::sqrt(bisector_x * bisector_x + bisector_y * bisector_y); + + std::vector unit_bisector = + {bisector_x / bisector_magnitude, bisector_y / bisector_magnitude}; + + start_coordinate_.x = corner.x + start_edge_unit_tangent[0] * tangent_length; + start_coordinate_.y = corner.y + start_edge_unit_tangent[1] * tangent_length; + + end_coordinate_.x = corner.x + end_edge_unit_tangent[0] * tangent_length; + end_coordinate_.y = corner.y + end_edge_unit_tangent[1] * tangent_length; + + float bisector_length = minimum_radius / std::sin(angle / 2); + + circle_center_coordinate_.x = corner.x + unit_bisector[0] * bisector_length; + circle_center_coordinate_.y = corner.y + unit_bisector[1] * bisector_length; + + valid_corner_ = true; + } + } + + /** + * @brief A destructor for nav2_route::CornerArc + */ + ~CornerArc() = default; + + /** + * @brief interpolates the arc for a path of certain density + * @param max_angle_resolution Resolution to interpolate path to + * @param poses Pose output + */ + void interpolateArc( + const float & max_angle_resolution, + std::vector & poses) + { + std::vector r_start{start_coordinate_.x - circle_center_coordinate_.x, + start_coordinate_.y - circle_center_coordinate_.y}; + std::vector r_end{end_coordinate_.x - circle_center_coordinate_.x, + end_coordinate_.y - circle_center_coordinate_.y}; + float cross = r_start[0] * r_end[1] - r_start[1] * r_end[0]; + float dot = r_start[0] * r_end[0] + r_start[1] * r_end[1]; + float signed_angle = std::atan2(cross, dot); + // lower limit for N is 1 to protect against divide by 0 + int N = std::max(1, static_cast(std::ceil(std::abs(signed_angle) / max_angle_resolution))); + float angle_resolution = signed_angle / N; + + float x, y; + for (int i = 0; i <= N; i++) { + float angle = i * angle_resolution; + x = circle_center_coordinate_.x + + (r_start[0] * std::cos(angle) - r_start[1] * std::sin(angle)); + y = circle_center_coordinate_.y + + (r_start[0] * std::sin(angle) + r_start[1] * std::cos(angle)); + poses.push_back(utils::toMsg(x, y)); + } + } + + /** + * @brief return if a valid corner arc (one that doesn't overrun the edge lengths) is generated + * @return if the a corner was able to be generated + */ + bool isCornerValid() const {return valid_corner_;} + + /** + * @brief return the start coordinate of the corner arc + * @return start coordinate of the arc + */ + Coordinates getCornerStart() const {return start_coordinate_;} + + /** + * @brief return the end coordinate of the corner arc + * @return end coordinate of the arc + */ + Coordinates getCornerEnd() const {return end_coordinate_;} + +protected: + /** + * @brief find the signed angle between a corner generated by 3 points + * @param start start coordinate of corner to be smoothed + * @param corner corner coordinate of corner to be smoothed + * @param end end coordinate of corner to be smoothed + * @return signed angle of the corner + */ + float getAngleBetweenEdges( + const Coordinates & start, const Coordinates & corner, + const Coordinates & end) + { + float start_dx = start.x - corner.x; + float start_dy = start.y - corner.y; + + float end_dx = end.x - corner.x; + float end_dy = end.y - corner.y; + + float angle = + acos((start_dx * end_dx + start_dy * end_dy) / (start_edge_length_ * end_edge_length_)); + + return angle; + } + +private: + bool valid_corner_{false}; + float start_edge_length_; + float end_edge_length_; + Coordinates start_coordinate_; + Coordinates end_coordinate_; + Coordinates circle_center_coordinate_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__CORNER_SMOOTHING_HPP_ diff --git a/nav2_route/include/nav2_route/path_converter.hpp b/nav2_route/include/nav2_route/path_converter.hpp index 90b61898160..c4454f768ba 100644 --- a/nav2_route/include/nav2_route/path_converter.hpp +++ b/nav2_route/include/nav2_route/path_converter.hpp @@ -26,6 +26,7 @@ #include "nav2_util/node_utils.hpp" #include "nav2_route/types.hpp" #include "nav2_route/utils.hpp" +#include "nav2_route/corner_smoothing.hpp" namespace nav2_route { @@ -80,7 +81,10 @@ class PathConverter protected: rclcpp_lifecycle::LifecyclePublisher::SharedPtr path_pub_; + rclcpp::Logger logger_{rclcpp::get_logger("PathConverter")}; float density_; + float smoothing_radius_; + bool smooth_corners_; }; } // namespace nav2_route diff --git a/nav2_route/include/nav2_route/types.hpp b/nav2_route/include/nav2_route/types.hpp index dc31c5e01da..0f021a0e23f 100644 --- a/nav2_route/include/nav2_route/types.hpp +++ b/nav2_route/include/nav2_route/types.hpp @@ -138,6 +138,8 @@ struct DirectionalEdge EdgeCost edge_cost; // Cost information associated with edge Metadata metadata; // Any metadata stored in the graph file of interest Operations operations; // Operations to perform related to the edge + + float getEdgeLength(); }; typedef DirectionalEdge * EdgePtr; @@ -194,6 +196,13 @@ struct Node } }; +inline float DirectionalEdge::getEdgeLength() +{ + return hypotf( + end->coords.x - start->coords.x, + end->coords.y - start->coords.y); +} + /** * @struct nav2_route::Route * @brief An ordered set of nodes and edges corresponding to the planned route diff --git a/nav2_route/src/path_converter.cpp b/nav2_route/src/path_converter.cpp index 38adea20c25..85607a5491b 100644 --- a/nav2_route/src/path_converter.cpp +++ b/nav2_route/src/path_converter.cpp @@ -30,9 +30,16 @@ void PathConverter::configure(nav2_util::LifecycleNode::SharedPtr node) nav2_util::declare_parameter_if_not_declared( node, "path_density", rclcpp::ParameterValue(0.05)); density_ = static_cast(node->get_parameter("path_density").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "smoothing_radius", rclcpp::ParameterValue(1.0)); + smoothing_radius_ = static_cast(node->get_parameter("smoothing_radius").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "smooth_corners", rclcpp::ParameterValue(false)); + smooth_corners_ = node->get_parameter("smooth_corners").as_bool(); path_pub_ = node->create_publisher("plan", 1); path_pub_->on_activate(); + logger_ = node->get_logger(); } nav_msgs::msg::Path PathConverter::densify( @@ -54,17 +61,50 @@ nav_msgs::msg::Path PathConverter::densify( interpolateEdge(start.x, start.y, end.x, end.y, path.poses); } - // Fill in path via route edges - for (unsigned int i = 0; i != route.edges.size(); i++) { - const EdgePtr edge = route.edges[i]; - const Coordinates & start = edge->start->coords; - const Coordinates & end = edge->end->coords; - interpolateEdge(start.x, start.y, end.x, end.y, path.poses); + Coordinates start; + Coordinates end; + + if (!route.edges.empty()) { + start = route.edges[0]->start->coords; + + // Fill in path via route edges + for (unsigned int i = 0; i < route.edges.size() - 1; i++) { + const EdgePtr edge = route.edges[i]; + const EdgePtr & next_edge = route.edges[i + 1]; + end = edge->end->coords; + + CornerArc corner_arc(start, end, next_edge->end->coords, smoothing_radius_); + if (corner_arc.isCornerValid() && smooth_corners_) { + // if an arc exists, end of the first edge is the start of the arc + end = corner_arc.getCornerStart(); + + // interpolate to start of arc + interpolateEdge(start.x, start.y, end.x, end.y, path.poses); + + // interpolate arc + corner_arc.interpolateArc(density_ / smoothing_radius_, path.poses); + + // new start of next edge is end of smoothing arc + start = corner_arc.getCornerEnd(); + } else { + if (smooth_corners_) { + RCLCPP_WARN( + logger_, "Unable to smooth corner between edge %i and edge %i", edge->edgeid, + next_edge->edgeid); + } + interpolateEdge(start.x, start.y, end.x, end.y, path.poses); + start = end; + } + } } if (route.edges.empty()) { path.poses.push_back(utils::toMsg(route.start_node->coords.x, route.start_node->coords.y)); } else { + interpolateEdge( + start.x, start.y, route.edges.back()->end->coords.x, + route.edges.back()->end->coords.y, path.poses); + path.poses.push_back( utils::toMsg(route.edges.back()->end->coords.x, route.edges.back()->end->coords.y)); } diff --git a/nav2_route/test/CMakeLists.txt b/nav2_route/test/CMakeLists.txt index 186c93dbfca..6ff732b35e4 100644 --- a/nav2_route/test/CMakeLists.txt +++ b/nav2_route/test/CMakeLists.txt @@ -127,3 +127,11 @@ ament_add_gtest(test_goal_intent_search target_link_libraries(test_goal_intent_search ${library_name} ) + +# Test corner smoothing +ament_add_gtest(test_corner_smoothing + test_corner_smoothing.cpp +) +target_link_libraries(test_corner_smoothing + ${library_name} +) diff --git a/nav2_route/test/test_corner_smoothing.cpp b/nav2_route/test/test_corner_smoothing.cpp new file mode 100644 index 00000000000..928ab07f3cd --- /dev/null +++ b/nav2_route/test/test_corner_smoothing.cpp @@ -0,0 +1,115 @@ +// Copyright (c) 2025, Polymath Robotics +// +// 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. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_route/corner_smoothing.hpp" +// #include "nav2_route/types.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +TEST(CornerSmoothingTest, test_corner_smoothing) +{ + Node test_node1, test_node2, test_node3; + test_node1.nodeid = 0; + test_node1.coords.x = 0.0; + test_node1.coords.y = 0.0; + test_node2.nodeid = 1; + test_node2.coords.x = 10.0; + test_node2.coords.y = 0.0; + test_node3.nodeid = 2; + test_node3.coords.x = 10.0; + test_node3.coords.y = 10.0; + + double smoothing_radius = 2.0; + + CornerArc corner_arc(test_node1.coords, test_node2.coords, test_node3.coords, smoothing_radius); + + Coordinates start = corner_arc.getCornerStart(); + Coordinates end = corner_arc.getCornerEnd(); + + EXPECT_TRUE(corner_arc.isCornerValid()); + EXPECT_EQ(start.x, 8.0); + EXPECT_EQ(start.y, 0.0); + EXPECT_EQ(end.x, 10.0); + EXPECT_EQ(end.y, 2.0); +} + +TEST(LargeRadiusTest, test_large_radius_smoothing) +{ + Node test_node1, test_node2, test_node3; + test_node1.nodeid = 0; + test_node1.coords.x = 0.0; + test_node1.coords.y = 0.0; + test_node2.nodeid = 1; + test_node2.coords.x = 10.0; + test_node2.coords.y = 0.0; + test_node3.nodeid = 2; + test_node3.coords.x = 10.0; + test_node3.coords.y = 10.0; + + double smoothing_radius = 20.0; + + CornerArc corner_arc(test_node1.coords, test_node2.coords, test_node3.coords, smoothing_radius); + + EXPECT_FALSE(corner_arc.isCornerValid()); +} + +TEST(ColinearSmoothingTest, test_colinear_smoothing) +{ + Node test_node1, test_node2, test_node3; + test_node1.nodeid = 0; + test_node1.coords.x = 0.0; + test_node1.coords.y = 0.0; + test_node2.nodeid = 1; + test_node2.coords.x = 10.0; + test_node2.coords.y = 0.0; + test_node3.nodeid = 2; + test_node3.coords.x = 20.0; + test_node3.coords.y = 0.0; + + double smoothing_radius = 2.0; + + CornerArc corner_arc(test_node1.coords, test_node2.coords, test_node3.coords, smoothing_radius); + + EXPECT_FALSE(corner_arc.isCornerValid()); +} + +TEST(DegeneratePointsTest, test_degenerate_points_smoothing) +{ + Node test_node1; + test_node1.nodeid = 0; + test_node1.coords.x = 0.0; + test_node1.coords.y = 0.0; + + double smoothing_radius = 2.0; + + CornerArc corner_arc(test_node1.coords, test_node1.coords, test_node1.coords, smoothing_radius); + + EXPECT_FALSE(corner_arc.isCornerValid()); +}