Route server corner smoothing

nav2_route/CMakeLists.txt

@@ -34,6 +34,7 @@ add_library(${library_name} SHARED
   src/route_server.cpp
   src/route_planner.cpp
   src/route_tracker.cpp
+  src/corner_arc.cpp
   src/edge_scorer.cpp
   src/operations_manager.cpp
   src/node_spatial_tree.cpp

nav2_route/include/nav2_route/corner_arc.hpp

@@ -0,0 +1,65 @@
+// 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_ARC_HPP_
+#define NAV2_ROUTE__CORNER_ARC_HPP_
+
+#include <vector>
+#include <cmath>
+
+#include "nav2_route/types.hpp"
+#include "nav2_route/utils.hpp"
+
+namespace nav2_route
+{
+class CornerArc
+{
+public:
+  /**
+   * @brief Constructor
+   */
+  CornerArc(EdgePtr start_edge, EdgePtr end_edge, double minimum_radius);
+
+  ~CornerArc() = default;
+
+  void interpolateArc(const double & max_angle_resolution, std::vector<geometry_msgs::msg::PoseStamped> & poses);
+
+  double getEdgeLength(const EdgePtr edge);
+
+  double getAngleBetweenEdges(const EdgePtr start_edge, const EdgePtr end_edge);
+
+  double getSignedAngleBetweenEdges(const EdgePtr start_edge, const EdgePtr end_edge);
+
+  bool isCornerValid() const { return valid_corner_; }
+
+  Coordinates getCornerStart() const { return start_coordinate_; }
+
+  Coordinates getCornerEnd() const { return end_coordinate_; }
+
+private:
+  EdgePtr start_edge_;
+  EdgePtr end_edge_;
+  double start_edge_length_;
+  double end_edge_length_;
+  double minimum_radius_;
+  double signed_angle_;
+  bool valid_corner_{false};
+  Coordinates start_coordinate_;
+  Coordinates end_coordinate_;
+  Coordinates circle_center_coordinate_;
+};
+
+}  // namespace nav2_route
+
+#endif  // NAV2_ROUTE__CORNER_ARC_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_arc.hpp"
 
 namespace nav2_route
 {
@@ -78,9 +79,15 @@ class PathConverter
     float x0, float y0, float x1, float y1,
     std::vector<geometry_msgs::msg::PoseStamped> & poses);
 
+  double getEdgeLength(const EdgePtr edge);
+
 protected:
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr path_pub_;
+  rclcpp::Logger logger_{rclcpp::get_logger("PathConverter")};
   float density_;
+  double smoothing_radius_;
+  double angle_of_interpolation_;
+  bool smooth_corners_;
 };
 
 }  // namespace nav2_route

nav2_route/src/corner_arc.cpp

