From f2f34623a75e11e2dc8c5445c17182f8791d858e Mon Sep 17 00:00:00 2001
From: Hilary Luo <hluo@clearpathrobotics.com>
Date: Tue, 24 Jun 2025 15:33:53 -0400
Subject: [PATCH 1/3] Populate the pose covariance matrix

---
 mecanum_drive_controller/src/mecanum_drive_controller.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp
index 9615d3e0c8..ead080050f 100644
--- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp
+++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp
@@ -175,13 +175,14 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(
   rt_odom_state_publisher_->msg_.child_frame_id = base_frame_id;
   rt_odom_state_publisher_->msg_.pose.pose.position.z = 0;
 
-  auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance;
+  auto & pose_covariance = rt_odom_state_publisher_->msg_.pose.covariance;
+  auto & twist_covariance = rt_odom_state_publisher_->msg_.twist.covariance;
   constexpr size_t NUM_DIMENSIONS = 6;
   for (size_t index = 0; index < 6; ++index)
   {
     const size_t diagonal_index = NUM_DIMENSIONS * index + index;
-    covariance[diagonal_index] = params_.pose_covariance_diagonal[index];
-    covariance[diagonal_index] = params_.twist_covariance_diagonal[index];
+    pose_covariance[diagonal_index] = params_.pose_covariance_diagonal[index];
+    twist_covariance[diagonal_index] = params_.twist_covariance_diagonal[index];
   }
   rt_odom_state_publisher_->unlock();
 

From 683de49638acc01009a63e2841a23e498f86e39f Mon Sep 17 00:00:00 2001
From: Hilary Luo <hluo@clearpathrobotics.com>
Date: Thu, 3 Jul 2025 17:36:41 +0000
Subject: [PATCH 2/3] Add test coverage for odom message covariances

---
 .../test/mecanum_drive_controller_params.yaml |  2 +-
 .../test/test_mecanum_drive_controller.cpp    | 24 +++++++++++++++++++
 2 files changed, 25 insertions(+), 1 deletion(-)

diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml
index 658cd3b2fa..dfa3e5de94 100644
--- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml
+++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml
@@ -17,7 +17,7 @@
       odom_frame_id: "odom"
       enable_odom_tf: true
       twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
-      pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
+      pose_covariance_diagonal: [0.0, 6.0, 12.0, 18.0, 24.0, 30.0]
 
 
   test_mecanum_drive_controller_with_rotation:
diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp
index ae7f551693..09795addbd 100644
--- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp
+++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp
@@ -62,6 +62,11 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_para
   ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0);
   ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0);
 
+  ASSERT_EQ(controller_->params_.pose_covariance_diagonal,
+            std::vector<double>({0.0, 6.0, 12.0, 18.0, 24.0, 30.0}));
+  ASSERT_EQ(controller_->params_.twist_covariance_diagonal,
+            std::vector<double>({0.0, 7.0, 14.0, 21.0, 28.0, 35.0}));
+
   ASSERT_EQ(
     controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME);
   ASSERT_EQ(
@@ -140,6 +145,25 @@ TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_false_no_na
   /* tf_frame_prefix_enable is false so no modifications to the frame id's */
   ASSERT_EQ(test_odom_frame_id, odom_id);
   ASSERT_EQ(test_base_frame_id, base_link_id);
+
+  std::array<double, 36> pose_covariance = {{
+              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+              0.0, 6.0, 0.0, 0.0, 0.0, 0.0,
+              0.0, 0.0, 12.0, 0.0, 0.0, 0.0,
+              0.0, 0.0, 0.0, 18.0, 0.0, 0.0,
+              0.0, 0.0, 0.0, 0.0, 24.0, 0.0,
+              0.0, 0.0, 0.0, 0.0, 0.0, 30.0}};
+
+  std::array<double, 36> twist_covariance = {{
+              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+              0.0, 7.0, 0.0, 0.0, 0.0, 0.0,
+              0.0, 0.0, 14.0, 0.0, 0.0, 0.0,
+              0.0, 0.0, 0.0, 21.0, 0.0, 0.0,
+              0.0, 0.0, 0.0, 0.0, 28.0, 0.0,
+              0.0, 0.0, 0.0, 0.0, 0.0, 35.0}};
+
+  ASSERT_EQ(odometry_message.pose.covariance, pose_covariance);
+  ASSERT_EQ(odometry_message.twist.covariance, twist_covariance);
 }
 
 TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_true_no_namespace)

