Odometry Position Frame (#21)

* Add hardware interface testing dependency * Use orientation w.r.t. odom to set position
ros-controls · Jul 31, 2024 · 57fac95 · 57fac95
1 parent 4852699
commit 57fac95
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 1 deletion.
diff --git 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
@@ -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_ =