Add ROS 2 Jazzy to CI/CD and dev workflows

.github/workflows/ci_pipeline.yml

@@ -38,6 +38,7 @@ jobs:
           - noetic
           - humble
           - iron
+          - jazzy
         include:
           - ros_distro: humble
             upload_artifacts: true
@@ -92,6 +93,7 @@ jobs:
       - name: Perform static analysis
         working-directory: ${{ github.workspace }}
         run: ./src/beluga/tools/run-clang-tidy.sh
+        if: ${{ matrix.ros_distro != "jazzy" }}
 
       - name: Upload code coverage report
         uses: codecov/codecov-action@v3

.github/workflows/docker_ci_pipeline.yml

@@ -20,6 +20,7 @@ jobs:
           - noetic
           - humble
           - iron
+          - jazzy
           - rolling
     steps:
       - name: Checkout repository

.github/workflows/weekly_pipeline.yml

@@ -24,6 +24,7 @@ jobs:
           - noetic
           - humble
           - iron
+          - jazzy
           - rolling
     container:
       image: ros:${{ matrix.ros_distro }}-ros-base

beluga/test/beluga/motion/test_differential_drive_model.cpp

@@ -106,7 +106,7 @@ auto get_statistics(Range&& range) {
 }
 
 TEST(DifferentialDriveModelSamples, Translate) {
-  const double tolerance = 0.01;
+  const double tolerance = 0.015;
   const double alpha = 0.2;
   const double origin = 5.0;
   const double distance = 3.0;
@@ -119,9 +119,9 @@ TEST(DifferentialDriveModelSamples, Translate) {
                 const auto pose = SE2d{SO2d{0.0}, Vector2d{origin, 0.0}};
                 return state_sampling_function(pose, generator).translation().x();
               }) |
-              ranges::views::take_exactly(1'000'000) | ranges::views::common;
+              ranges::views::take_exactly(100'000) | ranges::views::common;
   const auto [mean, stddev] = get_statistics(view);
-  ASSERT_NEAR(mean, origin + distance, 0.01);
+  ASSERT_NEAR(mean, origin + distance, tolerance);
   ASSERT_NEAR(stddev, std::sqrt(alpha * distance * distance), tolerance);
 }
 
@@ -139,7 +139,7 @@ TEST(DifferentialDriveModelSamples, RotateFirstQuadrant) {
                 const auto pose = SE2d{SO2d{initial_angle}, Vector2d{0.0, 0.0}};
                 return state_sampling_function(pose, generator).so2().log();
               }) |
-              ranges::views::take_exactly(1'000'000) | ranges::views::common;
+              ranges::views::take_exactly(100'000) | ranges::views::common;
   const auto [mean, stddev] = get_statistics(view);
   ASSERT_NEAR(mean, initial_angle + motion_angle, tolerance);
   ASSERT_NEAR(stddev, std::sqrt(alpha * motion_angle * motion_angle), tolerance);
@@ -159,7 +159,7 @@ TEST(DifferentialDriveModelSamples, RotateThirdQuadrant) {
                 const auto pose = SE2d{SO2d{initial_angle}, Vector2d{0.0, 0.0}};
                 return state_sampling_function(pose, generator).so2().log();
               }) |
-              ranges::views::take_exactly(1'000'000) | ranges::views::common;
+              ranges::views::take_exactly(100'000) | ranges::views::common;
   const auto [mean, stddev] = get_statistics(view);
   ASSERT_NEAR(mean, initial_angle + motion_angle, tolerance);
 
@@ -180,7 +180,7 @@ TEST(DifferentialDriveModelSamples, RotateTranslateRotateFirstQuadrant) {
                 const auto pose = SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}};
                 return state_sampling_function(pose, generator).translation().norm();
               }) |
-              ranges::views::take_exactly(1'000'000) | ranges::views::common;
+              ranges::views::take_exactly(100'000) | ranges::views::common;
   const auto [mean, stddev] = get_statistics(view);
   ASSERT_NEAR(mean, 1.41, tolerance);
 
@@ -206,7 +206,7 @@ TEST(DifferentialDriveModelSamples, RotateTranslateRotateThirdQuadrant) {
                 const auto pose = SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}};
                 return state_sampling_function(pose, generator).translation().norm();
               }) |
