P3 Autoparking

 In this practise we have to make an autonomus car park in a place between two cars. For this task we have three Lidars that would help it make the maniouver to place correctly without chrashing with anything.

To make this exercise im going to divide the problem into three main parts:

1. Detect the place

2. Position the car to start the maneuvering

3. Maniouvers

1. Detect the place

Before be able to detect the place we should know where hare positioned the Lidars and which information they give to us.

The platform provides us three functions:

• HAL.getFrontLaserData() - to obtain the front laser sensor data It is composed of 180 pairs of values: (0-180º distance in millimeters)

• HAL.getRightLaserData() - to obtain the right laser sensor data It is composed of 180 pairs of values: (0-180º distance in millimeters)

• HAL.getBackLaserData() - to obtain the back laser sensor data It is composed of 180 pairs of values: (0-180º distance in millimeters)

But, where is the value that corresponds to 0 and where the one that corresponds to 180?

This is the first question we have to solve before anything.

In the picture above you can see what is exactly what the lidar return. With this info now the car can detect when is a free space or not.

A first aproximation is check the central values from the right laser (from 40 to 140 more or less) to know if there is apparently any free park. 

But i thought i could make the detection more robust if i use the information provided by the front and back laser. So i don't only use the right laser, i check the front and rear laser too. In case of the front i would look to the right values of the laser. And for the one that is at the back i look for the left side.

If in that hole space the laser data gives a minimum number of positions in the array that holds a value of inf (which means that there is not detecting anything), then it can park there.

Perfect, now the car is able to detect if there is free space to park. 

Here i leave you some videos of how it works. In the videos you will see that when is detecting the place it commands a slower lineal velocity.

First taking into account only the laser on the right side:

Taking into acount the three lasers as i explained before:

2. Position the car to start maneuvering

Once the car has detected a free space it should position in order to park the car. The way i programm this is using the back laser. The car will move forward until the left side of the laser detect a big distance or even if it detect infinite.

As we can see in the above picture here is detecting the car behind it but the distance is more than x so it has a good position to start manouvering. In my case to start the maniouvers it has to be a distance more or equal to the value that gives maxRange-1.

But when i calculate the distance i cannot have infinite numbers, becouse as soon as one appears the total distance would be infinite, and will start parking when it dont have space already. 

In this last, gif as soon as it detect a place an the green laser return one infinite beam (in the zone we are calculating the distance) it start maniouvering.

So i calculate the distance only of thouse how has a value different than infinite. Also i check if all the left side return inifinites, in that case it can also start parking.

In this last case the laser suddenly start sensing that there is no car (it returns inifite), as you can see in the laser data (the right side of the green lines) so the car can start maniouvering.

Here is a video of how it is positioned in a normal case (to make it more visually, it slow down when detects the parking place):

3. Maniouvers

The car is ready to start parking. Let's programm the maniouvers to complete it task.

I resolve this using some if-else conditions as if it were a switch case conditions. 

• Case 0:

First of all the car should turn the steering wheel to the right and at the same time go backwards. It will make this until it reach an angle of 50 degrees. So in this case i command a negative lineal velocity and a positive angular velocity.

• Case 1: 

Once it reach the 50 degrees it will go backwards (without applying any angular velocity) until the right laser at its left detects that there is a car. In that case we have to make next maniouver.

The above image is the moment of transition to the next case. The yellow segemted line represents the area that is waiting to detect something to go to the next case.

• Case 2:  

It has detected the car that is in front of it with the right laser. Now it has to straighten the car in the place applying a negative linear velocity and a negative angular velocity. Until the center zone of the front laser detects a car (when it happens means that has the front car centered).

The above image is the moment of transition to the next case. The red segemted line represents the area that is waiting to detect something to go to the next case.

• Case 3: 

Now it only has to go forward until the distance between it and the front car is the same as the distance between it and the car behind. Also if the car is not straight (the orientation gives a number different than 0) it would apply a little angular velocity to improve it's position.

• Case 4:

The car has finish its maniouvering to park in the detected place so it has to command 0 lineal velocity and 0 angular velocity.

Here we can see step by step all this cases:

 

Also to avoid any collision i made two conditions for emergency stops. In case when the car is going forward or backwards any of the cars behind or in front decides to move.

Emergency stop behind:

Emergency stop in front:

While it is going backwards

Once it start going forward

First aproximation final result:

It can also park in other situations as for example:

It will have a little bit more dificulties but it can do it.

But i decide to make a more robust parking so i changed some things:

First in the Case 1 instead of look to the front car as a reference i will watch for the one that is at the back. So when it is detecting something at a certain distance less than x on right side of the back laser, it will change os case.

Other small change is that for transition to the third case it will look the orientation. If its orientation is more or less 0 it will start the next maniouver. Also to avoid collide i check the backwars laser so if the distance to the back car is less than x it will start making the next maniouver.

And the last modification is that now instead of make the car park in the middle of the park it will be more close to the front car. With this we can optimise the amount of cars that can park.

With this last changes we obtain:

When there is no front car:

When there is no back car: