Development of path planning, controls and Gazebo simulation for autonomous parking

1.0) Background Information
Automatic parking or called as autonomous parking is a car manoeuvring system that move a car from traffic lane into parking slot autonomously, but some car still required the driver to step on the brake for safety reason, and some car is able to park by itself just by pressing a Park button in the car or using application in mobile device.
A car is a non holonomic system which means that the number of control commands available is less than the number of coordinates that represent its position and orientation. Which means a non holonomic robot cannot move laterally.
Automatic parking requires the car to use sensors to get the information and measurement about its environment during auto parking, example the measuring the car park size.
The first prototypes of automatic parallel parking was developed on an electric car Ligier at INRIA in mid 1990. Nowadays many car company have develop their own automatic parking system, company that have develop this system are Toyota, Volkswagen and Volvo.
2.0) Introduction
2.1) Introduction to Path Planning
Path planning is producing a path from one state to another. When path planning task is being requested, path planning need to know the start position and goal position in order to plan a path. Path planning have to produce a path that avoid obstacle, finding a shortest and most optimal path from the start position to goal position. Some example of path planning algorithm are Dijkstra, A* and Rapidly Exploring Random Trees.
2.2) Introduction to Auto Parking
Auto parking is an autonomous car manoeuvring system, which this system can move a vehicle from a traffic lane into a parking spot. The auto parking can perform 3 types of parking which are parallel, perpendicular and angle parking.
Objective of auto parking is to enhance the comfort, safety and convenient of driving, especially in constrained environment. Example in urban cities where parking slot spaces are small. Auto parking requires the use of sensors and these sensors have names like Park Distance Control, Park Assist and EPS.
Sensors are placed at the front and back of the car to detect objects around the car.
These sensors send signals that will bounce off the object and the signal will come back to the car, which the system will calculate time taken for signal to return, this allows the system to calculate the object around the car. With these information received by the sensors, the computer is able to control the steering wheel and the brake pedals of the car.
2.3) Introduction to Types of Sensor

There are 2 types of sensors which are Ultrasonic Sensor and Electromagnetic Sensor.
The Ultrasonic Sensor will emit radio wave or ultrasonic wave to detect object around the vehicle. But some objects that are too small or flat object like narrow pole, the sensor cannot detect it as the waves that are emitted cannot reflect back to the sensor.
The returned wave are registered and analysed by a computer that is inside the car. The computer will note 2 factors to determine the size and proximity of the object that is behind the car. This 2 factors are the time taken for the wave to return to the sensor and the changes in length of the wave. If wave returned quickly it means that the car is close to an object, the computer will produce an audio alarm signal to inform the driver when car is near to an object.
Ultrasonic Sensor system required at least 4 to 6 individual sensor mounted on outside of the car bumper, this means that the sensor are placed at different part of the car. But the Ultrasonic Sensor accuracy and capability of detecting objects will be affected if there is dirt and grime on the sensor as it disrupts the Ultrasonic Sensor signals. And there are blind spots that Ultrasonic Sensor may not be able to detect.
Electromagnetic Sensors use electromagnetic wave. Electromagnetic Sensors uses a transceiver strip which will generate an elliptical field behind the car. When an object that meet a certain mass requirement, it will disturb the elliptical field and the detector will pick up the voltage change and send this information to the computer in the car.
The computer will analyse this data and determine the distance of the object, and when the car is near to the object the computer will produce an audio alarm to alert the driver that the car is close to an object, the computer will produce a louder audio alarm tone when the car is nearer to the object.
Electromagnetic Sensor are mounted inside of the car bumper instead of outside of the car bumper, which means there is no dirt that will interfere the system accuracy and capability on the detection of sensor.
Both Ultrasonic Sensor and Electromagnetic Sensor are able to detect moving object as well. If a moving object is near to the sensory area, these 2 sensor will produce a continuous audio alarm, and the audio alarm will stop when the object is outside of the sensory area. Moving objects are like human, but if there is a tow bar in the sensory area the audio alarm will not sound.
In conclusion, I think that by using both Ultrasonic Sensor and Electromagnetic Sensor will be better as it can solve the problem of dirt and grime that is on the sensor. By using 2 types of sensor it provides grater and better detection capability and is more accurate.
But for the NYP car we are not using Ultrasonic Sensor and Electromagnetic Sensor, we are actually using 2D Lidar and 3D Velodyne to detect obstacles around the surrounding.

