Skip to content

Integration of the perception, planning, and control subsystems of an autonomous vehicle using the Robot Operating System (ROS)

License

Notifications You must be signed in to change notification settings

pabaq/CarND-System-Integration

Folders and files

NameName
Last commit message
Last commit date

Latest commit

 

History

5 Commits
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

Repository files navigation

This is the final project of Udacity's Self-Driving-Car Nanodegree. The project description and build instructions can be found here, the required simulator here.

The goal of this project is the integration of the perception, planning, and control subsystems of an autonomous vehicle using the Robot Operating System (ROS) to guide a self-driving car on a simulated highway track while taking into account upcoming traffic signals.

System Architecture

The system architecture depicted above shows all the ROS nodes and topics involved in the project. The main focus lies on the implementation and integration of the following three nodes:

Traffic light detection: This node is part of the perception subsystem. It is responsible for the detection and classification of the traffic lights. If a red signal is detected, the node will publish the position of the traffic light's stop line to the /traffic_waypoint topic.

Waypoint updater: This node is part of the planning subsystem. It is responsible for the generation of a suitable trajectory to be followed by the vehicle. In general, the vehicle will follow the reference trajectory provided by the /base_waypoints topic, and should stop only if a red traffic light was detected. In these cases, the node will update the trajectory in such a way that vehicle comes to a halt at the location specified by the /traffic_wapoint topic. The updated trajectory will be published to the /final_waypoints topic.

Drive by wire controller: This node is part of the control subsystem. It is responsible for the longitudinal and lateral motion control of the vehicle. PID and Stanley controllers are implemented to provide the appropriate throttle, brake, and steering commands that make the vehicle follow the trajectory proposed by planning subsystem above.

In the following, we will take a closer look at the implementation details of each subsystem.

Perception subsystem

Traffic Light Detection

The traffic light detection node is part of the /ros/src/tl_detector/ package and it is implemented in tl_detector.py.

As mentioned above, the node is in charge of detecting the incoming traffic lights and classifying their signal states. In order to do so, it subscribes to the following topics:

  • /base_waypoints - A list of waypoints representing the reference trajectory on the map. Published by the Waypoint Loader node.
  • /image_color - The current RGB image received from the vehicle camera. Published by the simulator.
  • /current_pose - The current position of the vehicle. Published by the simulator.
  • /vehicle/traffic_lights - The positions of all the traffic lights on the map. Published by the simulator.

The node will search for the next upcoming traffic light within a look ahead distance of NUM_LOOKAHEAD waypoints in the front of the vehicle's current pose. If a traffic light is detected, the node will classify its signal state using the images from the incoming camera stream. If the traffic light signal is red, the node will communicate the position of the corresponding stop line. This is done by searching the reference waypoint that is closest to the stop line in the /base_waypoints list, and publishing its index to the /traffic_waypoint topic. This topic will then be used by the Waypoint Updater node in the planning subsystem to update the trajectory such that the vehicle comes to a halt at the stop line.

Traffic Light Classification

The traffic light classifier is implemented in tl_detector/light_classifier/tl_classifier.py. Up to now, the classification is solved by using color thresholding in the hsv color space. Three masks are defined to isolate the red, yellow and green areas in the image. The summation of the active pixels in each mask represent the intensities of each color. Finally, the color with the highest intensity indicates the color of the current traffic light. In the future, we may come back to this and replace this classification approach with a more robust one that uses Tensorflow's Object Detection API.

Obstacle Detection

The obstacle detection node is not required in this project.

Planning subsystem

The planning subsystem is responsible for the generation of a suitable trajectory to be followed by the vehicle. Two nodes are implemented to achieve this.

Waypoint Loader

The waypoint loader node is part of the /ros/src/waypoint_loader/ package and it is implemented in waypoint_loader.py. Its only purpose is to load the reference trajectory to be followed by the vehicle and to publish it to the /base_waypoints topic.

The reference trajectory is represented by a list of waypoints, of which each holds the information about a future target Pose (location/orientation) and Twist (linear/angular velocity) of the vehicle. The reference trajecory simply guides the vehicle along a predetermined path on the highway while maintaining a certain speed limit specified in the corresponding launch file. However, the reference waypoints are static and do not yet allow the vehicle to stop at red lights. To achieve this, they need to updated, and this is performed by the waypoint updater node.

Waypoint Updater

The waypoint updater node is part of the /ros/src/waypoint_updater/ package and it is implemented in waypoint_updater.py.

