Fix CI on humble, jazzy and kilted

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

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

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

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

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<std::pair<double, double>>{
     {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::size_t>(std::round(p * static_cast<double>(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) {

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

localizers/easynav_navmap_localizer/tests/navmap_localizer_threads_tests.cpp

@@ -195,7 +195,7 @@ TEST_F(NavmapAmclThreadsTest, ReseededPosesAreAppliedByTheRtThread)
   std::vector<tf2::Transform> before;
   for (const auto & p : amcl_->particles_) {
     before.push_back(p.pose);
-                                                                     }
+  }
 
   amcl_->reseed();
   ASSERT_TRUE(amcl_->pending_.load());

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<NavCelId> differ;

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<std::uint8_t>("inflated_obstacles", cid),
+  EXPECT_NEAR(
+    nm.layer_get<std::uint8_t>("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<std::uint8_t>("obstacles", cell_in(nm, 3.03f, 3.03f, slope * 3.03f),
+    nm.layer_set<std::uint8_t>(
+      "obstacles", cell_in(nm, 3.03f, 3.03f, slope * 3.03f),
       LETHAL_OBSTACLE);
     easynav::navmap::InflationFilter f;
     ASSERT_TRUE(

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

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

planners/easynav_navmap_planner/tests/navmap_planner_terrain_tests.cpp

@@ -43,7 +43,7 @@ using ::navmap::NavMap;
 namespace
 {
 
-using Height = std::function<float(float, float)>;
+using Height = std::function<float (float, float)>;
 
 // 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<float> 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<int>(std::lround(x * 10.0f)),
@@ -339,7 +339,8 @@ TEST_F(TerrainPlannerTest, LinksJoinSurfacesByTheirKind)
   make({rclcpp::Parameter("links.types", std::vector<std::string>{"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<std::string>{"stairs"})});
   set_map(nm);
   EXPECT_TRUE(plan(0.55f, 1.05f, 0.0f, 4.45f, 1.05f, 3.0f).poses.empty());