Navigation during TUG

[vvasco]

This PR integrates navigation into the TUG. Before starting the TUG, a localization phase described below is required.

Localization The robot has to localize itself with respect to the world, which is set from the start-line. For doing so, we bring the robot through the joystick to a spot where the line to detect is clearly visible (we adjust the head pitch through the motor gui). Start-line should be detected first, as finish-line is expressed with respect to it. Then we connect to /lineDetector/cmd:rpc and send the commands:

• detect start-line: to detect the start-line;
• detect finish-line: to detect the finish-line.

Both commands are blocking, waiting until a good estimate of the line is provided. Line is considered good when the median filter has enough samples for the estimation and, when so, the detected is added to the opc. To retrieve lines' poses with respect to the world, we can use the commands get_line_pose start-line and get_line_pose finish-line (the start line will be at 0.0, 0.0, 0.0).

TUG After the localization phase, we bring the robot to an initial spot facing the chair and give a start to /managerTUG/cmd:rpc. The robot navigates as follows:

• the robot explains the test and navigates to the finish line;
• during the exercise, the robot tracks the skeleton while navigating on a straight path;
• when the exercise finishes, the robot goes back to the initial position.