Issues with calculated velocities in odom message

PGotzmann commented 2 years ago

First of all thank you very much for taking the time to port this package to ros2 and sharing it . While testing this package I noticed a few small issues with the twist part of the odometry message.

Summary:

Ubuntu 20.04 with ROS galactic

The velocities in the odom message are currently calulated here with respect to the the odom frame, however, according to the odom message definition this should be done with respect to odom_msg.child_frame_id instead.

Additional to the angular velocity beeing set to the wrong axis (#3) I also noticed that the calculation here lacks some kind of normalization of the angle difference. This might lead to unexpected spikes if the orientation is near +-PI.

Steps to reproduce this issue:

run ros2_laser_scan_matcher with publish_odom enabled
set robot to constant speed and drive around on a circular path
one can see that the published values for v_x and v_y depend on the robots current orientation

Possible fixes:

Transform the velocities to base_link. Normalize the angle difference or use quaternions/tfs to avoid that.

PGotzmann commented 2 years ago

I have opened a corresponding PR that would address this Issue. Please let me know what you think about it.

AlexKaravaev commented 2 years ago

@PGotzmann Thank you so much for taking time to create PR! :) Pretty much all of the mentioned here makes sense, so I merged #5 and will close #3, #4. Cheers!

AlexKaravaev / ros2_laser_scan_matcher