Handling obstacles in TUG demo

[vvasco]

This PR modifies managerTUG in order to deal with obstacles along the path during the TUG. If an obstacle is found within a radius of 1.5m around the robot (within the rear and front laser FOVs), the interaction is frozen and the robot asks to remove the obstacle. The interaction does not start until the obstacle is removed (within a timeout).

Here's a video showing the functionality in Gazebo (low quality audio and slow interaction as it runs on my system). The scenario includes two obstacles, that are the nurse in front of the robot and the visitor behind it:

• when the robot moves towards the finish line, the nurse falls within the obstacle detection radius and thus the interaction is frozen. To simulate that the obstacle moves away from the obstacle detection radius, I manually move it in Gazebo and the interaction starts again.
• when the robot starts tracking the human skeleton and moves backwards, the visitor falls within the obstacle detection radius and the interaction is frozen. Since the obstacle is not removed within the timeout, the interaction stops.

[vvasco]

The seated person should not be considered as an obstacle, since the robot navigates keeping a distance from the person skeleton higher than the radius for the obstacle detection.

I'll have a look at the failures on the code quality before merging.

[vvasco]

Fixed BCH issues and Codacy duplications (Codacy still complains, but it's related to yarp infrastructure). Waiting for CI to finish and then merging.