From 19281aaa01ffb8c5668a28025288509f9ef775f6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 23 Oct 2026 16:06:47 +0200 Subject: [PATCH] Fix CI on humble, jazzy and kilted MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Refs #141 Signed-off-by: Francisco Martín Rico --- .../regulated_pp_goal_approach_tests.cpp | 3 ++- .../AMCLLocalizer.cpp | 5 ++-- .../tests/navmap_localizer_correct_tests.cpp | 15 ++++++++---- .../tests/navmap_localizer_predict_tests.cpp | 3 ++- .../tests/navmap_localizer_recovery_tests.cpp | 23 +++++++++++-------- .../tests/navmap_localizer_slip_tests.cpp | 9 +++++--- .../tests/navmap_localizer_threads_tests.cpp | 2 +- .../navmap_incremental_filters_tests.cpp | 6 +++-- .../tests/navmap_inflation_filter_tests.cpp | 6 +++-- .../tests/navmap_local_mode_tests.cpp | 12 ++++++---- .../tests/navmap_obstacle_memory_tests.cpp | 6 ++--- .../tests/navmap_planner_terrain_tests.cpp | 7 +++--- 12 files changed, 60 insertions(+), 37 deletions(-) diff --git a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_goal_approach_tests.cpp b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_goal_approach_tests.cpp index ad1f1e9f..2d4c63cd 100644 --- a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_goal_approach_tests.cpp +++ b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_goal_approach_tests.cpp @@ -61,7 +61,8 @@ class RppGoalApproachTest : public ::testing::Test // The playgrounds' tolerances: the controller's own equal to the system's. void SetUp() override { - make({rclcpp::Parameter("rpp.xy_goal_tolerance", 0.3), + make( + {rclcpp::Parameter("rpp.xy_goal_tolerance", 0.3), rclcpp::Parameter("rpp.yaw_goal_tolerance", 0.15)}); } diff --git a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp index 64129d6b..ec87e9d3 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -836,8 +836,9 @@ void AMCLLocalizer::predict(NavState & nav_state) } double rot_len = std::abs(yaw); // How far the robot has gone, for reseed() (single writer: this thread) - travelled_.store(travelled_.load(std::memory_order_relaxed) + trans_len, - std::memory_order_relaxed); + travelled_.store( + travelled_.load(std::memory_order_relaxed) + trans_len, + std::memory_order_relaxed); turned_.store(turned_.load(std::memory_order_relaxed) + rot_len, std::memory_order_relaxed); // Zero-mean noise; a zero deviation (e.g. the robot still) is no noise: std::normal_distribution diff --git a/localizers/easynav_navmap_localizer/tests/navmap_localizer_correct_tests.cpp b/localizers/easynav_navmap_localizer/tests/navmap_localizer_correct_tests.cpp index b612b514..50adac0c 100644 --- a/localizers/easynav_navmap_localizer/tests/navmap_localizer_correct_tests.cpp +++ b/localizers/easynav_navmap_localizer/tests/navmap_localizer_correct_tests.cpp @@ -186,10 +186,12 @@ class NavmapAmclCorrectTest : public ::testing::Test TEST_F(NavmapAmclCorrectTest, ThePoseThatSeesTheCloudScoresBest) { - make({rclcpp::Parameter("loc.min_point_height", 0.3), + make( + {rclcpp::Parameter("loc.min_point_height", 0.3), rclcpp::Parameter("loc.max_point_height", 1.8)}); nav_state_.set("laser", see(2.0, 3.5, 0.3, now_)); - place({{2.0, 3.5, 0.3}, {2.5, 3.5, 0.3}, {2.0, 3.0, 0.3}, {1.6, 3.8, 0.3}, {2.0, 3.5, 0.6}, + place( + {{2.0, 3.5, 0.3}, {2.5, 3.5, 0.3}, {2.0, 3.0, 0.3}, {1.6, 3.8, 0.3}, {2.0, 3.5, 0.6}, {2.0, 3.5, 0.0}}); amcl_->correct(nav_state_); EXPECT_GT(score(0), 0.6); // the map's probability where the points fall @@ -201,7 +203,8 @@ TEST_F(NavmapAmclCorrectTest, ThePoseThatSeesTheCloudScoresBest) TEST_F(NavmapAmclCorrectTest, OnlyPointsWithinTheHeightBandAreScored) { // Floor and ceiling fit any particle under them: out of the band, they are not scored - make({rclcpp::Parameter("loc.min_point_height", 0.3), + make( + {rclcpp::Parameter("loc.min_point_height", 0.3), rclcpp::Parameter("loc.max_point_height", 1.8)}); auto cloud = see(3.0, 3.0, 0.0, now_); std::size_t in_band = 0; @@ -229,13 +232,15 @@ TEST_F(NavmapAmclCorrectTest, ASensorNearTheFloorIsNotBlindedByIt) // occlusion check many look blocked and the true pose scores far less; without it, the true // pose is the best. sensor_z_ = 0.05; - make({rclcpp::Parameter("loc.min_point_height", 0.3), + make( + {rclcpp::Parameter("loc.min_point_height", 0.3), rclcpp::Parameter("loc.ray_occlusion", true)}); nav_state_.set("laser", see(3.0, 2.5, 0.2, now_)); place({{3.0, 2.5, 0.2}, {3.4, 2.5, 0.2}}); amcl_->correct(nav_state_); const double occluded = score(0); - make({rclcpp::Parameter("loc.min_point_height", 0.3), + make( + {rclcpp::Parameter("loc.min_point_height", 0.3), rclcpp::Parameter("loc.ray_occlusion", false)}); nav_state_.set("laser", see(3.0, 2.5, 0.2, now_)); place({{3.0, 2.5, 0.2}, {3.4, 2.5, 0.2}}); diff --git a/localizers/easynav_navmap_localizer/tests/navmap_localizer_predict_tests.cpp b/localizers/easynav_navmap_localizer/tests/navmap_localizer_predict_tests.cpp index cd9f1e1d..980ec587 100644 --- a/localizers/easynav_navmap_localizer/tests/navmap_localizer_predict_tests.cpp +++ b/localizers/easynav_navmap_localizer/tests/navmap_localizer_predict_tests.cpp @@ -307,7 +307,8 @@ TEST_F(NavmapAmclPredictTest, WithoutImuOnARampTheEstimateFollowsTheSurface) tf2::Matrix3x3(amcl_->getEstimatedPose().getRotation()).getRPY(r, p, y); EXPECT_NEAR(std::abs(p), std::atan(0.25), 0.02); // facing up the ramp EXPECT_NEAR(r, 0.0, 0.02); - EXPECT_NEAR(amcl_->getEstimatedPose().getOrigin().z(), + EXPECT_NEAR( + amcl_->getEstimatedPose().getOrigin().z(), 0.25 * amcl_->getEstimatedPose().getOrigin().x(), 0.02); } diff --git a/localizers/easynav_navmap_localizer/tests/navmap_localizer_recovery_tests.cpp b/localizers/easynav_navmap_localizer/tests/navmap_localizer_recovery_tests.cpp index c4fef6d5..75273af0 100644 --- a/localizers/easynav_navmap_localizer/tests/navmap_localizer_recovery_tests.cpp +++ b/localizers/easynav_navmap_localizer/tests/navmap_localizer_recovery_tests.cpp @@ -175,7 +175,7 @@ TEST_F(NavmapAmclRecoveryTest, ADropSpreadsInProportionAndFadesWhenItComesBack) make(); for (int i = 0; i < 100; ++i) { amcl_->update_recovery_(0.8); - } + } double last = 0.0; for (int i = 0; i < 10; ++i) { amcl_->update_recovery_(0.2); @@ -188,7 +188,7 @@ TEST_F(NavmapAmclRecoveryTest, ADropSpreadsInProportionAndFadesWhenItComesBack) EXPECT_GT(last, 0.4); for (int i = 0; i < 60; ++i) { amcl_->update_recovery_(0.8); - } + } EXPECT_LT(amcl_->recovery_probability_(), 0.01); } @@ -218,14 +218,15 @@ TEST_F(NavmapAmclRecoveryTest, TheLikelihoodIsThatOfTheBestOnes) TEST_F(NavmapAmclRecoveryTest, ZeroAlphasTurnItOff) { - make({rclcpp::Parameter("loc.recovery_alpha_slow", 0.0), + make( + {rclcpp::Parameter("loc.recovery_alpha_slow", 0.0), rclcpp::Parameter("loc.recovery_alpha_fast", 0.0)}); for (int i = 0; i < 100; ++i) { amcl_->update_recovery_(0.8); - } + } for (int i = 0; i < 20; ++i) { amcl_->update_recovery_(0.01); - } + } EXPECT_DOUBLE_EQ(amcl_->recovery_probability_(), 0.0); } @@ -234,7 +235,8 @@ TEST_F(NavmapAmclRecoveryTest, InvalidAlphasTurnItOff) for (auto [slow, fast] : std::vector>{ {0.1, 0.1}, {0.2, 0.1}, {-0.1, 0.1}, {0.001, 1.5}}) { - make({rclcpp::Parameter("loc.recovery_alpha_slow", slow), + make( + {rclcpp::Parameter("loc.recovery_alpha_slow", slow), rclcpp::Parameter("loc.recovery_alpha_fast", fast)}); for (int i = 0; i < 100; ++i) { amcl_->update_recovery_(0.8); @@ -252,7 +254,7 @@ TEST_F(NavmapAmclRecoveryTest, WithoutADropReseedKeepsToTheBest) cluster(); for (int i = 0; i < 50; ++i) { amcl_->update_recovery_(0.8); - } + } const auto before = origins(); amcl_->reseed(); amcl_->apply_pending_poses_(); @@ -267,10 +269,10 @@ TEST_F(NavmapAmclRecoveryTest, ADropReplacesExactlyTheWorstOnes) cluster(); for (int i = 0; i < 100; ++i) { amcl_->update_recovery_(0.8); - } + } for (int i = 0; i < 5; ++i) { amcl_->update_recovery_(0.2); - } + } const double p = amcl_->recovery_probability_(); ASSERT_GT(p, 0.0); const auto k = static_cast(std::round(p * static_cast(N - N_TOP))); @@ -306,7 +308,8 @@ TEST_F(NavmapAmclRecoveryTest, ConvergedOneMetreOffItComesBack) // Localized at (2, 2), then the robot is really 1 m away: the cluster only crawls without // recovery, and jumps back with it for (bool recovery : {false, true}) { - make({rclcpp::Parameter("loc.recovery_alpha_fast", recovery ? 0.1 : 0.0), + make( + {rclcpp::Parameter("loc.recovery_alpha_fast", recovery ? 0.1 : 0.0), rclcpp::Parameter("loc.recovery_alpha_slow", recovery ? 0.001 : 0.0)}); const tf2::Vector3 start(2.0, 2.0, 0.0), truth(3.0, 2.0, 0.0); for (int i = 0; i < 30; ++i) { diff --git a/localizers/easynav_navmap_localizer/tests/navmap_localizer_slip_tests.cpp b/localizers/easynav_navmap_localizer/tests/navmap_localizer_slip_tests.cpp index b08a3cbe..bfb7c698 100644 --- a/localizers/easynav_navmap_localizer/tests/navmap_localizer_slip_tests.cpp +++ b/localizers/easynav_navmap_localizer/tests/navmap_localizer_slip_tests.cpp @@ -90,7 +90,8 @@ class NavmapAmclSlipTest : public ::testing::Test void move(double dx) { amcl_->last_odom_ = amcl_->odom_; - amcl_->odom_ = amcl_->odom_ * tf2::Transform(tf2::Quaternion::getIdentity(), + amcl_->odom_ = amcl_->odom_ * tf2::Transform( + tf2::Quaternion::getIdentity(), tf2::Vector3(dx, 0.0, 0.0)); amcl_->predict(nav_state_); } @@ -175,7 +176,8 @@ TEST_F(NavmapAmclSlipTest, ReseededParticlesTakeTheirOdometryScaleAlong) x0.push_back(p.pose.getOrigin().x()); k.push_back(p.odom_scale); } - make({rclcpp::Parameter("loc.odom_slip_fraction", 1.0), + make( + {rclcpp::Parameter("loc.odom_slip_fraction", 1.0), rclcpp::Parameter("loc.noise_translation", 1e-9)}); // (a new filter: its particles at the same places and scales) amcl_->particles_.resize(x0.size()); @@ -196,7 +198,8 @@ TEST_F(NavmapAmclSlipTest, ARobotWhoseWheelsSlipIsStillFollowed) // the slip hypothesis the estimate stays with it; without it, it goes on with the odometry. for (double slip : {0.0, 0.1}) { // (without the slip hypothesis the scale stays fixed: no noise either) - make({rclcpp::Parameter("loc.odom_slip_fraction", slip), + make( + {rclcpp::Parameter("loc.odom_slip_fraction", slip), rclcpp::Parameter("loc.odom_scale_noise", slip > 0.0 ? 0.02 : 0.0)}); double x = 2.0; for (int i = 0; i < 20; ++i) { // 0.1 m a cycle, really moving diff --git a/localizers/easynav_navmap_localizer/tests/navmap_localizer_threads_tests.cpp b/localizers/easynav_navmap_localizer/tests/navmap_localizer_threads_tests.cpp index 7a2ea6f6..3d4328a9 100644 --- a/localizers/easynav_navmap_localizer/tests/navmap_localizer_threads_tests.cpp +++ b/localizers/easynav_navmap_localizer/tests/navmap_localizer_threads_tests.cpp @@ -195,7 +195,7 @@ TEST_F(NavmapAmclThreadsTest, ReseededPosesAreAppliedByTheRtThread) std::vector before; for (const auto & p : amcl_->particles_) { before.push_back(p.pose); - } + } amcl_->reseed(); ASSERT_TRUE(amcl_->pending_.load()); diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_incremental_filters_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_incremental_filters_tests.cpp index 746e5239..2e4595df 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/navmap_incremental_filters_tests.cpp +++ b/maps_managers/easynav_navmap_maps_manager/tests/navmap_incremental_filters_tests.cpp @@ -241,8 +241,10 @@ TEST(IncrementalInflation, MatchesAFullRecomputationAfterEveryChange) filter.update(nav_state); NavMap ref = *map; - ASSERT_TRUE(filter.inflate_layer_u8(ref, "obstacles", "ref", 0.6f, 3.0f, filter.inscribed(), - true)); + ASSERT_TRUE( + filter.inflate_layer_u8( + ref, "obstacles", "ref", 0.6f, 3.0f, filter.inscribed(), + true)); const auto inc = map->layers.get("inflated_obstacles"); ASSERT_TRUE(inc); std::set differ; diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_inflation_filter_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_inflation_filter_tests.cpp index 121622da..c2c0f9d8 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/navmap_inflation_filter_tests.cpp +++ b/maps_managers/easynav_navmap_maps_manager/tests/navmap_inflation_filter_tests.cpp @@ -238,7 +238,8 @@ TEST(NavmapInflationBorders, BorderCellsAreNotLethalAndCostByTheirDistance) // Cell centroid ~0.5 m from the x = 0 border: between inscribed and free. const auto cid = cell_in(nm, 0.53f, 2.03f, 0.0f); const float d = nm.navcel_centroid(cid).x(); - EXPECT_NEAR(nm.layer_get("inflated_obstacles", cid), + EXPECT_NEAR( + nm.layer_get("inflated_obstacles", cid), expected_cost(d, 0.3f, 1.0f, 3.0f), 1); EXPECT_EQ(cost_in(nm, 2.0f, 2.0f, 0.0f), FREE_SPACE); // 2 m from every border } @@ -277,7 +278,8 @@ TEST(NavmapInflationBorders, SlopeDistanceIsMeasuredAlongTheSurface) ::navmap::NavMap nm; add_grid(nm, 60, 60, 0.1f, 0.0f, 0.0f, [slope](float x, float) {return slope * x;}); finish(nm); - nm.layer_set("obstacles", cell_in(nm, 3.03f, 3.03f, slope * 3.03f), + nm.layer_set( + "obstacles", cell_in(nm, 3.03f, 3.03f, slope * 3.03f), LETHAL_OBSTACLE); easynav::navmap::InflationFilter f; ASSERT_TRUE( diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_local_mode_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_local_mode_tests.cpp index 36f3d80e..aaa2af47 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/navmap_local_mode_tests.cpp +++ b/maps_managers/easynav_navmap_maps_manager/tests/navmap_local_mode_tests.cpp @@ -54,10 +54,11 @@ class NavMapLocalModeTest : public ::testing::Test // What the robot sees, in its own frame: flat ground 10 x 10 m around it, a 0.6 m box 2 m // ahead, and a far wall 20 m away (out of the window). Stamped dt s from now. - void see(double dt) + void see(double dt, const std::string & frame = "") { easynav::PointPerception p; - p.frame_id = easynav::RTTFBuffer::getInstance()->get_tf_info().robot_frame; + p.frame_id = frame.empty() ? easynav::RTTFBuffer::getInstance()->get_tf_info().robot_frame : + frame; p.stamp = node_->now() + rclcpp::Duration::from_seconds(dt); p.valid = true; for (float x = -5.0f; x <= 5.0f; x += 0.05f) { @@ -165,8 +166,11 @@ TEST_F(NavMapLocalModeTest, DisabledByDefault) TEST_F(NavMapLocalModeTest, SafeWithoutOdometry) { - auto on = make(true); // no odom -> robot transform at all - see(0.0); + // A frame with no transform at all: before Rolling, tf2's clear() keeps the static + // odom -> robot transforms of the previous tests + node_->declare_parameter("navmap.local.frame", "robot_without_odom"); + auto on = make(true); + see(0.0, "robot_without_odom"); EXPECT_NO_THROW(on->update(nav_state_)); EXPECT_TRUE(nav_state_.get<::navmap::NavMap>("map.navmap").navcels.empty()); } diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_obstacle_memory_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_obstacle_memory_tests.cpp index 8ab5aa4b..63f44b3d 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/navmap_obstacle_memory_tests.cpp +++ b/maps_managers/easynav_navmap_maps_manager/tests/navmap_obstacle_memory_tests.cpp @@ -153,7 +153,7 @@ TEST_F(ObstacleMemoryTest, AnObstacleOutOfSightIsRemembered) EXPECT_EQ(nm.layer_type_name("obstacles_logodds"), "int16"); for (int i = 0; i < 5; ++i) { cycle({}); - } // nothing seen, no ray crosses it + } // nothing seen, no ray crosses it EXPECT_EQ(at(3.03f, 2.03f), LETHAL); EXPECT_EQ(at(1.03f, 1.03f), FREE); EXPECT_EQ(at(3.55f, 2.05f), LETHAL); // static @@ -186,7 +186,7 @@ TEST_F(ObstacleMemoryTest, SensorsFillInUnknownButNeverEraseTheStaticMap) // The ground seen on the unknown strip, and beyond the static obstacle for (int i = 0; i < 5; ++i) { cycle({}, {{3.86f, 2.02f}, {3.16f, 1.12f}}); - } + } EXPECT_EQ(at(3.16f, 1.12f), FREE); EXPECT_EQ(at(3.55f, 2.05f), LETHAL); // the static obstacle stays EXPECT_EQ(at(3.86f, 2.02f), FREE); @@ -223,7 +223,7 @@ TEST_F(ObstacleMemoryTest, WithoutClearingRaysChangeNothing) cycle({{3.03f, 2.03f}}); for (int i = 0; i < 5; ++i) { cycle({}, {{3.53f, 2.03f}}); - } + } EXPECT_EQ(at(3.03f, 2.03f), LETHAL); } diff --git a/planners/easynav_navmap_planner/tests/navmap_planner_terrain_tests.cpp b/planners/easynav_navmap_planner/tests/navmap_planner_terrain_tests.cpp index 3c5c54b9..b037893c 100644 --- a/planners/easynav_navmap_planner/tests/navmap_planner_terrain_tests.cpp +++ b/planners/easynav_navmap_planner/tests/navmap_planner_terrain_tests.cpp @@ -43,7 +43,7 @@ using ::navmap::NavMap; namespace { -using Height = std::function; +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( @@ -284,7 +284,7 @@ TEST_F(TerrainPlannerTest, RoughPatchesAreAvoidedWithMaxRoughness) 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)), @@ -339,7 +339,8 @@ TEST_F(TerrainPlannerTest, LinksJoinSurfacesByTheirKind) 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), + 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());