pose_follower: Algorithm tends to oscillate for nonholonomic vehicles

[nxdefiant]

As also reported in #39 the pose_follower tends to oscillate left-right for nonholonomic robots when close to the translation tolerance (tolerance_trans). Consider the following situation as taken from a gazebo simulation of my 4wd robot:

PoseFollower: current robot pose 0.011384 0.006735 ==> -2.764947 PoseFollower: target robot pose 0.011278 0.007573 ==> -0.004133 PoseFollower: diff -0.000209 -0.000818 ==> -1.821162

(units in meter, x axis to the right)

Here, the robots current position (red cross) is close to the goal position (green, most upper cross). Now when the robot turns there is also a bit of translational movement, so sometimes the robot is inside the tolerance_trans, sometimes outside. Now the code in PoseFollower::diff2D() does the following for the given situation in the image:

1. When inside tolerance_trans rotate counter clockwise towards the direction of the goal (early return).
2. When outside tolerance_trans drive towards the goal with includes a clockwise rotation as visualized by the blue cross in the lower left of the image.

Because there is always the fraction of translational movement the robot does 1. and 2. in alternation when its position is close to the tolerance_trans border. Unfortunately I do not have an idea on how to resolve the situation at this moment.