Add allow_changing_direction flag to servo_until_pose #178
Description
As it stands, servo_until_pose keeps moving until it reaches the goal pose. As a result of this, if it overshoots the goal, it changes direction to correct.
However, with MoveToMouth, if the robot moves closer to the user's mouth than it intended, it should just stop -- having it move away from the mouth to correct is unintuitive.
Similarly, it would be better to move acquisition to servo_until_pose, because sometimes it takes some time for the controller to start up so the robot doesn't exactly get the entire duration for the twist. However, even in acquisition, it doesn't make much sense for the robot to move backwards if it overshoots.
Thus, servo_until_pose should take in a flag, allow_changing_direction, that defaults to False. If True, it stores the previous value of a computed twist, and if any of the twists value's sign changes relative to the previous message, it sets that twist element to 0 instead. Essentially what this does is tells the robot that "if you overshoot, don't correct it, instead just complete the motion with the overshot dimension."