-              ranges::views::take_exactly(1'000'000) | ranges::views::common;
+              ranges::views::take_exactly(100'000) | ranges::views::common;
   const auto [mean, stddev] = get_statistics(view);
   ASSERT_NEAR(mean, 1.41, 0.01);
 

beluga/test/beluga/motion/test_omnidirectional_drive_model.cpp

@@ -135,7 +135,7 @@ TEST(OmnidirectionalDriveModelSamples, RotateFirstQuadrant) {
                 const auto pose = SE2d{SO2d{initial_angle}, Vector2d{0.0, 0.0}};
                 return state_sampling_function(pose, generator).so2().log();
               }) |
-              ranges::views::take_exactly(1'000'000) | ranges::views::common;
+              ranges::views::take_exactly(100'000) | ranges::views::common;
   const auto [mean, stddev] = get_statistics(view);
   ASSERT_NEAR(mean, initial_angle + motion_angle, tolerance);
   ASSERT_NEAR(stddev, std::sqrt(alpha * motion_angle * motion_angle), tolerance);
@@ -155,7 +155,7 @@ TEST(OmnidirectionalDriveModelSamples, RotateThirdQuadrant) {
                 const auto pose = SE2d{SO2d{initial_angle}, Vector2d{0.0, 0.0}};
                 return state_sampling_function(pose, generator).so2().log();
               }) |
-              ranges::views::take_exactly(1'000'000) | ranges::views::common;
+              ranges::views::take_exactly(100'000) | ranges::views::common;
   const auto [mean, stddev] = get_statistics(view);
   ASSERT_NEAR(mean, initial_angle + motion_angle, tolerance);
 

beluga_amcl/src/amcl_node.cpp

@@ -550,18 +550,6 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_
     return;
   }
 
-  const auto& [base_pose_in_map, base_pose_covariance] = last_known_estimate_.value();
-
-  // New pose messages are only published on updates to the filter.
-  if (new_estimate.has_value()) {
-    auto message = geometry_msgs::msg::PoseWithCovarianceStamped{};
-    message.header.stamp = laser_scan->header.stamp;
-    message.header.frame_id = get_parameter("global_frame_id").as_string();
-    tf2::toMsg(base_pose_in_map, message.pose.pose);
-    tf2::covarianceEigenToRowMajor(base_pose_covariance, message.pose.covariance);
-    pose_pub_->publish(message);
-  }
-
   // Transforms are always published to keep them current.
   if (enable_tf_broadcast_ && get_parameter("tf_broadcast").as_bool()) {
     auto message = geometry_msgs::msg::TransformStamped{};
@@ -574,6 +562,17 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_
     message.transform = tf2::toMsg(*last_known_odom_transform_in_map_);
     tf_broadcaster_->sendTransform(message);
   }