2.4) Introduction to Parallel and Perpendicular Parking
This picture actually show the movement of parallel parking. Parallel parking is harder as compared to perpendicular parking, as parallel parking manoeuvre the car in a S shape movement. Where a car first have to be in parallel with a parked car in the carpark and the car must not be too close to the parked car, which means both car are separated by distance y, this distance y is to prevent both car to get scratches during parking.
This picture actually show the movement of parallel parking.There are two ways of perpendicular parking. One I call it L-shape parking as the below left picture shows, another way I call it vertical parking as the below right picture shows. There is no right way for perpendicular parking but the vertical parking is just how the human will park the car in the daily life.

2.5) Introduction to path planning algorithms
There are two types of path planing which are dynamic path planing and static path plaining. Dynamic path planing is aimed to avoid unexpected obstacles along the robot’s trajectory to reach the goal. There are some algorithms like R*, Bug Algorithms, APF and so on if need can search online to see how does it work. However, for my auto-parking project, static path planning is the most suitable which is base on a map and goal location, finding a geometric path. The method i using in my project is geometric method what requires more mathematical calculation. https://www.slideshare.net/dare2kreate/dynamic- path-planning
2.6) Introduction to Geometric method
Geometric path planning is a robust alternative algorithm for computing a collision free path connecting the initial and final configurations of a robot with a minimal number of waypoints in 2D and 3D environments. It provides a geometric description of the robot motion given a mapping and a description of the obstacles in the workspace. The algorithm outputs the (x,y,z) coordinates of the path in the workspace which is then passed to a custom inverse kinematics block to compute the revolute and prismatic joint variables in the configuration space. I illustrate the application of this algorithm by designing fixed path for perpendicularly parking and parallel parking. In my project I enable the car follow the path I designed and can park into the parking slot.
3.0) Simulators
3.1 Stage Simulator
The Stage Simulator is a 2D simulator, I choose to use Stage Simulator because many of the online users actually suggested beginners to use Stage Simulator instead of Gazebo Simulator for beginner level as it is easy to use.
With the Stage Simulator we are able to move a robot by use teleop or by sending a 2D Navigation Goal. The Stage Simulator is also able to be visualized in Rviz, and the map used in Stage Simulator can be changed by drawing or change the configuration file (world file).
Please refer to the right picture below, as we can see the robot in Stage Simulator detect the obstacle in front of it and it can be visualized in Rviz (Zoom In Version). The teleop panel allow us to move the robot by pressing the keyboard.
I think that the Stage Simulator is a very good platform for beginners to learn what is a 2D simulator looks like, as in Stage Simulator they have a teleop panel that allows beginner to move the robot around the map, the map can also be changed which this let beginners to get a feel of what a 2D simulator is like.

3.2 Gazebo Simulator
The Gazebo Simulator that I used was found in the Learning ROS For Robotics Programming book, Robot simulation is an essential tool in every roboticist’s toolbox. A well-designed simulator makes it possible to rapidly test algorithms, design robots, perform regression testing, and train AI system using realistic scenarios. Gazebo offers the ability to accurately and efficiently simulate populations of robots in complex indoor and outdoor environments. At your fingertips is a robust physics engine, high-quality graphics, and convenient programmatic and graphical interfaces. Best of all, Gazebo is free with a vibrant community. The Gazebo Simulator is which company that do path planning use this simulator as they provide a more realistic environment.

