HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
HiepLM
2026-03-30 17:48:44 +07:00
parent 39bc1796af
commit ca9bfca40c
6 changed files with 64 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/src/odometry.cpp
View File

@@ -40,7 +40,7 @@
*/
#include <steer_drive_controller/odometry.h>
#include <ros/ros.h>
#include <boost/bind.hpp>
namespace steer_drive_controller
@@ -86,10 +86,9 @@ namespace steer_drive_controller
rear_wheel_old_pos_ = rear_wheel_cur_pos;
/// Compute linear and angular diff:
const double linear = rear_wheel_est_vel * cos(front_steer_pos);//(right_wheel_est_vel + left_wheel_est_vel) * 0.5;
//const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_;
const double linear = rear_wheel_est_vel * cos(front_steer_pos);
const double angular = tan(front_steer_pos) * linear / wheel_separation_h_;
// ROS_INFO("front_steer_pos: %.3f tan(front_steer_pos) %.3f, angular %.3f", front_steer_pos, tan(front_steer_pos), angular);
/// Integrate odometry:
integrate_fun_(linear, angular);