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()); +}