3.3) Stage Simulator VS Gazebo Simulator
I have tried both Stage Simulator and Gazebo Simulator. I think that the Stage Simulator which is a 2D simulator is a good platform for beginner to start on, if the Stage Simulator is familiarize already then we can upgrade to the Gazebo Simulator, as Gazebo Simulator is a 3D simulator which provide a more realistic environment which is good for visualization. Example Gazebo Simulator car robot and car park environment are in 3D as compared to Stage Simulator which only provide a car robot and car park environment in 2D.
I tried to compare Stage Simulator and Gazebo Simulator, and I have found out that the Stage Simulator which is a 2D simulator takes 2 second to open up when I run it and the processor wake up per seconds is 2000. The Gazebo Simulator which is a 3D simulator takes 6 seconds to open up and the processor wake up per seconds is 7000.
Stage Simulator is able to open up at a fast timing and it does not crash that often, even if we were to add more obstacle to the 2D map in Stage Simulator, it will not crash and it will not increase computation time. I would recommend beginner to use Stage Simulator as it is easier to use and learn and their tutorial can be easily understand.
Gazebo Simulator requires more time to open up, and sometimes when I run Gazebo Simulator it will crash, if we were to add in more obstacle in the map it will definitely require more computation time as it is a 3D simulator, and it will crash more often which means that we need to use a more better PC which require more money.
I would recommend to use Gazebo Simulator if we want to simulate a more realistic environment, to add obstacle in Gazebo Simulator it cannot be drawn like in Stage Simulator. The obstacle must be added in the simulator itself, which means that we have to go through their tutorial in order to do so, which it will take some time to learn.
In conclusion we have decide that we will be using Stage Simulator for my project. As Stage Simulator is an easier version to be use, it requires lesser computation time and does not crash that often. Most importantly the map in Stage Simulator can be drawn to simulate obstacle.

4.0) Path planning
4.1) Parallel parking
First, Setup your ros environment install from this website and follow the steps: http://wiki.ros.org/indigo/Installation/Ubuntu
After that, create your own workspace folder, follow this website
http://wiki.ros.org/catkin/Tutorials/create_a_workspace
After that before start this project I advise you can go through all beginning level rod tutorials and read through the chapter2 and chapter9 of the book.
This is the paper where can get reference to develop the codes for parallel parking using Geometry Method.(below left)
http://ac.els-cdn.com/S1474667016436852/1-s2.0-S1474667016436852-main.pdf?
_tid=a0d3f6c0-7598-11e7-8b2b-00000aab0f26&acdnat=1501468585_14c9cf64aa26c892e872aa4 74d4d474f

According to what the previous students had done I already get such a path as the picture shown upper right which is exactly the same as the reference. My first job is to modify this path to do some enhancement.
As you can see the turning angle for his path now is about 59 degrees. The turning angle is defined as the angle between the tangent line of the intersection point of the two circle and the horizontal line.
Normally for human driving this turning angle should be 45 degrees because if the angle is too large it is not easy for the vehicle to turn such fast and it need to stop at the intersection point to make another direction full turn which is not a continuous process. To reduce the turning angle there is one way to reduce the y distance shown in the graph.

The y distance is the safe distance between two cars. Image below shows the distance before modification is 2.4m.
To modify this distance or any other parameters we can use parameter configuration which will be very convenient than we zoom into the code to change these and recompile again every time. To use parameter reconfiguration firstly we need a yaml file which contains all parameters you want to modify later. In my case I just put two parameters inside that are radius and safety distance as the left picture below. There is one thing should be very careful that the format of the yaml is very important. Like there must have at least one space before each parameter name. Meanwhile the first character of each parameter name have to be aligned very well otherwise there will be some strange error.

After we generated the yaml file I should add these two lines in the source code. You can find my code in /catkin_workspace/src/FYP/geometric/src/geometric.cpp
These two line make the parameters inside the yaml file work. You may find there are two names within the bracket. The first name is the name I defined in the yaml file while the second name is same as the name I defined in the header file shown below and the followed number is the default value for these two parameters.

Finally to test this parameter configuration just add this line at the end of source code to see any difference happen.
Now, just modify the value in yaml file and click save then just
roslaunch teb_local_planner_tutorials geometryic.launch
no need catkin_make again you will see the value of radius is shown on the terminal as the picture below which means the parameter reconfiguration is working.

With the help of parameter reconfiguration now I can easily change the value. I found that in order to modify the angle to 45 degrees the safety distance should be 1.46m.
However this doesn’t make sense as the distance between two cars are only 0.06m now which is impossible.
Hence, I come out with another solution. With reference to the paper, we know that parking a car into the parallel parking lot requires 2 circle. Now I separate these two circles and connect them with one straight lin