@@ -0,0 +1,133 @@
+// 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.
+
+#include "nav2_route/corner_arc.hpp"
+#include <iostream>
+
+namespace nav2_route
+{
+
+CornerArc::CornerArc(EdgePtr start_edge, EdgePtr end_edge, double minimum_radius)
+{
+  start_edge_ = start_edge;
+  end_edge_ = end_edge;
+
+  double angle = getAngleBetweenEdges(start_edge, end_edge);
+
+  double tangent_length = minimum_radius/(std::tan(angle/2));
+
+  start_edge_length_ = getEdgeLength(start_edge);
+  end_edge_length_ = getEdgeLength(end_edge);
+
+  if(tangent_length < start_edge_length_ && tangent_length < end_edge_length_){
+    std::vector<double> start_edge_unit_tangent;
+    std::vector<double> end_edge_unit_tangent;
+    std::vector<double> unit_bisector;
+
+    start_edge_unit_tangent.push_back((start_edge->start->coords.x -start_edge->end->coords.x)/start_edge_length_);
+    start_edge_unit_tangent.push_back((start_edge->start->coords.y -start_edge->end->coords.y)/start_edge_length_);
+    end_edge_unit_tangent.push_back((end_edge->end->coords.x - end_edge->start->coords.x)/end_edge_length_);
+    end_edge_unit_tangent.push_back((end_edge->end->coords.y - end_edge->start->coords.y)/end_edge_length_);
+
+    double bisector_x = start_edge_unit_tangent[0]+end_edge_unit_tangent[0];
+    double bisector_y = start_edge_unit_tangent[1]+end_edge_unit_tangent[1];
+    double bisector_magnitude = std::sqrt(bisector_x*bisector_x + bisector_y*bisector_y);
+
+    unit_bisector.push_back(bisector_x/bisector_magnitude);
+    unit_bisector.push_back(bisector_y/bisector_magnitude);
+
+    start_coordinate_.x = start_edge->end->coords.x + start_edge_unit_tangent[0]*tangent_length;
+    start_coordinate_.y = start_edge->end->coords.y + start_edge_unit_tangent[1]*tangent_length;
+
+    end_coordinate_.x = end_edge->start->coords.x + end_edge_unit_tangent[0]*tangent_length;
+    end_coordinate_.y = end_edge->start->coords.y + end_edge_unit_tangent[1]*tangent_length;
+
+    double signed_angle = getSignedAngleBetweenEdges(start_edge, end_edge);
+
+    double bisector_length = minimum_radius/std::sin(signed_angle/2);
+
+    circle_center_coordinate_.x = end_edge->start->coords.x + unit_bisector[0]*bisector_length;
+    circle_center_coordinate_.y = end_edge->start->coords.y + unit_bisector[1]*bisector_length;
+
+    minimum_radius_ = minimum_radius;
+    valid_corner_ = true;
+
+  }
+}
+
+void CornerArc::interpolateArc(const double & max_angle_resolution, std::vector<geometry_msgs::msg::PoseStamped> & poses)
+{
+
+  std::vector<double> r_start{ start_coordinate_.x-circle_center_coordinate_.x, start_coordinate_.y-circle_center_coordinate_.y };
+  std::vector<double> r_end{ end_coordinate_.x-circle_center_coordinate_.x,  end_coordinate_.y-circle_center_coordinate_.y };
+  double cross = r_start[0]*r_end[1] - r_start[1]*r_end[0];
+  double dot = r_start[0]*r_end[0] + r_start[1]*r_end[1];
+  signed_angle_ = std::atan2(cross, dot);
+  int N = std::ceil(std::abs(signed_angle_)/max_angle_resolution);
+  double angle_resolution = signed_angle_/N;
+
+  float x, y;
+
+  for(int i = 0; i <= N; i++){
+    double 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));
+  }
+}
+
+double CornerArc::getEdgeLength(const EdgePtr edge){
+    const Coordinates & start = edge->start->coords;
+    const Coordinates & end = edge->end->coords;
+
+    float dx = end.x - start.x;
+    float dy = end.y - start.y;
+
+    return double(sqrt(dx*dx + dy*dy));
+  }
+
+double CornerArc::getAngleBetweenEdges(const EdgePtr start_edge, const EdgePtr end_edge){
+
+    double start_dx = start_edge->start->coords.x - start_edge->end->coords.x;
+    double start_dy = start_edge->start->coords.y - start_edge->end->coords.y;
+
+    double end_dx = end_edge->end->coords.x - end_edge->start->coords.x;
+    double end_dy = end_edge->end->coords.y - end_edge->start->coords.y;
+
+    double angle = acos((start_dx*end_dx + start_dy*end_dy)/(getEdgeLength(start_edge)*getEdgeLength(end_edge)));
+
+    return angle;
+}
+
+double CornerArc::getSignedAngleBetweenEdges(const EdgePtr start_edge, const EdgePtr end_edge){
+
+  double start_edge_length = getEdgeLength(start_edge);
+  double end_edge_length = getEdgeLength(end_edge);
+
+  double start_dx = (start_edge->start->coords.x - start_edge->end->coords.x)/start_edge_length;
+  double start_dy = (start_edge->start->coords.y - start_edge->end->coords.y)/start_edge_length;
+
+  double end_dx = (end_edge->end->coords.x - end_edge->start->coords.x)/end_edge_length;
+  double end_dy = (end_edge->end->coords.y - end_edge->start->coords.y)/end_edge_length;
+
+  double dot = start_dx*end_dx + start_dy*end_dy;
+  dot = std::clamp(dot, -1.0, 1.0);
+
+  double angle = std::acos(dot);
+
+  return angle;
+}
+
+
+}  // namespace nav2_route

