Mecanum Drive: Populate the pose covariance matrix (#1772)

Author: hilary-luo

mecanum_drive_controller/src/mecanum_drive_controller.cpp

@@ -173,13 +173,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();
 

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:

mecanum_drive_controller/test/test_mecanum_drive_controller.cpp

@@ -62,6 +62,13 @@ 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 +147,17 @@ 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)