Feat/path smoothing

navyu/package.xml

@@ -10,6 +10,8 @@
 
   <depend>navyu_navigation</depend>
   <depend>navyu_simulator</depend>
+  <depend>navyu_path_planner</depend>
+  <depend>navyu_costmap_2d</depend>
   <depend>rclcpp</depend>
 
   <exec_depend>launch_ros</exec_depend>

navyu_planner/CMakeLists.txt → navyu_path_planner/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-project(navyu_planner)
+project(navyu_path_planner)
 
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
@@ -10,7 +10,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic )
 endif()
 
-find_package(backward_ros REQUIRED)
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
@@ -23,8 +22,6 @@ rclcpp_components_register_node(${PROJECT_NAME}
   EXECUTABLE navyu_global_planner_node
 )
 
-add_backward(navyu_global_planner_node)
-
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()

navyu_planner/config/navyu_global_planner_params.yaml → 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_planner/include/navyu_planner/astar_planner.hpp → navyu_path_planner/include/navyu_path_planner/astar_planner.hpp

@@ -15,14 +15,15 @@
 #ifndef NAVYU_PLANNER__ASTAR_PLANNER_HPP_
 #define NAVYU_PLANNER__ASTAR_PLANNER_HPP_
 
-#include "navyu_planner/base_global_planner.hpp"
-#include "navyu_planner/node.hpp"
+#include "navyu_path_planner/base_global_planner.hpp"
+#include "navyu_path_planner/node.hpp"
 
 #include <Eigen/Core>
 
 #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_planner/include/navyu_planner/navyu_global_planner.hpp → navyu_path_planner/include/navyu_path_planner/navyu_global_planner.hpp

@@ -15,7 +15,8 @@
 #ifndef NAVYU_PLANNER__NAVYU_GLOBAL_PLANNER_HPP_
 #define NAVYU_PLANNER__NAVYU_GLOBAL_PLANNER_HPP_
 
-#include "navyu_planner/astar_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_planner/launch/navyu_global_planner.launch.py → navyu_path_planner/launch/navyu_global_planner.launch.py

@@ -25,14 +25,14 @@ def generate_launch_description():
     use_sim_time = LaunchConfiguration("use_sim_time", default="true")
 
     navyu_global_planner_config = PathJoinSubstitution(
-        [FindPackageShare("navyu_planner"), "config", "navyu_global_planner_params.yaml"]
+        [FindPackageShare("navyu_path_planner"), "config", "navyu_global_planner_params.yaml"]
     )
 
     return LaunchDescription(
         [
             DeclareLaunchArgument("use_sim_time", default_value="true"),
             Node(
-                package="navyu_planner",
+                package="navyu_path_planner",
                 executable="navyu_global_planner_node",
                 name="navyu_global_planner_node",
                 output="screen",

navyu_planner/package.xml → navyu_path_planner/package.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package format="3">
-  <name>navyu_planner</name>
+  <name>navyu_path_planner</name>
   <version>0.1.0</version>
-  <description>The navyu_planner package</description>
+  <description>The navyu_path_planner package</description>
   <maintainer email="[email protected]">RyuYamamoto</maintainer>
   <license>Apache License 2.0</license>
 

navyu_planner/src/navyu_global_planner.cpp → navyu_path_planner/src/navyu_global_planner.cpp

@@ -12,9 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License
 
-#include "navyu_planner/navyu_global_planner.hpp"
+#include "navyu_path_planner/navyu_global_planner.hpp"
 
-#include <geometry_msgs/msg/detail/transform_stamped__struct.hpp>
+#include "navyu_path_planner/smoother.hpp"
 
 NavyuGlobalPlanner::NavyuGlobalPlanner(const rclcpp::NodeOptions & node_options)
 : Node("navyu_global_planner", node_options)
@@ -23,6 +23,9 @@ NavyuGlobalPlanner::NavyuGlobalPlanner(const rclcpp::NodeOptions & node_options)
   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>(
@@ -32,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));
 }
@@ -101,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)
@@ -147,6 +158,17 @@ void NavyuGlobalPlanner::callback_costmap(const nav_msgs::msg::OccupancyGrid & m
 
     return;
   }
+
+  if (planner_ == nullptr) {
+    RCLCPP_ERROR_STREAM(get_logger(), "Planner is not set.");
+    return;
+  }
+
+  // set costmap info
+  planner_->set_origin(msg.info.origin.position.x, msg.info.origin.position.y);
+  planner_->set_size(msg.info.width, msg.info.height);
+  planner_->set_resolution(msg.info.resolution);
+  planner_->set_costmap(msg.data);
 }
 
 #include "rclcpp_components/register_node_macro.hpp"