Add test coverage for odom message covariances

Author: hilary-luo

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,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)