diff --git a/planners/easynav_bonxai_planner/CMakeLists.txt b/planners/easynav_bonxai_planner/CMakeLists.txt new file mode 100644 index 00000000..2709b551 --- /dev/null +++ b/planners/easynav_bonxai_planner/CMakeLists.txt @@ -0,0 +1,87 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_bonxai_planner) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Unoptimized, A* over millions of cells takes seconds +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +find_package(ament_cmake REQUIRED) +find_package(easynav_bonxai_maps_manager REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) + +add_library(${PROJECT_NAME} SHARED + src/easynav_bonxai_planner/BonxaiPlanner.cpp + src/easynav_bonxai_planner/Planner3D.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_bonxai_maps_manager::easynav_bonxai_maps_manager + easynav_common::easynav_common + easynav_core::easynav_core + pluginlib::pluginlib + rclcpp::rclcpp + Eigen3::Eigen + ${geometry_msgs_TARGETS} + ${nav_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) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(planner3d_tests tests/planner3d_tests.cpp) + target_link_libraries(planner3d_tests ${PROJECT_NAME}) + ament_add_gtest(bonxai_planner_tests tests/bonxai_planner_tests.cpp) + target_link_libraries(bonxai_planner_tests ${PROJECT_NAME}) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) + +pluginlib_export_plugin_description_file(easynav_core easynav_bonxai_planner_plugins.xml) + +ament_export_dependencies( + easynav_bonxai_maps_manager + easynav_common + easynav_core + pluginlib + rclcpp + geometry_msgs + nav_msgs + Eigen3 +) + +ament_package() diff --git a/planners/easynav_bonxai_planner/README.md b/planners/easynav_bonxai_planner/README.md new file mode 100644 index 00000000..3c3735e5 --- /dev/null +++ b/planners/easynav_bonxai_planner/README.md @@ -0,0 +1,70 @@ +# easynav_bonxai_planner + +## Description + +3D path planner for aerial robots: A* over a [Bonxai](https://github.com/facontidavide/Bonxai) map (`map.bonxai`, from `easynav_bonxai_maps_manager`), keeping a minimum distance from every occupied voxel. + +A Bonxai map holds what the sensors saw: surfaces, with gaps, and free space behind them (under the ground, inside buildings). `fill_below` makes what is below them solid. From the occupied voxels it builds a grid of the distance to the nearest obstacle (an exact Euclidean distance transform), once per map. A* (26-connected) only goes through cells at least `min_clearance` from the obstacles, and each step costs more the closer it is than `preferred_clearance`. The path is then made of straight segments wherever the clearance along them allows, sampled every `path_step` and smoothed: corners become curves without coming closer to the obstacles. A start or goal closer than `min_clearance` to an obstacle is moved to the nearest cell that is not, within `snap_radius`. + +## Authors and Maintainers + +- **Authors:** Intelligent Robotics Lab +- **Maintainers:** Francisco Martín Rico + +## Supported ROS 2 Distributions + +| Distribution | Status | +|---|---| +| humble | ![humble](https://img.shields.io/badge/humble-supported-brightgreen) | +| jazzy | ![jazzy](https://img.shields.io/badge/jazzy-supported-brightgreen) | +| kilted | ![kilted](https://img.shields.io/badge/kilted-supported-brightgreen) | +| lyrical | ![lyrical](https://img.shields.io/badge/lyrical-supported-brightgreen) | +| rolling | ![rolling](https://img.shields.io/badge/rolling-supported-brightgreen) | + +## Plugin (pluginlib) + +- **Plugin Name:** `easynav_bonxai_planner/BonxaiPlanner` +- **Type:** `easynav::BonxaiPlanner` +- **Base Class:** `easynav::PlannerMethodBase` +- **Library:** `easynav_bonxai_planner` + +## Parameters + +All under the plugin's name (`.`): + +| Name | Type | Default | Description | +|---|---|---:|---| +| `resolution` | `double` | `1.0` | Cell size of the planning grid (m). The grid covers the map's bounding box plus `margin`: at 1 m a town of 400 x 400 x 60 m is about 10 million cells. | +| `margin` | `double` | `10.0` | Space around the map's bounding box the robot may use (m). | +| `min_altitude` | `double` | NaN | Lowest z of the grid (m); NaN: the map's lowest point minus `margin`. | +| `fill_below` | `string` | `lowest` | What is solid below the map's points in each column: `none` (only the points), `lowest` (below the lowest one: the ground, so a gap in its scan is no way underground) or `highest` (below the highest one: buildings as solid blocks, as an elevation map; nothing can be flown under). Columns with no points take their neighbours' height. | +| `max_altitude` | `double` | NaN | Highest z of the grid (m); NaN: the map's highest point plus `margin`. | +| `min_clearance` | `double` | `1.5` | Distance from the obstacles the path never comes closer than (m). | +| `preferred_clearance` | `double` | `4.0` | Closer than this, a step costs more (m); 0: no preference. | +| `clearance_weight` | `double` | `2.0` | Extra cost per meter of a step with no clearance (it falls quadratically to 0 at `preferred_clearance`). | +| `snap_radius` | `double` | `5.0` | How far a start or goal too close to an obstacle may be moved (m). | +| `max_expansions` | `int` | `3000000` | A* gives up after expanding this many cells. | +| `path_step` | `double` | `0.5` | Distance between the path's poses (m). | +| `smooth_iterations` | `int` | `200` | Relaxation passes of the smoother (0: the straight segments, with their corners). | +| `smooth_weight` | `double` | `0.5` | Pull of each point towards its neighbours' mean, per pass. | +| `smooth_data_weight` | `double` | `0.01` | Pull back towards where A* put it (0.01 rounds a 90 deg corner to about 2.5 m of radius; higher, tighter). The smoother never moves a point closer to an obstacle than `min_clearance`; the ends do not move. | +| `continuous_replan` | `bool` | `true` | Plan in every cycle; false: only for a new goal. | + +## Interfaces + +| Direction | Topic | Type | Purpose | +|---|---|---|---| +| Publisher | `//path` | `nav_msgs/msg/Path` | The path (only when subscribed). | + +## NavState Keys + +| Key | Type | Access | Notes | +|---|---|---|---| +| `map.bonxai` | `Bonxai::ProbabilisticMap` | **Read** | Its occupied voxels are the obstacles. | +| `goals` | `nav_msgs::msg::Goals` | **Read** | The first one, in the map frame. | +| `robot_pose` | `nav_msgs::msg::Odometry` | **Read** | Start of the path. | +| `path` | `nav_msgs::msg::Path` | **Write** | Poses facing along the path; the last one with the goal's orientation. Empty if there is no path. | + +## License + +Apache-2.0 diff --git a/planners/easynav_bonxai_planner/easynav_bonxai_planner_plugins.xml b/planners/easynav_bonxai_planner/easynav_bonxai_planner_plugins.xml new file mode 100644 index 00000000..c82bdd05 --- /dev/null +++ b/planners/easynav_bonxai_planner/easynav_bonxai_planner_plugins.xml @@ -0,0 +1,9 @@ + + + + + A* in 3D over a Bonxai map, for aerial robots, keeping a clearance from the obstacles. + + + + diff --git a/planners/easynav_bonxai_planner/include/easynav_bonxai_planner/BonxaiPlanner.hpp b/planners/easynav_bonxai_planner/include/easynav_bonxai_planner/BonxaiPlanner.hpp new file mode 100644 index 00000000..d1a5c272 --- /dev/null +++ b/planners/easynav_bonxai_planner/include/easynav_bonxai_planner/BonxaiPlanner.hpp @@ -0,0 +1,75 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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 EASYNAV_BONXAI_PLANNER__BONXAIPLANNER_HPP_ +#define EASYNAV_BONXAI_PLANNER__BONXAIPLANNER_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "nav_msgs/msg/path.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "easynav_bonxai_planner/Planner3D.hpp" +#include "easynav_common/types/NavState.hpp" +#include "easynav_core/PlannerMethodBase.hpp" + +namespace easynav +{ + +/// @brief A* in 3D over a Bonxai map ("map.bonxai"), for aerial robots: the path keeps +/// min_clearance from every occupied voxel, and prefers preferred_clearance. +class BonxaiPlanner : public PlannerMethodBase +{ +public: + BonxaiPlanner() = default; + ~BonxaiPlanner() override = default; + + void on_initialize() override; + void update(NavState & nav_state) override; + + /// The grid the last plan used (tests). + const bonxai_planner::ClearanceGrid & grid() const {return grid_;} + +protected: + void clear_path(NavState & nav_state); + /// The occupied voxels to the clearance grid, when the map changed. + bool refresh_grid(const NavState & nav_state); + + bonxai_planner::ClearanceGrid grid_; + bonxai_planner::PlannerParams params_; + bonxai_planner::SmootherParams smoother_; + bonxai_planner::FillBelow fill_below_ {bonxai_planner::FillBelow::Lowest}; + double resolution_ {1.0}; + double margin_ {10.0}; + double min_altitude_ {std::numeric_limits::quiet_NaN()}; + double max_altitude_ {std::numeric_limits::quiet_NaN()}; + double path_step_ {0.5}; + bool continuous_replan_ {true}; + + const void * map_ptr_ {nullptr}; + std::size_t map_voxels_ {0}; + + nav_msgs::msg::Path current_path_; + geometry_msgs::msg::Pose current_goal_; + bool path_published_ {false}; + rclcpp::Publisher::SharedPtr path_pub_; +}; + +} // namespace easynav + +#endif // EASYNAV_BONXAI_PLANNER__BONXAIPLANNER_HPP_ diff --git a/planners/easynav_bonxai_planner/include/easynav_bonxai_planner/Planner3D.hpp b/planners/easynav_bonxai_planner/include/easynav_bonxai_planner/Planner3D.hpp new file mode 100644 index 00000000..d67aa239 --- /dev/null +++ b/planners/easynav_bonxai_planner/include/easynav_bonxai_planner/Planner3D.hpp @@ -0,0 +1,133 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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. + +/// \file +/// \brief Path planning in 3D space for aerial robots: a grid of the distance to the nearest +/// obstacle, and A* through the cells far enough from them. + +#ifndef EASYNAV_BONXAI_PLANNER__PLANNER3D_HPP_ +#define EASYNAV_BONXAI_PLANNER__PLANNER3D_HPP_ + +#include +#include +#include + +#include "Eigen/Core" + +namespace easynav +{ +namespace bonxai_planner +{ + +/// @brief What a map of surfaces (what the sensors saw) leaves solid below them, in each column. +enum class FillBelow +{ + None, ///< Only the observed points are obstacles. + Lowest, ///< Below the lowest point of each column: the ground is solid. + Highest, ///< Below the highest one: buildings are solid blocks (an elevation map). +}; + +/// @brief "none", "lowest" or "highest" to \p fill; false if it is none of them. +bool parse_fill_below(const std::string & name, FillBelow & fill); + +/// @brief A box of cubic cells with the distance (m) from each cell's centre to the nearest +/// obstacle point (Euclidean distance transform). +class ClearanceGrid +{ +public: + /// @brief Builds the grid over the obstacles' bounding box grown by \p margin, from z_min to + /// z_max (NaN: the obstacles' own), with what \p fill makes solid below them. Columns with no + /// points (gaps in the scans) take their neighbours' height. + /// @return false if there are no obstacles or the grid would exceed \p max_cells. + bool build( + const std::vector & obstacles, double resolution, double margin, + double z_min, double z_max, FillBelow fill = FillBelow::None, + std::size_t max_cells = 60000000); + + bool empty() const {return dist_.empty();} + double resolution() const {return res_;} + const Eigen::Vector3d & origin() const {return origin_;} + int nx() const {return nx_;} + int ny() const {return ny_;} + int nz() const {return nz_;} + + bool inside(int x, int y, int z) const + { + return x >= 0 && y >= 0 && z >= 0 && x < nx_ && y < ny_ && z < nz_; + } + std::size_t index(int x, int y, int z) const + { + return (static_cast(z) * ny_ + y) * nx_ + x; + } + /// Distance (m) at a cell; 0 outside the grid. + double distance(int x, int y, int z) const + { + return inside(x, y, z) ? dist_[index(x, y, z)] : 0.0; + } + /// Distance (m) at a point (its cell's); 0 outside the grid. + double distance_at(const Eigen::Vector3d & p) const; + bool cell_of(const Eigen::Vector3d & p, int & x, int & y, int & z) const; + Eigen::Vector3d center(int x, int y, int z) const; + +private: + double res_ {1.0}; + Eigen::Vector3d origin_ {Eigen::Vector3d::Zero()}; ///< Corner of cell (0, 0, 0). + int nx_ {0}, ny_ {0}, nz_ {0}; + std::vector dist_; +}; + +struct PlannerParams +{ + double min_clearance {1.5}; ///< Cells nearer than this to an obstacle are not used (m). + double preferred_clearance {4.0}; ///< Nearer than this costs more (m). + double clearance_weight {2.0}; ///< Extra cost per m at no clearance (quadratic). + std::size_t max_expansions {3000000}; + /// Start or goal nearer than min_clearance: the nearest cell that is not, within this (m). + double snap_radius {5.0}; +}; + +/// @brief Why plan() found no path, or Found. +enum class PlanStatus {Found, NoGrid, StartBlocked, GoalBlocked, Unreachable}; + +/// @brief A* over the grid's cells (26-connected), then straight segments wherever the +/// clearance along them allows. Empty if there is no path (\p status says why). +std::vector plan( + const ClearanceGrid & grid, const Eigen::Vector3d & start, const Eigen::Vector3d & goal, + const PlannerParams & params, PlanStatus * status = nullptr); + +struct SmootherParams +{ + int iterations {200}; + double smooth_weight {0.5}; ///< Pull of each point towards its neighbours' mean. + /// Pull back towards where the planner put it: 0.01 rounds a 90 deg corner to ~2.5 m radius. + double data_weight {0.01}; +}; + +/// @brief \p path sampled every \p step, then relaxed: each point moves towards its neighbours' +/// mean (corners become curves), kept near its original place, and never closer to an obstacle +/// than \p clearance (a point that would be stays). The ends do not move. +std::vector smooth( + const ClearanceGrid & grid, const std::vector & path, double step, + double clearance, const SmootherParams & params); + +/// @brief Whether every point of the segment keeps \p clearance (sampled every half cell). +bool segment_clear( + const ClearanceGrid & grid, const Eigen::Vector3d & a, const Eigen::Vector3d & b, + double clearance); + +} // namespace bonxai_planner +} // namespace easynav + +#endif // EASYNAV_BONXAI_PLANNER__PLANNER3D_HPP_ diff --git a/planners/easynav_bonxai_planner/package.xml b/planners/easynav_bonxai_planner/package.xml new file mode 100644 index 00000000..edc2201f --- /dev/null +++ b/planners/easynav_bonxai_planner/package.xml @@ -0,0 +1,29 @@ + + + + easynav_bonxai_planner + 0.5.1 + Easy Navigation: 3D path planner for aerial robots, A* over a Bonxai map keeping a clearance from the obstacles. + Francisco Martín Rico + Apache-2.0 + + ament_cmake + + easynav_bonxai_maps_manager + easynav_common + easynav_core + eigen + geometry_msgs + nav_msgs + pluginlib + rclcpp + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + rclcpp_lifecycle + + + ament_cmake + + diff --git a/planners/easynav_bonxai_planner/pixi.toml b/planners/easynav_bonxai_planner/pixi.toml new file mode 100644 index 00000000..54ac8daf --- /dev/null +++ b/planners/easynav_bonxai_planner/pixi.toml @@ -0,0 +1,7 @@ +# Conda package built by pixi (pixi-build-ros): name, version and deps come from package.xml +[package] +publish = true + +[package.build.backend] +name = "pixi-build-ros" +workspace = true diff --git a/planners/easynav_bonxai_planner/src/easynav_bonxai_planner/BonxaiPlanner.cpp b/planners/easynav_bonxai_planner/src/easynav_bonxai_planner/BonxaiPlanner.cpp new file mode 100644 index 00000000..69f0deb7 --- /dev/null +++ b/planners/easynav_bonxai_planner/src/easynav_bonxai_planner/BonxaiPlanner.cpp @@ -0,0 +1,211 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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. + +#include "easynav_bonxai_planner/BonxaiPlanner.hpp" + +#include +#include +#include +#include + +#include "bonxai/probabilistic_map.hpp" +#include "nav_msgs/msg/goals.hpp" +#include "nav_msgs/msg/odometry.hpp" + +#include "easynav_common/Parameters.hpp" +#include "easynav_common/RTTFBuffer.hpp" + +namespace easynav +{ + +void BonxaiPlanner::on_initialize() +{ + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + auto param = [&](const std::string & name, auto & value) { + using T = std::decay_t; + easynav::declare_parameter_if_absent(*node, plugin_name + "." + name, value); + node->get_parameter(plugin_name + "." + name, value); + }; + int max_expansions = static_cast(params_.max_expansions); + param("resolution", resolution_); + param("margin", margin_); + param("min_clearance", params_.min_clearance); + param("preferred_clearance", params_.preferred_clearance); + param("clearance_weight", params_.clearance_weight); + param("snap_radius", params_.snap_radius); + param("max_expansions", max_expansions); + param("min_altitude", min_altitude_); + param("max_altitude", max_altitude_); + param("path_step", path_step_); + param("smooth_iterations", smoother_.iterations); + param("smooth_weight", smoother_.smooth_weight); + param("smooth_data_weight", smoother_.data_weight); + param("continuous_replan", continuous_replan_); + std::string fill_below = "lowest"; + param("fill_below", fill_below); + if (!bonxai_planner::parse_fill_below(fill_below, fill_below_)) { + throw std::runtime_error( + "[" + plugin_name + "] fill_below '" + fill_below + "' is not none, lowest or highest"); + } + params_.max_expansions = static_cast(std::max(1, max_expansions)); + if (!(resolution_ > 0.0) || margin_ < 0.0 || params_.min_clearance < 0.0 || + params_.preferred_clearance < 0.0 || params_.clearance_weight < 0.0 || + !(path_step_ > 0.0) || smoother_.iterations < 0 || smoother_.smooth_weight < 0.0 || + smoother_.smooth_weight > 1.0 || smoother_.data_weight < 0.0 || smoother_.data_weight > 1.0) + { + throw std::runtime_error( + "[" + plugin_name + "] invalid parameters: resolution > 0, path_step > 0, " + "margin, clearances and clearance_weight >= 0, smooth_iterations >= 0, " + "smooth_weight and smooth_data_weight in [0, 1]"); + } + map_ptr_ = nullptr; + map_voxels_ = 0; + path_pub_ = node->create_publisher( + node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + + plugin_name + "/path", 10); +} + +bool BonxaiPlanner::refresh_grid(const NavState & nav_state) +{ + const auto & map = nav_state.get_ptr("map.bonxai"); + std::vector coords; + map->getOccupiedVoxels(coords); + if (map.get() == map_ptr_ && coords.size() == map_voxels_ && !grid_.empty()) { + return true; + } + std::vector points; + points.reserve(coords.size()); + for (const auto & c : coords) { + const auto p = map->grid().coordToPos(c); + points.emplace_back(p.x, p.y, p.z); + } + const auto start = std::chrono::steady_clock::now(); + if (!grid_.build(points, resolution_, margin_, min_altitude_, max_altitude_, fill_below_)) { + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 5000, + "[%s] cannot build the clearance grid (%zu occupied voxels): empty map, or too large for " + "resolution %.2f", get_plugin_name().c_str(), points.size(), resolution_); + return false; + } + map_ptr_ = map.get(); + map_voxels_ = coords.size(); + RCLCPP_INFO( + get_node()->get_logger(), "[%s] clearance grid %dx%dx%d at %.2f m from %zu voxels in %.0f ms", + get_plugin_name().c_str(), grid_.nx(), grid_.ny(), grid_.nz(), resolution_, points.size(), + std::chrono::duration(std::chrono::steady_clock::now() - start).count()); + return true; +} + +void BonxaiPlanner::update(NavState & nav_state) +{ + if (!nav_state.has("goals") || !nav_state.has("robot_pose") || !nav_state.has("map.bonxai")) { + return; + } + const auto & goals = nav_state.get("goals"); + if (goals.goals.empty()) { + clear_path(nav_state); + return; + } + const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); + if (goals.header.frame_id != tf_info.map_frame) { + RCLCPP_WARN( + get_node()->get_logger(), "[%s] goals frame is not '%s': %s", get_plugin_name().c_str(), + tf_info.map_frame.c_str(), goals.header.frame_id.c_str()); + clear_path(nav_state); + return; + } + const auto & goal = goals.goals.front().pose; + if (!continuous_replan_ && !current_path_.poses.empty() && goal == current_goal_) { + nav_state.set("path", current_path_); + return; + } + if (!refresh_grid(nav_state)) { + clear_path(nav_state); + return; + } + + const auto & robot = nav_state.get_safe("robot_pose").pose.pose; + const Eigen::Vector3d start(robot.position.x, robot.position.y, robot.position.z); + const Eigen::Vector3d target(goal.position.x, goal.position.y, goal.position.z); + bonxai_planner::PlanStatus status; + const auto points = bonxai_planner::plan(grid_, start, target, params_, &status); + current_goal_ = goal; + if (points.size() < 2) { + const char * why = status == bonxai_planner::PlanStatus::StartBlocked ? "the start is" : + status == bonxai_planner::PlanStatus::GoalBlocked ? "the goal is" : "nothing is"; + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 2000, + "[%s] no path from (%.1f, %.1f, %.1f) to (%.1f, %.1f, %.1f): %s at %.1f m from the " + "obstacles within %.1f m", get_plugin_name().c_str(), start.x(), start.y(), start.z(), + target.x(), target.y(), target.z(), why, params_.min_clearance, params_.snap_radius); + clear_path(nav_state); + return; + } + + // Smoothed and evenly spaced poses, facing along the path; the last one, the goal's heading + const auto dense = bonxai_planner::smooth( + grid_, points, path_step_, params_.min_clearance, smoother_); + current_path_.header.frame_id = goals.header.frame_id; + current_path_.header.stamp = get_node()->now(); + current_path_.poses.clear(); + double yaw = 0.0; + for (std::size_t i = 0; i + 1 < dense.size(); ++i) { + // The tangent: from the previous point to the next one + const Eigen::Vector3d d = dense[i + 1] - dense[i > 0 ? i - 1 : 0]; + if (d.head<2>().norm() > 1e-6) { + yaw = std::atan2(d.y(), d.x()); + } else if (i == 0) { + yaw = 2.0 * std::atan2(robot.orientation.z, robot.orientation.w); // straight up: as now + } + geometry_msgs::msg::PoseStamped ps; + ps.header = current_path_.header; + ps.pose.position.x = dense[i].x(); + ps.pose.position.y = dense[i].y(); + ps.pose.position.z = dense[i].z(); + ps.pose.orientation.z = std::sin(0.5 * yaw); + ps.pose.orientation.w = std::cos(0.5 * yaw); + current_path_.poses.push_back(ps); + } + geometry_msgs::msg::PoseStamped last; + last.header = current_path_.header; + last.pose.position.x = points.back().x(); + last.pose.position.y = points.back().y(); + last.pose.position.z = points.back().z(); + last.pose.orientation = goal.orientation; + current_path_.poses.push_back(last); + + nav_state.set("path", current_path_); + if (path_pub_->get_subscription_count() > 0) { + path_pub_->publish(current_path_); + path_published_ = true; + } +} + +void BonxaiPlanner::clear_path(NavState & nav_state) +{ + current_path_.poses.clear(); + if (path_published_ && path_pub_->get_subscription_count() > 0) { + current_path_.header.stamp = get_node()->now(); + path_pub_->publish(current_path_); + } + path_published_ = false; + nav_state.set("path", current_path_); +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::BonxaiPlanner, easynav::PlannerMethodBase) diff --git a/planners/easynav_bonxai_planner/src/easynav_bonxai_planner/Planner3D.cpp b/planners/easynav_bonxai_planner/src/easynav_bonxai_planner/Planner3D.cpp new file mode 100644 index 00000000..657af59a --- /dev/null +++ b/planners/easynav_bonxai_planner/src/easynav_bonxai_planner/Planner3D.cpp @@ -0,0 +1,452 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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. + +#include "easynav_bonxai_planner/Planner3D.hpp" + +#include +#include +#include +#include +#include +#include + +namespace easynav +{ +namespace bonxai_planner +{ + +namespace +{ + +constexpr double kInf = 1e20; + +/// Squared distance transform of a sampled function (Felzenszwalb & Huttenlocher). +void transform_1d( + const std::vector & f, int n, std::vector & d, std::vector & v, + std::vector & z) +{ + int k = 0; + v[0] = 0; + z[0] = -kInf; + z[1] = kInf; + auto intersection = [&](int q, int p) { + return ((f[q] + static_cast(q) * q) - (f[p] + static_cast(p) * p)) / + (2.0 * (q - p)); + }; + for (int q = 1; q < n; ++q) { + double s = intersection(q, v[k]); + while (s <= z[k]) { // z[0] is -inf: it stops there + --k; + s = intersection(q, v[k]); + } + ++k; + v[k] = q; + z[k] = s; + z[k + 1] = kInf; + } + k = 0; + for (int q = 0; q < n; ++q) { + while (z[k + 1] < q) {++k;} + const double dq = q - v[k]; + d[q] = dq * dq + f[v[k]]; + } +} + +constexpr int kGapFillPasses = 8; // a gap up to 16 cells across is filled + +} // namespace + +bool parse_fill_below(const std::string & name, FillBelow & fill) +{ + if (name == "none") { + fill = FillBelow::None; + } else if (name == "lowest") { + fill = FillBelow::Lowest; + } else if (name == "highest") { + fill = FillBelow::Highest; + } else { + return false; + } + return true; +} + +bool ClearanceGrid::build( + const std::vector & obstacles, double resolution, double margin, + double z_min, double z_max, FillBelow fill, std::size_t max_cells) +{ + dist_.clear(); + if (obstacles.empty() || !(resolution > 0.0)) { + return false; + } + Eigen::Vector3d lo = obstacles.front(), hi = obstacles.front(); + for (const auto & p : obstacles) { + lo = lo.cwiseMin(p); + hi = hi.cwiseMax(p); + } + lo.array() -= margin; + hi.array() += margin; + if (std::isfinite(z_min)) {lo.z() = z_min;} + if (std::isfinite(z_max)) {hi.z() = z_max;} + if (hi.z() <= lo.z()) { + return false; + } + res_ = resolution; + origin_ = lo; + nx_ = static_cast(std::ceil((hi.x() - lo.x()) / res_)); + ny_ = static_cast(std::ceil((hi.y() - lo.y()) / res_)); + nz_ = static_cast(std::ceil((hi.z() - lo.z()) / res_)); + const std::size_t n = static_cast(nx_) * ny_ * nz_; + if (nx_ <= 0 || ny_ <= 0 || nz_ <= 0 || n > max_cells) { + nx_ = ny_ = nz_ = 0; + return false; + } + + std::vector sq(n, kInf); + // The height of each column (cell index), for fill; -1: no points in it + std::vector top(static_cast(nx_) * ny_, -1); + for (const auto & p : obstacles) { + int x, y, z; + if (!cell_of(p, x, y, z)) { + // Below the grid (e.g. z_min above the ground) still counts as the column's bottom + if (fill == FillBelow::None || x < 0 || y < 0 || x >= nx_ || y >= ny_ || z >= nz_) { + continue; + } + z = -1; + } else { + sq[index(x, y, z)] = 0.0; + } + int & t = top[static_cast(y) * nx_ + x]; + if (t < 0 && z < 0) { + t = 0; // a column whose only points are below the grid: solid at its bottom cell + } else if (fill == FillBelow::Highest) { + t = std::max(t, z); + } else if (fill == FillBelow::Lowest) { + t = t < 0 ? std::max(z, 0) : std::min(t, std::max(z, 0)); + } + } + if (fill != FillBelow::None) { + // Gaps in the scans: the highest of the neighbouring columns', a few passes + for (int pass = 0; pass < kGapFillPasses; ++pass) { + std::vector next = top; + bool changed = false; + for (int y = 0; y < ny_; ++y) { + for (int x = 0; x < nx_; ++x) { + if (top[static_cast(y) * nx_ + x] >= 0) {continue;} + int best = -1; + for (int dy = -1; dy <= 1; ++dy) { + for (int dx = -1; dx <= 1; ++dx) { + const int qx = x + dx, qy = y + dy; + if (qx >= 0 && qy >= 0 && qx < nx_ && qy < ny_) { + best = std::max(best, top[static_cast(qy) * nx_ + qx]); + } + } + } + if (best >= 0) { + next[static_cast(y) * nx_ + x] = best; + changed = true; + } + } + } + top.swap(next); + if (!changed) {break;} + } + for (int y = 0; y < ny_; ++y) { + for (int x = 0; x < nx_; ++x) { + for (int z = 0; z <= top[static_cast(y) * nx_ + x]; ++z) { + sq[index(x, y, z)] = 0.0; + } + } + } + } + // Separable: along x, then y, then z + const int longest = std::max({nx_, ny_, nz_}); + std::vector f(longest), d(longest), zz(longest + 1); + std::vector v(longest); + for (int z = 0; z < nz_; ++z) { + for (int y = 0; y < ny_; ++y) { + for (int x = 0; x < nx_; ++x) { + f[x] = sq[index(x, y, z)]; + } + transform_1d(f, nx_, d, v, zz); + for (int x = 0; x < nx_; ++x) { + sq[index(x, y, z)] = d[x]; + } + } + } + for (int z = 0; z < nz_; ++z) { + for (int x = 0; x < nx_; ++x) { + for (int y = 0; y < ny_; ++y) { + f[y] = sq[index(x, y, z)]; + } + transform_1d(f, ny_, d, v, zz); + for (int y = 0; y < ny_; ++y) { + sq[index(x, y, z)] = d[y]; + } + } + } + for (int y = 0; y < ny_; ++y) { + for (int x = 0; x < nx_; ++x) { + for (int z = 0; z < nz_; ++z) { + f[z] = sq[index(x, y, z)]; + } + transform_1d(f, nz_, d, v, zz); + for (int z = 0; z < nz_; ++z) { + sq[index(x, y, z)] = d[z]; + } + } + } + dist_.resize(n); + for (std::size_t i = 0; i < n; ++i) { + dist_[i] = static_cast(std::sqrt(std::min(sq[i], 1e12)) * res_); + } + return true; +} + +bool ClearanceGrid::cell_of(const Eigen::Vector3d & p, int & x, int & y, int & z) const +{ + x = static_cast(std::floor((p.x() - origin_.x()) / res_)); + y = static_cast(std::floor((p.y() - origin_.y()) / res_)); + z = static_cast(std::floor((p.z() - origin_.z()) / res_)); + return inside(x, y, z); +} + +Eigen::Vector3d ClearanceGrid::center(int x, int y, int z) const +{ + return origin_ + res_ * Eigen::Vector3d(x + 0.5, y + 0.5, z + 0.5); +} + +double ClearanceGrid::distance_at(const Eigen::Vector3d & p) const +{ + int x, y, z; + return cell_of(p, x, y, z) ? dist_[index(x, y, z)] : 0.0; +} + +std::vector smooth( + const ClearanceGrid & grid, const std::vector & path, double step, + double clearance, const SmootherParams & params) +{ + std::vector dense; + if (path.empty() || !(step > 0.0)) { + return path; + } + dense.push_back(path.front()); + for (std::size_t i = 0; i + 1 < path.size(); ++i) { + const Eigen::Vector3d d = path[i + 1] - path[i]; + const int n = std::max(1, static_cast(std::ceil(d.norm() / step))); + for (int k = 1; k <= n; ++k) { + dense.push_back(path[i] + d * (static_cast(k) / n)); + } + } + const std::vector original = dense; + for (int it = 0; it < params.iterations; ++it) { + for (std::size_t i = 1; i + 1 < dense.size(); ++i) { + const Eigen::Vector3d moved = dense[i] + + params.smooth_weight * (0.5 * (dense[i - 1] + dense[i + 1]) - dense[i]) + + params.data_weight * (original[i] - dense[i]); + if (grid.distance_at(moved) >= clearance) { + dense[i] = moved; + } + } + } + return dense; +} + +bool segment_clear( + const ClearanceGrid & grid, const Eigen::Vector3d & a, const Eigen::Vector3d & b, + double clearance) +{ + const double len = (b - a).norm(); + const int steps = std::max(1, static_cast(std::ceil(len / (0.5 * grid.resolution())))); + for (int i = 0; i <= steps; ++i) { + if (grid.distance_at(a + (b - a) * (static_cast(i) / steps)) < clearance) { + return false; + } + } + return true; +} + +namespace +{ + +/// The cell nearest to \p p with the clearance, within \p radius; false if none. +bool snap( + const ClearanceGrid & grid, const Eigen::Vector3d & p, double clearance, double radius, + int & cx, int & cy, int & cz) +{ + int x, y, z; + grid.cell_of(p, x, y, z); + if (grid.inside(x, y, z) && grid.distance(x, y, z) >= clearance) { + cx = x; + cy = y; + cz = z; + return true; + } + const int r = static_cast(std::ceil(radius / grid.resolution())); + double best = std::numeric_limits::infinity(); + for (int dz = -r; dz <= r; ++dz) { + for (int dy = -r; dy <= r; ++dy) { + for (int dx = -r; dx <= r; ++dx) { + const int qx = x + dx, qy = y + dy, qz = z + dz; + if (!grid.inside(qx, qy, qz) || grid.distance(qx, qy, qz) < clearance) { + continue; + } + const double d = (grid.center(qx, qy, qz) - p).norm(); + if (d < best && d <= radius) { + best = d; + cx = qx; + cy = qy; + cz = qz; + } + } + } + } + return std::isfinite(best); +} + +} // namespace + +std::vector plan( + const ClearanceGrid & grid, const Eigen::Vector3d & start, const Eigen::Vector3d & goal, + const PlannerParams & params, PlanStatus * status) +{ + std::vector path; + PlanStatus dummy; + PlanStatus & st = status ? *status : dummy; + int sx, sy, sz, gx, gy, gz; + if (grid.empty()) { + st = PlanStatus::NoGrid; + return path; + } + if (!snap(grid, start, params.min_clearance, params.snap_radius, sx, sy, sz)) { + st = PlanStatus::StartBlocked; + return path; + } + if (!snap(grid, goal, params.min_clearance, params.snap_radius, gx, gy, gz)) { + st = PlanStatus::GoalBlocked; + return path; + } + st = PlanStatus::Unreachable; + const bool goal_free = grid.distance_at(goal) >= params.min_clearance; + + const std::size_t n = static_cast(grid.nx()) * grid.ny() * grid.nz(); + std::vector g(n, std::numeric_limits::infinity()); + std::vector parent(n, -1); + std::vector closed(n, 0); + using Entry = std::pair; + std::priority_queue, std::greater> open; + + const double res = grid.resolution(); + auto h = [&](int x, int y, int z) { + return static_cast(res * std::sqrt( + static_cast((x - gx) * (x - gx) + (y - gy) * (y - gy) + + (z - gz) * (z - gz)))); + }; + auto penalty = [&](double d) { + if (d >= params.preferred_clearance || params.preferred_clearance <= 0.0) {return 0.0;} + const double t = (params.preferred_clearance - d) / params.preferred_clearance; + return params.clearance_weight * t * t; + }; + + const std::size_t s = grid.index(sx, sy, sz), t = grid.index(gx, gy, gz); + g[s] = 0.0f; + open.emplace(h(sx, sy, sz), static_cast(s)); + std::size_t expansions = 0; + bool found = false; + const int nx = grid.nx(), ny = grid.ny(); + while (!open.empty() && expansions < params.max_expansions) { + const auto [f, ci] = open.top(); + open.pop(); + const std::size_t c = static_cast(ci); + if (closed[c]) {continue;} + closed[c] = 1; + ++expansions; + if (c == t) { + found = true; + break; + } + const int cx = static_cast(c % nx); + const int cy = static_cast((c / nx) % ny); + const int cz = static_cast(c / (static_cast(nx) * ny)); + for (int dz = -1; dz <= 1; ++dz) { + for (int dy = -1; dy <= 1; ++dy) { + for (int dx = -1; dx <= 1; ++dx) { + if (!dx && !dy && !dz) {continue;} + const int qx = cx + dx, qy = cy + dy, qz = cz + dz; + if (!grid.inside(qx, qy, qz)) {continue;} + const std::size_t q = grid.index(qx, qy, qz); + const double d = grid.distance(qx, qy, qz); + if (closed[q] || d < params.min_clearance) {continue;} + const double step = res * std::sqrt(static_cast(dx * dx + dy * dy + dz * dz)); + const float cost = g[c] + static_cast(step * (1.0 + penalty(d))); + if (cost < g[q]) { + g[q] = cost; + parent[q] = static_cast(c); + open.emplace(cost + h(qx, qy, qz), static_cast(q)); + } + } + } + } + } + if (!found) { + return path; + } + st = PlanStatus::Found; + + std::vector cells; + for (int32_t c = static_cast(t); c >= 0; c = parent[c]) { + const std::size_t u = static_cast(c); + cells.push_back( + grid.center( + static_cast(u % nx), static_cast((u / nx) % ny), + static_cast(u / (static_cast(nx) * ny)))); + } + std::reverse(cells.begin(), cells.end()); + if (cells.size() == 1) { + cells.push_back(cells.front()); // start and goal in one cell: a path from one to the other + } + cells.front() = grid.distance_at(start) >= params.min_clearance ? start : cells.front(); + if (goal_free) { + cells.back() = goal; + } + + // Straight segments where the clearance allows: as much as A*'s cells had along that stretch + // (up to preferred_clearance), so that a shortcut does not undo the preference + std::vector kept(cells.size()); + for (std::size_t k = 0; k < cells.size(); ++k) { + kept[k] = std::min( + grid.distance_at(cells[k]), + std::max(params.preferred_clearance, params.min_clearance)); + } + path.push_back(cells.front()); + std::size_t i = 0; + while (i + 1 < cells.size()) { + std::size_t j = cells.size() - 1; + while (j > i + 1) { + const double needed = std::max( + params.min_clearance, + *std::min_element(kept.begin() + i, kept.begin() + j + 1) - 0.5 * grid.resolution()); + if (segment_clear(grid, cells[i], cells[j], needed)) { + break; + } + --j; + } + path.push_back(cells[j]); + i = j; + } + return path; +} + +} // namespace bonxai_planner +} // namespace easynav diff --git a/planners/easynav_bonxai_planner/tests/bonxai_planner_tests.cpp b/planners/easynav_bonxai_planner/tests/bonxai_planner_tests.cpp new file mode 100644 index 00000000..46f6e977 --- /dev/null +++ b/planners/easynav_bonxai_planner/tests/bonxai_planner_tests.cpp @@ -0,0 +1,163 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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. + +/// \file +/// \brief BonxaiPlanner in the NavState: the path to the goal in 3D, and when there is none. + +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#include "bonxai/probabilistic_map.hpp" +#include "nav_msgs/msg/goals.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "easynav_bonxai_planner/BonxaiPlanner.hpp" +#include "easynav_common/RTTFBuffer.hpp" +#include "easynav_common/types/NavState.hpp" + +namespace +{ + +/// A building: a closed 10 x 10 x 20 m box at the origin, on a floor at z = 0. +std::shared_ptr town() +{ + auto map = std::make_shared(0.5); + for (double a = -5.0; a <= 5.0; a += 0.5) { + for (double z = 0.0; z <= 20.0; z += 0.5) { + for (double s : {-5.0, 5.0}) { + map->addHitPoint(Eigen::Vector3d(s, a, z)); + map->addHitPoint(Eigen::Vector3d(a, s, z)); + } + } + for (double b = -5.0; b <= 5.0; b += 0.5) { + map->addHitPoint(Eigen::Vector3d(a, b, 20.0)); + } + } + for (double x = -30.0; x <= 30.0; x += 0.5) { + for (double y = -30.0; y <= 30.0; y += 0.5) { + map->addHitPoint(Eigen::Vector3d(x, y, 0.0)); + } + } + return map; +} + +nav_msgs::msg::Goals goal_at(double x, double y, double z, const std::string & frame) +{ + nav_msgs::msg::Goals goals; + goals.header.frame_id = frame; + geometry_msgs::msg::PoseStamped g; + g.header.frame_id = frame; + g.pose.position.x = x; + g.pose.position.y = y; + g.pose.position.z = z; + g.pose.orientation.z = std::sin(0.5); + g.pose.orientation.w = std::cos(0.5); + goals.goals.push_back(g); + return goals; +} + +} // namespace + +class BonxaiPlannerTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) {rclcpp::init(0, nullptr);} + const std::vector overrides = { + {"bonxai.resolution", 1.0}, {"bonxai.min_clearance", 2.0}, + {"bonxai.preferred_clearance", 0.0}, {"bonxai.margin", 5.0}, + {"bonxai.max_altitude", 30.0}}; + node_ = std::make_shared( + "planner_node_" + std::to_string(count_++), + rclcpp::NodeOptions().parameter_overrides(overrides)); + planner_.initialize(node_, "bonxai"); + nav_msgs::msg::Odometry robot; + robot.pose.pose.position.x = -15.0; + robot.pose.pose.position.z = 5.0; + robot.pose.pose.orientation.w = 1.0; + nav_state_.set("robot_pose", robot); + } + + std::string map_frame() const + { + return easynav::RTTFBuffer::getInstance()->get_tf_info().map_frame; + } + + static int count_; + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + easynav::BonxaiPlanner planner_; + easynav::NavState nav_state_; +}; + +int BonxaiPlannerTest::count_ = 0; + +TEST_F(BonxaiPlannerTest, AroundTheBuildingAtItsDistanceAndToTheGoalsHeight) +{ + nav_state_.set("map.bonxai", town()); + nav_state_.set("goals", goal_at(15.0, 0.0, 12.0, map_frame())); + planner_.update(nav_state_); + ASSERT_TRUE(nav_state_.has("path")); + const auto path = nav_state_.get("path"); + ASSERT_GT(path.poses.size(), 10u); + const auto & first = path.poses.front().pose.position; + const auto & last = path.poses.back().pose; + EXPECT_NEAR(first.x, -15.0, 1e-6); + EXPECT_NEAR(last.position.x, 15.0, 1e-6); + EXPECT_NEAR(last.position.z, 12.0, 1e-6); + EXPECT_NEAR(last.orientation.z, std::sin(0.5), 1e-9) << "the goal's heading"; + for (std::size_t i = 1; i < path.poses.size(); ++i) { + const auto & a = path.poses[i - 1].pose.position; + const auto & b = path.poses[i].pose.position; + EXPECT_LE(std::hypot(b.x - a.x, b.y - a.y, b.z - a.z), 0.5 + 1e-6) << "evenly spaced"; + EXPECT_GE(planner_.grid().distance_at({b.x, b.y, b.z}), 2.0 - 1.0) << i; + } +} + +TEST_F(BonxaiPlannerTest, NoPathWithoutAMapForAnotherFrameOrToAGoalInsideTheBuilding) +{ + nav_state_.set("goals", goal_at(15.0, 0.0, 12.0, map_frame())); + planner_.update(nav_state_); + EXPECT_FALSE(nav_state_.has("path")) << "no map: nothing yet"; + + nav_state_.set("map.bonxai", town()); + nav_state_.set("goals", goal_at(15.0, 0.0, 12.0, "odom_other")); + planner_.update(nav_state_); + ASSERT_TRUE(nav_state_.has("path")); + EXPECT_TRUE(nav_state_.get("path").poses.empty()); + + nav_state_.set("goals", goal_at(0.0, 0.0, 10.0, map_frame())); + planner_.update(nav_state_); + EXPECT_TRUE(nav_state_.get("path").poses.empty()) << "inside"; +} + +TEST_F(BonxaiPlannerTest, InvalidParametersFailToInitialize) +{ + for (const auto & p : std::vector{ + {"bonxai.resolution", 0.0}, {"bonxai.path_step", 0.0}, {"bonxai.min_clearance", -1.0}}) + { + auto node = std::make_shared( + "bad_planner_" + std::to_string(count_++), rclcpp::NodeOptions().parameter_overrides({p})); + easynav::BonxaiPlanner planner; + EXPECT_THROW(planner.initialize(node, "bonxai"), std::runtime_error) << p.get_name(); + } +} diff --git a/planners/easynav_bonxai_planner/tests/planner3d_tests.cpp b/planners/easynav_bonxai_planner/tests/planner3d_tests.cpp new file mode 100644 index 00000000..7b765c2c --- /dev/null +++ b/planners/easynav_bonxai_planner/tests/planner3d_tests.cpp @@ -0,0 +1,348 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// 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. + +/// \file +/// \brief The 3D planner's core: the clearance grid and A* with its clearance. + +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#include "easynav_bonxai_planner/Planner3D.hpp" + +using easynav::bonxai_planner::ClearanceGrid; +using easynav::bonxai_planner::PlannerParams; +using Eigen::Vector3d; + +namespace +{ + +constexpr double kNaN = std::numeric_limits::quiet_NaN(); + +/// A wall at x = 0 over y, z in [-10, 10], with a square window of half side \p hole (0: none). +std::vector wall(double hole) +{ + std::vector pts; + for (double y = -10.0; y <= 10.0; y += 0.25) { + for (double z = -10.0; z <= 10.0; z += 0.25) { + if (std::abs(y) < hole && std::abs(z) < hole) {continue;} + pts.emplace_back(0.0, y, z); + } + } + return pts; +} + +double length(const std::vector & path) +{ + double l = 0.0; + for (std::size_t i = 1; i < path.size(); ++i) { + l += (path[i] - path[i - 1]).norm(); + } + return l; +} + +/// The least clearance along the path, sampled every 10 cm. +double least_clearance(const ClearanceGrid & grid, const std::vector & path) +{ + double least = std::numeric_limits::infinity(); + for (std::size_t i = 1; i < path.size(); ++i) { + const int n = std::max(1, static_cast((path[i] - path[i - 1]).norm() / 0.1)); + for (int k = 0; k <= n; ++k) { + least = std::min(least, grid.distance_at(path[i - 1] + (path[i] - path[i - 1]) * k / n)); + } + } + return least; +} + +} // namespace + +TEST(ClearanceGrid, DistanceToTheNearestObstacle) +{ + ClearanceGrid grid; + ASSERT_TRUE(grid.build({{0.0, 0.0, 0.0}}, 0.5, 5.0, kNaN, kNaN)); + EXPECT_NEAR(grid.distance_at({0.0, 0.0, 0.0}), 0.0, 1e-6); + EXPECT_NEAR(grid.distance_at({3.1, 0.1, 0.1}), 3.0, 0.5); + EXPECT_NEAR(grid.distance_at({2.1, 2.1, 2.1}), std::sqrt(12.0), 0.5); + EXPECT_DOUBLE_EQ(grid.distance_at({50.0, 0.0, 0.0}), 0.0) << "outside: no clearance"; +} + +TEST(ClearanceGrid, AltitudeLimitsAndSize) +{ + ClearanceGrid grid; + EXPECT_FALSE(grid.build({}, 0.5, 5.0, kNaN, kNaN)) << "no obstacles"; + EXPECT_FALSE(grid.build({{0, 0, 0}}, 0.0, 5.0, kNaN, kNaN)) << "no resolution"; + EXPECT_FALSE(grid.build({{0, 0, 0}}, 0.5, 5.0, 3.0, 2.0)) << "empty altitude band"; + const std::vector origin = {{0, 0, 0}}; + EXPECT_FALSE( + grid.build(origin, 0.01, 100.0, kNaN, kNaN, easynav::bonxai_planner::FillBelow::None, 1000000)) + << "too many cells"; + ASSERT_TRUE(grid.build({{0, 0, 0}}, 1.0, 5.0, -1.0, 20.0)); + EXPECT_NEAR(grid.origin().z(), -1.0, 1e-9); + EXPECT_EQ(grid.nz(), 21); +} + +TEST(Planner3D, StraightThroughFreeSpace) +{ + ClearanceGrid grid; + ASSERT_TRUE(grid.build({{0, 0, -20}, {0, 0, 20}}, 0.5, 15.0, kNaN, kNaN)); + const auto path = easynav::bonxai_planner::plan(grid, {-10, 0, 0}, {10, 3, 2}, {}); + ASSERT_EQ(path.size(), 2u) << "one straight segment"; + EXPECT_TRUE(path.front().isApprox(Vector3d(-10, 0, 0))); + EXPECT_TRUE(path.back().isApprox(Vector3d(10, 3, 2))); +} + +TEST(Planner3D, AlreadyAtTheGoalIsAPathToIt) +{ + // At the goal (one cell): a path from the robot to the goal, so the controller can turn to + // the goal's heading, instead of none + ClearanceGrid grid; + ASSERT_TRUE(grid.build({{0, 0, -20}, {0, 0, 20}}, 1.0, 15.0, kNaN, kNaN)); + const auto path = easynav::bonxai_planner::plan(grid, {5.1, 0.2, 0.3}, {5.3, 0.1, 0.4}, {}); + ASSERT_EQ(path.size(), 2u); + EXPECT_TRUE(path.front().isApprox(Vector3d(5.1, 0.2, 0.3))); + EXPECT_TRUE(path.back().isApprox(Vector3d(5.3, 0.1, 0.4))); +} + +TEST(Planner3D, ThroughAWindowWideEnough) +{ + ClearanceGrid grid; + ASSERT_TRUE(grid.build(wall(2.5), 0.25, 6.0, kNaN, kNaN)); + PlannerParams p; + p.min_clearance = 1.0; + p.preferred_clearance = 0.0; + const auto path = easynav::bonxai_planner::plan(grid, {-8, 0, 0}, {8, 0, 0}, p); + ASSERT_GE(path.size(), 2u); + EXPECT_LT(length(path), 17.0) << "straight through the window"; + EXPECT_GE(least_clearance(grid, path), 1.0 - 0.25); +} + +TEST(Planner3D, AroundTheWallWhenTheWindowIsTooNarrow) +{ + ClearanceGrid grid; + ASSERT_TRUE(grid.build(wall(2.0), 0.25, 6.0, kNaN, kNaN)); + PlannerParams p; + p.min_clearance = 2.5; // more than the window's half side + p.preferred_clearance = 0.0; + const auto path = easynav::bonxai_planner::plan(grid, {-8, 0, 0}, {8, 0, 0}, p); + ASSERT_GE(path.size(), 3u); + EXPECT_GT(length(path), 25.0) << "around the wall's edge"; + EXPECT_GE(least_clearance(grid, path), 2.5 - 0.25); +} + +TEST(Planner3D, PrefersToKeepItsDistance) +{ + // With a preferred clearance, the path through a wide window stays near its centre + ClearanceGrid grid; + ASSERT_TRUE(grid.build(wall(4.0), 0.25, 6.0, kNaN, kNaN)); + PlannerParams p; + p.min_clearance = 1.0; + p.preferred_clearance = 4.0; + p.clearance_weight = 5.0; + const auto path = easynav::bonxai_planner::plan(grid, {-8, 3, 3}, {8, 3, 3}, p); + ASSERT_GE(path.size(), 2u); + double at_wall = 0.0; + for (std::size_t i = 1; i < path.size(); ++i) { + if ((path[i - 1].x() < 0.0) != (path[i].x() < 0.0)) { + const double t = -path[i - 1].x() / (path[i].x() - path[i - 1].x()); + at_wall = grid.distance_at(path[i - 1] + t * (path[i] - path[i - 1])); + } + } + EXPECT_GT(at_wall, 2.0) << "crosses the window farther from its edges than it must"; +} + +TEST(Planner3D, NoPathToAnEnclosedGoal) +{ + std::vector box; + for (double a = -3.0; a <= 3.0; a += 0.25) { + for (double b = -3.0; b <= 3.0; b += 0.25) { + for (double s : {-3.0, 3.0}) { + box.emplace_back(s, a, b); + box.emplace_back(a, s, b); + box.emplace_back(a, b, s); + } + } + } + ClearanceGrid grid; + ASSERT_TRUE(grid.build(box, 0.25, 6.0, kNaN, kNaN)); + PlannerParams p; + p.min_clearance = 1.0; + p.snap_radius = 1.0; + EXPECT_TRUE(easynav::bonxai_planner::plan(grid, {-7, 0, 0}, {0, 0, 0}, p).empty()); +} + +TEST(Planner3D, AStartTooCloseToAnObstacleMovesAwayFirst) +{ + ClearanceGrid grid; + ASSERT_TRUE(grid.build(wall(0.0), 0.25, 6.0, kNaN, kNaN)); + PlannerParams p; + p.min_clearance = 1.0; + // 0.4 m from the wall: the first cell with 1 m of clearance, then on + const auto path = easynav::bonxai_planner::plan(grid, {-0.4, 0, 0}, {-5, 2, 0}, p); + ASSERT_GE(path.size(), 2u); + EXPECT_NEAR(path.front().x(), -1.1, 0.3); + EXPECT_TRUE(path.back().isApprox(Vector3d(-5, 2, 0))); +} + +TEST(Planner3D, TheAltitudeBandStopsItFromFlyingOver) +{ + // A wall from the floor up to 10 m, 40 m long: over it is shorter, unless the band is lower + std::vector pts; + for (double y = -20.0; y <= 20.0; y += 0.5) { + for (double z = 0.0; z <= 10.0; z += 0.5) { + pts.emplace_back(0.0, y, z); + } + } + for (double x = -10.0; x <= 10.0; x += 0.5) { + for (double y = -25.0; y <= 25.0; y += 0.5) { + pts.emplace_back(x, y, 0.0); + } // the floor + } + PlannerParams p; + p.min_clearance = 1.0; + p.preferred_clearance = 0.0; + ClearanceGrid high, low; + ASSERT_TRUE(high.build(pts, 0.5, 5.0, 0.0, 20.0)); + ASSERT_TRUE(low.build(pts, 0.5, 5.0, 0.0, 8.0)); + const auto over = easynav::bonxai_planner::plan(high, {-5, 0, 3}, {5, 0, 3}, p); + const auto around = easynav::bonxai_planner::plan(low, {-5, 0, 3}, {5, 0, 3}, p); + ASSERT_FALSE(over.empty()); + ASSERT_FALSE(around.empty()); + double top = 0.0; + for (const auto & q : over) { + top = std::max(top, q.z()); + } + EXPECT_GT(top, 10.5) << "over the wall"; + for (const auto & q : around) { + EXPECT_LE(q.z(), 8.0); + } + EXPECT_GT(length(around), length(over)); +} + +TEST(Planner3D, FillBelowKeepsItOutOfTheGroundAndOfBuildings) +{ + using easynav::bonxai_planner::FillBelow; + // A floor at z = 0 with a 6 m gap, and a building scanned only from outside: its walls and + // roof, nothing inside (a hollow shell for the sensors) + std::vector pts; + for (double x = -20.0; x <= 20.0; x += 0.5) { + for (double y = -20.0; y <= 20.0; y += 0.5) { + if (std::abs(x + 10.0) < 3.0 && std::abs(y) < 3.0) {continue;} // the gap + pts.emplace_back(x, y, 0.0); + } + } + for (double a = -4.0; a <= 4.0; a += 0.5) { + for (double z = 0.0; z <= 12.0; z += 0.5) { + for (double s : {-4.0, 4.0}) { + pts.emplace_back(5.0 + s, a, z); + pts.emplace_back(5.0 + a, s, z); + } + } + for (double b = -4.0; b <= 4.0; b += 0.5) { + pts.emplace_back(5.0 + a, b, 12.0); + } + } + PlannerParams p; + p.min_clearance = 1.5; + p.preferred_clearance = 0.0; + p.snap_radius = 0.5; + + ClearanceGrid none, lowest, highest; + ASSERT_TRUE(none.build(pts, 0.5, 6.0, -6.0, 20.0, FillBelow::None)); + ASSERT_TRUE(lowest.build(pts, 0.5, 6.0, -6.0, 20.0, FillBelow::Lowest)); + ASSERT_TRUE(highest.build(pts, 0.5, 6.0, -6.0, 20.0, FillBelow::Highest)); + + // Under the floor, through its gap: only without fill + EXPECT_FALSE(easynav::bonxai_planner::plan(none, {-10, 0, 5}, {-15, 10, -4}, p).empty()); + EXPECT_TRUE(easynav::bonxai_planner::plan(lowest, {-10, 0, 5}, {-15, 10, -4}, p).empty()) + << "the gap is filled from its neighbours: the ground is solid"; + + // Inside the building: reachable through nothing, but its inside is free space unless it + // is filled up to the roof + EXPECT_GT(none.distance_at({5, 0, 6}), 2.0); + EXPECT_DOUBLE_EQ(highest.distance_at({5, 0, 6}), 0.0) << "a solid block"; + EXPECT_GT(lowest.distance_at({5, 0, 6}), 2.0) << "lowest only fills the ground"; + + // Over it with highest: the path keeps its distance from the roof + const auto over = easynav::bonxai_planner::plan(highest, {-5, 0, 4}, {15, 0, 4}, p); + ASSERT_FALSE(over.empty()); + EXPECT_GE(least_clearance(highest, over), 1.5 - 0.5); +} + +TEST(Planner3D, FillBelowNames) +{ + using easynav::bonxai_planner::FillBelow; + FillBelow f; + EXPECT_TRUE(easynav::bonxai_planner::parse_fill_below("highest", f)); + EXPECT_EQ(f, FillBelow::Highest); + EXPECT_TRUE(easynav::bonxai_planner::parse_fill_below("none", f)); + EXPECT_EQ(f, FillBelow::None); + EXPECT_FALSE(easynav::bonxai_planner::parse_fill_below("top", f)); +} + +namespace +{ + +/// The largest turn (rad) between consecutive segments of a path. +double sharpest_turn(const std::vector & path) +{ + double worst = 0.0; + for (std::size_t i = 1; i + 1 < path.size(); ++i) { + const Vector3d a = (path[i] - path[i - 1]).normalized(); + const Vector3d b = (path[i + 1] - path[i]).normalized(); + worst = std::max(worst, std::acos(std::clamp(a.dot(b), -1.0, 1.0))); + } + return worst; +} + +} // namespace + +TEST(Planner3D, TheSmootherRoundsCornersAndKeepsTheEnds) +{ + ClearanceGrid grid; + ASSERT_TRUE(grid.build({{0, 0, -30}, {0, 0, 30}}, 0.5, 30.0, kNaN, kNaN)); + // A 90 deg corner in the horizontal plane and then a climb + const std::vector corner = {{10, 0, 0}, {20, 0, 0}, {20, 10, 0}, {20, 10, 8}}; + const auto smooth = easynav::bonxai_planner::smooth(grid, corner, 0.5, 1.0, {}); + ASSERT_GT(smooth.size(), 50u); + EXPECT_TRUE(smooth.front().isApprox(corner.front())); + EXPECT_TRUE(smooth.back().isApprox(corner.back())); + EXPECT_LT(sharpest_turn(smooth), 0.35) << "no corner left (it was 90 deg)"; + for (std::size_t i = 1; i < smooth.size(); ++i) { + EXPECT_LT((smooth[i] - smooth[i - 1]).norm(), 0.6); + } + // No smoothing: the corners stay + easynav::bonxai_planner::SmootherParams none; + none.iterations = 0; + const auto kept = easynav::bonxai_planner::smooth(grid, corner, 0.5, 1.0, none); + EXPECT_NEAR(sharpest_turn(kept), M_PI / 2.0, 1e-6); +} + +TEST(Planner3D, TheSmootherNeverComesCloserToAnObstacle) +{ + // A corner around the edge of a wall: rounding it would cut through the wall's clearance + ClearanceGrid grid; + ASSERT_TRUE(grid.build(wall(0.0), 0.25, 8.0, kNaN, kNaN)); + PlannerParams p; + p.min_clearance = 1.5; + p.preferred_clearance = 0.0; + const auto path = easynav::bonxai_planner::plan(grid, {-5, 0, 0}, {5, 0, 0}, p); + ASSERT_GE(path.size(), 3u) << "around the wall"; + const auto smooth = easynav::bonxai_planner::smooth(grid, path, 0.25, p.min_clearance, {}); + EXPECT_GE(least_clearance(grid, smooth), p.min_clearance - 0.25); + EXPECT_LE(sharpest_turn(smooth), sharpest_turn(path) + 1e-9); +}