nav2_route/src/path_converter.cpp

@@ -20,7 +20,6 @@
 #include <algorithm>
 
 #include "nav2_route/path_converter.hpp"
-
 namespace nav2_route
 {
 
@@ -30,9 +29,19 @@ 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_ = node->get_parameter("smoothing_radius").as_double();
+  nav2_util::declare_parameter_if_not_declared(
+    node, "angle_of_interpolation", rclcpp::ParameterValue(0.1));
+  angle_of_interpolation_ = node->get_parameter("angle_of_interpolation").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,19 +63,66 @@ nav_msgs::msg::Path PathConverter::densify(
     interpolateEdge(start.x, start.y, end.x, end.y, path.poses);
   }
 
+  // Make a copy of all the edges as we'll modify the edges during
+  // smoothing and we don't want to modify the original edges on the route
+  std::vector<EdgePtr> edges{route.edges.begin(), route.edges.end()};
+
+  // Make new edges and new nodes as end points will change depending
+  // On presence of smoothed corner. However we also do not want to
+  // modify the original route
+  std::vector<std::unique_ptr<DirectionalEdge>> new_edges;
+  std::vector<std::unique_ptr<Node>>            new_nodes;
+
+  Coordinates start = edges[0]->start->coords;
+  Coordinates end;
+
   // 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);
+
+  for (unsigned int i = 0; i < edges.size() - 1; i++) {
+
+    const EdgePtr edge = edges[i];
+    const EdgePtr next_edge = edges[i+1];
+
+    end = edge->end->coords;
+
+    CornerArc corner_arc(edge, next_edge, 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();
+      interpolateEdge(start.x, start.y, end.x, end.y, path.poses);
+
+      corner_arc.interpolateArc(angle_of_interpolation_, path.poses);
+
+      //if an arc exists, start of the subsequent edge is end of the arc
+      auto new_start_node = std::make_unique<Node>(*next_edge->start);
+      new_start_node->coords = corner_arc.getCornerEnd();
+      auto new_next_edge   = std::make_unique<DirectionalEdge>(*next_edge);
+      new_next_edge->start = new_start_node.get();
+
+      //reassign the next edge to have the modified start coordinate
+      edges[i+1] = new_next_edge.get();
+      start = corner_arc.getCornerEnd();
+
+      //Store pointer to edges to keep them alive in this method
+      new_nodes.emplace_back(std::move(new_start_node));
+      new_edges.emplace_back(std::move(new_next_edge));
+
+    }else{
+      interpolateEdge(start.x, start.y, end.x, end.y, path.poses);
+      start = end;
+    }
   }
 
-  if (route.edges.empty()) {
+  if (edges.empty()) {
     path.poses.push_back(utils::toMsg(route.start_node->coords.x, route.start_node->coords.y));
   } else {
+
+    interpolateEdge(edges.back()->start->coords.x, edges.back()->start->coords.y,
+                    edges.back()->end->coords.x, 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));
+      utils::toMsg(edges.back()->end->coords.x, edges.back()->end->coords.y));
   }
 
   // Set path poses orientations for each point
@@ -121,4 +177,14 @@ void PathConverter::interpolateEdge(
   }
 }
 
+double PathConverter::getEdgeLength(const EdgePtr edge){
+  const Coordinates & start = edge->start->coords;
+  const Coordinates & end = edge->end->coords;
+
+  float dx = end.x - start.x;
+  float dy = end.y - start.y;
+
+  return double(sqrt(dx*dx + dy*dy));
+}
+
 }  // namespace nav2_route