Add service to reset odometry

[alcantara09]

Why:
The odometry state relies on the assumption that the node that implements the controller_manager interface has the same life cycle as it. In other words is assumes that the internal state is zero and any incoming messages are due robot movement. However this is not true when restarting the nodes but not the actual physical system. In this case the first reading of encoder position will be the current encoder position of the controller, what will be interpreted as being a valid input and will generate a spike in velocity, moving the internal state to a position far way from the origin.

How:
This PR exposes a service to reset the odometry state of the diff_drive_controller. This allow us to reset the odometry in cases when you have to restart your system, but the actual controller is not resetted.
To overcome the initialization issue, the first reading of the controller will also be considered as an offset. This also ensures that no jump will occur when resetting the odometry state.

[matthew-reynolds]

This looks reasonable, but I don't think we can accept it into ros1 since it will break ABI. I'm not sure how much of an issue ABI compatibility is when using pluginlib though... @bmagyar @ipa-mdl thoughts?

[matthew-reynolds]

Nit: Please alphabetize

[alcantara09]

done

[bmagyar]

I have two points:

• The variable name first_data is unclear when you first read it. Using something more specific, such as odometry_reset would be desirable.
• Using a service in a controller is not fortunate as it is a blocking call unless explicitly handled on a separate thread. I suggest introducing a topic for this with a realtime subscriber and triggering the reset in the next update loop after receiving the reset request. That way you can also eliminate the subject of the first point as you won't need a flag to convey this change ;)

Additionally: services are generally not preferred for any deployment where you want to record/monitor the behaviour of the system.

[caioaamaral]

friendly ping, would be nice having this feature on melodic

[bmagyar]

@alcantara09 @caioaamaral
I'd be happy to merge this to Noetic provided that there are some responses to the questions we asked.

[alcantara09]

Hi @bmagyar,
I believe I pushed your requested changes, however I forgot to ping you =)

[nblkStudy]

I want to know if this "reset odom" method is feasible. How should I use it to solve the problem? Can it be used on the Husky robot with Melodic?