Test lidar in simulation about assistive-rehab

[vvasco]

Below we can see how obstacles are detected in simulation.
On the left terminal there's obstacleDetector running, on the right the commands provided to navController.
At the moment, the obstacle is a human model created here, since the actor we use for the demo is not seen by lasers (as described in #286). The human model is useful to see how legs are perceived by lasers

obstacleDetector clusters the data received from the lasers, splitting them by means of a Euclidean distance. When the robot reaches a threshold distance (1.5m) from the closest obstacle, obstacleDetector sends a stop to navController. Only if the obstacle disappears, the robot can move again.

One obstacle in front of laser front

When starting, the obstacle is at 3m in front of the robot.
obstacleDetector detects two obstacles (the two legs), with the closest at ~2.9m.

Note: the legs can be detected as a single obstacle or as two obstacles depending on the distance from the robot. The closer the robot, the more the legs are seen as two obstacles.

When sending the command to navController for moving the robot at 5m from the starting position along the x, the distance from the obstacle starts decreasing and the robot stops when the distance threshold is reached (1.5m).

When the obstacle is removed, obstacleDetector does not detect any obstacle anymore and the robot is free to move.

One obstacle in front of laser back

This is the same scenario as before, with the obstacle being behind the robot and thus perceived by the back laser.

Several obstacles

In case of several obstacles, the robot stops when it approaches the closest.
obstacleDetector detects 3 obstacles (2 legs from the closest and a single obstacle from the furthest) from laser front and 1 from laser back.

[pattacini]

Super sweet 🚀

I've noticed how the legs are detected sometimes as 1 sometimes as 2 distinct obstacles. At this stage, we're not reconstructing the person entity from the laser detections so it's good 👍

As agreed, when the obstacle disappears, the supervisor will be responsible for driving the robot again toward the original target 👍