update old pos before publishing odom

[1hada]

The PR solves a bug where the joint state positions used by odometry are non-zero by the time a controller starts.

We have found that as our hardware interface begins, and the peripheral devices initialize, the first joint states read by the controller are nonzero.

Fixing the bug from within the odometry class is more robust than implementing a position offset in the hardware interface. The reason it’s more robust is because :

• it allows the odometry’s velocity-accumulator to more quickly converge towards a more accurate velocity than if an erroneous value from a non zero previous joint state should have been used.

example :

• robot initialization
• controller stops
  it solves an edge case where the controller and it’s odometry instance are destroyed, the destruction results in a similar scenario as the robot initialization where a non-zero old position is essential to maintaining reasonable velocity values / converging to the more accurate velocity values faster.

The PR maintains undefined behavior if the hardware interface shuts down and thus creates a large change in odometry based on a hardware failure, such a case would have still existed with out the fix.