Compute twist on publish

[efernandez]

This PR is on top of #22

This PR adds:

1. Computes the twist at the odometry publish rate. This is very important because any software that uses the odometry can only retrieve the dt used to compute the twist from the difference between the msgs timestamps.
2. Add a unit test to verify that the current pose P_1 can be computed from the previous pose P_0, the current twist T_1 and the dt obtained as the difference between the msgs timestamps, as P_1 = P_0 * T_1 / dt (which is the rigid motion equation in SE(2), in matrix form).
3. Extend the rostests with the unittest checks.
4. All the test checks include the covariance, i.e. the pose covariance of P_1 as reported in the odometry msg, can be retrieved using the pose covariance of P_0 and the twist covariance of T_1, using the (analytical) Jacobian of the rigid motion equation shown before (implemented in element-wise form).

@ayrton04 @servos @afakihcpr

[efernandez]

@ayrton04 This should be equivalent to the equations from https://github.com/clearpathrobotics/robot_localization/blob/indigo-devel/src/ekf.cpp#L209, but in SE(2) only

[ayrton04]

Awesome! Hopefully I'll get around to adding a 2D version of the EKF anyway.

[ayrton04]

These look good.

[efernandez]

Thanks for confirming it.

[ayrton04]

I'm not an expert on the ros_controller package, but what I see looks correct! Nice work on the very thorough tests.

[efernandez]

@ayrton04 This one is actually wrong. I don't want to bias you... can you review it again?

[ayrton04]

My unbiased correction:

 J_twist << -cos_yaw * dt, sin_yaw * dt,  0.0,
            -sin_yaw * dt,  -cos_yaw * dt, 0.0,
            0.0,           0.0,            1.0;

[efernandez]

Are you sure?, I don't see why the sign changes.

The error I found is on the derivative wrt v_yaw, which should be dt instead of 1.0.
I've just updated the PR with that fix. I think the rest it's fine, indeed now the relevant tests are passing 😃

[efernandez]

#22 was closed and merged in #31
Closing this PR to create a new one on top of indigo-devel