@@ -62,6 +62,13 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_para
62
62
ASSERT_EQ (controller_->params_ .kinematics .base_frame_offset .y , 0.0 );
63
63
ASSERT_EQ (controller_->params_ .kinematics .base_frame_offset .theta , 0.0 );
64
64
65
+ ASSERT_EQ (
66
+ controller_->params_ .pose_covariance_diagonal ,
67
+ std::vector<double >({0.0 , 6.0 , 12.0 , 18.0 , 24.0 , 30.0 }));
68
+ ASSERT_EQ (
69
+ controller_->params_ .twist_covariance_diagonal ,
70
+ std::vector<double >({0.0 , 7.0 , 14.0 , 21.0 , 28.0 , 35.0 }));
71
+
65
72
ASSERT_EQ (
66
73
controller_->params_ .front_left_wheel_command_joint_name , TEST_FRONT_LEFT_CMD_JOINT_NAME);
67
74
ASSERT_EQ (
@@ -140,6 +147,17 @@ TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_false_no_na
140
147
/* tf_frame_prefix_enable is false so no modifications to the frame id's */
141
148
ASSERT_EQ (test_odom_frame_id, odom_id);
142
149
ASSERT_EQ (test_base_frame_id, base_link_id);
150
+
151
+ std::array<double , 36 > pose_covariance = {
152
+ {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 ,
153
+ 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 }};
154
+
155
+ std::array<double , 36 > twist_covariance = {
156
+ {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 ,
157
+ 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 }};
158
+
159
+ ASSERT_EQ (odometry_message.pose .covariance , pose_covariance);
160
+ ASSERT_EQ (odometry_message.twist .covariance , twist_covariance);
143
161
}
144
162
145
163
TEST_F (MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_true_no_namespace)
0 commit comments