From 6ddc25c64036a205cdb11f6f5c4d05444e521fc3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 16 Oct 2026 06:51:35 +0200 Subject: [PATCH] NavMap A*: terrain costs and limits, and links between surfaces MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit All off by default. terrain.slope_weight adds slope_weight * tan^2 of the slope of each NavCel per meter: gentle slopes cost little and steep ones a lot, up, down or across, so a gentle ramp is preferred to a steep one to the same height. The line-of-sight shortcut never replaces part of the path by a steeper or costlier segment. terrain.max_slope_deg, max_step, max_roughness and min_clearance make the NavCels beyond the robot's limits untraversable, from the NavMap descriptors computed once per geometry. Links between surfaces of the allowed kinds (links.types, generic, ramp and door by default) are search edges, and the path keeps their two ends. The graph cache now tells a new map by its geometry id. Signed-off-by: Francisco Martín Rico --- planners/easynav_navmap_planner/README.md | 9 + .../easynav_navmap_planner/AStarPlanner.hpp | 26 ++ .../easynav_navmap_planner/AStarPlanner.cpp | 144 ++++++- .../tests/CMakeLists.txt | 7 + .../tests/navmap_planner_terrain_tests.cpp | 375 ++++++++++++++++++ 5 files changed, 549 insertions(+), 12 deletions(-) create mode 100644 planners/easynav_navmap_planner/tests/navmap_planner_terrain_tests.cpp diff --git a/planners/easynav_navmap_planner/README.md b/planners/easynav_navmap_planner/README.md index aca9313b..164d67b5 100644 --- a/planners/easynav_navmap_planner/README.md +++ b/planners/easynav_navmap_planner/README.md @@ -57,6 +57,15 @@ All parameters are declared under the plugin namespace, i.e. | `.cost_factor` | `double` | `2.0` | Multiplicative weight for geometric distance; values > 1 increase the relative importance of distance. | | `.cost_weight` | `double` | `5.0` | Weight of the cell cost (inflation) against the path length; higher values keep paths farther from obstacles. | | `.continuous_replan` | `bool` | `true` | If true, recomputes the path whenever `NavState` updates; if false, plans once per goal. | +| `.terrain.slope_weight` | `double` | `0.0` | Extra cost per meter on a sloped NavCel: `slope_weight * tan²(slope)`, up, down or across. Gentle slopes cost little and steep ones a lot: with `20`, 5° adds 0.15 per meter, 10° 0.6, 20° 2.6 and 30° 6.7 (the flat costs `cost_factor`, 2). `0` ignores the slope. | +| `.terrain.max_slope_deg` | `double` | `90.0` | NavCels steeper than this are not traversable. | +| `.terrain.max_step` | `double` | `0.0` | `> 0`: NavCels with a larger height step around them (m) are not traversable. | +| `.terrain.max_roughness` | `double` | `0.0` | `> 0`: NavCels rougher than this (RMS distance to the local plane, m) are not traversable. | +| `.terrain.min_clearance` | `double` | `0.0` | `> 0`: NavCels with less free height above (to the next surface of the map, m) are not traversable; e.g. the robot's height. | +| `.links.enabled` | `bool` | `true` | Take the links between surfaces of the map (`navmap::SurfaceLink`). | +| `.links.types` | `list` | `[generic, ramp, door]` | Kinds of link the robot can take: `generic`, `ramp`, `stairs`, `elevator`, `door`. | + +The terrain limits use the NavMap descriptors (`navmap::compute_descriptors`), computed once per map geometry: a few tens of ms on maps of tens of thousands of NavCels (more if the map is rebuilt continuously, as in the maps manager's local mode). The line-of-sight shortcut never replaces part of the path by a steeper or costlier straight segment, and never cuts across a link. **Note:** The planner internally uses hardcoded values for `layer_name` (prefers `"inflated_obstacles"`, fallback to `"obstacles"`), `cost_axial`, and `cost_diagonal`. These are not runtime-configurable parameters. diff --git a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp index 7e38273d..f6c54040 100644 --- a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp +++ b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp @@ -43,6 +43,11 @@ namespace navmap /// favored by how the mesh is split. Then the path is shortened by line of sight: waypoints are /// skipped while the straight segment stays on traversable NavCels no costlier than the ones /// it replaces. +/// +/// Terrain (off by default): a step costs more the steeper the NavCel (`terrain.slope_weight`), and +/// NavCels beyond the robot's limits of slope, step, roughness or free height are not +/// traversable (navmap::compute_descriptors, once per map). Links between surfaces +/// (navmap::SurfaceLink) of the allowed types are taken as edges of the search. class AStarPlanner : public PlannerMethodBase { public: @@ -79,6 +84,15 @@ class AStarPlanner : public PlannerMethodBase double cost_diagonal_ {1.0}; ///< Cost multiplier for diagonal moves. std::string layer_name_; bool continuous_replan_ {true}; ///< Whether to replan the path at control frequency. + + // Terrain: all off by default + double slope_weight_ {0.0}; ///< Extra cost per meter: weight * tan^2 of the NavCel's slope + double max_slope_deg_ {90.0}; ///< NavCels steeper than this are not traversable + double max_step_ {0.0}; ///< > 0: NavCels with a larger height step are not traversable + double max_roughness_ {0.0}; ///< > 0: NavCels rougher than this (RMS, m) are not traversable + double min_clearance_ {0.0}; ///< > 0: NavCels with less free height above are not traversable + bool use_links_ {true}; ///< Take the links between surfaces of the allowed types + std::vector link_type_ok_; ///< By navmap::LinkType value nav_msgs::msg::Path current_path_; ///< Most recently computed path. geometry_msgs::msg::Pose current_goal_; ///< Current goal. @@ -94,6 +108,15 @@ class AStarPlanner : public PlannerMethodBase /// Cached centroids for each NavCel (same indexing as ::navmap::NavMap::navcels). std::vector centroids_; + /// Geometry the caches were built for (navmap::NavMap::geometry_id()). + std::uint64_t geometry_id_ {0}; + + /// Per NavCel: within the terrain limits. + std::vector terrain_ok_; + + /// Per NavCel: tan^2 of its slope (the slope cost before slope_weight). + std::vector slope_cost_; + /// A NavCel reachable from another: across an edge, or only through a shared vertex. struct Neighbor { @@ -116,6 +139,9 @@ class AStarPlanner : public PlannerMethodBase std::vector g_; std::vector<::navmap::NavCelId> parent_; + /// Which NavCels are within the terrain limits (terrain_ok_), for a new geometry. + void build_terrain(const ::navmap::NavMap & map); + /** * @brief Ensure internal caches (centroids and A* buffers) are sized for the given map. * diff --git a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp index bcaebc91..67f7a83b 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -30,6 +30,7 @@ #include "nav_msgs/msg/goals.hpp" #include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/path.hpp" +#include "navmap_core/Descriptors.hpp" #include "navmap_core/NavMap.hpp" #include "navmap_ros/conversions.hpp" @@ -74,6 +75,46 @@ void AStarPlanner::on_initialize() node->get_parameter(plugin_name + ".cost_weight", cost_weight_); node->get_parameter(plugin_name + ".continuous_replan", continuous_replan_); + // Terrain (off by default) + const std::string t = plugin_name + ".terrain."; + easynav::declare_parameter_if_absent(*node, t + "slope_weight", slope_weight_); + easynav::declare_parameter_if_absent(*node, t + "max_slope_deg", max_slope_deg_); + easynav::declare_parameter_if_absent(*node, t + "max_step", max_step_); + easynav::declare_parameter_if_absent(*node, t + "max_roughness", max_roughness_); + easynav::declare_parameter_if_absent(*node, t + "min_clearance", min_clearance_); + node->get_parameter(t + "slope_weight", slope_weight_); + node->get_parameter(t + "max_slope_deg", max_slope_deg_); + node->get_parameter(t + "max_step", max_step_); + node->get_parameter(t + "max_roughness", max_roughness_); + node->get_parameter(t + "min_clearance", min_clearance_); + if (!(slope_weight_ >= 0.0)) {slope_weight_ = 0.0;} + if (!(max_slope_deg_ > 0.0)) {max_slope_deg_ = 90.0;} + + // Links between surfaces: which kinds the robot can take + std::vector link_types{"generic", "ramp", "door"}; + easynav::declare_parameter_if_absent(*node, plugin_name + ".links.enabled", use_links_); + easynav::declare_parameter_if_absent(*node, plugin_name + ".links.types", link_types); + node->get_parameter(plugin_name + ".links.enabled", use_links_); + node->get_parameter(plugin_name + ".links.types", link_types); + const std::vector> kinds{ + {"generic", ::navmap::LinkType::GENERIC}, {"ramp", ::navmap::LinkType::RAMP}, + {"stairs", ::navmap::LinkType::STAIRS}, {"elevator", ::navmap::LinkType::ELEVATOR}, + {"door", ::navmap::LinkType::DOOR}}; + link_type_ok_.assign(kinds.size(), 0); + for (const auto & name : link_types) { + const auto it = std::find_if( + kinds.begin(), kinds.end(), [&](const auto & k) {return k.first == name;}); + if (it == kinds.end()) { + RCLCPP_WARN( + node->get_logger(), "[%s] unknown link type '%s' (generic, ramp, stairs, elevator, door)", + plugin_name.c_str(), name.c_str()); + continue; + } + link_type_ok_[static_cast(it->second)] = 1; + } + geometry_id_ = 0; // new limits: the terrain cache is rebuilt on the next plan + centroids_.clear(); + path_pub_ = node->create_publisher( node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/path", 10); @@ -303,22 +344,51 @@ static inline bool layer_exists(const ::navmap::NavMap & nm, const std::string & return static_cast(nm.layers.get(name)); } +void AStarPlanner::build_terrain(const ::navmap::NavMap & map) +{ + using ::navmap::NavCelId; + const std::size_t N = map.navcels.size(); + terrain_ok_.assign(N, 1); + slope_cost_.assign(N, 0.0f); + const float cos_max = static_cast(std::cos(max_slope_deg_ * M_PI / 180.0)); + for (NavCelId c = 0; c < N; ++c) { + const float nz = std::clamp(std::abs(map.navcels[c].normal.z()), 1e-3f, 1.0f); + if (max_slope_deg_ < 90.0 && nz < cos_max) {terrain_ok_[c] = 0;} + slope_cost_[c] = (1.0f - nz * nz) / (nz * nz); // tan^2 of the slope + } + if (max_step_ <= 0.0 && max_roughness_ <= 0.0 && min_clearance_ <= 0.0) {return;} + // Descriptors on a copy: the map in NavState is not written by the planner + ::navmap::NavMap tmp = map; + ::navmap::DescriptorParams dp; + dp.slope = false; + dp.step = max_step_ > 0.0; + dp.roughness = max_roughness_ > 0.0; + dp.clearance = min_clearance_ > 0.0; + ::navmap::compute_descriptors(tmp, dp); + for (NavCelId c = 0; c < N; ++c) { + if (dp.step && tmp.layer_get("step", c) > max_step_) {terrain_ok_[c] = 0;} + if (dp.roughness && tmp.layer_get("roughness", c) > max_roughness_) { + terrain_ok_[c] = 0; + } + if (dp.clearance && tmp.layer_get("clearance", c) < min_clearance_) { + terrain_ok_[c] = 0; + } + } +} + void AStarPlanner::ensure_graph_cache(const ::navmap::NavMap & map) { using ::navmap::NavCelId; const std::size_t N = map.navcels.size(); const std::size_t V = map.positions.x.size(); - // Same geometry as the cached one? (sizes, and the first and last centroids) - bool same = centroids_.size() == N && vertex_cels_.size() == V && neighbors_.size() == N; - if (same && N > 0) { - const auto c0 = map.navcel_centroid(0); - const auto cn = map.navcel_centroid(static_cast(N - 1)); - same = (Eigen::Vector3f{c0.x(), c0.y(), c0.z()} - centroids_.front()).norm() < 1e-6f && - (Eigen::Vector3f{cn.x(), cn.y(), cn.z()} - centroids_.back()).norm() < 1e-6f; - } + // Same geometry as the cached one? + const bool same = centroids_.size() == N && vertex_cels_.size() == V && + neighbors_.size() == N && geometry_id_ == map.geometry_id(); if (!same) { + geometry_id_ = map.geometry_id(); + build_terrain(map); centroids_.resize(N); for (NavCelId c = 0; c < static_cast(N); ++c) { const auto cc = map.navcel_centroid(c); @@ -444,12 +514,29 @@ std::vector AStarPlanner::shortcut_path( return nm.locate_navcel(p, sidx, out, bary, &hit, opts); }; - // Straight segment i -> j: on traversable NavCels no costlier than the waypoints it replaces + // A step through a link (not neighbors in the mesh) is kept as is: no shortcut across it + std::vector jump(n, 0); + for (std::size_t k = 1; k < n; ++k) { + const auto & nb = neighbors_[cels[k - 1]]; + jump[k] = cels[k] != cels[k - 1] && std::none_of( + nb.begin(), nb.end(), [&](const Neighbor & x) {return x.cid == cels[k];}); + } + + // Straight segment i -> j: on traversable NavCels no costlier, and no steeper, than the + // waypoints it replaces auto visible = [&](std::size_t i, std::size_t j) -> bool { + for (std::size_t k = i + 1; k <= j; ++k) { + if (jump[k]) {return false;} + } std::uint8_t allowed = 0; for (std::size_t k = i + 1; k < j; ++k) { allowed = std::max(allowed, occ_[cels[k]]); } + float allowed_slope = 0.0f; + for (std::size_t k = i; k <= j; ++k) { + allowed_slope = std::max(allowed_slope, slope_cost_[cels[k]]); + } + allowed_slope = allowed_slope * 1.05f + 1e-3f; const Eigen::Vector3f a = points[i], b = points[j]; const double len = (b - a).head<2>().norm(); const int steps = std::max(1, static_cast(std::ceil(len / (0.25 * cel_spacing_)))); @@ -458,8 +545,11 @@ std::vector AStarPlanner::shortcut_path( const Eigen::Vector3f p = a + (b - a) * (static_cast(s) / steps); NavCelId c; if (!locate_near(p, cur, c)) {return false;} + if (slope_weight_ > 0.0 && slope_cost_[c] > allowed_slope) {return false;} const std::uint8_t v = occ_[c]; - if (v >= navmap_ros::INSCRIBED_INFLATED_OBSTACLE || v > allowed) {return false;} + if (v >= navmap_ros::INSCRIBED_INFLATED_OBSTACLE || v > allowed || !terrain_ok_[c]) { + return false; + } cur = c; } return true; @@ -494,6 +584,10 @@ std::vector AStarPlanner::shortcut_path( std::vector out{points.front()}; for (std::size_t k = 1; k < kept.size(); ++k) { const Eigen::Vector3f a = points[kept[k - 1]], b = points[kept[k]]; + if (kept[k] == kept[k - 1] + 1 && jump[kept[k]]) { // through a link: its two ends + out.push_back(b); + continue; + } const double len = (b - a).head<2>().norm(); const int steps = std::max(1, static_cast(std::round(len / cel_spacing_))); NavCelId cur = cels[kept[k - 1]]; @@ -571,7 +665,7 @@ std::vector AStarPlanner::a_star_path( // as the costmap planner does. auto traversable = [&](NavCelId c) -> bool { const std::uint8_t v = occ_[c]; - return v < INSCRIBED_INFLATED_OBSTACLE; + return v < INSCRIBED_INFLATED_OBSTACLE && terrain_ok_[c]; }; // How far from the start inscribed cells are still traversable (m). constexpr double kEscapeDistance = 1.0; @@ -617,7 +711,12 @@ std::vector AStarPlanner::a_star_path( // Ensure the multiplier is at least 1.0 so h = euclid remains admissible. // If your cost_factor_ is already ≥ 1, this holds. Otherwise we clamp. const double cf = std::max(1.0, static_cast(cost_factor_)); - const double mult = cf + static_cast(cost_weight_) * ncost; + double mult = cf + static_cast(cost_weight_) * ncost; + if (slope_weight_ > 0.0) { + // tan^2 of the slope of the NavCel: gentle slopes cost little, steep ones a lot, and + // across a slope as much as up or down it + mult += slope_weight_ * static_cast(slope_cost_[to]); + } return base * mult; }; @@ -681,6 +780,27 @@ std::vector AStarPlanner::a_star_path( open.push(Node{v, f}); } } + + // Links to other surfaces (stairs, ramps, doors...) of the allowed kinds + if (!use_links_) {continue;} + for (const auto li : nm.links_from(u)) { + const auto & l = nm.links()[li]; + const auto kind = static_cast(l.type); + if (kind >= link_type_ok_.size() || !link_type_ok_[kind]) {continue;} + const NavCelId v = l.from == u ? l.to : l.from; + if (v >= N || !traversable(v)) {continue;} + const double ncost = normalized_cost(v); + if (!std::isfinite(ncost)) {continue;} + // Never shorter than the straight line, so the heuristic stays admissible + const double len = std::max(static_cast(l.length), euclid(u, v)); + const double cf = std::max(1.0, static_cast(cost_factor_)); + const double tentative = g_[u] + len * (cf + static_cast(cost_weight_) * ncost); + if (tentative < g_[v]) { + g_[v] = tentative; + parent_[v] = u; + open.push(Node{v, tentative + h(v, cid_goal)}); + } + } } if (!std::isfinite(g_[cid_goal])) { diff --git a/planners/easynav_navmap_planner/tests/CMakeLists.txt b/planners/easynav_navmap_planner/tests/CMakeLists.txt index e1fa449d..b95f1ce2 100644 --- a/planners/easynav_navmap_planner/tests/CMakeLists.txt +++ b/planners/easynav_navmap_planner/tests/CMakeLists.txt @@ -14,3 +14,10 @@ target_link_libraries(navmap_planner_reconfigure_tests rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle ) + +ament_add_gtest(navmap_planner_terrain_tests navmap_planner_terrain_tests.cpp) +target_link_libraries(navmap_planner_terrain_tests + ${PROJECT_NAME} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/planners/easynav_navmap_planner/tests/navmap_planner_terrain_tests.cpp b/planners/easynav_navmap_planner/tests/navmap_planner_terrain_tests.cpp new file mode 100644 index 00000000..3c5c54b9 --- /dev/null +++ b/planners/easynav_navmap_planner/tests/navmap_planner_terrain_tests.cpp @@ -0,0 +1,375 @@ +// 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. + +// A* over the terrain: slope cost, limits of slope, step, roughness and free height, and links +// between surfaces. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#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 "navmap_core/NavMap.hpp" +#include "easynav_common/types/NavState.hpp" +#include "easynav_navmap_planner/AStarPlanner.hpp" + +using ::navmap::NavCelId; +using ::navmap::NavMap; + +namespace +{ + +using Height = std::function; + +// nx x ny cells of 0.1 m from (x0, y0), at height h, as one surface; cells where keep(x, y) +void add_grid( + NavMap & nm, int nx, int ny, float x0, float y0, const Height & h, + const std::function & keep = nullptr) +{ + const float r = 0.1f; + const std::size_t s = nm.create_surface("map"); + const auto base = static_cast(nm.positions.size()); + for (int j = 0; j <= ny; ++j) { + for (int i = 0; i <= nx; ++i) { + const float x = x0 + i * r, y = y0 + j * r; + nm.add_vertex({x, y, h(x, y)}); + } + } + auto v = [&](int i, int j) {return base + static_cast(j * (nx + 1) + i);}; + for (int j = 0; j < ny; ++j) { + for (int i = 0; i < nx; ++i) { + if (keep && !keep(x0 + (i + 0.5f) * r, y0 + (j + 0.5f) * r)) {continue;} + nm.add_navcel_to_surface(s, nm.add_navcel(v(i, j), v(i + 1, j), v(i + 1, j + 1))); + nm.add_navcel_to_surface(s, nm.add_navcel(v(i, j), v(i + 1, j + 1), v(i, j + 1))); + } + } +} + +float flat(float, float) {return 0.0f;} + +class TerrainPlannerTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) {rclcpp::init(0, nullptr);} + } + + void make(const std::vector & params = {}) + { + static int count = 0; + rclcpp::NodeOptions opts; + for (const auto & p : params) { + opts.append_parameter_override("planner." + p.get_name(), p.get_parameter_value()); + } + node_ = rclcpp_lifecycle::LifecycleNode::make_shared( + "navmap_terrain_test_" + std::to_string(count++), opts); + planner_ = std::make_shared(); + planner_->initialize(node_, "planner"); + } + + void set_map(NavMap nm) + { + nm.rebuild_geometry_accels(); + nm.add_layer("obstacles", "", "", 0); + nav_state_.set("map.navmap", nm); + } + + nav_msgs::msg::Path plan(float sx, float sy, float sz, float gx, float gy, float gz) + { + nav_msgs::msg::Odometry robot; + robot.header.frame_id = "map"; + robot.pose.pose.position.x = sx; + robot.pose.pose.position.y = sy; + robot.pose.pose.position.z = sz; + robot.pose.pose.orientation.w = 1.0; + nav_state_.set("robot_pose", robot); + nav_msgs::msg::Goals goals; + goals.header.frame_id = "map"; + goals.header.stamp = node_->now(); + geometry_msgs::msg::PoseStamped g; + g.header = goals.header; + g.pose.position.x = gx; + g.pose.position.y = gy; + g.pose.position.z = gz; + g.pose.orientation.w = 1.0; + goals.goals.push_back(g); + nav_state_.set("goals", goals); + planner_->update(nav_state_); + return nav_state_.get("path"); + } + + static double max_abs_dy(const nav_msgs::msg::Path & p, double y) + { + double m = 0.0; + for (const auto & ps : p.poses) {m = std::max(m, std::abs(ps.pose.position.y - y));} + return m; + } + + static double max_z(const nav_msgs::msg::Path & p) + { + double m = -1e9; + for (const auto & ps : p.poses) {m = std::max(m, ps.pose.position.z);} + return m; + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + std::shared_ptr planner_; + easynav::NavState nav_state_; +}; + +// 6 x 3 m with a 1 m high bump in the middle (radius about 0.8 m) +float hill(float x, float y) +{ + const float d2 = (x - 3.0f) * (x - 3.0f) + (y - 1.5f) * (y - 1.5f); + return std::exp(-d2 / 0.35f); +} + +// A ridge across the whole width at x = 3: up 0.6 m at 40 deg, a plateau, and down again +float ridge(float x, float) +{ + const float k = std::tan(40.0f * static_cast(M_PI) / 180.0f); + return std::clamp(std::min((x - 2.5f) * k, (3.8f - x) * k), 0.0f, 0.6f); +} + +} // namespace + +TEST_F(TerrainPlannerTest, ByDefaultTheTerrainIsIgnored) +{ + make(); + NavMap nm; + add_grid(nm, 60, 30, 0.0f, 0.0f, hill); + set_map(nm); + const auto p = plan(0.55f, 1.55f, 0.0f, 5.45f, 1.55f, 0.0f); + ASSERT_FALSE(p.poses.empty()); + EXPECT_LT(max_abs_dy(p, 1.55), 0.3); // straight over the hill + EXPECT_GT(max_z(p), 0.8); +} + +TEST_F(TerrainPlannerTest, ASlopeWeightGoesAroundAHill) +{ + make({rclcpp::Parameter("terrain.slope_weight", 6.0)}); + NavMap nm; + add_grid(nm, 60, 30, 0.0f, 0.0f, hill); + set_map(nm); + const auto p = plan(0.55f, 1.55f, 0.0f, 5.45f, 1.55f, 0.0f); + ASSERT_FALSE(p.poses.empty()); + EXPECT_GT(max_abs_dy(p, 1.55), 0.6); // around it + EXPECT_LT(max_z(p), 0.5); +} + +TEST_F(TerrainPlannerTest, AGentleRampIsPreferredToASteepOneToTheSameHeight) +{ + // A 0.5 m plateau at x > 4: reached by a steep ramp (30 deg) for y < 1.5 and by a gentle one + // (8 deg) for y > 1.5. Climbing the same height, the gentle one must win (not only less climb) + const Height ramps = [](float x, float y) { + const float deg = y < 1.5f ? 30.0f : 8.0f; + const float run = 0.5f / std::tan(deg * static_cast(M_PI) / 180.0f); + return 0.5f * std::clamp((x - (4.0f - run)) / run, 0.0f, 1.0f); + }; + NavMap nm; + add_grid(nm, 60, 30, 0.0f, 0.0f, ramps); + // Where the path climbs: on the steep ramp (y < 1.5) or on the gentle one + auto climbs_gently = [&](const nav_msgs::msg::Path & p) { + bool any = false, gentle = true; + for (const auto & ps : p.poses) { + const auto & q = ps.pose.position; + if (q.z > 0.1 && q.z < 0.4) { + any = true; + gentle = gentle && q.y > 1.5; + } + } + return any && gentle; + }; + make(); + set_map(nm); + const auto direct = plan(0.55f, 0.55f, 0.0f, 5.45f, 0.55f, 0.5f); + ASSERT_FALSE(direct.poses.empty()); + EXPECT_FALSE(climbs_gently(direct)); // without a weight: straight up the steep one + make({rclcpp::Parameter("terrain.slope_weight", 50.0)}); + set_map(nm); + const auto gentle = plan(0.55f, 0.55f, 0.0f, 5.45f, 0.55f, 0.5f); + ASSERT_FALSE(gentle.poses.empty()); + EXPECT_TRUE(climbs_gently(gentle)); +} + +TEST_F(TerrainPlannerTest, TooSteepCellsAreNotTraversable) +{ + NavMap nm; + add_grid(nm, 60, 30, 0.0f, 0.0f, ridge); + for (const double max_deg : {30.0, 45.0}) { + make({rclcpp::Parameter("terrain.max_slope_deg", max_deg)}); + set_map(nm); + const auto p = plan(0.55f, 1.55f, 0.0f, 5.45f, 1.55f, 0.0f); + EXPECT_EQ(p.poses.empty(), max_deg < 40.0) << max_deg; + } + // The goal itself too steep: no plan + make({rclcpp::Parameter("terrain.max_slope_deg", 30.0)}); + set_map(nm); + EXPECT_TRUE(plan(0.55f, 1.55f, 0.0f, 2.85f, 1.55f, 0.25f).poses.empty()); +} + +TEST_F(TerrainPlannerTest, ACurbHigherThanMaxStepIsAvoided) +{ + // Up 0.15 m at x = 3: a curb (over 0.1 m) for y < 2, a gentle ramp (over 1.5 m) for y > 2 + const Height curb = [](float x, float y) { + const float run = y < 2.0f ? 0.1f : 1.5f; + return 0.15f * std::clamp((x - 3.0f) / run, 0.0f, 1.0f); + }; + NavMap nm; + add_grid(nm, 60, 30, 0.0f, 0.0f, curb); + make({rclcpp::Parameter("terrain.max_step", 0.1)}); + set_map(nm); + const auto p = plan(0.55f, 0.55f, 0.0f, 5.45f, 0.55f, 0.15f); + ASSERT_FALSE(p.poses.empty()); + // Goes up by the ramp, never over the curb + for (std::size_t i = 1; i < p.poses.size(); ++i) { + const auto & a = p.poses[i - 1].pose.position, & b = p.poses[i].pose.position; + if ((a.x - 3.05) * (b.x - 3.05) <= 0.0) {EXPECT_GT(std::min(a.y, b.y), 2.1) << i;} + } + // Without the limit, straight across + make(); + set_map(nm); + EXPECT_LT(max_abs_dy(plan(0.55f, 0.55f, 0.0f, 5.45f, 0.55f, 0.15f), 0.55), 0.3); +} + +TEST_F(TerrainPlannerTest, LowCeilingsAreAvoidedWithMinClearance) +{ + // A 0.6 m high table over x in [2, 4], y in [0, 2] (another surface, not reachable) + NavMap nm; + add_grid(nm, 60, 30, 0.0f, 0.0f, flat); + add_grid(nm, 20, 20, 2.0f, 0.0f, [](float, float) {return 0.6f;}); + make({rclcpp::Parameter("terrain.min_clearance", 1.0)}); + set_map(nm); + const auto p = plan(0.55f, 0.95f, 0.0f, 5.45f, 0.95f, 0.0f); + ASSERT_FALSE(p.poses.empty()); + for (const auto & ps : p.poses) { + const auto & q = ps.pose.position; + EXPECT_FALSE(q.x > 2.1 && q.x < 3.9 && q.y < 1.9) << q.x << " " << q.y; + EXPECT_LT(q.z, 0.1); // never onto the table + } + make(); // a robot lower than the table goes under it + set_map(nm); + EXPECT_LT(max_abs_dy(plan(0.55f, 0.95f, 0.0f, 5.45f, 0.95f, 0.0f), 0.95), 0.3); +} + +TEST_F(TerrainPlannerTest, RoughPatchesAreAvoidedWithMaxRoughness) +{ + std::mt19937 rng(5); + std::normal_distribution N(0.0f, 0.05f); + std::vector noise(61 * 31); + for (auto & v : noise) { + v = N(rng); + } + // Rubble over x in [2, 4], y in [0, 2] + const Height rubble = [&](float x, float y) { + const int i = static_cast(std::lround(x * 10.0f)), + j = static_cast(std::lround(y * 10.0f)); + return x > 2.0f && x < 4.0f && y < 2.0f ? noise[j * 61 + i] : 0.0f; + }; + NavMap nm; + add_grid(nm, 60, 30, 0.0f, 0.0f, rubble); + make({rclcpp::Parameter("terrain.max_roughness", 0.02)}); + set_map(nm); + const auto p = plan(0.55f, 0.95f, 0.0f, 5.45f, 0.95f, 0.0f); + ASSERT_FALSE(p.poses.empty()); + for (const auto & ps : p.poses) { + const auto & q = ps.pose.position; + EXPECT_FALSE(q.x > 2.3 && q.x < 3.7 && q.y < 1.7) << q.x << " " << q.y; + } +} + +TEST_F(TerrainPlannerTest, LinksJoinSurfacesByTheirKind) +{ + // Ground floor x in [0, 2] and an upper one 3 m above at x in [3, 5]; a link between them + NavMap base; + add_grid(base, 20, 20, 0.0f, 0.0f, flat); + add_grid(base, 20, 20, 3.0f, 0.0f, [](float, float) {return 3.0f;}); + base.rebuild_geometry_accels(); + std::size_t s; + NavCelId a, b; + Eigen::Vector3f bary; + ASSERT_TRUE(base.locate_navcel(Eigen::Vector3f(1.93f, 1.02f, 0.1f), s, a, bary, nullptr)); + ASSERT_TRUE(base.locate_navcel(Eigen::Vector3f(3.07f, 1.02f, 3.1f), s, b, bary, nullptr)); + + for (const auto kind : {::navmap::LinkType::RAMP, ::navmap::LinkType::STAIRS}) { + NavMap nm = base; + nm.add_link(a, b, kind, 4.0f); + // Default kinds: generic, ramp and door + make(); + set_map(nm); + const auto p = plan(0.55f, 1.05f, 0.0f, 4.45f, 1.05f, 3.0f); + EXPECT_EQ(p.poses.empty(), kind == ::navmap::LinkType::STAIRS); + if (!p.poses.empty()) { + EXPECT_NEAR(p.poses.back().pose.position.z, 3.0, 0.05); + // No waypoint in the air between the floors: the link is taken from end to end + for (const auto & ps : p.poses) { + const double z = ps.pose.position.z; + EXPECT_TRUE(z<0.05 || z>2.95) << z; + } + } + } + // Stairs, when the robot can take them; and no links at all + NavMap nm = base; + nm.add_link(a, b, ::navmap::LinkType::STAIRS, 4.0f); + make({rclcpp::Parameter("links.types", std::vector{"stairs", "nonsense"})}); + set_map(nm); + EXPECT_FALSE(plan(0.55f, 1.05f, 0.0f, 4.45f, 1.05f, 3.0f).poses.empty()); + make({rclcpp::Parameter("links.enabled", false), + rclcpp::Parameter("links.types", std::vector{"stairs"})}); + set_map(nm); + EXPECT_TRUE(plan(0.55f, 1.05f, 0.0f, 4.45f, 1.05f, 3.0f).poses.empty()); + // One-way link: down only + NavMap down = base; + down.add_link(b, a, ::navmap::LinkType::RAMP, 4.0f, false); + make(); + set_map(down); + EXPECT_TRUE(plan(0.55f, 1.05f, 0.0f, 4.45f, 1.05f, 3.0f).poses.empty()); + EXPECT_FALSE(plan(4.45f, 1.05f, 3.0f, 0.55f, 1.05f, 0.0f).poses.empty()); +} + +TEST_F(TerrainPlannerTest, NewLimitsAndNewTerrainApplyOnTheNextPlan) +{ + NavMap nm; + add_grid(nm, 60, 30, 0.0f, 0.0f, ridge); + make({rclcpp::Parameter("terrain.max_slope_deg", 30.0)}); + set_map(nm); + EXPECT_TRUE(plan(0.55f, 1.55f, 0.0f, 5.45f, 1.55f, 0.0f).poses.empty()); + // Reconfigured: initialize() again with another limit + node_->set_parameter(rclcpp::Parameter("planner.terrain.max_slope_deg", 60.0)); + planner_->initialize(node_, "planner"); + EXPECT_FALSE(plan(0.55f, 1.55f, 0.0f, 5.45f, 1.55f, 0.0f).poses.empty()); + // Same size, flat now: the terrain is recomputed for the new geometry + node_->set_parameter(rclcpp::Parameter("planner.terrain.max_slope_deg", 30.0)); + planner_->initialize(node_, "planner"); + NavMap flat_nm; + add_grid(flat_nm, 60, 30, 0.0f, 0.0f, flat); + set_map(flat_nm); + EXPECT_FALSE(plan(0.55f, 1.55f, 0.0f, 5.45f, 1.55f, 0.0f).poses.empty()); + set_map(nm); + EXPECT_TRUE(plan(0.55f, 1.55f, 0.0f, 5.45f, 1.55f, 0.0f).poses.empty()); +}