New easynav_bonxai_planner: 3D A* with clearance over Bonxai maps

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+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()

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 <fmrico@gmail.com>
+
+## 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 (`<plugin>.`):
+
+| 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 | `<node_fqn>/<plugin>/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

planners/easynav_bonxai_planner/easynav_bonxai_planner_plugins.xml

@@ -0,0 +1,9 @@
+<class_libraries>
+  <library path="easynav_bonxai_planner">
+    <class name="easynav_bonxai_planner/BonxaiPlanner" type="easynav::BonxaiPlanner" base_class_type="easynav::PlannerMethodBase">
+      <description>
+      A* in 3D over a Bonxai map, for aerial robots, keeping a clearance from the obstacles.
+      </description>
+    </class>
+  </library>
+</class_libraries>

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 <cstddef>
+#include <limits>
+#include <vector>
+
+#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<double>::quiet_NaN()};
+  double max_altitude_ {std::numeric_limits<double>::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<nav_msgs::msg::Path>::SharedPtr path_pub_;
+};
+
+}  // namespace easynav
+
+#endif  // EASYNAV_BONXAI_PLANNER__BONXAIPLANNER_HPP_

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 <cstdint>
+#include <string>
+#include <vector>
+
+#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<Eigen::Vector3d> & 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<std::size_t>(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<float> 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<Eigen::Vector3d> 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<Eigen::Vector3d> smooth(
+  const ClearanceGrid & grid, const std::vector<Eigen::Vector3d> & 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_