Skip to content

Commit 922e49b

Browse files
authored
Mecanum Drive: Populate the pose covariance matrix (#1772)
1 parent 9e01142 commit 922e49b

File tree

3 files changed

+23
-4
lines changed

3 files changed

+23
-4
lines changed

mecanum_drive_controller/src/mecanum_drive_controller.cpp

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -173,13 +173,14 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(
173173
rt_odom_state_publisher_->msg_.child_frame_id = base_frame_id;
174174
rt_odom_state_publisher_->msg_.pose.pose.position.z = 0;
175175

176-
auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance;
176+
auto & pose_covariance = rt_odom_state_publisher_->msg_.pose.covariance;
177+
auto & twist_covariance = rt_odom_state_publisher_->msg_.twist.covariance;
177178
constexpr size_t NUM_DIMENSIONS = 6;
178179
for (size_t index = 0; index < 6; ++index)
179180
{
180181
const size_t diagonal_index = NUM_DIMENSIONS * index + index;
181-
covariance[diagonal_index] = params_.pose_covariance_diagonal[index];
182-
covariance[diagonal_index] = params_.twist_covariance_diagonal[index];
182+
pose_covariance[diagonal_index] = params_.pose_covariance_diagonal[index];
183+
twist_covariance[diagonal_index] = params_.twist_covariance_diagonal[index];
183184
}
184185
rt_odom_state_publisher_->unlock();
185186

mecanum_drive_controller/test/mecanum_drive_controller_params.yaml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@
1717
odom_frame_id: "odom"
1818
enable_odom_tf: true
1919
twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
20-
pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
20+
pose_covariance_diagonal: [0.0, 6.0, 12.0, 18.0, 24.0, 30.0]
2121

2222

2323
test_mecanum_drive_controller_with_rotation:

mecanum_drive_controller/test/test_mecanum_drive_controller.cpp

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -62,6 +62,13 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_para
6262
ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0);
6363
ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0);
6464

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+
6572
ASSERT_EQ(
6673
controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME);
6774
ASSERT_EQ(
@@ -140,6 +147,17 @@ TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_false_no_na
140147
/* tf_frame_prefix_enable is false so no modifications to the frame id's */
141148
ASSERT_EQ(test_odom_frame_id, odom_id);
142149
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);
143161
}
144162

145163
TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_true_no_namespace)

0 commit comments

Comments
 (0)