diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml
index aa718b57f0..fc6c03ad8b 100644
--- a/mecanum_drive_controller/package.xml
+++ b/mecanum_drive_controller/package.xml
@@ -35,6 +35,7 @@
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>controller_manager</test_depend>
+  <test_depend>hardware_interface_testing</test_depend>
   <test_depend>ros2_control_test_assets</test_depend>
 
   <export>
diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp
index 32b3bb0dbd..e5760f18e6 100644
--- a/mecanum_drive_controller/src/odometry.cpp
+++ b/mecanum_drive_controller/src/odometry.cpp
@@ -101,7 +101,7 @@ bool Odometry::update(
   orientation_z_in_base_frame_ += velocity_in_base_frame_angular_z * dt;
 
   tf2::Quaternion orientation_R_b_odom;
-  orientation_R_b_odom.setRPY(0.0, 0.0, -base_frame_offset_[2]);
+  orientation_R_b_odom.setRPY(0.0, 0.0, orientation_z_in_base_frame_);
 
   tf2::Matrix3x3 angular_transformation_from_base_2_odom = tf2::Matrix3x3((orientation_R_b_odom));
   tf2::Vector3 velocity_in_base_frame_w_r_t_odom_frame_ =