diff --git a/.github/thirdparty.repos b/.github/thirdparty.repos index e2b12e2..9be16d6 100644 --- a/.github/thirdparty.repos +++ b/.github/thirdparty.repos @@ -11,4 +11,7 @@ repositories: type: git url: https://github.com/kiko2r/navigation2.git version: nav2_msgs - \ No newline at end of file + ThirdParty/yaets: + type: git + url: https://github.com/fmrico/yaets.git + version: rolling \ No newline at end of file diff --git a/.github/workflows/rolling.yaml b/.github/workflows/rolling.yaml index b00d916..53b3a7d 100644 --- a/.github/workflows/rolling.yaml +++ b/.github/workflows/rolling.yaml @@ -28,19 +28,17 @@ jobs: - name: build and test uses: ros-tooling/action-ros-ci@0.4.3 with: - package-name: easynav_gridmap_maps_manager + package-name: easynav_gridmap_maps_manager easynav_gridmap_astar_planner easynav_gridmap_rrtstar_planner target-ros2-distro: rolling vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos - skip-test: true colcon-defaults: | { - "build": { - "packages-up-to": true, - "mixin": ["coverage-gcc"] + "test": { + "parallel-workers" : 1 } } + colcon-mixin-name: coverage-gcc colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - name: Codecov uses: codecov/codecov-action@v5.4.0 with: @@ -48,4 +46,4 @@ jobs: flags: unittests name: codecov-umbrella # yml: ./codecov.yml - fail_ci_if_error: false + fail_ci_if_error: false \ No newline at end of file diff --git a/easynav_gridmap_rrtstar_planner/CMakeLists.txt b/easynav_gridmap_rrtstar_planner/CMakeLists.txt new file mode 100644 index 0000000..6ff8b99 --- /dev/null +++ b/easynav_gridmap_rrtstar_planner/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_gridmap_rrtstar_planner) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CMAKE_CXX_STANDARD 23) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +find_package(ament_cmake REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(pluginlib REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(grid_map_ros REQUIRED) +find_package(grid_map_msgs REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/easynav_gridmap_rrtstar_planner/GridMapRRTStarPlanner.cpp + src/easynav_gridmap_rrtstar_planner/KDTree.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + pluginlib::pluginlib + grid_map_ros::grid_map_ros + ${nav_msgs_TARGETS} + ${grid_map_msgs_TARGETS} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + # add_subdirectory(tests) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) + +# Register the planning plugins +pluginlib_export_plugin_description_file(easynav_core easynav_gridmap_rrtstar_planner_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + easynav_simple_common + pluginlib + nav_msgs + grid_map_ros + grid_map_msgs +) +ament_package() diff --git a/easynav_gridmap_rrtstar_planner/easynav_gridmap_rrtstar_planner_plugins.xml b/easynav_gridmap_rrtstar_planner/easynav_gridmap_rrtstar_planner_plugins.xml new file mode 100644 index 0000000..f90e805 --- /dev/null +++ b/easynav_gridmap_rrtstar_planner/easynav_gridmap_rrtstar_planner_plugins.xml @@ -0,0 +1,9 @@ + + + + + A implementation for the RRT* path planner. + + + + diff --git a/easynav_gridmap_rrtstar_planner/include/easynav_gridmap_rrtstar_planner/GridMapRRTStarPlanner.hpp b/easynav_gridmap_rrtstar_planner/include/easynav_gridmap_rrtstar_planner/GridMapRRTStarPlanner.hpp new file mode 100644 index 0000000..09d994b --- /dev/null +++ b/easynav_gridmap_rrtstar_planner/include/easynav_gridmap_rrtstar_planner/GridMapRRTStarPlanner.hpp @@ -0,0 +1,350 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// licensed under the GNU General Public License v3.0. +// See for details. +// +// Easy Navigation program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +/// \file +/// \brief Declaration of the GridMapRRTStarPlanner class implementing RRT* path planning using elevation maps. + +#ifndef EASYNAV_GRIDMAP_RRTSTAR_PLANNER__GRIDMAPRRTSTARPLANNER_HPP_ +#define EASYNAV_GRIDMAP_RRTSTAR_PLANNER__GRIDMAPRRTSTARPLANNER_HPP_ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "grid_map_core/GridMap.hpp" +#include "nav_msgs/msg/goals.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" +#include "rclcpp/rclcpp.hpp" +#include "easynav_core/PlannerMethodBase.hpp" +#include "easynav_gridmap_rrtstar_planner/KDTree.hpp" +#include "easynav_gridmap_rrtstar_planner/RRTNode.hpp" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "visualization_msgs/msg/marker.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +namespace easynav +{ + +class KDTree; + +/** + * @class GridMapRRTStarPlanner + * @brief RRT* path planner implementation using elevation grid maps. + * + * This planner generates collision-free and slope-constrained paths. + * Features include: + * - RRT* optimization + * - KD-tree accelerated nearest neighbor search + * - Path smoothing (Catmull-Rom and linear interpolation) + * - Visualization markers for debugging and RViz display + */ +class GridMapRRTStarPlanner : public PlannerMethodBase +{ +public: +/** + * @brief Constructor. Initializes KD-tree and sets default parameters. + */ + GridMapRRTStarPlanner(); + +/** + * @brief Initializes the planner plugin. + * + * Declares parameters and sets up internal state. + * @return std::expected Empty if successful, error string otherwise. + */ + std::expected on_initialize() override; + +/** + * @brief Main planner update function. + * + * Computes a path from the robot to the goal, updates internal state, + * and publishes visualization if enabled. + * + * @param nav_state Navigation state containing robot pose, map, and goals. + */ + void update(NavState & nav_state) override; + +protected: +// ------------------------------------------------------------------ +// Core RRT* methods +// ------------------------------------------------------------------ + +/** + * @brief Runs the RRT* algorithm to plan a path from start to goal. + * @param map Grid map used for planning. + * @param start Start pose in map frame. + * @param goal Goal pose in map frame. + * @return Vector of smoothed poses representing the planned path. + * Empty if start/goal invalid or no path found. + */ + std::vector rrt_star( + const grid_map::GridMap & map, + const geometry_msgs::msg::Pose & start, + const geometry_msgs::msg::Pose & goal); + +/** + * @brief Generate a random grid index inside the map bounds. + * @param map Grid map to sample from. + * @param rng Random number generator. + * @return Random index within map bounds. + */ + grid_map::Index random_index(const grid_map::GridMap & map, std::mt19937 & rng); + +/** + * @brief Generate a biased random index pointing forward from robot pose. + * @param map Grid map. + * @param robot_pose Current robot pose. + * @param robot_yaw Robot orientation in radians. + * @param rng Random generator. + * @return Random index biased in forward direction. + */ + grid_map::Index biased_random_index( + const grid_map::GridMap & map, + const geometry_msgs::msg::Pose & robot_pose, + double robot_yaw, + std::mt19937 & rng); + +/** + * @brief Steer from one index toward another within max step size. + * @param map Grid map. + * @param from Starting index. + * @param to Target index. + * @return New index reached after stepping, or {-1,-1} if outside map. + */ + grid_map::Index steer( + const grid_map::GridMap & map, + const grid_map::Index & from, + const grid_map::Index & to); + +/** + * @brief Compute traversal cost between two indices. + * + * Considers elevation changes and slope constraints. + * + * @param map Grid map. + * @param from Source index. + * @param to Destination index. + * @return Traversal cost, or infinity if slope exceeds max. + */ + double traversal_cost( + const grid_map::GridMap & map, + const grid_map::Index & from, + const grid_map::Index & to); + +/** + * @brief Compute Euclidean distance between two map indices. + * @param map Grid map. + * @param a First index. + * @param b Second index. + * @return Distance in meters. + */ + double distance( + const grid_map::GridMap & map, + const grid_map::Index & a, + const grid_map::Index & b); + +/** + * @brief Generate a sample index for RRT* expansion. + * + * Sampling may be goal-biased, path-biased, or uniform random. + * + * @param map Grid map. + * @param goal_idx Goal index. + * @param best_goal_node Current best goal node. + * @param rng Random generator. + * @param prob_dist Probability distribution. + * @return Sampled grid index. + */ + grid_map::Index generate_sample( + const grid_map::GridMap & map, + const grid_map::Index & goal_idx, + std::shared_ptr best_goal_node, + std::mt19937 & rng, + std::uniform_real_distribution & prob_dist); + +/** + * @brief Find the nearest node in the tree to a target index. + * @param tree RRT* tree of nodes. + * @param target Target index. + * @param map Grid map. + * @return Shared pointer to nearest node, or nullptr if empty. + */ + std::shared_ptr find_nearest( + const std::vector> & tree, + const grid_map::Index & target, + const grid_map::GridMap & map); + +/** + * @brief Backtrack from goal node to extract the full path. + * @param goal Goal node in the tree. + * @param map Grid map. + * @param goal_pose Final goal pose (orientation). + * @return Vector of poses representing path. + */ + std::vector extract_path( + std::shared_ptr goal, + const grid_map::GridMap & map, + const geometry_msgs::msg::Pose & goal_pose); + +// ------------------------------------------------------------------ +// Path management and smoothing +// ------------------------------------------------------------------ + +/** + * @brief Merge new path poses with stored path and publish results. + * @param new_poses Vector of new poses. + * @param frame_id Map frame ID. + */ + void combine_paths( + const std::vector & new_poses, + const std::string & frame_id); + +/** + * @brief Generate a new path given robot and goal poses. + * @param map Grid map. + * @param robot_pose Current robot pose. + * @param goal_pose Target goal pose. + * @return Vector of poses forming a path. + */ + std::vector generate_new_path( + const grid_map::GridMap & map, + const geometry_msgs::msg::Pose & robot_pose, + const geometry_msgs::msg::Pose & goal_pose); + +/** + * @brief Publish path visualization markers for RViz. + * @param path Path message. + */ + void publish_path_markers(const nav_msgs::msg::Path & path); + +/** + * @brief Smooth path using Catmull-Rom splines. + * @param input_path Original path. + * @param interpolation_points_per_segment Number of interpolated points. + * @return Smoothed path. + */ + std::vector smooth_path( + const std::vector & input_path, + int interpolation_points_per_segment); + +/** + * @brief Linearly interpolate between path points. + * @param input_path Original path. + * @param interpolation_points_per_segment Points per segment. + * @return Interpolated path. + */ + std::vector linear_interpolate_path( + const std::vector & input_path, + int interpolation_points_per_segment); + +/** + * @brief Trim path to start from current robot pose. + * @param path Original path. + * @param robot_pose Current robot pose. + * @return Trimmed path message. + */ + nav_msgs::msg::Path trim_path_from_robot( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Pose & robot_pose) const; + +/** + * @brief Check if the goal has changed significantly since last update. + * @param goal_pose New goal pose. + * @return True if changed, false otherwise. + */ + bool check_goal_changed(const geometry_msgs::msg::Pose & goal_pose); + +/** + * @brief Compute lateral deviation of robot pose from a path. + * @param path Path. + * @param robot_pose Current pose. + * @return Lateral deviation distance. + */ + double calculate_lateral_distance_to_path( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Pose & robot_pose) const; + +// ------------------------------------------------------------------ +// Parameters +// ------------------------------------------------------------------ + double max_allowed_slope_deg_ = 50; ///< Maximum slope in degrees + double max_allowed_slope_ = 50.0 * M_PI / 180.0; ///< Maximum slope in radians + int max_iters_ = 2000; ///< Maximum RRT* iterations + double step_size_ = 0.8; ///< Step size in meters + double neighbor_radius_ = 1.5; ///< Neighbor radius in meters + double goal_bias_ = 0.1; ///< Probability of sampling goal + double goal_threshold_ = 1.0; ///< Goal connection threshold (m) + int kdtree_rebuild_interval_ = 200; ///< KD-tree rebuild interval + double spacing_ = 0.2; ///< Min spacing for smoothed path points + double max_lateral_deviation_ = 1.5; ///< Allowed lateral deviation (m) + int final_poses_with_goal_orientation_ = 2; ///< Preserve orientation for last poses + +// ------------------------------------------------------------------ +// Runtime state +// ------------------------------------------------------------------ + rclcpp::Publisher::SharedPtr path_pub_; ///< Publisher for path + rclcpp::Publisher::SharedPtr marker_pub_; ///< Publisher for markers + nav_msgs::msg::Path current_path_; ///< Current stored path + std::unique_ptr kd_tree_; ///< KD-tree for NN search + std::unordered_map cost_cache_; ///< Traversal cost cache + uint32_t cached_map_width_ = 0; ///< Cached map width + uint32_t cached_map_height_ = 0; ///< Cached map height + uint64_t last_map_timestamp_ = 0; ///< Cached map timestamp + double last_path_cost_ = 0.0; ///< Cost of previous path + double current_path_cost_ = std::numeric_limits::max(); ///< Cost of current path + double new_path_cost_ = std::numeric_limits::max(); ///< Cost of new candidate path + int kdtree_rebuild_counter_ = 0; ///< Counter for KD-tree rebuilds + geometry_msgs::msg::Pose last_goal_pose_; ///< Last processed goal pose + +// ------------------------------------------------------------------ +// Cache utilities +// ------------------------------------------------------------------ + +/** + * @brief Clear traversal cost cache. + */ + void clear_cost_cache(); + +/** + * @brief Compute flat index from 2D index. + * @param idx Grid index. + * @param width Map width. + * @return Flat index. + */ + static uint32_t flat_index(const grid_map::Index & idx, uint32_t width); + +/** + * @brief Generate unique key for an edge between two nodes. + * @param a_flat Flat index of first node. + * @param b_flat Flat index of second node. + * @return Unique edge key. + */ + static uint64_t edge_key(uint32_t a_flat, uint32_t b_flat); +}; + +} // namespace easynav + +#endif // EASYNAV_GRIDMAP_RRTSTAR_PLANNER__GRIDMAPRRTSTARPLANNER_HPP_ diff --git a/easynav_gridmap_rrtstar_planner/include/easynav_gridmap_rrtstar_planner/KDTree.hpp b/easynav_gridmap_rrtstar_planner/include/easynav_gridmap_rrtstar_planner/KDTree.hpp new file mode 100644 index 0000000..70d1e16 --- /dev/null +++ b/easynav_gridmap_rrtstar_planner/include/easynav_gridmap_rrtstar_planner/KDTree.hpp @@ -0,0 +1,106 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// licensed under the GNU General Public License v3.0. +// See for details. +// +// Easy Navigation program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +/// \file +/// \brief Declaration of the KDTree class for efficient nearest neighbor search of RRT nodes. +#ifndef EASYNAV_GRIDMAP_RRTSTAR_PLANNER__KDTREE_HPP_ +#define EASYNAV_GRIDMAP_RRTSTAR_PLANNER__KDTREE_HPP_ + +#include +#include +#include +#include "grid_map_core/GridMap.hpp" +#include "easynav_gridmap_rrtstar_planner/GridMapRRTStarPlanner.hpp" +#include "easynav_gridmap_rrtstar_planner/RRTNode.hpp" + +namespace easynav +{ + +struct RRTNode; + +/** + * @brief KD-tree for efficient nearest neighbor search of RRT nodes in a grid map. + * + * Supports incremental rebuilding and radius-based neighbor queries. + */ +class KDTree { +private: +/** + * @brief Internal node of the KD-tree. + */ + struct KDNode + { + std::shared_ptr rrt_node; ///< Associated RRT node + std::unique_ptr left; ///< Left subtree + std::unique_ptr right; ///< Right subtree + int depth; ///< Depth of this node in the tree + + /** + * @brief KDNode constructor + * @param node Pointer to associated RRTNode + * @param d Depth in the KD-tree + */ + KDNode(std::shared_ptr node, int d) + : rrt_node(node), depth(d) + { + } + }; + + std::unique_ptr root; ///< Root of the KD-tree + +/** + * @brief Recursively builds a balanced KD-tree from a list of nodes. + * @param nodes List of RRT nodes to include in the tree + * @param depth Current depth (used to determine split axis) + * @return Unique pointer to the root KDNode of the subtree + */ + std::unique_ptr build(std::vector> & nodes, int depth); + +/** + * @brief Recursively searches the KD-tree for nodes within a given squared radius. + * @param node Current subtree node + * @param target Index to search neighbors around + * @param radius_sq Squared radius for neighbor search + * @param result Vector to store found neighbor nodes + */ + void search_radius( + const std::unique_ptr & node, const grid_map::Index & target, + double radius_sq, std::vector> & result); + +public: +/** + * @brief Rebuilds the KD-tree from a given set of RRT nodes. + * @param nodes Vector of RRT nodes to include + */ + void rebuild(std::vector> & nodes); + +/** + * @brief Finds all RRT nodes within a given radius of a target index. + * @param index Target index for neighbor search + * @param radius Search radius in map units + * @return Vector of shared pointers to RRT nodes within the radius + */ + std::vector> find_neighbors( + const grid_map::Index & index, + double radius); +}; + +} // namespace easynav + +#endif // EASYNAV_GRIDMAP_RRTSTAR_PLANNER__KDTREE_HPP_ diff --git a/easynav_gridmap_rrtstar_planner/include/easynav_gridmap_rrtstar_planner/RRTNode.hpp b/easynav_gridmap_rrtstar_planner/include/easynav_gridmap_rrtstar_planner/RRTNode.hpp new file mode 100644 index 0000000..0f3f48d --- /dev/null +++ b/easynav_gridmap_rrtstar_planner/include/easynav_gridmap_rrtstar_planner/RRTNode.hpp @@ -0,0 +1,74 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// licensed under the GNU General Public License v3.0. +// See for details. +// +// Easy Navigation program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +/// \file +/// \brief Declaration of the RRTNode structure which represents a single node in the RRT* tree. + +#ifndef EASYNAV_GRIDMAP_RRTSTAR_PLANNER__RRTNODE_HPP_ +#define EASYNAV_GRIDMAP_RRTSTAR_PLANNER__RRTNODE_HPP_ + +#include +#include +#include "grid_map_core/GridMap.hpp" + +namespace easynav +{ + +/** + * @brief Represents a node in the RRT* tree. + * + * Stores the grid map index, parent and children pointers, and cumulative cost + * from the root. Used by the KDTree and RRT* planner for path generation. + */ +struct RRTNode +{ + grid_map::Index index; ///< Grid cell index (x, y) + std::shared_ptr parent = nullptr; ///< Pointer to parent node in the tree + double cost = 0.0; ///< Accumulated cost from root + + /// @brief Default constructor + RRTNode() = default; + + /** + * @brief Construct node with specific index and optional cost + * @param idx Grid cell index + * @param c Initial cost (default 0.0) + */ + explicit RRTNode(const grid_map::Index & idx, double c = 0.0) + : index(idx), cost(c) + { + } + + /** + * @brief Change parent of this node and update cumulative cost + * @param new_parent New parent node + * @param new_cost Updated cumulative cost + * + * Note: children list is not automatically updated for performance reasons. + */ + void change_parent(std::shared_ptr new_parent, double new_cost) + { + parent = new_parent; + cost = new_cost; + } +}; + +} // namespace easynav + +#endif // EASYNAV_GRIDMAP_RRTSTAR_PLANNER__RRTNODE_HPP_ diff --git a/easynav_gridmap_rrtstar_planner/package.xml b/easynav_gridmap_rrtstar_planner/package.xml new file mode 100644 index 0000000..3b6dd49 --- /dev/null +++ b/easynav_gridmap_rrtstar_planner/package.xml @@ -0,0 +1,26 @@ + + + + easynav_gridmap_rrtstar_planner + 0.0.1 + Easy Navigation: RRT star planner package. + estherag + GPL-3.0-only + + ament_cmake + + easynav_common + easynav_core + pluginlib + nav_msgs + grid_map_ros + grid_map_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + \ No newline at end of file diff --git a/easynav_gridmap_rrtstar_planner/src/easynav_gridmap_rrtstar_planner/GridMapRRTStarPlanner.cpp b/easynav_gridmap_rrtstar_planner/src/easynav_gridmap_rrtstar_planner/GridMapRRTStarPlanner.cpp new file mode 100644 index 0000000..2506ee1 --- /dev/null +++ b/easynav_gridmap_rrtstar_planner/src/easynav_gridmap_rrtstar_planner/GridMapRRTStarPlanner.cpp @@ -0,0 +1,1070 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// licensed under the GNU General Public License v3.0. +// See for details. +// +// Easy Navigation program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include +#include +#include +#include +#include +#include +#include + +#include "grid_map_core/GridMap.hpp" +#include "nav_msgs/msg/goals.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" +#include "rclcpp/rclcpp.hpp" +#include "easynav_gridmap_rrtstar_planner/GridMapRRTStarPlanner.hpp" +#include "easynav_gridmap_rrtstar_planner/KDTree.hpp" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "visualization_msgs/msg/marker.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +namespace easynav +{ +grid_map::Index GridMapRRTStarPlanner::random_index( + const grid_map::GridMap & map, + std::mt19937 & rng) +{ + std::uniform_int_distribution dist_x(0, map.getSize()[0] - 1); + std::uniform_int_distribution dist_y(0, map.getSize()[1] - 1); + return {dist_x(rng), dist_y(rng)}; +} + +double compute_path_length(const nav_msgs::msg::Path & path) +{ + if (path.poses.size() < 2) { + return 0.0; + } + + double total_length = 0.0; + for (size_t i = 1; i < path.poses.size(); ++i) { + const auto & p1 = path.poses[i - 1].pose.position; + const auto & p2 = path.poses[i].pose.position; + total_length += std::hypot(p2.x - p1.x, p2.y - p1.y); + } + return total_length; +} + +GridMapRRTStarPlanner::GridMapRRTStarPlanner() +: kd_tree_(std::make_unique()) +{ + NavState::register_printer( + [](const nav_msgs::msg::Path & path) + { + std::ostringstream ret; + ret << "Path with " << path.poses.size() << " poses, length " + << compute_path_length(path) << " m."; + return ret.str(); + }); +} + +std::expected GridMapRRTStarPlanner::on_initialize() +{ + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + + node->declare_parameter(plugin_name + ".max_allowed_slope_deg", 40.0); + node->declare_parameter(plugin_name + ".max_iters", 3000); + node->declare_parameter(plugin_name + ".step_size", 1.0); + node->declare_parameter(plugin_name + ".neighbor_radius", 2.0); + node->declare_parameter(plugin_name + ".goal_bias", 0.15); + node->declare_parameter(plugin_name + ".goal_threshold", 0.5); + node->declare_parameter(plugin_name + ".kdtree_rebuild_interval", 800); + node->declare_parameter(plugin_name + ".spacing", 0.2); + node->declare_parameter(plugin_name + ".max_lateral_deviation", 0.5); + node->declare_parameter(plugin_name + ".final_poses_with_goal_orientation", 2); + + node->get_parameter(plugin_name + ".max_allowed_slope_deg", max_allowed_slope_deg_); + node->get_parameter(plugin_name + ".max_iters", max_iters_); + node->get_parameter(plugin_name + ".step_size", step_size_); + node->get_parameter(plugin_name + ".neighbor_radius", neighbor_radius_); + node->get_parameter(plugin_name + ".goal_bias", goal_bias_); + node->get_parameter(plugin_name + ".goal_threshold", goal_threshold_); + node->get_parameter(plugin_name + ".kdtree_rebuild_interval", kdtree_rebuild_interval_); + node->get_parameter(plugin_name + ".spacing", spacing_); + node->get_parameter(plugin_name + ".max_lateral_deviation", max_lateral_deviation_); + node->get_parameter(plugin_name + ".final_poses_with_goal_orientation", + final_poses_with_goal_orientation_); + + max_allowed_slope_ = max_allowed_slope_deg_ * M_PI / 180.0; + + path_pub_ = node->create_publisher("planner/path", 10); + marker_pub_ = node->create_publisher("planner/marker", 10); + + node->get_logger().set_level(rclcpp::Logger::Level::Debug); + + last_goal_pose_.position.x = 0.0; + last_goal_pose_.position.y = 0.0; + last_goal_pose_.position.z = 0.0; + last_goal_pose_.orientation.x = 0.0; + last_goal_pose_.orientation.y = 0.0; + last_goal_pose_.orientation.z = 0.0; + last_goal_pose_.orientation.w = 1.0; + + return {}; +} + +grid_map::Index GridMapRRTStarPlanner::steer( + const grid_map::GridMap & map, + const grid_map::Index & from, + const grid_map::Index & to) +{ + + // Convert indices to continuous map positions + grid_map::Position p_from, p_to; + map.getPosition(from, p_from); + map.getPosition(to, p_to); + + // Compute vector from 'from' to 'to' and its Euclidean distance + double dx = p_to.x() - p_from.x(); + double dy = p_to.y() - p_from.y(); + double dist = std::hypot(dx, dy); + + grid_map::Position p_new; + if (dist <= step_size_) { + // Target is within one step: move directly to it + p_new = p_to; + } else { + // Move along the line by 'step_size_' toward target + double ratio = step_size_ / dist; + p_new = grid_map::Position(p_from.x() + dx * ratio, + p_from.y() + dy * ratio); + } + + // Convert continuous position back to map index + grid_map::Index new_idx; + if (!map.getIndex(p_new, new_idx)) { + // New position is outside map bounds + return grid_map::Index(-1, -1); + } + + return new_idx; +} + +std::vector GridMapRRTStarPlanner::rrt_star( + const grid_map::GridMap & map, + const geometry_msgs::msg::Pose & start, + const geometry_msgs::msg::Pose & goal) +{ + std::vector> tree; + static uint32_t seed_counter = 0; + std::mt19937 rng(seed_counter++); + std::uniform_real_distribution prob_dist(0.0, 1.0); + + grid_map::Index start_idx, goal_idx; + if (!map.getIndex(grid_map::Position(start.position.x, start.position.y), start_idx) || + !map.getIndex(grid_map::Position(goal.position.x, goal.position.y), goal_idx)) + { + RCLCPP_WARN(get_node()->get_logger(), "Start or goal position is outside map bounds"); + return {}; + } + + // get robot yaw from the start pose for forward-biased sampling + tf2::Quaternion robot_q; + tf2::fromMsg(start.orientation, robot_q); + double robot_roll, robot_pitch, robot_yaw; + tf2::Matrix3x3(robot_q).getRPY(robot_roll, robot_pitch, robot_yaw); + + auto root = std::make_shared(); + root->index = start_idx; + root->cost = 0.0; + tree.push_back(root); + + std::shared_ptr best_goal_node = nullptr; + double best_goal_cost = std::numeric_limits::max(); + + int iterations_without_improvement = 0; + const int max_no_improvement = 500; + const int min_iterations = max_iters_ / 2; + + for (int iter = 0; iter < max_iters_; ++iter) { + // adaptive goal bias: increase bias if a candidate path exists, else grow modestly + double adaptive_goal_bias = goal_bias_; + + if (best_goal_node) { + adaptive_goal_bias = std::min(0.9, goal_bias_ + 0.15 * (iter / 100.0)); + } else { + adaptive_goal_bias = std::min(0.6, goal_bias_ * 2.0); + } + + grid_map::Index rand_idx; + if (prob_dist(rng) < adaptive_goal_bias) { + rand_idx = goal_idx; + } else { + // early iterations: bias samples forward relative to robot heading, otherwise sample uniformly + if (iter < 100 && tree.size() < 50) { + rand_idx = biased_random_index(map, start, robot_yaw, rng); + } else { + rand_idx = random_index(map, rng); + } + } + + auto nearest = find_nearest(tree, rand_idx, map); + if (!nearest) { + continue; + } + + auto new_idx = steer(map, nearest->index, rand_idx); + if ((new_idx == grid_map::Index(-1, -1)).all()) { + continue; + } + + // skip exact duplicates and very close nodes when tree is large + bool duplicate = false; + for (const auto & node : tree) { + if ((node->index == new_idx).all()) { + duplicate = true; + break; + } + if (tree.size() > 500 && distance(map, node->index, new_idx) < 0.1) { + duplicate = true; + break; + } + } + if (duplicate) { + continue; + } + + // compute full traversal cost for the new edge and skip infeasible edges + double edge_cost = traversal_cost(map, nearest->index, new_idx); + if (!std::isfinite(edge_cost)) { + continue; + } + + auto new_node = std::make_shared(); + new_node->index = new_idx; + new_node->cost = nearest->cost + edge_cost; + new_node->parent = nearest; + + // collect nearby nodes for potential rewiring using a dynamic radius + std::vector> nearby_nodes; + double search_radius = std::min(neighbor_radius_, + neighbor_radius_ * + std::sqrt(std::log(tree.size()) / tree.size())); + + for (const auto & near_node : tree) { + double dist = distance(map, near_node->index, new_idx); + if (dist <= search_radius) { + nearby_nodes.push_back(near_node); + } + } + + // limit number of neighbors considered, keeping closest ones + if (nearby_nodes.size() > 8) { + std::nth_element(nearby_nodes.begin(), nearby_nodes.begin() + 15, nearby_nodes.end(), + [&](const auto & a, const auto & b) + { + return distance(map, a->index, new_idx) < distance(map, b->index, new_idx); + }); + nearby_nodes.resize(8); + } + + // choose the best parent among nearby nodes to minimize cost + for (const auto & near_node : nearby_nodes) { + double cost = traversal_cost(map, near_node->index, new_idx); + if (!std::isfinite(cost)) { + continue; + } + + double total = near_node->cost + cost; + if (total < new_node->cost) { + new_node->cost = total; + new_node->parent = near_node; + } + } + + tree.push_back(new_node); + + // try to rewire nearby nodes through the new node if it lowers their cost + for (auto & near_node : nearby_nodes) { + if (near_node == new_node || near_node == root) { + continue; + } + + double cost = traversal_cost(map, new_idx, near_node->index); + if (!std::isfinite(cost)) { + continue; + } + + double total = new_node->cost + cost; + if (total < near_node->cost - 0.01) { // small threshold to avoid oscillation + near_node->cost = total; + near_node->parent = new_node; + } + } + + // check proximity to goal and update best solution if improved + double dist_to_goal = distance(map, new_idx, goal_idx); + if (dist_to_goal < goal_threshold_) { + double goal_cost = traversal_cost(map, new_idx, goal_idx); + if (std::isfinite(goal_cost)) { + double total = new_node->cost + goal_cost; + if (total < best_goal_cost) { + best_goal_node = new_node; + best_goal_cost = total; + iterations_without_improvement = 0; + + RCLCPP_DEBUG(get_node()->get_logger(), + "New best goal found at iteration %d with cost %.2f (dist: %.2f)", + iter, best_goal_cost, dist_to_goal); + + // adaptive early termination: evaluate path efficiency against direct distance + if (iter > min_iterations && best_goal_node) { + double direct_distance = distance(map, start_idx, goal_idx); + double path_efficiency = best_goal_cost / direct_distance; + + if (path_efficiency < 1.3 && iter > 2000) { + RCLCPP_DEBUG(get_node()->get_logger(), + "Early termination: efficient path found (%.1f%% of direct) at iter %d", + path_efficiency * 100, iter); + break; + } + + if (path_efficiency < 1.1 && iter > 1000) { + RCLCPP_DEBUG(get_node()->get_logger(), + "Very efficient path found (%.1f%%), early exit at iter %d", + path_efficiency * 100, iter); + break; + } + } + } else { + iterations_without_improvement++; + } + } + } else { + iterations_without_improvement++; + } + + // adaptive termination when no improvement for many iterations, scaled by tree size + int max_no_improvement_adaptive = max_no_improvement; + if (tree.size() > 1000) { + max_no_improvement_adaptive = 50; + } + if (tree.size() > 2000) { + max_no_improvement_adaptive = 30; + } + + if (iterations_without_improvement > max_no_improvement_adaptive && best_goal_node) { + RCLCPP_DEBUG(get_node()->get_logger(), + "No improvement termination at iteration %d (tree size: %zu)", + iter, tree.size()); + break; + } + } + + if (!best_goal_node) { + RCLCPP_WARN(get_node()->get_logger(), "No path found to goal after %d iterations", max_iters_); + return {}; + } + + auto raw_path = extract_path(best_goal_node, map, goal); + + if (raw_path.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "Extracted path is empty"); + return {}; + } + + // apply conservative smoothing to the raw path + raw_path = smooth_path(raw_path, 4); + + RCLCPP_DEBUG(get_node()->get_logger(), + "Generated RRT* path with %zu points, cost %.2f, tree size: %zu", + raw_path.size(), best_goal_cost, tree.size()); + + new_path_cost_ = best_goal_cost; + return raw_path; +} + +grid_map::Index GridMapRRTStarPlanner::biased_random_index( + const grid_map::GridMap & map, + const geometry_msgs::msg::Pose & robot_pose, + double robot_yaw, + std::mt19937 & rng) +{ + // Generate samples biased forward relative to the robot heading. + // Angle in ±60° in front, distance between 1 and 5 meters. + std::uniform_real_distribution angle_dist(-M_PI / 3, M_PI / 3); + std::uniform_real_distribution dist_dist(1.0, 5.0); + + double bias_angle = robot_yaw + angle_dist(rng); + double bias_distance = dist_dist(rng); + + // Compute biased target position in world coordinates + double target_x = robot_pose.position.x + bias_distance * std::cos(bias_angle); + double target_y = robot_pose.position.y + bias_distance * std::sin(bias_angle); + + grid_map::Index biased_idx; + grid_map::Position target_pos(target_x, target_y); + + // If biased point is inside the map, return its index, otherwise fall back + if (map.getIndex(target_pos, biased_idx)) { + return biased_idx; + } else { + return random_index(map, rng); + } +} + +std::shared_ptr GridMapRRTStarPlanner::find_nearest( + const std::vector> & tree, + const grid_map::Index & target, + const grid_map::GridMap & map) +{ + if (tree.empty()) { + return nullptr; + } + + // Return the node in 'tree' with minimum Euclidean distance to 'target' + return *std::min_element(tree.begin(), tree.end(), + [&](const auto & a, const auto & b) + { + return distance(map, a->index, target) < distance(map, b->index, target); + }); +} + +double GridMapRRTStarPlanner::distance( + const grid_map::GridMap & map, + const grid_map::Index & a, + const grid_map::Index & b) +{ + grid_map::Position pa, pb; + map.getPosition(a, pa); + map.getPosition(b, pb); + + return std::hypot(pb.x() - pa.x(), pb.y() - pa.y()); +} + +// Replace previous implementation: trim path by removing waypoints behind or already passed by the robot +nav_msgs::msg::Path GridMapRRTStarPlanner::trim_path_from_robot( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Pose & robot_pose) const +{ + nav_msgs::msg::Path trimmed_path = path; + + if (path.poses.size() < 2) { + return trimmed_path; + } + + const double trim_distance = 1.0; // meters: threshold to consider a waypoint "passed" + size_t closest_index = 0; + double min_distance = std::numeric_limits::max(); + + // Find the closest waypoint to the robot + for (size_t i = 0; i < path.poses.size(); ++i) { + const auto & wp = path.poses[i].pose.position; + const auto & rp = robot_pose.position; + double d = std::hypot(wp.x - rp.x, wp.y - rp.y); + if (d < min_distance) { + min_distance = d; + closest_index = i; + } + } + + // Identify previous waypoints that are very close (already visited) + size_t start_index = closest_index; + for (size_t i = 0; i <= closest_index; ++i) { + const auto & wp = path.poses[i].pose.position; + const auto & rp = robot_pose.position; + double d = std::hypot(wp.x - rp.x, wp.y - rp.y); + if (d < trim_distance) { + start_index = i + 1; // mark for removal + } else { + break; + } + } + + // Ensure we keep at least the closest waypoint + if (start_index > closest_index) { + start_index = closest_index; + } + + // Erase the earlier segment only if enough waypoints remain after trimming + if (start_index > 0 && (path.poses.size() - start_index) >= 3) { + trimmed_path.poses.erase(trimmed_path.poses.begin(), trimmed_path.poses.begin() + start_index); + RCLCPP_DEBUG(get_node()->get_logger(), + "Path trimmed: removed %zu waypoints, %zu remaining (closest at %.2fm)", + start_index, trimmed_path.poses.size(), min_distance); + } + + return trimmed_path; +} + +std::vector GridMapRRTStarPlanner::extract_path( + std::shared_ptr goal_node, + const grid_map::GridMap & map, + const geometry_msgs::msg::Pose & goal) +{ + std::vector> nodes; + nodes.reserve(100); + + // collect nodes from the RRT tree back to the root + for (auto current = goal_node; current; current = current->parent) { + nodes.push_back(current); + } + std::reverse(nodes.begin(), nodes.end()); + + std::vector path; + path.reserve(nodes.size()); + + // convert tree nodes to basic poses (no smoothing here) + for (size_t i = 0; i < nodes.size(); ++i) { + const auto & node = nodes[i]; + grid_map::Position pos; + map.getPosition(node->index, pos); + + geometry_msgs::msg::Pose pose; + pose.position.x = pos.x(); + pose.position.y = pos.y(); + pose.position.z = map.at("elevation", node->index); + + // DEFAULT ORIENTATION: neutral (facing along x-axis) + tf2::Quaternion q; + q.setRPY(0, 0, 0); + pose.orientation = tf2::toMsg(q); + + path.push_back(pose); + } + + // for the final raw waypoint, apply the goal orientation + if (!path.empty()) { + path.back().orientation = goal.orientation; + } + + RCLCPP_DEBUG(get_node()->get_logger(), + "Path extracted with %zu raw RRT* nodes (no smoothing)", path.size()); + + return path; +} + +double GridMapRRTStarPlanner::calculate_lateral_distance_to_path( + const nav_msgs::msg::Path & path, + const geometry_msgs::msg::Pose & robot_pose) const +{ + if (path.poses.size() < 2) { + return 0.0; + } + + double min_distance = std::numeric_limits::max(); + + // Find the closest point on any path segment to the robot + for (size_t i = 0; i < path.poses.size() - 1; ++i) { + const auto & p1 = path.poses[i].pose.position; + const auto & p2 = path.poses[i + 1].pose.position; + const auto & rp = robot_pose.position; + + // Project robot position onto the vector p1->p2 and clamp to the segment + double vx = p2.x - p1.x; + double vy = p2.y - p1.y; + double wx = rp.x - p1.x; + double wy = rp.y - p1.y; + + double dot = wx * vx + wy * vy; + double len_sq = vx * vx + vy * vy; + + if (len_sq < 1e-6) { + // degenerate segment, skip + continue; + } + + double t = dot / len_sq; + double cx, cy; + if (t <= 0.0) { + cx = p1.x; + cy = p1.y; + } else if (t >= 1.0) { + cx = p2.x; + cy = p2.y; + } else { + cx = p1.x + t * vx; + cy = p1.y + t * vy; + } + + double d = std::hypot(rp.x - cx, rp.y - cy); + min_distance = std::min(min_distance, d); + } + + return (min_distance == std::numeric_limits::max()) ? 0.0 : min_distance; +} + +double GridMapRRTStarPlanner::traversal_cost( + const grid_map::GridMap & map, + const grid_map::Index & from, + const grid_map::Index & to) +{ + // Use a unique key (based on flattened indices) to identify the edge. + const uint32_t width = static_cast(map.getSize()[0]); + const uint64_t key = edge_key(flat_index(from, width), flat_index(to, width)); + + if (auto it = cost_cache_.find(key); it != cost_cache_.end()) { + return it->second; + } + + grid_map::Position pos_from, pos_to; + map.getPosition(from, pos_from); + map.getPosition(to, pos_to); + + if (!map.isInside(pos_from) || !map.isInside(pos_to)) { + return std::numeric_limits::infinity(); + } + + if (!map.isValid(from, "elevation") || !map.isValid(to, "elevation")) { + return std::numeric_limits::infinity(); + } + + double z1 = map.at("elevation", from); + double z2 = map.at("elevation", to); + double dx = map.getResolution() * (to.x() - from.x()); + double dy = map.getResolution() * (to.y() - from.y()); + double dz = z2 - z1; + double horizontal_dist = std::hypot(dx, dy); + + // Flat surface: cost = horizontal distance + if (std::abs(dz) < 1e-3) { + return cost_cache_[key] = horizontal_dist; + } + + // Calculate slope + double slope_rad = std::atan2(std::abs(dz), horizontal_dist); + + // Reject edges with slope exceeding maximum allowed + if (slope_rad > max_allowed_slope_) { + return std::numeric_limits::infinity(); + } + + const double distance_3d = std::sqrt(dx * dx + dy * dy + dz * dz); + + const double slope_factor = 1.0 + std::pow(slope_rad / max_allowed_slope_, 2.0); + + const double final_cost = distance_3d * slope_factor; + + return cost_cache_[key] = final_cost; +} + +void GridMapRRTStarPlanner::clear_cost_cache() +{ + cost_cache_.clear(); + cached_map_width_ = 0; + cached_map_height_ = 0; + last_map_timestamp_ = 0; +} + +uint32_t GridMapRRTStarPlanner::flat_index(const grid_map::Index & idx, uint32_t width) +{ + return idx[0] + idx[1] * width; +} + +uint64_t GridMapRRTStarPlanner::edge_key(uint32_t a_flat, uint32_t b_flat) +{ + return (static_cast(a_flat) << 32) | b_flat; +} + +bool GridMapRRTStarPlanner::check_goal_changed(const geometry_msgs::msg::Pose & goal_pose) +{ + // tolerance used for comparing goal positions + const double tolerance = goal_threshold_; + + // check if the goal has changed significantly + bool goal_changed = (std::abs(goal_pose.position.x - last_goal_pose_.position.x) > tolerance || + std::abs(goal_pose.position.y - last_goal_pose_.position.y) > tolerance || + std::abs(goal_pose.position.z - last_goal_pose_.position.z) > tolerance); + + if (goal_changed) { + last_goal_pose_ = goal_pose; // update last known goal + } + + return goal_changed; +} + +// Replace entire update() function: +void GridMapRRTStarPlanner::update(NavState & nav_state) +{ + // initial validations + if (!nav_state.has("goals") || !nav_state.has("robot_pose") || !nav_state.has("map")) { + RCLCPP_DEBUG(get_node()->get_logger(), "goals, robot_pose or map missing. Returning"); + return; + } + + const auto goals = nav_state.get("goals"); + if (goals.goals.empty()) { + RCLCPP_DEBUG(get_node()->get_logger(), "goals empty. Returning empty path"); + current_path_.poses.clear(); + nav_state.set("path", current_path_); + return; + } + + const auto & robot_pose = nav_state.get("robot_pose"); + const auto & goal_pose = goals.goals.front().pose; + const auto & map = nav_state.get("map"); + + // evaluate conditions once + bool goal_changed = check_goal_changed(goal_pose); + bool robot_deviated = false; + bool path_too_short = current_path_.poses.size() < 3; + double lateral_distance = 0.0; + + // check robot deviation only if a path exists + if (!current_path_.poses.empty()) { + lateral_distance = calculate_lateral_distance_to_path(current_path_, robot_pose.pose.pose); + robot_deviated = (lateral_distance > max_lateral_deviation_); + + if (robot_deviated) { + RCLCPP_INFO(get_node()->get_logger(), + "Robot deviated from path (%.2f m > %.2f m threshold), replanning", + lateral_distance, max_lateral_deviation_); + } else { + RCLCPP_DEBUG(get_node()->get_logger(), + "Robot deviation: %.2f m (within %.2f m threshold)", + lateral_distance, max_lateral_deviation_); + } + } + + // decide once whether to replan + bool needs_replan = goal_changed || robot_deviated || path_too_short; + bool needs_new_path = needs_replan || current_path_.poses.empty(); + + // perform actions based on decision + if (needs_replan) { + if (goal_changed) { + RCLCPP_INFO(get_node()->get_logger(), "New goal detected, regenerating path"); + last_goal_pose_ = goal_pose; + } else if (robot_deviated) { + RCLCPP_INFO(get_node()->get_logger(), "Robot deviated, regenerating path"); + } else if (path_too_short) { + RCLCPP_DEBUG(get_node()->get_logger(), + "Path too short (%zu waypoints), regenerating", + current_path_.poses.size()); + } + + // reset state for replanning + current_path_.poses.clear(); + clear_cost_cache(); + current_path_cost_ = std::numeric_limits::max(); + kd_tree_ = std::make_unique(); + } + + // generate a new path if needed + if (needs_new_path) { + auto new_poses = generate_new_path(map, robot_pose.pose.pose, goal_pose); + if (new_poses.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "Failed to generate a new path"); + return; + } + + nav_msgs::msg::Path new_path; + new_path.header.stamp = get_node()->now(); + new_path.header.frame_id = goals.header.frame_id; + + for (const auto & pose : new_poses) { + geometry_msgs::msg::PoseStamped ps; + ps.header = new_path.header; + ps.pose = pose; + new_path.poses.push_back(ps); + } + + // accept or compare new path + if (needs_replan) { + current_path_ = new_path; + current_path_cost_ = new_path_cost_; + RCLCPP_DEBUG(get_node()->get_logger(), + "Using new path: cost %.2f, length %.2f m", + new_path_cost_, compute_path_length(new_path)); + } else if (current_path_.poses.empty()) { + current_path_ = new_path; + current_path_cost_ = new_path_cost_; + RCLCPP_DEBUG(get_node()->get_logger(), + "Using new path (no previous path): cost %.2f, length %.2f m", + new_path_cost_, compute_path_length(new_path)); + } else { + const double improvement_threshold = 5 * goal_threshold_; + + if (current_path_cost_ - new_path_cost_ > improvement_threshold) { + double cost_improvement = current_path_cost_ - new_path_cost_; + double old_cost = current_path_cost_; + + current_path_ = new_path; + current_path_cost_ = new_path_cost_; + + RCLCPP_DEBUG(get_node()->get_logger(), + "Updated to better cost path: %.2f -> %.2f (improvement: %.2f)", + old_cost, new_path_cost_, cost_improvement); + } else { + double cost_difference = new_path_cost_ - current_path_cost_; + RCLCPP_DEBUG(get_node()->get_logger(), + "Keeping current path: cost %.2f < %.2f (difference: %.2f, threshold: %.2f)", + current_path_cost_, new_path_cost_, cost_difference, + improvement_threshold); + } + } + } + + // apply trimming and publish + if (!current_path_.poses.empty()) { + size_t original_size = current_path_.poses.size(); + current_path_ = trim_path_from_robot(current_path_, robot_pose.pose.pose); + + if (current_path_.poses.size() != original_size) { + RCLCPP_DEBUG(get_node()->get_logger(), + "Path trimmed: %zu -> %zu waypoints", + original_size, current_path_.poses.size()); + } + } + + nav_state.set("path", current_path_); + + if (path_pub_->get_subscription_count() > 0) { + path_pub_->publish(current_path_); + } + + if (marker_pub_->get_subscription_count() > 0) { + publish_path_markers(current_path_); + } +} + +std::vector GridMapRRTStarPlanner::generate_new_path( + const grid_map::GridMap & map, + const geometry_msgs::msg::Pose & robot_pose, + const geometry_msgs::msg::Pose & goal_pose) +{ + std::vector new_poses; + + // ensure goal is inside map bounds + grid_map::Index goal_idx; + if (!map.getIndex(grid_map::Position(goal_pose.position.x, goal_pose.position.y), goal_idx)) { + RCLCPP_WARN(get_node()->get_logger(), "Goal is outside the map bounds"); + return new_poses; + } + + // always plan from robot pose for consistency + RCLCPP_DEBUG(get_node()->get_logger(), "Generating new path from robot pose"); + new_poses = rrt_star(map, robot_pose, goal_pose); + + return new_poses; +} + +void GridMapRRTStarPlanner::publish_path_markers(const nav_msgs::msg::Path & path) +{ + visualization_msgs::msg::MarkerArray marker_array; + + // clear previous markers + visualization_msgs::msg::Marker clear; + clear.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array.markers.push_back(clear); + + // create markers for each path pose + int id = 0; + for (const auto & pose_stamped : path.poses) { + visualization_msgs::msg::Marker marker; + marker.header = path.header; + marker.ns = "rrt_star_path"; + marker.id = id++; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; + + // set marker pose + marker.pose = pose_stamped.pose; + + // set marker scale + marker.scale.x = 0.3; // arrow length + marker.scale.y = 0.05; // arrow thickness + marker.scale.z = 0.05; + + // set marker color + marker.color.r = 0.1f; + marker.color.g = 0.1f; + marker.color.b = 1.0f; + marker.color.a = 1.0f; + + marker_array.markers.push_back(marker); + } + + // publish markers + marker_pub_->publish(marker_array); +} +std::vector GridMapRRTStarPlanner::smooth_path( + const std::vector & input_path, + int interpolation_points_per_segment) +{ + // Return the input path if it's too short to smooth + if (input_path.size() < 3) { + return input_path; + } + + std::vector smoothed; + const size_t N = input_path.size(); + const size_t preserve_n = std::max(1, final_poses_with_goal_orientation_); + const double max_yaw_step = M_PI / 8.0; + + // Precompute positions and tangents + std::vector pts(N); + std::vector z_values(N); + for (size_t i = 0; i < N; ++i) { + pts[i] = {input_path[i].position.x, input_path[i].position.y}; + z_values[i] = input_path[i].position.z; + } + + auto compute_tangent = [&](size_t idx) -> grid_map::Position + { + if (idx == 0) { + return 0.5 * (pts[1] - pts[0]); + } else if (idx + 1 >= N) { + return 0.5 * (pts[N - 1] - pts[N - 2]); + } else { + return 0.5 * (pts[idx + 1] - pts[idx - 1]); + } + }; + + std::vector tangents(N); + std::vector z_tangents(N); + for (size_t i = 0; i < N; ++i) { + tangents[i] = compute_tangent(i); + z_tangents[i] = (i == 0) ? 0.5 * (z_values[1] - z_values[0]) : (i + 1 >= + N) ? 0.5 * (z_values[N - 1] - z_values[N - 2]) : + 0.5 * (z_values[i + 1] - z_values[i - 1]); + } + + // Add the first point with corrected orientation + geometry_msgs::msg::Pose first_pose = input_path.front(); + + // Calculate orientation for the first point based on direction to second point + if (input_path.size() > 1) { + double dx = input_path[1].position.x - first_pose.position.x; + double dy = input_path[1].position.y - first_pose.position.y; + if (std::hypot(dx, dy) > 1e-6) { + double yaw = std::atan2(dy, dx); + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + first_pose.orientation = tf2::toMsg(q); + } + } + + smoothed.push_back(first_pose); + + // Interpolate between points + for (size_t i = 0; i < N - 1; ++i) { + const auto & p0 = pts[i], & p1 = pts[i + 1]; + const auto & m0 = tangents[i], & m1 = tangents[i + 1]; + double z0 = z_values[i], z1 = z_values[i + 1]; + double z_m0 = z_tangents[i], z_m1 = z_tangents[i + 1]; + + for (int s = 1; s <= interpolation_points_per_segment; ++s) { + double t = static_cast(s) / (interpolation_points_per_segment + 1); + double t2 = t * t, t3 = t2 * t; + + // Hermite interpolation + double h00 = 2.0 * t3 - 3.0 * t2 + 1.0; + double h10 = t3 - 2.0 * t2 + t; + double h01 = -2.0 * t3 + 3.0 * t2; + double h11 = t3 - t2; + + geometry_msgs::msg::Pose sample; + sample.position.x = h00 * p0.x() + h10 * m0.x() + h01 * p1.x() + h11 * m1.x(); + sample.position.y = h00 * p0.y() + h10 * m0.y() + h01 * p1.y() + h11 * m1.y(); + sample.position.z = h00 * z0 + h10 * z_m0 + h01 * z1 + h11 * z_m1; + + // Calculate orientation based on path direction + double yaw = 0.0; + if (!smoothed.empty()) { + const auto & prev = smoothed.back().position; + double dx = sample.position.x - prev.x; + double dy = sample.position.y - prev.y; + if (std::hypot(dx, dy) > 1e-6) { + yaw = std::atan2(dy, dx); + + // Smooth yaw transitions + tf2::Quaternion prev_q; + tf2::fromMsg(smoothed.back().orientation, prev_q); + double pr, pp, prev_yaw; + tf2::Matrix3x3(prev_q).getRPY(pr, pp, prev_yaw); + + double dyaw = yaw - prev_yaw; + while (dyaw > M_PI) { + dyaw -= 2.0 * M_PI; + } + while (dyaw < -M_PI) { + dyaw += 2.0 * M_PI; + } + + if (std::abs(dyaw) > max_yaw_step) { + yaw = prev_yaw + (dyaw > 0 ? max_yaw_step : -max_yaw_step); + } + } else { + // Keep previous orientation if no significant movement + tf2::Quaternion prev_q; + tf2::fromMsg(smoothed.back().orientation, prev_q); + double pr, pp; + tf2::Matrix3x3(prev_q).getRPY(pr, pp, yaw); + } + } + + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + sample.orientation = tf2::toMsg(q); + + // Avoid adding points too close + const auto & last_pos = smoothed.back().position; + if (std::hypot(sample.position.x - last_pos.x, + sample.position.y - last_pos.y) >= spacing_) + { + smoothed.push_back(sample); + } + } + + // Add the next raw waypoint with corrected orientation + geometry_msgs::msg::Pose next_pose = input_path[i + 1]; + + // Calculate orientation for the waypoint based on path direction + if (!smoothed.empty()) { + const auto & prev = smoothed.back().position; + double dx = next_pose.position.x - prev.x; + double dy = next_pose.position.y - prev.y; + if (std::hypot(dx, dy) > 1e-6) { + double yaw = std::atan2(dy, dx); + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + next_pose.orientation = tf2::toMsg(q); + } + } + + smoothed.push_back(next_pose); + } + + // Ensure the final `preserve_n` points have the goal orientation + tf2::Quaternion goal_orientation; + tf2::fromMsg(input_path.back().orientation, goal_orientation); + + // Apply goal orientation to the last preserve_n points + size_t start_preserve = smoothed.size() > preserve_n ? smoothed.size() - preserve_n : 0; + for (size_t i = start_preserve; i < smoothed.size(); ++i) { + smoothed[i].orientation = tf2::toMsg(goal_orientation); + } + if (!smoothed.empty()) { + smoothed.back().orientation = tf2::toMsg(goal_orientation); + } + RCLCPP_DEBUG(get_node()->get_logger(), + "Smoothed path: %zu -> %zu points (preserved %zu final orientations)", + input_path.size(), smoothed.size(), preserve_n); + + return smoothed; +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::GridMapRRTStarPlanner, easynav::PlannerMethodBase) diff --git a/easynav_gridmap_rrtstar_planner/src/easynav_gridmap_rrtstar_planner/KDTree.cpp b/easynav_gridmap_rrtstar_planner/src/easynav_gridmap_rrtstar_planner/KDTree.cpp new file mode 100644 index 0000000..b1f7583 --- /dev/null +++ b/easynav_gridmap_rrtstar_planner/src/easynav_gridmap_rrtstar_planner/KDTree.cpp @@ -0,0 +1,111 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// licensed under the GNU General Public License v3.0. +// See for details. +// +// Easy Navigation program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include "easynav_gridmap_rrtstar_planner/KDTree.hpp" +#include +#include + +namespace easynav +{ + +std::unique_ptr KDTree::build( + std::vector> & nodes, + int depth) +{ + if (nodes.empty()) { + return nullptr; + } + + // Determine splitting axis (0 = x, 1 = y) + int axis = depth % 2; + + // Sort nodes along the current axis + std::sort(nodes.begin(), nodes.end(), + [axis](const std::shared_ptr & a, const std::shared_ptr & b) + { + return axis == 0 ? a->index.x() < b->index.x() : a->index.y() < b->index.y(); + }); + + // Choose median as root of this subtree + int median = nodes.size() / 2; + auto node = std::make_unique(nodes[median], depth); + + // Recursively build left and right subtrees + std::vector> left_nodes(nodes.begin(), nodes.begin() + median); + std::vector> right_nodes(nodes.begin() + median + 1, nodes.end()); + + node->left = build(left_nodes, depth + 1); + node->right = build(right_nodes, depth + 1); + + return node; +} + +void KDTree::search_radius( + const std::unique_ptr & node, + const grid_map::Index & target, + double radius_sq, + std::vector> & result) +{ + if (!node) { + return; + } + + // Compute squared distance between node and target + double dx = node->rrt_node->index.x() - target.x(); + double dy = node->rrt_node->index.y() - target.y(); + double dist_sq = dx * dx + dy * dy; + + // If within radius and not the target itself, add to result + if (dist_sq <= radius_sq && !(node->rrt_node->index == target).all()) { + result.push_back(node->rrt_node); + } + + // Determine axis and distance along that axis + int axis = node->depth % 2; + double axis_dist = (axis == 0) ? dx : dy; + + // Recursively search child nodes based on axis distance + if (axis_dist <= 0) { + search_radius(node->left, target, radius_sq, result); + if (axis_dist * axis_dist <= radius_sq) { + search_radius(node->right, target, radius_sq, result); + } + } else { + search_radius(node->right, target, radius_sq, result); + if (axis_dist * axis_dist <= radius_sq) { + search_radius(node->left, target, radius_sq, result); + } + } +} + +void KDTree::rebuild(std::vector> & nodes) +{ + root = build(nodes, 0); +} + +std::vector> KDTree::find_neighbors( + const grid_map::Index & index, + double radius) +{ + std::vector> result; + search_radius(root, index, radius * radius, result); + return result; +} + +} // namespace easynav