add path smoother

navyu_path_planner/config/navyu_global_planner_params.yaml

@@ -4,4 +4,6 @@ navyu_global_planner_node:
 
     base_frame: base_link
 
+    displacement_threshold: 5.0
+
     lethal_cost_threshold: 45.0

navyu_path_planner/include/navyu_path_planner/astar_planner.hpp

@@ -23,6 +23,7 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 
+#include <algorithm>
 #include <queue>
 #include <unordered_map>
 #include <vector>
@@ -97,6 +98,8 @@ class AstarPlanner : public BaseGlobalPlanner
     }
     path.emplace_back(start);
 
+    std::reverse(path.begin(), path.end());
+
     return path;
   }
 };

navyu_path_planner/include/navyu_path_planner/navyu_global_planner.hpp

@@ -16,6 +16,7 @@
 #define NAVYU_PLANNER__NAVYU_GLOBAL_PLANNER_HPP_
 
 #include "navyu_path_planner/astar_planner.hpp"
+#include "navyu_path_planner/smoother.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -26,6 +27,7 @@
 
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
 
 class NavyuGlobalPlanner : public rclcpp::Node
 {
@@ -50,11 +52,14 @@ class NavyuGlobalPlanner : public rclcpp::Node
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_pose_subscriber_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_subscriber_;
   rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_publisher_;
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr raw_path_publisher_;
 
   tf2_ros::Buffer tf_buffer_{get_clock()};
   std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
+  tf2_ros::TransformListener tf_listener_{tf_buffer_};
 
   std::shared_ptr<AstarPlanner> planner_;
+  std::shared_ptr<Smoother> smoother_;
 
   std::string map_frame_;
   std::string base_frame_;

navyu_path_planner/include/navyu_path_planner/smoother.hpp

@@ -0,0 +1,133 @@
+// Copyright 2024 RyuYamamoto.
+//
+// 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 NAVYU_PATH_PLANNER__SMOOTHER_HPP_
+#define NAVYU_PATH_PLANNER__SMOOTHER_HPP_
+
+#include <nav_msgs/msg/path.hpp>
+
+#include <cmath>
+#include <vector>
+
+class Smoother
+{
+public:
+  Smoother(double displacement_threshold) { displacement_threshold_ = displacement_threshold; }
+
+  nav_msgs::msg::Path smooth(nav_msgs::msg::Path path)
+  {
+    std::vector<nav_msgs::msg::Path> segments_path = split_path_with_dist(path);
+    nav_msgs::msg::Path optimized_path;
+
+    auto factorial = [](int n) -> double {
+      double result = 1.0;
+      for (int i = 1; i <= n; i++) result *= i;
+      return result;
+    };
+
+    auto binominal = [&](int n, int i) -> double {
+      return factorial(n) / (factorial(i) * factorial(n - i));
+    };
+
+    auto bernstein = [&](int n, int i, double t) -> double {
+      return binominal(n, i) * std::pow(t, i) * std::pow(1 - t, n - i);
+    };
+
+    for (auto segment : segments_path) {
+      int path_size = segment.poses.size();
+      double t_step = 1.0 / (double)path_size;
+
+      for (double t = 0.0; t <= 1.0; t += t_step) {
+        geometry_msgs::msg::PoseStamped pose;
+
+        for (int idx = 0; idx < path_size; idx++) {
+          pose.pose.position.x +=
+            segment.poses[idx].pose.position.x * bernstein(path_size - 1, idx, t);
+          pose.pose.position.y +=
+            segment.poses[idx].pose.position.y * bernstein(path_size - 1, idx, t);
+        }
+        optimized_path.poses.emplace_back(pose);
+      }
+    }
+
+    return optimized_path;
+  }
+
+  std::vector<nav_msgs::msg::Path> split_path_with_dist(nav_msgs::msg::Path path)
+  {
+    std::vector<nav_msgs::msg::Path> segments_path;
+
+    auto calculate_distance =
+      [](geometry_msgs::msg::Point p1, geometry_msgs::msg::Point p2) -> double {
+      return std::hypot((p2.x - p1.x), (p2.y - p1.y));
+    };
+
+    double displacement = 0.0;
+    nav_msgs::msg::Path current_segments_path;
+    for (auto pose : path.poses) {
+      current_segments_path.poses.emplace_back(pose);
+
+      if (2 <= current_segments_path.poses.size()) {
+        displacement += calculate_distance(
+          current_segments_path.poses[current_segments_path.poses.size() - 2].pose.position,
+          pose.pose.position);
+      }
+
+      if (5.0 < displacement) {
+        segments_path.emplace_back(current_segments_path);
+        current_segments_path.poses.clear();
+        displacement = 0.0;
+      }
+    }
+
+    current_segments_path.poses.emplace_back(path.poses[path.poses.size() - 1]);
+    segments_path.emplace_back(current_segments_path);
+
+    return segments_path;
+  }
+
+  std::vector<nav_msgs::msg::Path> split_path(nav_msgs::msg::Path path)
+  {
+    std::vector<nav_msgs::msg::Path> segments_path;
+    nav_msgs::msg::Path current_segments_path;
+
+    // detect cusp and split path
+    for (int i = 0; i < path.poses.size() - 1; i++) {
+      // cross product
+      const double ux = path.poses[i].pose.position.x - path.poses[i - 1].pose.position.x;
+      const double uy = path.poses[i].pose.position.y - path.poses[i - 1].pose.position.y;
+      const double vx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x;
+      const double vy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y;
+      const double cp = ux * vy - uy * vx;
+      // const double cp = ux * vx + uy * vy;
+
+      if (0.1 < std::abs(cp)) {
+        segments_path.emplace_back(current_segments_path);
+        current_segments_path.poses.clear();
+      }
+      current_segments_path.poses.emplace_back(path.poses[i]);
+    }
+
+    // insert goal node
+    current_segments_path.poses.emplace_back(path.poses[path.poses.size() - 1]);
+    segments_path.emplace_back(current_segments_path);
+
+    return segments_path;
+  }
+
+private:
+  double displacement_threshold_;
+};
+
+#endif

navyu_path_planner/src/navyu_global_planner.cpp

@@ -14,13 +14,18 @@
 
 #include "navyu_path_planner/navyu_global_planner.hpp"
 
+#include "navyu_path_planner/smoother.hpp"
+
 NavyuGlobalPlanner::NavyuGlobalPlanner(const rclcpp::NodeOptions & node_options)
 : Node("navyu_global_planner", node_options)
 {
   map_frame_ = declare_parameter<std::string>("map_frame");
   base_frame_ = declare_parameter<std::string>("base_frame");
   declare_parameter<double>("lethal_cost_threshold");
 
+  const double displacement_threshold = declare_parameter<double>("displacement_threshold");
+  smoother_ = std::make_shared<Smoother>(displacement_threshold);
+
   broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
 
   goal_pose_subscriber_ = create_subscription<geometry_msgs::msg::PoseStamped>(
@@ -30,6 +35,8 @@ NavyuGlobalPlanner::NavyuGlobalPlanner(const rclcpp::NodeOptions & node_options)
     "costmap", rclcpp::QoS(10).transient_local().reliable().keep_last(1),
     std::bind(&NavyuGlobalPlanner::callback_costmap, this, std::placeholders::_1));
 
+  raw_path_publisher_ = create_publisher<nav_msgs::msg::Path>(
+    "raw_path", rclcpp::QoS(10).transient_local().reliable().keep_last(1));
   path_publisher_ = create_publisher<nav_msgs::msg::Path>(
     "path", rclcpp::QoS(10).transient_local().reliable().keep_last(1));
 }
@@ -99,8 +106,14 @@ void NavyuGlobalPlanner::publish_path(std::vector<Node2D *> path)
       node->x_, node->y_, path_pose.pose.position.x, path_pose.pose.position.y);
     path_msgs.poses.emplace_back(path_pose);
   }
-
-  path_publisher_->publish(path_msgs);
+  // publish raw path
+  raw_path_publisher_->publish(path_msgs);
+
+  // publish smoothing path
+  nav_msgs::msg::Path optimized_path = smoother_->smooth(path_msgs);
+  optimized_path.header.frame_id = map_frame_;
+  optimized_path.header.stamp = now();
+  path_publisher_->publish(optimized_path);
 }
 
 void NavyuGlobalPlanner::callback_goal_pose(const geometry_msgs::msg::PoseStamped & msg)