[ROS2] Odometry with Velocity feedback

edit

Configuration:

• ROS 2 Distro: Humble
• OS Version: Ubuntu 22.04

Hi there, I have a 2 wheeled robot with differential drive kinematics and odometry setup, I implemented EKF and added an IMU.

The robot has an emergency button feature that cuts any velocity command sent to the motors (while cmd_vel is still publishing in the background). I am using a position and velocity state_interface and I made sure that the feedback is reaching the diff drive plugin.

The problem is that my odometry twist values are taken directly from the cmd_vel topic, so even though the robot isn't moving the odometry keeps going on.

How can I integrate my wheel velocity feedback with the diff drive plugin??

ros2_control tag:

<ros2_control name="${name}" type="system">
  <hardware>
      <plugin>amr_control/DiffDriveSystemHardware</plugin>
      <param name="wheel_radius">0.098</param>
      <param name="wheel_seperation">0.705</param>
  </hardware>
  <joint name="left_wheel_joint">
    <command_interface name="velocity">
      <param name="min">-1.5</param>
      <param name="max">1.5</param>
    </command_interface>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
  </joint>
  <joint name="right_wheel_joint">
    <command_interface name="velocity">
      <param name="min">-1.5</param>
      <param name="max">1.5</param>
    </command_interface>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
  </joint>
</ros2_control>

HW Interface read Implementation:

hardware_interface::return_type DiffDriveSystemHardware::read(const rclcpp::Time &time, const rclcpp::Duration &period)
   {
for (uint i = 0; i < hw_commands_.size(); i++)
{
  hw_positions_[i] = comms->getPosition(info_.joints[i].name.c_str());
  hw_velocities_[i] = comms->getVelocity(info_.joints[i].name.c_str());
}

return hardware_interface::return_type::OK;
}

Example Scenario:

cmd_vel topic -> linear.x = 0.5
real_motor -> linear.x = 0.0 (because of the emergency stop)
HW interface joint state -> linear.x = 0.0 (reading from real hardware)
diff_drive/odom -> linear.x = 0.5 (same as cmd_vel)