Route server corner smoothing (backport #5226)

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"

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

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 <vector>
+#include <cmath>
+#include <algorithm>
+
+#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<float> start_edge_unit_tangent =
+      {(start.x - corner.x) / start_edge_length_, (start.y - corner.y) / start_edge_length_};
+      std::vector<float> 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<float> 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<geometry_msgs::msg::PoseStamped> & poses)
+  {
+    std::vector<float> r_start{start_coordinate_.x - circle_center_coordinate_.x,
+      start_coordinate_.y - circle_center_coordinate_.y};
+    std::vector<float> 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<int>(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_

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<nav_msgs::msg::Path>::SharedPtr path_pub_;
+  rclcpp::Logger logger_{rclcpp::get_logger("PathConverter")};
   float density_;
+  float smoothing_radius_;
+  bool smooth_corners_;
 };
 
 }  // namespace nav2_route

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

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<float>(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<float>(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<nav_msgs::msg::Path>("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));
   }

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