Fix odometry twist computation

[efernandez]

The twist is computed as the relative transformation in SE(2) between
the previous and current odometry pose, divided by the time step dt.

This is the first PR of a big list trying to bring the CPR fork changes upstream: https://github.com/clearpathrobotics/ros_controllers/commits/indigo-devel?after=35323e012116cee02e9c2e25f5f31be14d44298b+139 (from June 8, 2015)

This first PR is the same as clearpathrobotics#2 with a few conflicts resolved.

[efernandez]

FYI @wxmerkt @artivis

[efernandez]

This is between a "bug" and an "enhancement" b/c the twist should have a small (but still have) y component.

Please ignore the fact I used SE(2) from sophus here, as I'd later change that in newer PRs.

My intention is to iterate fast on the list of changes in the CPR fork, so we get to the latest commits, which are the most stable and tested ones (> year, thousands of km, in different robots, mainly the OTTO platforms).

[bmagyar]

I am a bit concerned about adding sophus as a dependency even if it gets removed later. I know it's a PITA but can that part be removed from this PR? It would make assessing the changes easier if a new library is not brought in especially if it is later removed (because then we have to review that the removal changes indeed keep functionality).

[bmagyar]

(The tests are timing out...feel free to restart them until they turn green or fail for real)

[efernandez]

I could use Eigen or tf transforms, but the could will change in future PRs anyway: https://github.com/clearpathrobotics/ros_controllers/blob/indigo-devel/diff_drive_controller/src/odometry.cpp#L176

I can still do it if you insist, but it'd slow things down. 😃

What's the problem with sophus as a dependency? I mean, it's well release under ROS as a debian.

BTW I also re-run the test job, now it timed out on the last/third subtest.

[bmagyar]

There's nothing wrong with sophus per se, it's about

• adding something only for the sake of removing it later
• adding a new dependency that may block our release with newer distros.

I see that there's actually not much code affected...would you mind using Eigen of tf instead please?

[efernandez]

I see that there's actually not much code affected...would you mind using Eigen of tf instead please?

Will do that

[bmagyar]

Hola @artivis , would you mind to revive this PR?