From 0b2af364a6aaffacfca13f713000bfd31f6a9548 Mon Sep 17 00:00:00 2001 From: RyuYamamoto Date: Sun, 28 Apr 2024 13:56:20 +0900 Subject: [PATCH 1/3] rename package --- .../CMakeLists.txt | 5 +---- .../config/navyu_global_planner_params.yaml | 0 .../include/navyu_path_planner}/astar_planner.hpp | 4 ++-- .../navyu_path_planner}/base_global_planner.hpp | 0 .../navyu_path_planner}/navyu_global_planner.hpp | 2 +- .../include/navyu_path_planner}/node.hpp | 0 .../launch/navyu_global_planner.launch.py | 4 ++-- {navyu_planner => navyu_path_planner}/package.xml | 4 ++-- .../src/navyu_global_planner.cpp | 15 ++++++++++++--- 9 files changed, 20 insertions(+), 14 deletions(-) rename {navyu_planner => navyu_path_planner}/CMakeLists.txt (88%) rename {navyu_planner => navyu_path_planner}/config/navyu_global_planner_params.yaml (100%) rename {navyu_planner/include/navyu_planner => navyu_path_planner/include/navyu_path_planner}/astar_planner.hpp (97%) rename {navyu_planner/include/navyu_planner => navyu_path_planner/include/navyu_path_planner}/base_global_planner.hpp (100%) rename {navyu_planner/include/navyu_planner => navyu_path_planner/include/navyu_path_planner}/navyu_global_planner.hpp (97%) rename {navyu_planner/include/navyu_planner => navyu_path_planner/include/navyu_path_planner}/node.hpp (100%) rename {navyu_planner => navyu_path_planner}/launch/navyu_global_planner.launch.py (90%) rename {navyu_planner => navyu_path_planner}/package.xml (88%) rename {navyu_planner => navyu_path_planner}/src/navyu_global_planner.cpp (92%) diff --git a/navyu_planner/CMakeLists.txt b/navyu_path_planner/CMakeLists.txt similarity index 88% rename from navyu_planner/CMakeLists.txt rename to navyu_path_planner/CMakeLists.txt index 0f604cf..b11f688 100644 --- a/navyu_planner/CMakeLists.txt +++ b/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() diff --git a/navyu_planner/config/navyu_global_planner_params.yaml b/navyu_path_planner/config/navyu_global_planner_params.yaml similarity index 100% rename from navyu_planner/config/navyu_global_planner_params.yaml rename to navyu_path_planner/config/navyu_global_planner_params.yaml diff --git a/navyu_planner/include/navyu_planner/astar_planner.hpp b/navyu_path_planner/include/navyu_path_planner/astar_planner.hpp similarity index 97% rename from navyu_planner/include/navyu_planner/astar_planner.hpp rename to navyu_path_planner/include/navyu_path_planner/astar_planner.hpp index 20899ed..8701dd0 100644 --- a/navyu_planner/include/navyu_planner/astar_planner.hpp +++ b/navyu_path_planner/include/navyu_path_planner/astar_planner.hpp @@ -15,8 +15,8 @@ #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 diff --git a/navyu_planner/include/navyu_planner/base_global_planner.hpp b/navyu_path_planner/include/navyu_path_planner/base_global_planner.hpp similarity index 100% rename from navyu_planner/include/navyu_planner/base_global_planner.hpp rename to navyu_path_planner/include/navyu_path_planner/base_global_planner.hpp diff --git a/navyu_planner/include/navyu_planner/navyu_global_planner.hpp b/navyu_path_planner/include/navyu_path_planner/navyu_global_planner.hpp similarity index 97% rename from navyu_planner/include/navyu_planner/navyu_global_planner.hpp rename to navyu_path_planner/include/navyu_path_planner/navyu_global_planner.hpp index 9a77f7d..e58abfb 100644 --- a/navyu_planner/include/navyu_planner/navyu_global_planner.hpp +++ b/navyu_path_planner/include/navyu_path_planner/navyu_global_planner.hpp @@ -15,7 +15,7 @@ #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 diff --git a/navyu_planner/include/navyu_planner/node.hpp b/navyu_path_planner/include/navyu_path_planner/node.hpp similarity index 100% rename from navyu_planner/include/navyu_planner/node.hpp rename to navyu_path_planner/include/navyu_path_planner/node.hpp diff --git a/navyu_planner/launch/navyu_global_planner.launch.py b/navyu_path_planner/launch/navyu_global_planner.launch.py similarity index 90% rename from navyu_planner/launch/navyu_global_planner.launch.py rename to navyu_path_planner/launch/navyu_global_planner.launch.py index a9971fa..20f123a 100644 --- a/navyu_planner/launch/navyu_global_planner.launch.py +++ b/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", diff --git a/navyu_planner/package.xml b/navyu_path_planner/package.xml similarity index 88% rename from navyu_planner/package.xml rename to navyu_path_planner/package.xml index 9e0d2c2..a6e92d2 100644 --- a/navyu_planner/package.xml +++ b/navyu_path_planner/package.xml @@ -1,8 +1,8 @@ - navyu_planner + navyu_path_planner 0.1.0 - The navyu_planner package + The navyu_path_planner package RyuYamamoto Apache License 2.0 diff --git a/navyu_planner/src/navyu_global_planner.cpp b/navyu_path_planner/src/navyu_global_planner.cpp similarity index 92% rename from navyu_planner/src/navyu_global_planner.cpp rename to navyu_path_planner/src/navyu_global_planner.cpp index 641775a..eb5d8bb 100644 --- a/navyu_planner/src/navyu_global_planner.cpp +++ b/navyu_path_planner/src/navyu_global_planner.cpp @@ -12,9 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License -#include "navyu_planner/navyu_global_planner.hpp" - -#include +#include "navyu_path_planner/navyu_global_planner.hpp" NavyuGlobalPlanner::NavyuGlobalPlanner(const rclcpp::NodeOptions & node_options) : Node("navyu_global_planner", node_options) @@ -147,6 +145,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" From c3fbc3c49e0757973ae182bc1236d81d0b8a0f07 Mon Sep 17 00:00:00 2001 From: RyuYamamoto Date: Mon, 29 Apr 2024 12:52:40 +0900 Subject: [PATCH 2/3] fix depent --- navyu/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/navyu/package.xml b/navyu/package.xml index ddc63c0..c6220f2 100644 --- a/navyu/package.xml +++ b/navyu/package.xml @@ -10,6 +10,8 @@ navyu_navigation navyu_simulator + navyu_path_planner + navyu_costmap_2d rclcpp launch_ros From 3cd488b658978c698aaeaf9fa1a64eaa52c47fa0 Mon Sep 17 00:00:00 2001 From: RyuYamamoto Date: Mon, 29 Apr 2024 12:53:43 +0900 Subject: [PATCH 3/3] add path smoother --- .../config/navyu_global_planner_params.yaml | 2 + .../navyu_path_planner/astar_planner.hpp | 3 + .../navyu_global_planner.hpp | 5 + .../include/navyu_path_planner/smoother.hpp | 133 ++++++++++++++++++ .../src/navyu_global_planner.cpp | 17 ++- 5 files changed, 158 insertions(+), 2 deletions(-) create mode 100644 navyu_path_planner/include/navyu_path_planner/smoother.hpp diff --git a/navyu_path_planner/config/navyu_global_planner_params.yaml b/navyu_path_planner/config/navyu_global_planner_params.yaml index 80e1d40..8947468 100644 --- a/navyu_path_planner/config/navyu_global_planner_params.yaml +++ b/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 diff --git a/navyu_path_planner/include/navyu_path_planner/astar_planner.hpp b/navyu_path_planner/include/navyu_path_planner/astar_planner.hpp index 8701dd0..0566169 100644 --- a/navyu_path_planner/include/navyu_path_planner/astar_planner.hpp +++ b/navyu_path_planner/include/navyu_path_planner/astar_planner.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -97,6 +98,8 @@ class AstarPlanner : public BaseGlobalPlanner } path.emplace_back(start); + std::reverse(path.begin(), path.end()); + return path; } }; diff --git a/navyu_path_planner/include/navyu_path_planner/navyu_global_planner.hpp b/navyu_path_planner/include/navyu_path_planner/navyu_global_planner.hpp index e58abfb..2c23bd8 100644 --- a/navyu_path_planner/include/navyu_path_planner/navyu_global_planner.hpp +++ b/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 @@ -26,6 +27,7 @@ #include #include +#include class NavyuGlobalPlanner : public rclcpp::Node { @@ -50,11 +52,14 @@ class NavyuGlobalPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr goal_pose_subscriber_; rclcpp::Subscription::SharedPtr costmap_subscriber_; rclcpp::Publisher::SharedPtr path_publisher_; + rclcpp::Publisher::SharedPtr raw_path_publisher_; tf2_ros::Buffer tf_buffer_{get_clock()}; std::shared_ptr broadcaster_; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; std::shared_ptr planner_; + std::shared_ptr smoother_; std::string map_frame_; std::string base_frame_; diff --git a/navyu_path_planner/include/navyu_path_planner/smoother.hpp b/navyu_path_planner/include/navyu_path_planner/smoother.hpp new file mode 100644 index 0000000..434c25c --- /dev/null +++ b/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 + +#include +#include + +class Smoother +{ +public: + Smoother(double displacement_threshold) { displacement_threshold_ = displacement_threshold; } + + nav_msgs::msg::Path smooth(nav_msgs::msg::Path path) + { + std::vector 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 split_path_with_dist(nav_msgs::msg::Path path) + { + std::vector 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 split_path(nav_msgs::msg::Path path) + { + std::vector 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 diff --git a/navyu_path_planner/src/navyu_global_planner.cpp b/navyu_path_planner/src/navyu_global_planner.cpp index eb5d8bb..234b540 100644 --- a/navyu_path_planner/src/navyu_global_planner.cpp +++ b/navyu_path_planner/src/navyu_global_planner.cpp @@ -14,6 +14,8 @@ #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) { @@ -21,6 +23,9 @@ NavyuGlobalPlanner::NavyuGlobalPlanner(const rclcpp::NodeOptions & node_options) base_frame_ = declare_parameter("base_frame"); declare_parameter("lethal_cost_threshold"); + const double displacement_threshold = declare_parameter("displacement_threshold"); + smoother_ = std::make_shared(displacement_threshold); + broadcaster_ = std::make_shared(this); goal_pose_subscriber_ = create_subscription( @@ -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( + "raw_path", rclcpp::QoS(10).transient_local().reliable().keep_last(1)); path_publisher_ = create_publisher( "path", rclcpp::QoS(10).transient_local().reliable().keep_last(1)); } @@ -99,8 +106,14 @@ void NavyuGlobalPlanner::publish_path(std::vector 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)