Add ROS 2 Jazzy to CI/CD and dev workflows (#378)

### Proposed changes Precisely what the title says. This patch also fixes the tests that started failing with the transition. #### Type of change - [ ] 🐛 Bugfix (change which fixes an issue) - [x] 🚀 Feature (change which adds functionality) - [ ] 📚 Documentation (change which fixes or extends documentation) ### Checklist - [x] Lint and unit tests (if any) pass locally with my changes - [x] I have added tests that prove my fix is effective or that my feature works - [x] I have added necessary documentation (if appropriate) - [x] All commits have been signed for [DCO](https://developercertificate.org/) ### Additional comments Here we exclude `beluga_example` and `beluga_benchmark`, as we cannot yet satisfy nav2 dependencies. --------- Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>
Ekumen-OS · May 21, 2024 · 530d8b1 · 530d8b1
1 parent 33c83f7
commit 530d8b1
Show file tree

Hide file tree

Showing 14 changed files with 178 additions and 39 deletions.
diff --git a/.github/workflows/ci_pipeline.yml b/.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

diff --git a/.github/workflows/docker_ci_pipeline.yml b/.github/workflows/docker_ci_pipeline.yml
@@ -20,6 +20,7 @@ jobs:
           - noetic
           - humble
           - iron
+          - jazzy
           - rolling
     steps:
       - name: Checkout repository

diff --git a/.github/workflows/weekly_pipeline.yml b/.github/workflows/weekly_pipeline.yml
@@ -24,6 +24,7 @@ jobs:
           - noetic
           - humble
           - iron
+          - jazzy
           - rolling
     container:
       image: ros:${{ matrix.ros_distro }}-ros-base

diff --git a/beluga/test/beluga/motion/test_differential_drive_model.cpp b/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);
 

diff --git a/beluga/test/beluga/motion/test_omnidirectional_drive_model.cpp b/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);
 

diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp
@@ -560,18 +560,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{};
@@ -584,6 +572,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) {

diff --git a/beluga_amcl/test/test_amcl_node.cpp b/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(
@@ -255,8 +260,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) {
@@ -267,7 +271,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"));
 }
 
@@ -280,8 +283,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) {
@@ -514,6 +516,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());
 }
@@ -526,9 +529,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) {
@@ -539,6 +543,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"));
@@ -552,9 +557,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();
@@ -564,9 +570,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();
@@ -584,9 +591,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();
@@ -598,7 +606,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();
@@ -677,6 +685,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);

diff --git a/beluga_amcl/test/test_ndt_amcl_node.cpp b/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);

diff --git a/beluga_example/package.xml b/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>
 

diff --git a/docker/docker-compose.yml b/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

diff --git a/docker/files/jazzy.repos b/docker/files/jazzy.repos
@@ -0,0 +1 @@
+repositories: {}
-Original file line number
+Diff line change
@@ Expand Up / @@ -20,6 +20,7 @@ jobs: @@
               - noetic
               - humble
               - iron
+              - jazzy
               - rolling
         steps:
           - name: Checkout repository
@@ Expand Down @@