Ensure the robot state is up-to-date before Servoing

[AndyZe]

Description

Previously we didn't wait to get a current robot state before servoing began. That caused the initial joint angles to be all-zero, which caused a small jump at the beginning of motion. This is a fix.

Test with:

ros2 launch moveit_servo demo_ros_api.launch.py

ros2 run moveit_servo servo_keyboard_input

[sea-bass]

Fixes look good at a glance!

One thing to watch out for, from my own experience, is what happens if you pause/stop servo, move the robot through some other free-space motion planner, and then resume servo. (Maybe @pac48 and @sjahr can test with MoveIt Pro and double check?)

Far as I remember, this was one reason for the liberal sprinkling of resetSmoothing(), but I don't doubt this PR fixes that. I just want to make sure it wasn't pruned too much to affect the interaction with other motions.

Essentially, there are 2 alternatives:

1. Keep the smoothing going even when servo is stopped,
2. Ensure that transitioning servo from stopped to not stopped triggers a reset to the latest state when restarting.

There are merits to both, but just wanted to bring it up and have people test this case.

[AndyZe]

One thing to watch out for, from my own experience, is what happens if you pause/stop servo, move the robot through some other free-space motion planner, and then resume servo. (Maybe @pac48 and @sjahr can test with MoveIt Pro and double check?)

I had to add one line to fix this. Good catch. Here's a video of several cycles of Servo -> pause -> joint command -> unpause -> Servo again

servo_pausing.webm

[sea-bass]

Awesome, thanks for that test!

[sjahr]

Nice work, thanks!