From b7ff624fd7baa71c3bd6c2e368870f6a2b008df0 Mon Sep 17 00:00:00 2001
From: Hilary Luo <hluo@clearpathrobotics.com>
Date: Fri, 4 Jul 2025 13:20:44 +0000
Subject: [PATCH 3/3] Linting

---
 .../test/test_mecanum_drive_controller.cpp    | 32 ++++++++-----------
 1 file changed, 13 insertions(+), 19 deletions(-)

diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp
index 09795addbd..66bc2d6ab9 100644
--- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp
+++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp
@@ -62,10 +62,12 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_para
   ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0);
   ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0);
 
-  ASSERT_EQ(controller_->params_.pose_covariance_diagonal,
-            std::vector<double>({0.0, 6.0, 12.0, 18.0, 24.0, 30.0}));
-  ASSERT_EQ(controller_->params_.twist_covariance_diagonal,
-            std::vector<double>({0.0, 7.0, 14.0, 21.0, 28.0, 35.0}));
+  ASSERT_EQ(
+    controller_->params_.pose_covariance_diagonal,
+    std::vector<double>({0.0, 6.0, 12.0, 18.0, 24.0, 30.0}));
+  ASSERT_EQ(
+    controller_->params_.twist_covariance_diagonal,
+    std::vector<double>({0.0, 7.0, 14.0, 21.0, 28.0, 35.0}));
 
   ASSERT_EQ(
     controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME);
@@ -146,21 +148,13 @@ TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_false_no_na
   ASSERT_EQ(test_odom_frame_id, odom_id);
   ASSERT_EQ(test_base_frame_id, base_link_id);
 
-  std::array<double, 36> pose_covariance = {{
-              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-              0.0, 6.0, 0.0, 0.0, 0.0, 0.0,
-              0.0, 0.0, 12.0, 0.0, 0.0, 0.0,
-              0.0, 0.0, 0.0, 18.0, 0.0, 0.0,
-              0.0, 0.0, 0.0, 0.0, 24.0, 0.0,
-              0.0, 0.0, 0.0, 0.0, 0.0, 30.0}};
-
-  std::array<double, 36> twist_covariance = {{
-              0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-              0.0, 7.0, 0.0, 0.0, 0.0, 0.0,
-              0.0, 0.0, 14.0, 0.0, 0.0, 0.0,
-              0.0, 0.0, 0.0, 21.0, 0.0, 0.0,
-              0.0, 0.0, 0.0, 0.0, 28.0, 0.0,
-              0.0, 0.0, 0.0, 0.0, 0.0, 35.0}};
+  std::array<double, 36> pose_covariance = {
+    {0.0, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0, 6.0, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0, 12.0, 0.0, 0.0, 0.0,
+     0.0, 0.0, 0.0, 18.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 24.0, 0.0, 0.0, 0.0, 0.0,  0.0, 0.0, 30.0}};
+
+  std::array<double, 36> twist_covariance = {
+    {0.0, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0, 7.0, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0, 14.0, 0.0, 0.0, 0.0,
+     0.0, 0.0, 0.0, 21.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 28.0, 0.0, 0.0, 0.0, 0.0,  0.0, 0.0, 35.0}};
 
   ASSERT_EQ(odometry_message.pose.covariance, pose_covariance);
   ASSERT_EQ(odometry_message.twist.covariance, twist_covariance);