diff drive controller not calculating accurate odom
When i send input cmd velocity the motor driver rotating at correct sped and returning correct angular velocity and angular position but diff driver controller is not calculating correct odom twist::linear::x value eg. input is 0.1 but outpt is 0 m/s but twist::angular::z is 300. At high speed the wheels starts to rotate instead of moving forward according to odometry data but actual motor is moving correctly and providing correct feedback data. Here is read and write code of hardware interface:
void RRBotHWInterface::read(ros::Duration &elapsed_time)
{
if(controller->bus_voltage < 21 && controller->bus_voltage>0) {
ROS_WARN("Under voltage Detected %f", controller->bus_voltage);
throw 10;
}
joint_position_[0] = -controller->lm_ang_pos;
joint_position_[1] = controller->rm_ang_pos;
joint_velocity_[0] = controller->lm_vel;
joint_velocity_[1] = controller->rm_vel;
}
void RRBotHWInterface::write(ros::Duration &elapsed_time)
{
// Safety
enforceLimits(elapsed_time);
int lm_vel = static_cast<int>(joint_velocity_command_[0]*RPM);
int rm_vel = -static_cast<int>(joint_velocity_command_[1]*RPM);
float head_pos = joint_velocity_command_[2];
float body_pos = joint_velocity_command_[3];
controller->send_vel(lm_vel, rm_vel);
ros::Duration(0.0001).sleep();
}
I am using ros noetic, ubuntu 20.04. jetson nano. Hers yaml of diff drive controller:
base_controller:
type : "diff_drive_controller/DiffDriveController"
left_wheel : 'left_wheel_joint'
right_wheel : 'right_wheel_joint'
publish_rate: 50.0 # default: 50
pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
# Wheel separation and diameter. These are both optional.
# diff_drive_controller will attempt to read either one or both from the
# URDF if not specified as a parameter
wheel_separation : 0.385
wheel_radius : 0.085
# Wheel separation and radius multipliers
wheel_separation_multiplier: 1.0 # default: 1.0
wheel_radius_multiplier : 1.0 # default: 1.0
# Velocity commands timeout [s], default 0.5
cmd_vel_timeout: 0.25
# Base frame_id
base_frame_id: base_footprint #default: base_link
# Velocity and acceleration limits
# Whenever a min_* is unspecified, default to -max_*
linear:
x:
has_velocity_limits : true
max_velocity : 0.5 # m/s
min_velocity : -0.5 # m/s
has_acceleration_limits: false
max_acceleration : 0.0 # m/s^2
min_acceleration : 0.0 # m/s^2
has_jerk_limits : false
max_jerk : 0.01 # m/s^3
angular:
z:
has_velocity_limits : true
max_velocity : 1.5708 # rad/s
has_acceleration_limits: false
max_acceleration : 0.7 # rad/s^2
has_jerk_limits : false
max_jerk : 0.01 # rad/s^3
diff drive is totally not calculating correct odom data at all.