+
+  // New pose messages are only published on updates to the filter.
+  if (new_estimate.has_value()) {
+    auto message = geometry_msgs::msg::PoseWithCovarianceStamped{};
+    message.header.stamp = laser_scan->header.stamp;
+    message.header.frame_id = get_parameter("global_frame_id").as_string();
+    const auto& [base_pose_in_map, base_pose_covariance] = new_estimate.value();
+    tf2::toMsg(base_pose_in_map, message.pose.pose);
+    tf2::covarianceEigenToRowMajor(base_pose_covariance, message.pose.covariance);
+    pose_pub_->publish(message);
+  }
 }
 
 void AmclNode::initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message) {

beluga_amcl/test/test_amcl_node.cpp

@@ -65,6 +65,11 @@ class BaseNodeFixture : public T {
     return spin_until([this] { return tester_node_->latest_pose().has_value(); }, 1000ms, amcl_node_, tester_node_);
   }
 
+  bool wait_for_transform(const std::string& target, const std::string& source) {
+    return spin_until(
+        [&, this] { return tester_node_->can_transform(target, source); }, 1000ms, amcl_node_, tester_node_);
+  }
+
   bool wait_for_particle_cloud() {
     tester_node_->create_particle_cloud_subscriber();
     return spin_until(
@@ -249,8 +254,7 @@ TEST_F(TestNode, BroadcastWhenInitialPoseSet) {
   ASSERT_TRUE(wait_for_initialization());
   ASSERT_FALSE(tester_node_->can_transform("map", "odom"));
   tester_node_->publish_laser_scan();
-  ASSERT_TRUE(wait_for_pose_estimate());
-  ASSERT_TRUE(tester_node_->can_transform("map", "odom"));
+  ASSERT_TRUE(wait_for_transform("map", "odom"));
 }
 
 TEST_F(TestNode, NoBroadcastWhenNoInitialPose) {
@@ -261,7 +265,6 @@ TEST_F(TestNode, NoBroadcastWhenNoInitialPose) {
   ASSERT_TRUE(wait_for_initialization());
   ASSERT_FALSE(tester_node_->can_transform("map", "odom"));
   tester_node_->publish_laser_scan();
-  ASSERT_TRUE(wait_for_pose_estimate());
   ASSERT_FALSE(tester_node_->can_transform("map", "odom"));
 }
 
@@ -274,8 +277,7 @@ TEST_F(TestNode, BroadcastWithGlobalLocalization) {
   ASSERT_FALSE(tester_node_->can_transform("map", "odom"));
   ASSERT_TRUE(request_global_localization());
   tester_node_->publish_laser_scan();
-  ASSERT_TRUE(wait_for_pose_estimate());
-  ASSERT_TRUE(tester_node_->can_transform("map", "odom"));
+  ASSERT_TRUE(wait_for_transform("map", "odom"));
 }
 
 TEST_F(TestNode, IgnoreGlobalLocalizationBeforeInitialize) {
@@ -508,6 +510,7 @@ TEST_F(TestNode, InitialPoseBeforeInitialize) {
   amcl_node_->configure();
   amcl_node_->activate();
   tester_node_->publish_default_initial_pose();
+  spin_for(10ms, amcl_node_);  // ensure orderly processing
   tester_node_->publish_laser_scan();
   ASSERT_FALSE(wait_for_pose_estimate());
 }
@@ -520,9 +523,10 @@ TEST_F(TestNode, InitialPoseAfterInitialize) {
   ASSERT_TRUE(wait_for_initialization());
   ASSERT_FALSE(tester_node_->can_transform("map", "odom"));
   tester_node_->publish_default_initial_pose();
+  spin_for(10ms, amcl_node_);  // ensure orderly processing
   tester_node_->publish_laser_scan();
   ASSERT_TRUE(wait_for_pose_estimate());
-  ASSERT_TRUE(tester_node_->can_transform("map", "odom"));
+  ASSERT_TRUE(wait_for_transform("map", "odom"));
 }
 
 TEST_F(TestNode, InitialPoseWithWrongFrame) {
@@ -533,6 +537,7 @@ TEST_F(TestNode, InitialPoseWithWrongFrame) {
   ASSERT_TRUE(wait_for_initialization());
   ASSERT_FALSE(tester_node_->can_transform("map", "odom"));
   tester_node_->publish_initial_pose_with_wrong_frame();
+  spin_for(10ms, amcl_node_);  // ensure orderly processing
   tester_node_->publish_laser_scan();
   ASSERT_TRUE(wait_for_pose_estimate());
   ASSERT_FALSE(tester_node_->can_transform("map", "odom"));
@@ -546,9 +551,10 @@ TEST_F(TestNode, CanUpdatePoseEstimate) {
   ASSERT_TRUE(wait_for_initialization());
   ASSERT_FALSE(tester_node_->can_transform("map", "odom"));
   tester_node_->publish_default_initial_pose();
+  spin_for(10ms, amcl_node_);  // ensure orderly processing
   tester_node_->publish_laser_scan();
   ASSERT_TRUE(wait_for_pose_estimate());
-  ASSERT_TRUE(tester_node_->can_transform("map", "odom"));
+  ASSERT_TRUE(wait_for_transform("map", "odom"));
 
   {
     const auto [pose, _] = amcl_node_->estimate();
@@ -558,9 +564,10 @@ TEST_F(TestNode, CanUpdatePoseEstimate) {
   }
 
   tester_node_->publish_initial_pose(1., 1.);
+  spin_for(10ms, amcl_node_);  // ensure orderly processing
   tester_node_->publish_laser_scan();
   ASSERT_TRUE(wait_for_pose_estimate());
-  ASSERT_TRUE(tester_node_->can_transform("map", "odom"));
+  ASSERT_TRUE(wait_for_transform("map", "odom"));
 
   {
     const auto [pose, _] = amcl_node_->estimate();
@@ -578,9 +585,10 @@ TEST_F(TestNode, CanForcePoseEstimate) {
   ASSERT_TRUE(wait_for_initialization());
   ASSERT_FALSE(tester_node_->can_transform("map", "odom"));
   tester_node_->publish_default_initial_pose();
+  spin_for(10ms, amcl_node_);  // ensure orderly processing
   tester_node_->publish_laser_scan();
   ASSERT_TRUE(wait_for_pose_estimate());
-  ASSERT_TRUE(tester_node_->can_transform("map", "odom"));
+  ASSERT_TRUE(wait_for_transform("map", "odom"));
 
   {
     const auto [pose, _] = amcl_node_->estimate();
@@ -592,7 +600,7 @@ TEST_F(TestNode, CanForcePoseEstimate) {
   ASSERT_TRUE(request_nomotion_update());
   tester_node_->publish_laser_scan();
   ASSERT_TRUE(wait_for_pose_estimate());
-  ASSERT_TRUE(tester_node_->can_transform("map", "odom"));
+  ASSERT_TRUE(wait_for_transform("map", "odom"));
 
   {
     const auto [pose, _] = amcl_node_->estimate();
@@ -663,6 +671,7 @@ TEST_F(TestNode, TransformValue) {
   ASSERT_NEAR(pose.translation().y(), 2.0, 0.01);
   ASSERT_NEAR(pose.so2().log(), Sophus::Constants<double>::pi() / 3, 0.01);
 
+  ASSERT_TRUE(wait_for_transform("map", "odom"));
   const auto transform = tester_node_->lookup_transform("map", "odom");
   EXPECT_NEAR(transform.translation().x(), -2.00, 0.01);
   EXPECT_NEAR(transform.translation().y(), -2.00, 0.01);

beluga_amcl/test/test_ndt_amcl_node.cpp

@@ -71,6 +71,11 @@ class BaseNodeFixture : public T {
     return spin_until([this] { return tester_node_->latest_pose().has_value(); }, 1000ms, ndt_amcl_node_, tester_node_);
   }
 
+  bool wait_for_transform(const std::string& target, const std::string& source) {
+    return spin_until(
+        [&, this] { return tester_node_->can_transform(target, source); }, 1000ms, ndt_amcl_node_, tester_node_);
+  }
+
   bool wait_for_particle_cloud() {
     tester_node_->create_particle_cloud_subscriber();
     return spin_until(
@@ -233,7 +238,7 @@ TEST_F(TestNode, BroadcastWhenInitialPoseSet) {
   ASSERT_FALSE(tester_node_->can_transform("map", "odom"));
   tester_node_->publish_laser_scan();
   ASSERT_TRUE(wait_for_pose_estimate());
-  ASSERT_TRUE(tester_node_->can_transform("map", "odom"));
+  ASSERT_TRUE(wait_for_transform("map", "odom"));
 }
 
 TEST_F(TestNode, NoBroadcastWhenNoInitialPose) {
@@ -385,6 +390,7 @@ TEST_F(TestNode, InitialPoseBeforeInitialize) {
   ndt_amcl_node_->activate();
   ASSERT_FALSE(wait_for_pose_estimate());
   tester_node_->publish_default_initial_pose();
+  spin_for(10ms, ndt_amcl_node_);  // ensure orderly processing
   tester_node_->publish_laser_scan();
   ASSERT_TRUE(wait_for_pose_estimate());
 }
@@ -396,9 +402,10 @@ TEST_F(TestNode, InitialPoseAfterInitialize) {
   ASSERT_TRUE(wait_for_initialization());
   ASSERT_FALSE(tester_node_->can_transform("map", "odom"));
   tester_node_->publish_default_initial_pose();
+  spin_for(10ms, ndt_amcl_node_);  // ensure orderly processing
   tester_node_->publish_laser_scan();
   ASSERT_TRUE(wait_for_pose_estimate());
-  ASSERT_TRUE(tester_node_->can_transform("map", "odom"));
+  ASSERT_TRUE(wait_for_transform("map", "odom"));
 }
 
 TEST_F(TestNode, InitialPoseWithWrongFrame) {
@@ -408,6 +415,7 @@ TEST_F(TestNode, InitialPoseWithWrongFrame) {
   ASSERT_TRUE(wait_for_initialization());
   ASSERT_FALSE(tester_node_->can_transform("map", "odom"));
   tester_node_->publish_initial_pose_with_wrong_frame();
+  spin_for(10ms, ndt_amcl_node_);  // ensure orderly processing
   tester_node_->publish_laser_scan();
   ASSERT_FALSE(wait_for_pose_estimate());
   ASSERT_FALSE(tester_node_->can_transform("map", "odom"));
@@ -420,9 +428,10 @@ TEST_F(TestNode, CanUpdatePoseEstimate) {
   ASSERT_TRUE(wait_for_initialization());
   ASSERT_FALSE(tester_node_->can_transform("map", "odom"));
   tester_node_->publish_default_initial_pose();
+  spin_for(10ms, ndt_amcl_node_);  // ensure orderly processing
   tester_node_->publish_laser_scan();
   ASSERT_TRUE(wait_for_pose_estimate());
-  ASSERT_TRUE(tester_node_->can_transform("map", "odom"));
+  ASSERT_TRUE(wait_for_transform("map", "odom"));
 
   {
     const auto [pose, _] = ndt_amcl_node_->estimate();
@@ -432,9 +441,10 @@ TEST_F(TestNode, CanUpdatePoseEstimate) {
   }
 
   tester_node_->publish_initial_pose(1., 1.);
+  spin_for(10ms, ndt_amcl_node_);  // ensure orderly processing
   tester_node_->publish_laser_scan();
   ASSERT_TRUE(wait_for_pose_estimate());
-  ASSERT_TRUE(tester_node_->can_transform("map", "odom"));
+  ASSERT_TRUE(wait_for_transform("map", "odom"));
 
   {
     const auto [pose, _] = ndt_amcl_node_->estimate();
@@ -492,6 +502,7 @@ TEST_F(TestNode, TransformValue) {
   ASSERT_NEAR(pose.translation().y(), 2.0, 0.01);
   ASSERT_NEAR(pose.so2().log(), Sophus::Constants<double>::pi() / 3, 0.01);
 
+  ASSERT_TRUE(wait_for_transform("map", "odom"));
   const auto transform = tester_node_->lookup_transform("map", "odom");
   EXPECT_NEAR(transform.translation().x(), -2.00, 0.01);
   EXPECT_NEAR(transform.translation().y(), -2.00, 0.01);

beluga_example/package.xml

@@ -24,7 +24,6 @@
   <exec_depend condition="$ROS_VERSION == 2">nav2_amcl</exec_depend>
   <exec_depend condition="$ROS_VERSION == 2">nav2_lifecycle_manager</exec_depend>
   <exec_depend condition="$ROS_VERSION == 2">nav2_map_server</exec_depend>
-  <exec_depend condition="$ROS_VERSION == 2">nav2_rviz_plugins</exec_depend>
   <exec_depend condition="$ROS_VERSION == 2">rviz2</exec_depend>
   <exec_depend>teleop_twist_keyboard</exec_depend>
 

docker/docker-compose.yml

@@ -4,6 +4,7 @@ services:
     build:
       context: ..
       dockerfile: docker/images/${ROSDISTRO:-humble}/Dockerfile
+    image: ekumenlabs/beluga-${ROSDISTRO:-humble}-dev
     container_name: beluga-dev
     environment:
       - DISPLAY

docker/files/jazzy.repos

@@ -0,0 +1 @@
+repositories: {}