diff drive controller not calculating accurate odom

asked 2023-01-10 03:08:57 -0500

dinesh gravatar image

updated 2023-01-10 03:09:23 -0500

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.

edit retag flag offensive close merge delete