This repository includes the planning algorithm based on frenet optimal polynomial and the commonly used control algorithms for vehicle motion like PID, LQR, MPC and other algorithms based on geometry model like Stanley, Pure Pursuit and Rear Wheel Feedback.
Two scenarios are tested here:
- forward driving in curved road with differernt velocity and radius
- reverse driving in curved road
The velocity of test trajectory is in the range from 10m/s to 20m/s, a part of result plot is post in below, all the plots can be found in the folder named result_plot.
The simulation is conducted in GAZEBO, the vehicle model and the senario are obtained from the Demo of Prius in ROS/GAZEBO(https://github.com/osrf/car_demo.git).
It is recommended to switch off the planning module to observe the performance of control module better by modifying the Activate_planning_function in src/plan/conf/planning_conf.txt to 0;
~$ cd catkin_ws/src/multi_launch/
# forward running in an empty world with smooth, artificial trjaectory
~/catkin_ws/src/multi_launch/$ ./forward.sh
# reverse running in an empty world with smooth, artificial trjaectory
~/catkin_ws/src/multi_launch/$ ./reverse.sh
# running in an simulated environment with recored trajectory
~/catkin_ws/src/multi_launch/$ ./simulated_world.sh
# running in an simulated environment with ramp
~/catkin_ws/src/multi_launch/$ ./run_in_ramp.sh
~$ cd catkin_ws/src/plan/launch/
# change the trajectory in empty world
~/catkin_ws/src/plan/launch$ gedit publish_mat_trajectory.launch
Modify the value of test trajectory file path to change the test trajectories. The avaliable test trajectories can be found in the folder named "test_trajectories".
In this folder, the trajectory file starting with "mat" means that it is a artificial trajectory, and the recorded trajectories are recorded in the GAZEBO senario, which are not so smooth, but closer to the real situation.
The last num of trajectory file name means the velocity of vehicle running in curve, only for the "mat trajectories", the velocity of recorded trajectories are not constant.
Recording a trajectory is a good method to generate an availble trajectory in the city environment.
- find the plugin code
~$ cd catkin_ws/src/control/plugins/
~/catkin_ws/src/control/plugins$ gedit PriusHybridPlugin.cc
-
Switch on the recording function
correct the record_enabled into True -
roslaunch
~$ roslaunch control record_trajectory.launch
-
record trajectory
"W","A","D" are the control keys
"S" is "reverse"
"E" is "brake" -
Get recorded trajectory
The recorded trajectory is stored in the folder (catkin_ws/src/car_demo/recorded_trajectories), you can trim it and move it to the test_trajectories folder.
~$ cd catkin_ws/src/control/conf/
~/catkin_ws/src/control/conf$ gedit control_conf.txt
This file includes the physical parameters of vehicle model (can not be modified) and all the parameters (can be adjusted) required by the controllers and other important objects necessary during the operation of control module.
The controller can be classified into the distributed controllers (control longitudinal and lateral motion respectively) and centralized controllers (control the vehicle motion coupled).
To active distributed controller, the CONTROLLER_NUM must be 2, the next step is to set the LONGITUDINAL_CONTROLLER and LATERAL_CONTROLLER, the options are listed below them.
To active centralized controller, the CONTROLLER_NUM must be 1, the next step is to set the CENTRALIZED_CONTROLLER.
Since the FPS is low if we plot more than one figures simultaneously, so now there is only one plot can be shown during the simulation (after simulation, we can get all the plots, see Other Plot), the steps to choose the plot shown is:
~$ cd catkin_ws/src/control/scripts/
~/catkin_ws/src/control/scripts$ gedit TrajectoryPlotter.py
change the TARGET_PLOT to choose which plot you want to see during simulation, now there are three options
- trajectory comparison
- velocity comparison with station
- heading angle comarison with station
- Longitudinal Controllers
- Lateral Controllers
- Centralized Controllers
- MPC (working in process)
- Error Comparison
- Velocity Error Comparison
- Lateral Distance Error Comparison
- Heading Error Comparison
Pure Pursuit Controller V=10m/s
Pure Pursuit Controller V=15m/s
Stanley Controller V=10m/s
Stanley Controller V=15m/s
Rear Wheel Feedback Controller V=10m/s
Rear Wheel Feedback Controller V=15m/s
LQR Controller V=10m/s
LQR Controller with prediction V=10m/s
LQR Controller with prediction V=15m/s
MPC Controller V=10m/s
~$ cd catkin_ws/src/control/plot_py/
~/catkin_ws/src/control/plot_py$ python result_plot.py
According to the output simulatin result, the scrtipt can plot the result of path tracking, velocity tracking and heading tracking, as well as the figures of error.