The node is responsible to update the reference waypoints taking into account the traffic lights in the vicinity. To accomplish this, it subscribes to the following topics:

  • /base_waypoints - A list of waypoints representing the reference trajectory on the map. Published by the Waypoint Loader node.
  • /current_pose - The current position of the vehicle. Published by the simulator.
  • /traffic_waypoint - Index to the reference waypoint in the /base_waypoints list that is closest to the stop line of the upcoming red traffic light. Published by the Traffic Light Detection node.
  • /obstacle_waypoints - Not required for this project.

The node will update the next NUM_LOOKAHEAD reference waypoints in front of the vehicle. If a red traffic light is detected (indicated by the /traffic_waypoint topic), the speed of each waypoint between the current position of the vehicle and the stop line will be reduced using a smooth decelerating curve. Otherwise, the original reference waypoints are kept unchanged and the vehicle will continue to follow its designated trajectory. The list of updated waypoints are published to the /final_waypoints topic that will be used by the control subsystem.

Control subsystem

The control subsystem is responsible for the longitudinal and lateral motion control of the vehicle. Two nodes are implemented to achieve this.

Waypoint Follower

The waypoint follower node is part of the /ros/src/waypoint_follower/ package and it is provided for this project using code from Autoware.

The node is responsible to generate the linear and angular target velocities that the vehicle shall adopt. To do this, it uses the updated trajectory that is published to the /final_waypoints topic by the Waypoint Updater node . The determined target velocities are then published themselves to the /twist_cmd topic in form of Twist messages. The target velocities will be used by the Drive by Wire node to generate approriate control commands.

Drive by Wire

The Drive by Wire node is part of the /ros/src/twist_controller/ package and it is implemented in dbw_node.py.

The node is responsible for the generation of appropriate throttle, brake, and steering commands that make the vehicle follow the updated trajectory proposed by planning subsystem. To achieve this, it subscribes to the following topics:

  • /base_waypoints - A list of waypoints representing the reference trajectory on the map. Published by the Waypoint Loader node.
  • /current_pose - The current position of the vehicle. Published by the simulator.
  • /current_velocity - The current linear and angular velocities of the vehicle. Published by the simulator.
  • /twist_cmd - The target linear and angular velocities for the vehicle. Published by the Waypoint Follower node.
  • /vehicle/dbw_enabled - The drive by wire status (true/false). Published by the simulator.

The node uses the Controller class implemented in twist_controller.py to generate the throttle, brake and steering commands and publishes them to the /vehicle/throttle_cmd, /vehicle/brake_cmd, and /vehicle/steering_cmd topics. These are then used as input for the simulator.

Lateral Control

A Stanley controller is implemented in stanley.py to take care of the lateral motion control. The Stanley method is the path tracking approach used by Stanford University’s Darpa Grand Challenge team. The theoretical details of this controller can be found in [1].

The Stanley method uses the front axle as its reference point and simultaneously corrects the heading error ψ and the cross-track error e, while obeying the maximum steering angle bounds δmin and δmax . It requires the two gains k and ks, that typically need to be determined experimentally. After some tuning in the simulator the gains were chosen as follows:

K = 0.1
Ks = 1.0

Longitudinal Control

Two seperate PID controllers are implemented for the longitudinal motion control, one of which generates the throttle commands and the other the brake commands. The speed error between the target and current velocity of the vehicle determines which of both controllers is active. For instance, if the speed error is positive, i.e. the vehicle is too slow, the throttle controller is activated and vice versa. Throttle and brake commands cannot be applied simultaneously. A low pass filter is implemented to remove the high frequency noise from the incoming vehicle velocity.

The simulator requires the throttle commands to be normalized to [0, 1], wheras the brake commands must be provided as a physical torque value. The torque can be computed as T = m * a * r , with the vehicle mass m, the deceleration a, and the wheel radius r, whereby the control value returned by the brake controller is used as deceleration a.

After some tuning in the simulator the gains were chosen as follows:

# PID gains for the throttle controller
Kp = 0.3
Ki = 0.1
Kd = 0.0

# PID gains for the brake controller
Kp = 0.2
Ki = 0.0
Kd = 0.1

The integral gain of the throttle controller ensures a higher accuracy to maintain the target speed while driving. However, no integral gain was defined for the brake controller, since no residual brake demand is required when the speed target changes, e.g. due to a traffic light switching from red to green.

References

[1] Hoffmann, Gabriel M., Claire J. Tomlin, Michael Montemerlo, and Sebastian Thrun. Autonomous Automobile Trajectory Tracking for Off-Road Driving: Controller Design, Experimental Validation and Racing. American Control Conference, 2007, pp. 2296–2301

Releases

No releases published

Packages

No packages published