Route server corner smoothing

nav2_route/include/nav2_route/corner_smoothing.hpp

@@ -0,0 +1,134 @@
+// 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 "nav2_route/types.hpp"
+#include "nav2_route/utils.hpp"
+
+namespace nav2_route
+{
+class CornerArc
+{
+public:
+  /**
+   * @brief Constructor
+   */
+  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);
+
+    double angle = getAngleBetweenEdges(start, corner, end);
+
+    double tangent_length = minimum_radius / (std::tan(std::fabs(angle) / 2));
+
+    if (tangent_length < start_edge_length_ && tangent_length < end_edge_length_) {
+      std::vector<double> start_edge_unit_tangent =
+      {(start.x - corner.x) / start_edge_length_, (start.y - corner.y) / start_edge_length_};
+      std::vector<double> end_edge_unit_tangent =
+      {(end.x - corner.x) / end_edge_length_, (end.y - corner.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);
+
+      std::vector<double> 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;
+
+      double 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;
+    }
+  }
+
+  ~CornerArc() = default;
+
+  void interpolateArc(
+    const float & 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];
+    double 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));
+    }
+  }
+
+
+  bool isCornerValid() const {return valid_corner_;}
+
+  Coordinates getCornerStart() const {return start_coordinate_;}
+
+  Coordinates getCornerEnd() const {return end_coordinate_;}
+
+protected:
+  double getAngleBetweenEdges(
+    const Coordinates & start, const Coordinates & corner,
+    const Coordinates & end)
+  {
+    double start_dx = start.x - corner.x;
+    double start_dy = start.y - corner.y;
+
+    double end_dx = end.x - corner.x;
+    double end_dy = end.y - corner.y;
+
+    double 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,44 @@ nav_msgs::msg::Path PathConverter::densify(
     interpolateEdge(start.x, start.y, end.x, end.y, path.poses);
   }
 
+  Coordinates start = route.edges[0]->start->coords;
+  Coordinates end;
+
   // Fill in path via route edges
-  for (unsigned int i = 0; i != route.edges.size(); i++) {
+
+  for (unsigned int i = 0; i < route.edges.size() - 1; 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);
+    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 {
+      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(
+      route.edges.back()->start->coords.x, route.edges.back()->start->coords.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));
   }