├── .gitignore ├── Dockerfile ├── README.md ├── data ├── churchlot_with_cars.csv ├── filter.csv ├── grasshopper_calibration.yml ├── maptf.launch ├── sim_waypoints.csv ├── sitesim.csv ├── wp_yaw.txt └── wp_yaw_const.csv ├── imgs ├── autoware_computing.png ├── autoware_tf1.png ├── autoware_tf2.png ├── open_simulator.png ├── select_waypoint.png └── unity.png ├── requirements.txt └── ros ├── .catkin_workspace ├── launch ├── dbwtest.launch ├── site.launch ├── sitesim.launch ├── styx.launch └── tltest.launch └── src ├── CMakeLists.txt ├── camera_info_publisher ├── CMakeLists.txt ├── launch │ └── camera_info_publisher.launch ├── package.xml └── yaml_to_camera_info_publisher.py ├── styx ├── CMakeLists.txt ├── bridge.py ├── conf.py ├── data │ └── .gitattributes ├── launch │ └── server.launch ├── package.xml ├── server.py └── unity_simulator_launcher.sh ├── styx_msgs ├── CMakeLists.txt ├── msg │ ├── Lane.msg │ ├── TrafficLight.msg │ ├── TrafficLightArray.msg │ └── Waypoint.msg └── package.xml ├── tl_detector ├── CMakeLists.txt ├── launch │ ├── tl_detector.launch │ └── tl_detector_site.launch ├── light_classification │ ├── __init__.py │ ├── model │ │ ├── sim_graph.pb │ │ ├── sim_labels.txt │ │ └── site_graph.pb │ ├── test_data │ │ └── test_green1.png │ └── tl_classifier.py ├── light_publisher.py ├── package.xml ├── sim_traffic_light_config.yaml ├── site_traffic_light_config.yaml ├── sitesim_traffic_light_config.yaml └── tl_detector.py ├── twist_controller ├── CMakeLists.txt ├── dbw_node.py ├── dbw_test.py ├── launch │ ├── dbw.launch │ ├── dbw_sim.launch │ └── dbw_test.launch ├── lowpass.py ├── package.xml ├── pid.py ├── twist_controller.py └── yaw_controller.py ├── waypoint_follower ├── CMakeLists.txt ├── include │ ├── libwaypoint_follower.h │ └── pure_pursuit_core.h ├── launch │ └── pure_pursuit.launch ├── lib │ └── libwaypoint_follower.cpp ├── package.xml └── src │ ├── pure_pursuit.cpp │ └── pure_pursuit_core.cpp ├── waypoint_loader ├── CMakeLists.txt ├── launch │ ├── waypoint_loader.launch │ ├── waypoint_loader_site.launch │ └── waypoint_loader_sitesim.launch ├── package.xml └── waypoint_loader.py └── waypoint_updater ├── CMakeLists.txt ├── launch └── waypoint_updater.launch ├── package.xml └── waypoint_updater.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.py[cod] 2 | 3 | build 4 | devel 5 | 6 | profile.tmp 7 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | # Udacity capstone project dockerfile 2 | FROM ros:kinetic-robot 3 | LABEL maintainer="olala7846@gmail.com" 4 | 5 | # Install Dataspeed DBW https://goo.gl/KFSYi1 from binary 6 | # adding Dataspeed server to apt 7 | RUN sh -c 'echo "deb [ arch=amd64 ] http://packages.dataspeedinc.com/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-dataspeed-public.list' 8 | RUN apt-key adv --keyserver keyserver.ubuntu.com --recv-keys FF6D3CDA 9 | RUN apt-get update 10 | 11 | # setup rosdep 12 | RUN sh -c 'echo "yaml http://packages.dataspeedinc.com/ros/ros-public-'$ROS_DISTRO'.yaml '$ROS_DISTRO'" > /etc/ros/rosdep/sources.list.d/30-dataspeed-public-'$ROS_DISTRO'.list' 13 | RUN rosdep update 14 | RUN apt-get install -y ros-$ROS_DISTRO-dbw-mkz 15 | RUN apt-get upgrade -y 16 | # end installing Dataspeed DBW 17 | 18 | # install python packages 19 | RUN apt-get install -y python-pip 20 | COPY requirements.txt ./requirements.txt 21 | RUN pip install -r requirements.txt 22 | 23 | # install required ros dependencies 24 | RUN apt-get install -y ros-$ROS_DISTRO-cv-bridge 25 | RUN apt-get install -y ros-$ROS_DISTRO-pcl-ros 26 | RUN apt-get install -y ros-$ROS_DISTRO-image-proc 27 | 28 | # socket io 29 | RUN apt-get install -y netbase 30 | 31 | RUN mkdir /capstone 32 | VOLUME ["/capstone"] 33 | VOLUME ["/root/.ros/log/"] 34 | WORKDIR /capstone/ros 35 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![Udacity - Self-Driving Car NanoDegree](https://s3.amazonaws.com/udacity-sdc/github/shield-carnd.svg)](http://www.udacity.com/drive) ![completion](https://img.shields.io/badge/completion%20state-100%25-blue.svg?style=plastic)


2 | ![level5-engineers](https://avatars1.githubusercontent.com/u/31551095?v=4&s=100) 3 | ### level5-engineers 4 | CarND Capstone Project: System Integration
5 | It’s not about the pieces but how they work together. 6 |
7 | 8 | #### Team 9 | 10 | * James Dunn, lead 11 | * Oleg Leizerov 12 | * Aman Agarwal 13 | * Rajesh Bhatia 14 | * Yousof Ebneddin 15 | 16 | ### Objective 17 | Create code to drive a vehicle in both a Unity-based simulator and a real-world Lincoln MKZ around a closed-circuit test-track. This repository contains all ROS nodes to implement the core functionality of an autonomous vehicle system. 18 | 19 | ### Results 20 | From the Udacity review: "Excellent work here! The car drove very smoothly around the waypoints, and made a full stop for the red light. Well done!" 21 | - Video from the dash camera onboard the test vehicle: on Vimeo (Drive-by-wire is engaged at 2s and disengaged at 38s.) 22 | - Point cloud vizualization: on Vimeo 23 | - A map of the on-site test run: here 24 | - Log file, ROS bag, and feedback: here 25 | 26 | Below is a visualization of the lidar point cloud from the team's test run on the autonomous Lincoln. 27 | 28 | ![Point cloud visualization](https://github.com/level5-engineers/assets/blob/master/images/pcgif.gif?raw=true) 29 | 30 | 31 | ### Implementation Notes 32 | 33 | The diagram below illustrates the system architecture. The autonomous vehicle controller is composed of three major units: perception, planning, and control. 34 | 35 | ![System Architecture](https://raw.githubusercontent.com/level5-engineers/assets/master/images/SystemArchitecture.png) 36 | Legend: the letters a-k indicate published ROS topics 37 | 38 | ``` 39 | a: /camera/image_raw 40 | b: /current_pose 41 | c: /current_velocity 42 | d: /vehicle/dbw_enabled 43 | e: /traffic_waypoint 44 | f: /base_waypoints 45 | g: /final_waypoints 46 | h: /twist_cmd 47 | i: /vehicle/throttle_cmd 48 | j: /vehicle/brake_cmd 49 | k: /vehicle/steering_cmd 50 | ``` 51 | 52 | ### Perception 53 | We employ the MobileNet architecture to efficiently detect / classify traffic lights. We applied transfer learning to further train two convolutional neural networks for the different modes of operation: 54 | - simulator mode: classifies whole images as either `red` or `none`. The model was trained with several datasets using the Tensorflow Image Retraining Example (tutorial: https://goo.gl/HgmiVo, code: https://goo.gl/KdVcMi). 55 | - test-site mode: we employ the "SSD: Single Shot MultiBox Detection" framework to locate a bounding box around a traffic light. We fine-tuned the pre-trained `ssd_mobilenet_v1_coco` model using the Tensorflow Object Detection API. The training dataset includes camera images from training, reference, and review rosbags. 56 | - [more...](#additional-specifications) 57 | 58 | ### Planning 59 | The waypoint updater node publishes a queue of `n` waypoints ahead of the vehicle position, each with a target velocity. For the simulator, `n=100` is sufficient. For the site (the real-world test track), we reduce to `n=20`. We dequeue traversed waypoints and enqueue new points, preserving and reusing those in the middle. When a light-state changes, the entire queue is updated. The vehicle stops at the final base waypoint. [more...](#additional-specifications) 60 | 61 | ### Control 62 | The drive-by-wire node adjusts throttle and brakes according to the velocity targets published by the waypoint follower (which is informed by the waypoint updater node). If the list of waypoints contains a series of descending velocity targets, the PID velocity controller (in the twist controller component of DBW) will attempt to match the target velocity. [more...](#additional-specifications) 63 | 64 | ### Operation 65 | There are three modes in which the controller operates: 66 | - site: When at the test site, this mode is launched. This mode can be run simultaneously with a rosbag to test the traffic light classifier. (See below) 67 | - sitesim: emulates the test site in the simulator at the first traffic light. 68 | - styx: When using the term3 simulator, this mode is launched. The simulator communicates through server.py and bridge.py 69 | 70 | These modes are started by roslaunch. For example, to run the styx (simulator) version we run: 71 | 72 | `roslaunch launch/styx.launch` 73 | 74 | ### Additional Specifications 75 | 76 | [Traffic light image classification](https://github.com/level5-engineers/system-integration/wiki/Traffic-Light-Image-Classification)
77 | [Waypoint updater](https://github.com/level5-engineers/system-integration/wiki/Waypoint-Updater)
78 | [Drive-by-wire](https://github.com/level5-engineers/system-integration/wiki/Drive-By-Wire)
79 | 80 | ### References 81 | 82 | [Traffic Light Detection and Classification](https://becominghuman.ai/traffic-light-detection-tensorflow-api-c75fdbadac62)
83 | [SSD: Single Shot MultiBox Detection](https://arxiv.org/pdf/1512.02325.pdf)
84 | [Machine learning](http://ftp.cs.wisc.edu/machine-learning/shavlik-group/torrey.handbook09.pdf)
85 | [MobileNets](https://arxiv.org/abs/1704.04861)
86 | [Transfer learning](http://ruder.io/transfer-learning/index.html)
87 | [Pure Pursuit Algorithm](http://ri.cmu.edu/pub_files/pub3/coulter_r_craig_1992_1/coulter_r_craig_1992_1.pdf)
88 | [Quaternion mathematics](https://web.archive.org/web/20120417090529/http://www.itu.dk/people/erikdam/DOWNLOAD/98-5.pdf)
89 | [Quaternions online visualization](http://quaternions.online/)
90 | [PID control](https://udacity-reviews-uploads.s3.amazonaws.com/_attachments/41330/1493863065/pid_control_document.pdf)
91 | 92 | --- 93 | 94 | This is the project repo for the final project of the Udacity Self-Driving Car Nanodegree: Programming a Real Self-Driving Car. For more information about the project, see the project wiki [here](https://github.com/level5-engineers/system-integration/wiki). 95 | 96 | ### Native Installation 97 | 98 | * Be sure that your workstation is running Ubuntu 16.04 Xenial Xerus or Ubuntu 14.04 Trusty Tahir. [Ubuntu downloads can be found here](https://www.ubuntu.com/download/desktop). 99 | * If using a Virtual Machine to install Ubuntu, use the following configuration as minimum: 100 | * 2 CPU 101 | * 2 GB system memory 102 | * 25 GB of free hard drive space 103 | 104 | The Udacity provided virtual machine has ROS and Dataspeed DBW already installed, so you can skip the next two steps if you are using this. 105 | 106 | * Follow these instructions to install ROS 107 | * [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu) if you have Ubuntu 16.04. 108 | * [ROS Indigo](http://wiki.ros.org/indigo/Installation/Ubuntu) if you have Ubuntu 14.04. 109 | * [Dataspeed DBW](https://bitbucket.org/DataspeedInc/dbw_mkz_ros) 110 | * Use this option to install the SDK on a workstation that already has ROS installed: [One Line SDK Install (binary)](https://bitbucket.org/DataspeedInc/dbw_mkz_ros/src/81e63fcc335d7b64139d7482017d6a97b405e250/ROS_SETUP.md?fileviewer=file-view-default) 111 | * Download the [Udacity Simulator](https://github.com/udacity/CarND-Capstone/releases/tag/v1.2). 112 | 113 | ### Docker Installation 114 | [Install Docker](https://docs.docker.com/engine/installation/) 115 | 116 | Build the docker container 117 | ```bash 118 | docker build . -t capstone 119 | ``` 120 | 121 | Run the docker file 122 | ```bash 123 | docker run -p 4567:4567 -v $PWD:/capstone -v /tmp/log:/root/.ros/ --rm -it capstone 124 | ``` 125 | 126 | ### Usage 127 | 128 | 1. Clone the project repository 129 | ```bash 130 | git clone https://github.com/level5-engineers/system-integration.git 131 | ``` 132 | 133 | 2. Install python dependencies 134 | ```bash 135 | cd system-integration 136 | pip install -r requirements.txt 137 | ``` 138 | 139 | 3. Make the controller 140 | ```bash 141 | cd ros 142 | catkin_make 143 | ``` 144 | 145 | 4. In a new terminal window, start roscore 146 | ```bash 147 | roscore 148 | ``` 149 | 150 | 5. Start the simulator, select screen resolution 800x600, click SELECT, uncheck the Manual checkbox. Ideally, run the simulator in the host environment (outside of the virtual machine). 151 | 152 | 6. In a new terminal window, start the controller 153 | ```bash 154 | cd system-integration/ros 155 | source devel/setup.sh 156 | roslaunch launch/styx.launch 157 | ``` 158 | 159 | ### Real world testing 160 | 1. Download [training bag](https://drive.google.com/file/d/0B2_h37bMVw3iYkdJTlRSUlJIamM/view?usp=sharing) that was recorded on the Udacity self-driving car (a bag demonstraing the correct predictions in autonomous mode can be found [here](https://drive.google.com/open?id=0B2_h37bMVw3iT0ZEdlF4N01QbHc)) 161 | 2. Unzip the file 162 | ```bash 163 | unzip traffic_light_bag_files.zip 164 | ``` 165 | 3. Play the bag file 166 | ```bash 167 | rosbag play -l traffic_light_bag_files/loop_with_traffic_light.bag 168 | ``` 169 | 4. Launch your project in site mode 170 | ```bash 171 | cd system-integration/ros 172 | roslaunch launch/site.launch 173 | ``` 174 | 5. Confirm that traffic light detection works on real life images 175 | -------------------------------------------------------------------------------- /data/churchlot_with_cars.csv: -------------------------------------------------------------------------------- 1 | 10.4062,15.581,0.775,-2.7419,12 2 | 9.3323,15.1076,0.7265,-2.7761,12 3 | 8.2133,14.6523,0.6463,-2.7875,12 4 | 7.1124,14.2238,0.5837,-2.7816,12 5 | 6.1445,13.8196,0.5186,-2.7624,12 6 | 5.1563,13.3232,0.4582,-2.7233,12 7 | 4.0306,12.8497,0.4001,-2.6551,12 8 | 2.9963,12.3614,0.3606,-2.5595,12 9 | 2.1433,11.6514,0.324,-2.4223,12 10 | 1.3549,10.8159,0.3129,-2.2498,12 11 | 0.6673,9.8075,0.2927,-2.0473,12 12 | 0.0982,8.8679,0.2952,-1.829,12 13 | -0.1674,7.7939,0.2971,-1.5273,12 14 | -0.016,6.7165,0.3308,-1.2993,12 15 | 0.4703,5.6266,0.3591,-1.0645,12 16 | 1.0478,4.6374,0.44,-0.8443,12 17 | 1.7823,3.662,0.5316,-0.643,12 18 | 2.8056,2.9294,0.618,-0.4675,12 19 | 3.752,2.5905,0.639,-0.3436,12 20 | 4.8491,2.3299,0.6923,-0.2324,12 21 | 5.9542,2.1258,0.7686,-0.1403,12 22 | 7.1193,1.9555,0.845,-0.072,12 23 | 8.329,1.8898,0.8918,-0.0299,12 24 | 9.5629,1.8863,0.9811,-0.0096,12 25 | 10.8273,1.9032,1.0635,0.0055,12 26 | 12.0401,1.9231,1.134,0.0154,12 27 | 13.2311,1.9756,1.2247,0.0259,12 28 | 14.454,2.0982,1.2894,0.0491,12 29 | 15.667,2.1955,1.358,0.0874,12 30 | 16.8044,2.3517,1.4531,0.142,12 31 | 17.9644,2.5628,1.5041,0.2087,12 32 | 19.1917,2.8108,1.5556,0.2976,12 33 | 20.2247,3.1073,1.6655,0.4049,12 34 | 21.1457,3.5801,1.7298,0.5225,12 35 | 21.9855,4.1788,1.7631,0.6443,12 36 | 22.7811,4.8316,1.8034,0.7734,12 37 | 23.5077,5.622,1.8373,0.9148,12 38 | 24.1083,6.4314,1.8431,1.0537,12 39 | 24.7628,7.5022,1.8801,1.2454,12 40 | 25.0165,8.5624,1.9044,1.439,12 41 | 24.9825,9.7477,1.8986,1.6241,12 42 | 24.8426,10.9168,1.8811,1.801,12 43 | 24.5885,12.16,1.8511,1.9861,12 44 | 24.1027,13.3017,1.8157,2.1751,12 45 | 23.3313,14.2742,1.7346,2.3716,12 46 | 22.4417,14.9339,1.6529,2.514,12 47 | 21.549,15.5242,1.5707,2.6533,12 48 | 20.4978,16.0282,1.4939,2.788,12 49 | 19.4593,16.3515,1.4019,2.9136,12 50 | 18.3935,16.6445,1.3187,3.0424,12 51 | 17.3152,16.7253,1.2257,3.1397,12 52 | 16.2105,16.6429,1.1768,-3.0689,12 53 | 15.1095,16.4975,1.0757,-3.0101,12 54 | 14.0054,16.3033,1.0277,-2.9785,12 55 | 12.9916,16.1129,0.9634,-2.9596,12 56 | 11.9502,15.8813,0.8892,-2.9351,12 57 | 10.8016,15.601,0.8146,-2.9138,12 58 | 9.7822,15.3713,0.7315,-2.8976,12 59 | 8.8213,15.0926,0.698,-2.8836,12 60 | 7.6275,14.7692,0.6189,-2.8695,12 61 | 6.6734,14.4438,0.5474,-2.8635,12 62 | -------------------------------------------------------------------------------- /data/filter.csv: -------------------------------------------------------------------------------- 1 | 18.5823,-0.1006,1.917,-2.7314 2 | 17.6029,-0.4658,1.8647,-2.7586 3 | 16.5194,-0.8204,1.7964,-2.7953 4 | 15.5304,-1.0683,1.6857,-2.8314 5 | 14.4029,-1.392,1.6891,-2.8769 6 | 13.343,-1.6056,1.629,-2.9188 7 | 12.1324,-1.7882,1.5459,-2.9623 8 | 11.0082,-1.9127,1.4919,-3.0054 9 | 9.9825,-1.9991,1.4366,-3.0386 10 | 8.7695,-2.0188,1.3677,-3.0857 11 | 7.7279,-2.0494,1.2835,-3.1221 12 | 6.677,-2.0044,1.2583,3.1259 13 | 5.6152,-1.9119,1.1624,3.0808 14 | 4.5767,-1.7968,1.1319,3.0231 15 | 3.5939,-1.5726,1.0709,2.9482 16 | 2.5599,-1.3617,1.0073,2.8731 17 | 1.5352,-1.0525,0.9372,2.8042 18 | 0.5575,-0.6867,0.9008,2.7398 19 | -0.5215,-0.1168,0.8273,2.6599 20 | -1.5183,0.5154,0.7422,2.5855 21 | -2.5668,1.1857,0.6468,2.5052 22 | -3.371,1.8118,0.5814,2.438 23 | -4.1252,2.4951,0.5011,2.3515 24 | -4.7803,3.2998,0.4317,2.2519 25 | -5.4865,4.2466,0.4,2.0881 26 | -6.1627,5.2986,0.3772,1.879 27 | -6.5431,6.3887,0.3161,1.7093 28 | -6.5982,7.5075,0.2587,1.5341 29 | -6.4658,8.7207,0.2392,1.3114 30 | -6.1309,9.6912,0.1923,1.0789 31 | -5.814,10.6621,0.1562,0.9107 32 | -5.0999,11.5741,0.198,0.7595 33 | -4.231,12.2941,0.2517,0.6083 34 | -3.3027,12.8061,0.2601,0.499 35 | -2.2996,13.3991,0.355,0.4342 36 | -1.1437,13.8183,0.3851,0.4036 37 | 0.0466,14.2605,0.4603,0.389 38 | 1.026,14.6107,0.5404,0.3834 39 | 2.0331,14.9719,0.5994,0.3761 40 | 3.0959,15.3267,0.6673,0.3666 41 | 4.0581,15.6389,0.7038,0.3578 42 | 5.0816,15.96,0.7695,0.348 43 | 6.0861,16.2388,0.8362,0.3404 44 | 7.3077,16.6437,0.9008,0.3247 45 | 8.3065,16.9484,0.9994,0.3131 46 | 9.3468,17.2339,1.0939,0.297 47 | 10.4225,17.4959,1.1265,0.2782 48 | 11.4929,17.707,1.1745,0.2453 49 | 12.7278,17.9654,1.2696,0.1979 50 | 13.8239,18.2032,1.3624,0.1314 51 | 15.0089,18.2654,1.4164,0.0331 52 | 16.0333,18.2674,1.4964,-0.0638 53 | 17.0387,18.1363,1.5563,-0.1702 54 | 18.1804,17.8773,1.6658,-0.3199 55 | 19.2442,17.5957,1.7895,-0.4583 56 | 20.243,16.9884,1.8428,-0.5951 57 | 21.0586,16.3732,1.9174,-0.7163 58 | 21.8151,15.5929,1.9829,-0.825 59 | 22.4817,14.7813,2.0611,-0.9303 60 | 23.1269,13.8423,2.115,-1.0357 61 | 23.7013,12.8443,2.1792,-1.1312 62 | 24.0966,11.8622,2.2177,-1.2224 63 | 24.3302,10.7692,2.2253,-1.3174 64 | 24.4664,9.6994,2.1941,-1.4245 65 | 24.713,8.5143,2.2356,-1.551 66 | 24.67,7.3907,2.2119,-1.6701 67 | 24.4691,6.2895,2.2405,-1.7855 68 | 24.1418,5.2507,2.2695,-1.9052 69 | 23.6646,4.3326,2.236,-2.04 70 | 23.2215,3.3197,2.1929,-2.1711 71 | 22.5403,2.4461,2.156,-2.2797 72 | 21.8149,1.6579,2.133,-2.3845 73 | 20.8341,0.9279,2.0607,-2.5057 74 | 19.8934,0.2885,2.0179,-2.612 75 | 18.8238,-0.2946,1.9504,-2.6942 76 | 17.8086,-0.6758,1.8845,-2.7531 77 | 16.6883,-1.03,1.8115,-2.8021 78 | 15.6309,-1.3051,1.7324,-2.833 79 | -------------------------------------------------------------------------------- /data/grasshopper_calibration.yml: -------------------------------------------------------------------------------- 1 | image_width: 800 2 | image_height: 600 3 | camera_name: narrow_stereo 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1352.729680, 0.000000, 426.573376, 0.000000, 1362.529057, 366.755119, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.050937, -0.096261, 0.011851, 0.008042, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [1345.200806, 0.000000, 429.549312, 0.000000, 0.000000, 1353.838257, 369.393325, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /data/maptf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /data/sitesim.csv: -------------------------------------------------------------------------------- 1 | 1148.479732,1165.305193,0,-3.034680717 2 | 1147.440428,1165.193653,0,-3.075306526 3 | 1146.302882,1165.118139,0,3.13718979 4 | 1145.283296,1165.122628,0,-3.112016634 5 | 1144.110762,1165.087939,0,3.09045744 6 | 1143.030967,1165.143203,0,3.04129844 7 | 1141.812825,1165.265787,0,3.001888628 8 | 1140.692772,1165.423289,0,2.975629422 9 | 1139.677583,1165.593337,0,2.907831951 10 | 1138.497418,1165.874351,0,2.920962087 11 | 1137.480629,1166.102398,0,2.848798357 12 | 1136.473532,1166.405996,0,2.80469582 13 | 1135.467625,1166.758314,0,2.781210227 14 | 1134.489886,1167.126764,0,2.667306998 15 | 1133.593107,1167.587143,0,2.690387341 16 | 1132.643429,1168.047303,0,2.598534013 17 | 1131.727082,1168.600405,0,2.533573188 18 | 1130.870276,1169.19672,0,2.405660527 19 | 1129.965815,1170.015852,0,2.326383883 20 | 1129.156412,1170.875011,0,2.322780311 21 | 1128.306342,1171.783876,0,2.230076183 22 | 1127.682043,1172.589474,0,2.155476294 23 | 1127.12034,1173.438124,0,2.004071206 24 | 1126.684692,1174.379882,0,1.961655971 25 | 1126.234688,1175.471965,0,1.892076153 26 | 1125.839778,1176.658556,0,1.656543283 27 | 1125.740899,1177.80888,0,1.370005763 28 | 1125.964307,1178.906531,0,1.212093642 29 | 1126.392742,1180.049259,0,0.988511416 30 | 1126.957336,1180.906733,0,1.005300335 31 | 1127.504589,1181.769048,0,0.656504818 32 | 1128.422122,1182.476025,0,0.441958425 33 | 1129.442141,1182.958672,0,0.254029259 34 | 1130.468253,1183.22509,0,0.283899613 35 | 1131.586879,1183.551485,0,0.097909396 36 | 1132.810557,1183.671678,0,0.105701273 37 | 1134.073255,1183.805647,0,0.093399039 38 | 1135.108849,1183.902652,0,0.094363107 39 | 1136.174003,1184.003463,0,0.072202124 40 | 1137.291542,1184.084292,0,0.063747724 41 | 1138.301069,1184.148734,0,0.054002683 42 | 1139.372193,1184.206634,0,0.020736324 43 | 1140.414441,1184.22825,0,0.070055099 44 | 1141.698239,1184.318334,0,0.046098069 45 | 1142.741372,1184.366454,0,0.01784557 46 | 1143.819966,1184.385704,0,-0.011089436 47 | 1144.927045,1184.373427,0,-0.055282797 48 | 1146.016395,1184.313143,0,-0.043728428 49 | 1147.276835,1184.257991,0,-0.036359782 50 | 1148.397692,1184.217219,0,-0.197558674 51 | 1149.561242,1183.984312,0,-0.24804764 52 | 1150.554291,1183.732809,0,-0.379664268 53 | 1151.496001,1183.357045,0,-0.473079061 54 | 1152.53813,1182.823635,0,-0.508776059 55 | 1153.49919,1182.287601,0,-0.796302735 56 | 1154.316692,1181.452073,0,-0.896241901 57 | 1154.954734,1180.654216,0,-1.050883651 58 | 1155.494667,1179.711012,0,-1.133177624 59 | 1155.939751,1178.759724,0,-1.218771173 60 | 1156.332581,1177.69029,0,-1.298547978 61 | 1156.642215,1176.581206,0,-1.438132475 62 | 1156.78225,1175.531839,0,-1.610240399 63 | 1156.738176,1174.415024,0,-1.694164061 64 | 1156.605469,1173.344785,0,-1.615640193 65 | 1156.551204,1172.135517,0,-1.859047507 66 | 1156.231558,1171.057485,0,-2.001249107 67 | 1155.764462,1170.040222,0,-2.126025459 68 | 1155.190334,1169.114692,0,-2.300133839 69 | 1154.500827,1168.343194,0,-2.233170514 70 | 1153.820907,1167.471408,0,-2.483074571 71 | 1152.944751,1166.793497,0,-2.564727787 72 | 1152.046899,1166.209267,0,-2.75175573 73 | 1150.915984,1165.744615,0,-2.794616651 74 | 1149.846338,1165.357825,0,-2.892475227 75 | 1148.665728,1165.057476,0,-3.03239026 76 | 1147.587778,1164.939291,0,-3.085372061 77 | 1146.414675,1164.873269,0,-3.137068695 78 | 1145.322086,1164.868326,0,2.962240354 79 | -------------------------------------------------------------------------------- /imgs/autoware_computing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/level5-engineers/system-integration/e59e4f3570a006f916fbacf690bc72477b81d716/imgs/autoware_computing.png -------------------------------------------------------------------------------- /imgs/autoware_tf1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/level5-engineers/system-integration/e59e4f3570a006f916fbacf690bc72477b81d716/imgs/autoware_tf1.png -------------------------------------------------------------------------------- /imgs/autoware_tf2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/level5-engineers/system-integration/e59e4f3570a006f916fbacf690bc72477b81d716/imgs/autoware_tf2.png -------------------------------------------------------------------------------- /imgs/open_simulator.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/level5-engineers/system-integration/e59e4f3570a006f916fbacf690bc72477b81d716/imgs/open_simulator.png -------------------------------------------------------------------------------- /imgs/select_waypoint.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/level5-engineers/system-integration/e59e4f3570a006f916fbacf690bc72477b81d716/imgs/select_waypoint.png -------------------------------------------------------------------------------- /imgs/unity.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/level5-engineers/system-integration/e59e4f3570a006f916fbacf690bc72477b81d716/imgs/unity.png -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | Flask==0.11.1 2 | attrdict==2.0.0 3 | eventlet==0.19.0 4 | python-socketio==1.6.1 5 | numpy==1.13.1 6 | Pillow==2.2.1 7 | scipy==0.19.1 8 | keras==2.0.8 9 | tensorflow==1.3.0 10 | h5py==2.6.0 11 | -------------------------------------------------------------------------------- /ros/.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /ros/launch/dbwtest.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /ros/launch/site.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /ros/launch/sitesim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /ros/launch/styx.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /ros/launch/tltest.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /ros/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/kinetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /ros/src/camera_info_publisher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(camera_info_publisher) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a run_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if you package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES camera_info_publisher 108 | # CATKIN_DEPENDS roscpp rospy std_msgs 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/camera_info_publisher.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/camera_info_publisher_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # install(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables and/or libraries for installation 168 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 169 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark cpp header files for installation 175 | # install(DIRECTORY include/${PROJECT_NAME}/ 176 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 177 | # FILES_MATCHING PATTERN "*.h" 178 | # PATTERN ".svn" EXCLUDE 179 | # ) 180 | 181 | ## Mark other files for installation (e.g. launch and bag files, etc.) 182 | # install(FILES 183 | # # myfile1 184 | # # myfile2 185 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 186 | # ) 187 | 188 | ############# 189 | ## Testing ## 190 | ############# 191 | 192 | ## Add gtest based cpp test target and link libraries 193 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_camera_info_publisher.cpp) 194 | # if(TARGET ${PROJECT_NAME}-test) 195 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 196 | # endif() 197 | 198 | ## Add folders to be run by python nosetests 199 | # catkin_add_nosetests(test) 200 | -------------------------------------------------------------------------------- /ros/src/camera_info_publisher/launch/camera_info_publisher.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /ros/src/camera_info_publisher/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | camera_info_publisher 4 | 0.0.0 5 | The camera_info_publisher package 6 | 7 | 8 | 9 | 10 | caleb 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /ros/src/camera_info_publisher/yaml_to_camera_info_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Thanks to Github user @rossbar for writing this script 5 | https://gist.github.com/rossbar/ebb282c3b73c41c1404123de6cea4771 6 | 7 | pointgrey_camera_driver (at least the version installed with apt-get) doesn't 8 | properly handle camera info in indigo. 9 | This node is a work-around that will read in a camera calibration .yaml 10 | file (as created by the cameracalibrator.py in the camera_calibration pkg), 11 | convert it to a valid sensor_msgs/CameraInfo message, and publish it on a 12 | topic. 13 | 14 | The yaml parsing is courtesy ROS-user Stephan: 15 | http://answers.ros.org/question/33929/camera-calibration-parser-in-python/ 16 | 17 | This file just extends that parser into a rosnode. 18 | """ 19 | import rospy 20 | import yaml 21 | from sensor_msgs.msg import CameraInfo 22 | 23 | def yaml_to_CameraInfo(calib_yaml): 24 | """ 25 | Parse a yaml file containing camera calibration data (as produced by 26 | rosrun camera_calibration cameracalibrator.py) into a 27 | sensor_msgs/CameraInfo msg. 28 | 29 | Parameters 30 | ---------- 31 | yaml_fname : str 32 | Path to yaml file containing camera calibration data 33 | 34 | Returns 35 | ------- 36 | camera_info_msg : sensor_msgs.msg.CameraInfo 37 | A sensor_msgs.msg.CameraInfo message containing the camera calibration 38 | data 39 | """ 40 | # Load data from file 41 | calib_data = yaml.load(calib_yaml) 42 | # Parse 43 | camera_info_msg = CameraInfo() 44 | camera_info_msg.width = calib_data["image_width"] 45 | camera_info_msg.height = calib_data["image_height"] 46 | camera_info_msg.K = calib_data["camera_matrix"]["data"] 47 | camera_info_msg.D = calib_data["distortion_coefficients"]["data"] 48 | camera_info_msg.R = calib_data["rectification_matrix"]["data"] 49 | camera_info_msg.P = calib_data["projection_matrix"]["data"] 50 | camera_info_msg.distortion_model = calib_data["distortion_model"] 51 | return camera_info_msg 52 | 53 | if __name__ == "__main__": 54 | 55 | calib_yaml = rospy.get_param("/grasshopper_calibration_yaml") 56 | 57 | # Parse yaml file 58 | camera_info_msg = yaml_to_CameraInfo(calib_yaml) 59 | 60 | # Initialize publisher node 61 | rospy.init_node("camera_info_publisher", anonymous=True) 62 | publisher = rospy.Publisher("camera_info", CameraInfo, queue_size=10) 63 | rate = rospy.Rate(10) 64 | 65 | # Run publisher 66 | while not rospy.is_shutdown(): 67 | publisher.publish(camera_info_msg) 68 | rate.sleep() 69 | -------------------------------------------------------------------------------- /ros/src/styx/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(styx) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | # add_definitions(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | dbw_mkz_msgs 12 | geometry_msgs 13 | roscpp 14 | rospy 15 | sensor_msgs 16 | std_msgs 17 | cv_bridge 18 | ) 19 | 20 | ## System dependencies are found with CMake's conventions 21 | # find_package(Boost REQUIRED COMPONENTS system) 22 | 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | # catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 38 | ## * add a build_depend tag for "message_generation" 39 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 40 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 41 | ## but can be declared for certainty nonetheless: 42 | ## * add a run_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | # add_message_files( 55 | # FILES 56 | # Message1.msg 57 | # Message2.msg 58 | # ) 59 | 60 | ## Generate services in the 'srv' folder 61 | # add_service_files( 62 | # FILES 63 | # Service1.srv 64 | # Service2.srv 65 | # ) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ## Generate added messages and services with any dependencies listed here 75 | # generate_messages( 76 | # DEPENDENCIES 77 | # dbw_mkz_msgs# geometry_msgs# sensor_msgs# std_msgs 78 | # ) 79 | 80 | ################################################ 81 | ## Declare ROS dynamic reconfigure parameters ## 82 | ################################################ 83 | 84 | ## To declare and build dynamic reconfigure parameters within this 85 | ## package, follow these steps: 86 | ## * In the file package.xml: 87 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 88 | ## * In this file (CMakeLists.txt): 89 | ## * add "dynamic_reconfigure" to 90 | ## find_package(catkin REQUIRED COMPONENTS ...) 91 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 92 | ## and list every .cfg file to be processed 93 | 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 95 | # generate_dynamic_reconfigure_options( 96 | # cfg/DynReconf1.cfg 97 | # cfg/DynReconf2.cfg 98 | # ) 99 | 100 | ################################### 101 | ## catkin specific configuration ## 102 | ################################### 103 | ## The catkin_package macro generates cmake config files for your package 104 | ## Declare things to be passed to dependent projects 105 | ## INCLUDE_DIRS: uncomment this if you package contains header files 106 | ## LIBRARIES: libraries you create in this project that dependent projects also need 107 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 108 | ## DEPENDS: system dependencies of this project that dependent projects also need 109 | catkin_package( 110 | # INCLUDE_DIRS include 111 | # LIBRARIES styx 112 | # CATKIN_DEPENDS dbw_mkz_msgs geometry_msgs roscpp rospy sensor_msgs std_msgs 113 | # DEPENDS system_lib 114 | ) 115 | 116 | ########### 117 | ## Build ## 118 | ########### 119 | 120 | ## Specify additional locations of header files 121 | ## Your package locations should be listed before other locations 122 | # include_directories(include) 123 | include_directories( 124 | ${catkin_INCLUDE_DIRS} 125 | ) 126 | 127 | ## Declare a C++ library 128 | # add_library(${PROJECT_NAME} 129 | # src/${PROJECT_NAME}/styx.cpp 130 | # ) 131 | 132 | ## Add cmake target dependencies of the library 133 | ## as an example, code may need to be generated before libraries 134 | ## either from message generation or dynamic reconfigure 135 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Declare a C++ executable 138 | ## With catkin_make all packages are built within a single CMake context 139 | ## The recommended prefix ensures that target names across packages don't collide 140 | # add_executable(${PROJECT_NAME}_node src/styx_node.cpp) 141 | 142 | ## Rename C++ executable without prefix 143 | ## The above recommended prefix causes long target names, the following renames the 144 | ## target back to the shorter version for ease of user use 145 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 146 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 147 | 148 | ## Add cmake target dependencies of the executable 149 | ## same as for the library above 150 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 151 | 152 | ## Specify libraries to link a library or executable target against 153 | # target_link_libraries(${PROJECT_NAME}_node 154 | # ${catkin_LIBRARIES} 155 | # ) 156 | 157 | ############# 158 | ## Install ## 159 | ############# 160 | 161 | # all install targets should use catkin DESTINATION variables 162 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 163 | 164 | ## Mark executable scripts (Python etc.) for installation 165 | ## in contrast to setup.py, you can choose the destination 166 | # install(PROGRAMS 167 | # scripts/my_python_script 168 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 169 | # ) 170 | 171 | ## Mark executables and/or libraries for installation 172 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 173 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 175 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 176 | # ) 177 | 178 | ## Mark cpp header files for installation 179 | # install(DIRECTORY include/${PROJECT_NAME}/ 180 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 181 | # FILES_MATCHING PATTERN "*.h" 182 | # PATTERN ".svn" EXCLUDE 183 | # ) 184 | 185 | ## Mark other files for installation (e.g. launch and bag files, etc.) 186 | # install(FILES 187 | # # myfile1 188 | # # myfile2 189 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 190 | # ) 191 | 192 | ############# 193 | ## Testing ## 194 | ############# 195 | 196 | ## Add gtest based cpp test target and link libraries 197 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_styx.cpp) 198 | # if(TARGET ${PROJECT_NAME}-test) 199 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 200 | # endif() 201 | 202 | ## Add folders to be run by python nosetests 203 | # catkin_add_nosetests(test) 204 | -------------------------------------------------------------------------------- /ros/src/styx/bridge.py: -------------------------------------------------------------------------------- 1 | 2 | import rospy 3 | 4 | import tf 5 | from geometry_msgs.msg import PoseStamped, Quaternion, TwistStamped 6 | from dbw_mkz_msgs.msg import SteeringReport, ThrottleCmd, BrakeCmd, SteeringCmd 7 | from std_msgs.msg import Float32 as Float 8 | from std_msgs.msg import Bool 9 | from sensor_msgs.msg import PointCloud2 10 | from sensor_msgs.msg import Image 11 | import sensor_msgs.point_cloud2 as pcl2 12 | from std_msgs.msg import Header 13 | from cv_bridge import CvBridge, CvBridgeError 14 | 15 | from styx_msgs.msg import TrafficLight, TrafficLightArray 16 | import numpy as np 17 | from PIL import Image as PIL_Image 18 | from io import BytesIO 19 | import base64 20 | 21 | import math 22 | 23 | TYPE = { 24 | 'bool': Bool, 25 | 'float': Float, 26 | 'pose': PoseStamped, 27 | 'pcl': PointCloud2, 28 | 'twist': TwistStamped, 29 | 'steer': SteeringReport, 30 | 'trafficlights': TrafficLightArray, 31 | 'steer_cmd': SteeringCmd, 32 | 'brake_cmd': BrakeCmd, 33 | 'throttle_cmd': ThrottleCmd, 34 | 'image':Image 35 | } 36 | seq= 0 37 | def saveImage(image): 38 | global seq 39 | if seq%2==0: 40 | img= PIL_Image.fromarray(image, 'RGB') # 'L' 41 | img.save('/home/student/data/out'+str(seq).zfill(5)+'.png', 'PNG') 42 | seq += 1 43 | 44 | 45 | class Bridge(object): 46 | def __init__(self, conf, server): 47 | rospy.init_node('styx_server') 48 | self.server = server 49 | self.vel = 0. 50 | self.yaw = None 51 | self.angular_vel = 0. 52 | self.bridge = CvBridge() 53 | 54 | self.callbacks = { 55 | '/vehicle/steering_cmd': self.callback_steering, 56 | '/vehicle/throttle_cmd': self.callback_throttle, 57 | '/vehicle/brake_cmd': self.callback_brake, 58 | } 59 | 60 | self.subscribers = [rospy.Subscriber(e.topic, TYPE[e.type], self.callbacks[e.topic]) 61 | for e in conf.subscribers] 62 | 63 | self.publishers = {e.name: rospy.Publisher(e.topic, TYPE[e.type], queue_size=1) 64 | for e in conf.publishers} 65 | while rospy.get_time() == 0: 66 | rospy.spin() 67 | self.prev_time = rospy.get_time() 68 | 69 | def create_light(self, x, y, z, yaw, state): 70 | light = TrafficLight() 71 | 72 | light.header = Header() 73 | light.header.stamp = rospy.Time.now() 74 | light.header.frame_id = '/world' 75 | 76 | light.pose = self.create_pose(x, y, z, yaw) 77 | light.state = state 78 | 79 | return light 80 | 81 | def create_pose(self, x, y, z, yaw=0.): 82 | pose = PoseStamped() 83 | 84 | pose.header = Header() 85 | pose.header.stamp = rospy.Time.now() 86 | pose.header.frame_id = '/world' 87 | 88 | pose.pose.position.x = x 89 | pose.pose.position.y = y 90 | pose.pose.position.z = z 91 | 92 | q = tf.transformations.quaternion_from_euler(0., 0., math.pi * yaw/180.) 93 | pose.pose.orientation = Quaternion(*q) 94 | 95 | return pose 96 | 97 | def create_float(self, val): 98 | fl = Float() 99 | fl.data = val 100 | return fl 101 | 102 | def create_twist(self, velocity, angular): 103 | tw = TwistStamped() 104 | tw.twist.linear.x = velocity 105 | tw.twist.angular.z = angular 106 | tw.header.stamp = rospy.Time.now() 107 | return tw 108 | 109 | def create_steer(self, val): 110 | st = SteeringReport() 111 | st.steering_wheel_angle_cmd = val * math.pi/180. 112 | st.enabled = True 113 | st.speed = self.vel 114 | return st 115 | 116 | def calc_angular(self, yaw): 117 | angular_vel = 0. 118 | if self.yaw is not None: 119 | angular_vel = (yaw - self.yaw)/(rospy.get_time() - self.prev_time) 120 | self.yaw = yaw 121 | self.prev_time = rospy.get_time() 122 | return angular_vel 123 | 124 | def create_point_cloud_message(self, pts): 125 | header = Header() 126 | header.stamp = rospy.Time.now() 127 | header.frame_id = '/world' 128 | cloud_message = pcl2.create_cloud_xyz32(header, pts) 129 | return cloud_message 130 | 131 | def broadcast_transform(self, name, position, orientation): 132 | br = tf.TransformBroadcaster() 133 | br.sendTransform(position, 134 | orientation, 135 | rospy.Time.now(), 136 | name, 137 | "world") 138 | 139 | def publish_odometry(self, data): 140 | pose = self.create_pose(data['x'], data['y'], data['z'], data['yaw']) 141 | 142 | position = (data['x'], data['y'], data['z']) 143 | orientation = tf.transformations.quaternion_from_euler(0, 0, math.pi * data['yaw']/180.) 144 | self.broadcast_transform("base_link", position, orientation) 145 | 146 | self.publishers['current_pose'].publish(pose) 147 | self.vel = data['velocity']* 0.44704 148 | self.angular = self.calc_angular(data['yaw'] * math.pi/180.) 149 | self.publishers['current_velocity'].publish(self.create_twist(self.vel, self.angular)) 150 | 151 | 152 | def publish_controls(self, data): 153 | steering, throttle, brake = data['steering_angle'], data['throttle'], data['brake'] 154 | self.publishers['steering_report'].publish(self.create_steer(steering)) 155 | self.publishers['throttle_report'].publish(self.create_float(throttle)) 156 | self.publishers['brake_report'].publish(self.create_float(brake)) 157 | 158 | def publish_obstacles(self, data): 159 | for obs in data['obstacles']: 160 | pose = self.create_pose(obs[0], obs[1], obs[2]) 161 | self.publishers['obstacle'].publish(pose) 162 | header = Header() 163 | header.stamp = rospy.Time.now() 164 | header.frame_id = '/world' 165 | cloud = pcl2.create_cloud_xyz32(header, data['obstacles']) 166 | self.publishers['obstacle_points'].publish(cloud) 167 | 168 | def publish_lidar(self, data): 169 | self.publishers['lidar'].publish(self.create_point_cloud_message(zip(data['lidar_x'], data['lidar_y'], data['lidar_z']))) 170 | 171 | def publish_traffic(self, data): 172 | x, y, z = data['light_pos_x'], data['light_pos_y'], data['light_pos_z'], 173 | yaw = [math.atan2(dy, dx) for dx, dy in zip(data['light_pos_dx'], data['light_pos_dy'])] 174 | status = data['light_state'] 175 | 176 | lights = TrafficLightArray() 177 | header = Header() 178 | header.stamp = rospy.Time.now() 179 | header.frame_id = '/world' 180 | lights.lights = [self.create_light(*e) for e in zip(x, y, z, yaw, status)] 181 | self.publishers['trafficlights'].publish(lights) 182 | 183 | def publish_dbw_status(self, data): 184 | self.publishers['dbw_status'].publish(Bool(data)) 185 | 186 | def publish_camera(self, data): 187 | imgString = data["image"] 188 | image = PIL_Image.open(BytesIO(base64.b64decode(imgString))) 189 | image_array = np.asarray(image) 190 | #saveImage(image_array) 191 | 192 | image_message = self.bridge.cv2_to_imgmsg(image_array, encoding="passthrough") 193 | self.publishers['image'].publish(image_message) 194 | 195 | def callback_steering(self, data): 196 | self.server('steer', data={'steering_angle': str(data.steering_wheel_angle_cmd)}) 197 | 198 | def callback_throttle(self, data): 199 | self.server('throttle', data={'throttle': str(data.pedal_cmd)}) 200 | 201 | def callback_brake(self, data): 202 | self.server('brake', data={'brake': str(data.pedal_cmd)}) 203 | -------------------------------------------------------------------------------- /ros/src/styx/conf.py: -------------------------------------------------------------------------------- 1 | from attrdict import AttrDict 2 | 3 | conf = AttrDict({ 4 | 'subscribers': [ 5 | {'topic':'/vehicle/steering_cmd', 'type': 'steer_cmd', 'name': 'steering'}, 6 | {'topic':'/vehicle/throttle_cmd', 'type': 'throttle_cmd', 'name': 'throttle'}, 7 | {'topic':'/vehicle/brake_cmd', 'type': 'brake_cmd', 'name': 'brake'}, 8 | ], 9 | 'publishers': [ 10 | {'topic': '/current_pose', 'type': 'pose', 'name': 'current_pose'}, 11 | {'topic': '/current_velocity', 'type': 'twist', 'name': 'current_velocity'}, 12 | {'topic': '/vehicle/steering_report', 'type': 'steer', 'name': 'steering_report'}, 13 | {'topic': '/vehicle/throttle_report', 'type': 'float', 'name': 'throttle_report'}, 14 | {'topic': '/vehicle/brake_report', 'type': 'float', 'name': 'brake_report'}, 15 | {'topic': '/vehicle/obstacle', 'type': 'pose', 'name': 'obstacle'}, 16 | {'topic': '/vehicle/obstacle_points', 'type': 'pcl', 'name': 'obstacle_points'}, 17 | {'topic': '/vehicle/lidar', 'type': 'pcl', 'name': 'lidar'}, 18 | {'topic': '/vehicle/traffic_lights', 'type': 'trafficlights', 'name': 'trafficlights'}, 19 | {'topic': '/vehicle/dbw_enabled', 'type': 'bool', 'name': 'dbw_status'}, 20 | {'topic': '/image_color', 'type': 'image', 'name': 'image'}, 21 | ] 22 | }) 23 | -------------------------------------------------------------------------------- /ros/src/styx/data/.gitattributes: -------------------------------------------------------------------------------- 1 | udacity_succesful_light_detection.bag filter=lfs diff=lfs merge=lfs -text 2 | -------------------------------------------------------------------------------- /ros/src/styx/launch/server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /ros/src/styx/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | styx 4 | 0.0.0 5 | The styx package 6 | 7 | 8 | 9 | 10 | yousuf 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | dbw_mkz_msgs 44 | geometry_msgs 45 | roscpp 46 | rospy 47 | sensor_msgs 48 | std_msgs 49 | cv_bridge 50 | 51 | dbw_mkz_msgs 52 | geometry_msgs 53 | roscpp 54 | rospy 55 | sensor_msgs 56 | std_msgs 57 | cv_bridge 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /ros/src/styx/server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import socketio 4 | import eventlet 5 | import eventlet.wsgi 6 | import time 7 | import rospy 8 | from flask import Flask, render_template 9 | 10 | from bridge import Bridge 11 | from conf import conf 12 | 13 | sio = socketio.Server() 14 | app = Flask(__name__) 15 | 16 | msgs = {} 17 | # To mitigate simulator lag, see 18 | # https://github.com/udacity/self-driving-car-sim/issues/53 19 | 20 | dbw_enable = False 21 | 22 | @sio.on('connect') 23 | def connect(sid, environ): 24 | print("connect ", sid) 25 | 26 | def send(topic, data): 27 | msgs[topic] = data 28 | 29 | bridge = Bridge(conf, send) 30 | 31 | @sio.on('telemetry') 32 | def telemetry(sid, data): 33 | #rospy.loginfo("x: %.2f, y: %.2f, z: %.2f, t: %.2f", data['x'], data['y'], data['z'], data['yaw']) 34 | 35 | global dbw_enable 36 | #rate = rospy.Rate(10) 37 | if data["dbw_enable"] != dbw_enable: 38 | dbw_enable = data["dbw_enable"] 39 | bridge.publish_dbw_status(dbw_enable) 40 | bridge.publish_odometry(data) 41 | for i in range(len(msgs)): 42 | topic, data = msgs.popitem() 43 | sio.emit(topic, data=data, skip_sid=True) 44 | #rate.sleep() # sleep a 1/10 of a second 45 | 46 | #@sio.on('control') 47 | def control(sid, data): 48 | #bridge.publish_controls(data) 49 | pass 50 | 51 | #@sio.on('obstacle') 52 | def obstacle(sid, data): 53 | #bridge.publish_obstacles(data) 54 | #rospy.loginfo("obstacle callback") 55 | pass 56 | 57 | #@sio.on('lidar') 58 | def obstacle(sid, data): 59 | #bridge.publish_lidar(data) 60 | pass 61 | 62 | @sio.on('trafficlights') 63 | def trafficlights(sid, data): 64 | bridge.publish_traffic(data) 65 | #pass 66 | 67 | count = 0 68 | skip = 2 69 | @sio.on('image') 70 | def image(sid, data): 71 | global count 72 | count += 1 73 | if count%(skip+1)==0: 74 | bridge.publish_camera(data) 75 | #rospy.loginfo("imagesize: %d", len(data["image"]) ) 76 | #pass 77 | 78 | if __name__ == '__main__': 79 | 80 | # wrap Flask application with engineio's middleware 81 | app = socketio.Middleware(sio, app) 82 | 83 | # deploy as an eventlet WSGI server 84 | eventlet.wsgi.server(eventlet.listen(('', 4567)), app) 85 | -------------------------------------------------------------------------------- /ros/src/styx/unity_simulator_launcher.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # 3 | # Script to launch the CarND Unity simulator 4 | 5 | THIS_DIR="$(cd "$(dirname "$0")" && pwd -P && cd - > /dev/null)" 6 | USER_PROFILE="$THIS_DIR/profile.tmp" 7 | 8 | if [ ! -f "$USER_PROFILE" ]; 9 | then 10 | echo "What is the full path to your Unity simulator?" 11 | read unity_path 12 | 13 | # write to the file 14 | echo "$unity_path" > $USER_PROFILE 15 | else 16 | unity_path=$(cat "$USER_PROFILE") 17 | fi 18 | 19 | $unity_path 20 | -------------------------------------------------------------------------------- /ros/src/styx_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(styx_msgs) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | # add_definitions(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | message_generation 12 | geometry_msgs 13 | roscpp 14 | rospy 15 | sensor_msgs 16 | std_msgs 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | # catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 40 | ## but can be declared for certainty nonetheless: 41 | ## * add a run_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | add_message_files( 54 | FILES 55 | TrafficLight.msg 56 | TrafficLightArray.msg 57 | Waypoint.msg 58 | Lane.msg 59 | ) 60 | 61 | ## Generate services in the 'srv' folder 62 | # add_service_files( 63 | # FILES 64 | # Service1.srv 65 | # Service2.srv 66 | # ) 67 | 68 | ## Generate actions in the 'action' folder 69 | # add_action_files( 70 | # FILES 71 | # Action1.action 72 | # Action2.action 73 | # ) 74 | 75 | ## Generate added messages and services with any dependencies listed here 76 | generate_messages( 77 | DEPENDENCIES 78 | geometry_msgs 79 | sensor_msgs 80 | std_msgs 81 | ) 82 | 83 | ################################################ 84 | ## Declare ROS dynamic reconfigure parameters ## 85 | ################################################ 86 | 87 | ## To declare and build dynamic reconfigure parameters within this 88 | ## package, follow these steps: 89 | ## * In the file package.xml: 90 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 91 | ## * In this file (CMakeLists.txt): 92 | ## * add "dynamic_reconfigure" to 93 | ## find_package(catkin REQUIRED COMPONENTS ...) 94 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 95 | ## and list every .cfg file to be processed 96 | 97 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 98 | # generate_dynamic_reconfigure_options( 99 | # cfg/DynReconf1.cfg 100 | # cfg/DynReconf2.cfg 101 | # ) 102 | 103 | ################################### 104 | ## catkin specific configuration ## 105 | ################################### 106 | ## The catkin_package macro generates cmake config files for your package 107 | ## Declare things to be passed to dependent projects 108 | ## INCLUDE_DIRS: uncomment this if you package contains header files 109 | ## LIBRARIES: libraries you create in this project that dependent projects also need 110 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 111 | ## DEPENDS: system dependencies of this project that dependent projects also need 112 | catkin_package( 113 | # INCLUDE_DIRS include 114 | # LIBRARIES styx_msgs 115 | # CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs std_msgs 116 | # DEPENDS system_lib 117 | ) 118 | 119 | ########### 120 | ## Build ## 121 | ########### 122 | 123 | ## Specify additional locations of header files 124 | ## Your package locations should be listed before other locations 125 | # include_directories(include) 126 | include_directories( 127 | ${catkin_INCLUDE_DIRS} 128 | ) 129 | 130 | ## Declare a C++ library 131 | # add_library(${PROJECT_NAME} 132 | # src/${PROJECT_NAME}/styx_msgs.cpp 133 | # ) 134 | 135 | ## Add cmake target dependencies of the library 136 | ## as an example, code may need to be generated before libraries 137 | ## either from message generation or dynamic reconfigure 138 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 139 | 140 | ## Declare a C++ executable 141 | ## With catkin_make all packages are built within a single CMake context 142 | ## The recommended prefix ensures that target names across packages don't collide 143 | # add_executable(${PROJECT_NAME}_node src/styx_msgs_node.cpp) 144 | 145 | ## Rename C++ executable without prefix 146 | ## The above recommended prefix causes long target names, the following renames the 147 | ## target back to the shorter version for ease of user use 148 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 149 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 150 | 151 | ## Add cmake target dependencies of the executable 152 | ## same as for the library above 153 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 154 | 155 | ## Specify libraries to link a library or executable target against 156 | # target_link_libraries(${PROJECT_NAME}_node 157 | # ${catkin_LIBRARIES} 158 | # ) 159 | 160 | ############# 161 | ## Install ## 162 | ############# 163 | 164 | # all install targets should use catkin DESTINATION variables 165 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 166 | 167 | ## Mark executable scripts (Python etc.) for installation 168 | ## in contrast to setup.py, you can choose the destination 169 | # install(PROGRAMS 170 | # scripts/my_python_script 171 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark executables and/or libraries for installation 175 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | # install(FILES 190 | # # myfile1 191 | # # myfile2 192 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 193 | # ) 194 | 195 | ############# 196 | ## Testing ## 197 | ############# 198 | 199 | ## Add gtest based cpp test target and link libraries 200 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_styx_msgs.cpp) 201 | # if(TARGET ${PROJECT_NAME}-test) 202 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 203 | # endif() 204 | 205 | ## Add folders to be run by python nosetests 206 | # catkin_add_nosetests(test) 207 | -------------------------------------------------------------------------------- /ros/src/styx_msgs/msg/Lane.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | Waypoint[] waypoints 3 | -------------------------------------------------------------------------------- /ros/src/styx_msgs/msg/TrafficLight.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/PoseStamped pose 3 | uint8 state 4 | 5 | uint8 UNKNOWN=4 6 | uint8 GREEN=2 7 | uint8 YELLOW=1 8 | uint8 RED=0 9 | -------------------------------------------------------------------------------- /ros/src/styx_msgs/msg/TrafficLightArray.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | TrafficLight[] lights 3 | -------------------------------------------------------------------------------- /ros/src/styx_msgs/msg/Waypoint.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped pose 2 | geometry_msgs/TwistStamped twist 3 | -------------------------------------------------------------------------------- /ros/src/styx_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | styx_msgs 4 | 0.0.0 5 | The styx_msgs package 6 | 7 | 8 | 9 | 10 | yousuf 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | geometry_msgs 44 | roscpp 45 | rospy 46 | sensor_msgs 47 | std_msgs 48 | geometry_msgs 49 | roscpp 50 | rospy 51 | sensor_msgs 52 | std_msgs 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /ros/src/tl_detector/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(tl_detector) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | # add_definitions(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | geometry_msgs 12 | roscpp 13 | rospy 14 | sensor_msgs 15 | std_msgs 16 | styx_msgs 17 | waypoint_updater 18 | ) 19 | 20 | ## System dependencies are found with CMake's conventions 21 | # find_package(Boost REQUIRED COMPONENTS system) 22 | 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | # catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 38 | ## * add a build_depend tag for "message_generation" 39 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 40 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 41 | ## but can be declared for certainty nonetheless: 42 | ## * add a run_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | # add_message_files( 55 | # FILES 56 | # Message1.msg 57 | # Message2.msg 58 | # ) 59 | 60 | ## Generate services in the 'srv' folder 61 | # add_service_files( 62 | # FILES 63 | # Service1.srv 64 | # Service2.srv 65 | # ) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ## Generate added messages and services with any dependencies listed here 75 | # generate_messages( 76 | # DEPENDENCIES 77 | # geometry_msgs# sensor_msgs# std_msgs# styx_msgs 78 | # ) 79 | 80 | ################################################ 81 | ## Declare ROS dynamic reconfigure parameters ## 82 | ################################################ 83 | 84 | ## To declare and build dynamic reconfigure parameters within this 85 | ## package, follow these steps: 86 | ## * In the file package.xml: 87 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 88 | ## * In this file (CMakeLists.txt): 89 | ## * add "dynamic_reconfigure" to 90 | ## find_package(catkin REQUIRED COMPONENTS ...) 91 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 92 | ## and list every .cfg file to be processed 93 | 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 95 | # generate_dynamic_reconfigure_options( 96 | # cfg/DynReconf1.cfg 97 | # cfg/DynReconf2.cfg 98 | # ) 99 | 100 | ################################### 101 | ## catkin specific configuration ## 102 | ################################### 103 | ## The catkin_package macro generates cmake config files for your package 104 | ## Declare things to be passed to dependent projects 105 | ## INCLUDE_DIRS: uncomment this if you package contains header files 106 | ## LIBRARIES: libraries you create in this project that dependent projects also need 107 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 108 | ## DEPENDS: system dependencies of this project that dependent projects also need 109 | catkin_package( 110 | # INCLUDE_DIRS include 111 | # LIBRARIES waypoint_loader 112 | # CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs std_msgs styx_msgs 113 | # DEPENDS system_lib 114 | ) 115 | 116 | ########### 117 | ## Build ## 118 | ########### 119 | 120 | ## Specify additional locations of header files 121 | ## Your package locations should be listed before other locations 122 | # include_directories(include) 123 | include_directories( 124 | ${catkin_INCLUDE_DIRS} 125 | ) 126 | 127 | ## Declare a C++ library 128 | # add_library(${PROJECT_NAME} 129 | # src/${PROJECT_NAME}/waypoint_loader.cpp 130 | # ) 131 | 132 | ## Add cmake target dependencies of the library 133 | ## as an example, code may need to be generated before libraries 134 | ## either from message generation or dynamic reconfigure 135 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Declare a C++ executable 138 | ## With catkin_make all packages are built within a single CMake context 139 | ## The recommended prefix ensures that target names across packages don't collide 140 | # add_executable(${PROJECT_NAME}_node src/waypoint_loader_node.cpp) 141 | 142 | ## Rename C++ executable without prefix 143 | ## The above recommended prefix causes long target names, the following renames the 144 | ## target back to the shorter version for ease of user use 145 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 146 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 147 | 148 | ## Add cmake target dependencies of the executable 149 | ## same as for the library above 150 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 151 | 152 | ## Specify libraries to link a library or executable target against 153 | # target_link_libraries(${PROJECT_NAME}_node 154 | # ${catkin_LIBRARIES} 155 | # ) 156 | 157 | ############# 158 | ## Install ## 159 | ############# 160 | 161 | # all install targets should use catkin DESTINATION variables 162 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 163 | 164 | ## Mark executable scripts (Python etc.) for installation 165 | ## in contrast to setup.py, you can choose the destination 166 | # install(PROGRAMS 167 | # scripts/my_python_script 168 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 169 | # ) 170 | 171 | ## Mark executables and/or libraries for installation 172 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 173 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 175 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 176 | # ) 177 | 178 | ## Mark cpp header files for installation 179 | # install(DIRECTORY include/${PROJECT_NAME}/ 180 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 181 | # FILES_MATCHING PATTERN "*.h" 182 | # PATTERN ".svn" EXCLUDE 183 | # ) 184 | 185 | ## Mark other files for installation (e.g. launch and bag files, etc.) 186 | # install(FILES 187 | # # myfile1 188 | # # myfile2 189 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 190 | # ) 191 | 192 | ############# 193 | ## Testing ## 194 | ############# 195 | 196 | ## Add gtest based cpp test target and link libraries 197 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_waypoint_loader.cpp) 198 | # if(TARGET ${PROJECT_NAME}-test) 199 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 200 | # endif() 201 | 202 | ## Add folders to be run by python nosetests 203 | # catkin_add_nosetests(test) 204 | -------------------------------------------------------------------------------- /ros/src/tl_detector/launch/tl_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /ros/src/tl_detector/launch/tl_detector_site.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /ros/src/tl_detector/light_classification/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/level5-engineers/system-integration/e59e4f3570a006f916fbacf690bc72477b81d716/ros/src/tl_detector/light_classification/__init__.py -------------------------------------------------------------------------------- /ros/src/tl_detector/light_classification/model/sim_graph.pb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/level5-engineers/system-integration/e59e4f3570a006f916fbacf690bc72477b81d716/ros/src/tl_detector/light_classification/model/sim_graph.pb -------------------------------------------------------------------------------- /ros/src/tl_detector/light_classification/model/sim_labels.txt: -------------------------------------------------------------------------------- 1 | none 2 | red 3 | -------------------------------------------------------------------------------- /ros/src/tl_detector/light_classification/model/site_graph.pb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/level5-engineers/system-integration/e59e4f3570a006f916fbacf690bc72477b81d716/ros/src/tl_detector/light_classification/model/site_graph.pb -------------------------------------------------------------------------------- /ros/src/tl_detector/light_classification/test_data/test_green1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/level5-engineers/system-integration/e59e4f3570a006f916fbacf690bc72477b81d716/ros/src/tl_detector/light_classification/test_data/test_green1.png -------------------------------------------------------------------------------- /ros/src/tl_detector/light_classification/tl_classifier.py: -------------------------------------------------------------------------------- 1 | import tensorflow as tf 2 | import cv2 3 | import numpy as np 4 | import time 5 | 6 | # Reference: https://github.com/udacity/CarND-Object-Detection-Lab 7 | class TLClassifier(object): 8 | def __init__(self, simulator): 9 | self.seq = 0 10 | self.simulator = simulator 11 | start = time.time() 12 | if simulator: 13 | graph_filename = 'light_classification/model/sim_graph.pb' 14 | else: 15 | graph_filename = 'light_classification/model/site_graph.pb' 16 | labels_filename = 'light_classification/model/sim_labels.txt' 17 | 18 | self.labels = self.load_labels(labels_filename) 19 | self.labels_dic = {'yellow':1, 20 | 'green':2, 21 | 'red':0, 22 | 'none':4} 23 | 24 | print("Initializing TensorFlow...") 25 | self.detection_graph = tf.Graph() 26 | # configure for a GPU 27 | config = tf.ConfigProto() 28 | config.gpu_options.allow_growth = True 29 | # load trained tensorflow graph 30 | with self.detection_graph.as_default(): 31 | graph_def = tf.GraphDef() 32 | with tf.gfile.GFile(graph_filename, 'rb') as f: 33 | graph_def.ParseFromString(f.read()) 34 | tf.import_graph_def(graph_def, name='') 35 | 36 | self.sess = tf.Session(graph=self.detection_graph, config=config) 37 | 38 | # configure input and output 39 | if simulator: 40 | self.image_tensor = self.detection_graph.get_tensor_by_name('input:0') 41 | self.softmax_tensor = self.detection_graph.get_tensor_by_name('final_result:0') 42 | image = np.asarray(np.random.rand(224,224,3)*2.-1.) 43 | else: 44 | self.image_tensor = self.detection_graph.get_tensor_by_name('image_tensor:0') 45 | self.num_detections = self.detection_graph.get_tensor_by_name('num_detections:0') 46 | self.dboxes = self.detection_graph.get_tensor_by_name('detection_boxes:0') 47 | self.dscores = self.detection_graph.get_tensor_by_name('detection_scores:0') 48 | self.dclasses = self.detection_graph.get_tensor_by_name('detection_classes:0') 49 | image = np.asarray(np.random.rand(300,300,3)*255, dtype="uint8") 50 | 51 | # initialize the network by running a randomized image 52 | image_expanded = np.expand_dims(image, axis=0) 53 | startA = time.time() 54 | if simulator: 55 | _ = self.sess.run(self.softmax_tensor, {self.image_tensor: image_expanded, 'Placeholder:0':1.0}) 56 | else: 57 | _ = self.sess.run([self.dboxes, self.dscores, self.dclasses, self.num_detections], 58 | feed_dict={self.image_tensor: image_expanded}) 59 | endA = time.time() 60 | print('Priming duration: ', endA-startA) 61 | end = time.time() 62 | print('Total initialization time: ', end-start) 63 | 64 | def load_labels(self, filename): 65 | """Read in labels, one label per line. 66 | From Tensorflow Example image retrain""" 67 | return [line.rstrip() for line in tf.gfile.GFile(filename)] 68 | 69 | # downsample the incoming image from the ROS message 70 | def scale_image(self, img): 71 | image_data = cv2.resize(img, (224,224)) 72 | image_data = (image_data - 128.)/128. 73 | image_data = np.reshape(image_data, (1, 224,224,3)) 74 | return image_data 75 | 76 | # Convert normalized box coordinates to pixels 77 | def to_image_coords(self, boxes, dim): 78 | """ 79 | The original box coordinate output is normalized, i.e [0, 1]. 80 | This converts it back to the original coordinates based on the 81 | image size. Optimized. 82 | """ 83 | h, w = dim[0], dim[1] 84 | box_coords = [int(boxes[0]*h), int(boxes[1]*w), int(boxes[2]*h), int(boxes[3]*w)] 85 | return np.array(box_coords) 86 | 87 | def locateTL(self, image): 88 | box = [0, 0, 0, 0] 89 | with self.detection_graph.as_default(): 90 | image_expanded = np.expand_dims(image, axis=0) 91 | (boxes, scores, classes, num_detections) = self.sess.run( 92 | [self.dboxes, self.dscores, self.dclasses, self.num_detections], 93 | feed_dict={self.image_tensor: image_expanded}) 94 | 95 | # Remove unnecessary dimensions 96 | boxes = np.squeeze(boxes) 97 | class_ = np.int32(np.squeeze(classes).tolist()) 98 | scores = np.squeeze(scores) 99 | 100 | # First occurrence where clsid<4 (traffic lights) 101 | index = next((i for i, clsid in enumerate(class_) if clsid < 4), None) 102 | if index == None: 103 | print 'No traffic light detected' 104 | elif scores[index] < 0.5: 105 | print 'Low confidence: ', scores[index] 106 | else: 107 | b = self.to_image_coords(boxes[index], image.shape[0:2]) 108 | b_w = b[3] - b[1] 109 | ratio = (b[2] - b[0]) / (b_w + 0.00001) 110 | if (b_w >= 20) and (ratio > 2.0) and (ratio < 3.8): 111 | #print 'Confidence: ', scores[index] 112 | box = b 113 | return box 114 | 115 | # Classify a traffic light based on simple geometric properties 116 | # Expects a gray-scale image 117 | def classifyTL(self, image_data): 118 | # get the image center geometry 119 | midX = int(image_data.shape[1]/2) 120 | midY = int(image_data.shape[0]/2) 121 | thirdY = int(image_data.shape[0]/3) 122 | p = int(thirdY/3) #patch size 123 | # get the center point of each ROI 124 | rROI = ( int(thirdY/2) , midX ) 125 | yROI = ( midY, midX ) 126 | gROI = ( midY+thirdY , midX ) 127 | # find the average from each center patch 128 | rROI = int(np.mean(image_data[rROI[0]-p:rROI[0]+p, rROI[1]-p:rROI[1]+p])) 129 | yROI = int(np.mean(image_data[yROI[0]-p:yROI[0]+p, yROI[1]-p:yROI[1]+p])) 130 | gROI = int(np.mean(image_data[gROI[0]-p:gROI[0]+p, gROI[1]-p:gROI[1]+p])) 131 | # perform simple brightness comparisons and print for humans 132 | if (gROI > yROI) and (gROI > rROI): 133 | print(">>> GREEN <<<") 134 | elif (yROI > gROI) and (yROI > rROI): 135 | print(">>> YELLOW <<<") 136 | elif (rROI > yROI) and (rROI > gROI): 137 | print(">>> RED <<<") 138 | if (gROI > yROI) and (gROI > rROI): 139 | return 1 # GO 140 | else: 141 | return 0 # STOP 142 | 143 | def saveImage(self,img): 144 | fname = '/home/student/xdrive/debug/out' + str(self.seq).zfill(5)+'.png' 145 | print "Saving tl image", fname 146 | cv2.imwrite(fname, img) 147 | self.seq += 1 148 | 149 | def get_classification(self, image): 150 | """Locates and determines the color of the traffic light in the image 151 | Args: 152 | image (cv::Mat): image containing the traffic light 153 | Returns: 154 | int: ID of traffic light color (specified in styx_msgs/TrafficLight) 155 | """ 156 | #print('____________________________________________________________________') 157 | #print image.shape[0], image.shape[1] 158 | 159 | if self.simulator: 160 | image_data = self.scale_image(image) 161 | predictions, = self.sess.run(self.softmax_tensor, {self.image_tensor: image_data, 'Placeholder:0':1.0}) 162 | 163 | # Sort to show labels in order of confidence 164 | top_k = predictions.argsort()[-1:][::-1] 165 | for node_id in top_k: 166 | predict_label = self.labels[node_id] 167 | score = predictions[node_id] 168 | print '%s (score = %.5f)' % (predict_label, score) 169 | return self.labels_dic[predict_label] 170 | 171 | else: 172 | start = time.time() 173 | b = self.locateTL(image) 174 | end = time.time() 175 | print 'Detection time: ', end-start 176 | 177 | # If there is no detection or low-confidence detection 178 | if np.array_equal(b, np.zeros(4)): 179 | #print ('unknown') 180 | signal_status = True # Go 181 | tlFound = False 182 | else: 183 | img_tl = cv2.cvtColor(image[b[0]:b[2], b[1]:b[3]], cv2.COLOR_RGB2HSV)[:,:,2] 184 | #self.saveImage(img_tl) 185 | signal_status = self.classifyTL(img_tl) 186 | #print("GO" if signal_status else "STOP") 187 | tlFound = True 188 | end = time.time() 189 | return 4 if signal_status else 0, tlFound, (b[0],b[1]) # return position 190 | -------------------------------------------------------------------------------- /ros/src/tl_detector/light_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import tf 4 | import cv2 5 | import time 6 | from styx_msgs.msg import TrafficLightArray, TrafficLight 7 | from std_msgs.msg import Header 8 | from geometry_msgs.msg import PoseStamped, Quaternion, TwistStamped 9 | 10 | import numpy as np 11 | import rospkg 12 | import math 13 | 14 | class TLPublisher(object): 15 | def __init__(self): 16 | rospy.init_node('tl_publisher') 17 | 18 | self.traffic_light_pubs = rospy.Publisher('/vehicle/traffic_lights', TrafficLightArray, queue_size=1) 19 | 20 | light = self.create_light(20.991, 22.837, 1.524, 0.08, 3) 21 | lights = TrafficLightArray() 22 | lights.header = light.header 23 | lights.lights = [light] 24 | self.lights = lights 25 | self.loop() 26 | 27 | def loop(self): 28 | rate = rospy.Rate(50) 29 | while not rospy.is_shutdown(): 30 | self.traffic_light_pubs.publish(self.lights) 31 | rate.sleep() 32 | 33 | def create_light(self, x, y, z, yaw, state): 34 | light = TrafficLight() 35 | 36 | light.header = Header() 37 | light.header.stamp = rospy.Time.now() 38 | light.header.frame_id = '/world' 39 | 40 | light.pose = self.create_pose(x, y, z, yaw) 41 | light.state = state 42 | 43 | return light 44 | 45 | def create_pose(self, x, y, z, yaw=0.): 46 | pose = PoseStamped() 47 | 48 | pose.header = Header() 49 | pose.header.stamp = rospy.Time.now() 50 | pose.header.frame_id = '/world' 51 | 52 | pose.pose.position.x = x 53 | pose.pose.position.y = y 54 | pose.pose.position.z = z 55 | 56 | q = tf.transformations.quaternion_from_euler(0., 0., math.pi * yaw/180.) 57 | pose.pose.orientation = Quaternion(*q) 58 | 59 | return pose 60 | 61 | 62 | if __name__ == '__main__': 63 | try: 64 | TLPublisher() 65 | except rospy.ROSInterruptException: 66 | rospy.logerr('Could not start traffic publisher node.') 67 | -------------------------------------------------------------------------------- /ros/src/tl_detector/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | tl_detector 4 | 0.0.0 5 | The traffic light detection package 6 | 7 | 8 | 9 | 10 | yousuf 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | geometry_msgs 44 | roscpp 45 | rospy 46 | sensor_msgs 47 | std_msgs 48 | styx_msgs 49 | waypoint_updater 50 | geometry_msgs 51 | roscpp 52 | rospy 53 | sensor_msgs 54 | std_msgs 55 | styx_msgs 56 | waypoint_updater 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /ros/src/tl_detector/sim_traffic_light_config.yaml: -------------------------------------------------------------------------------- 1 | camera_info: 2 | image_width: 800 3 | image_height: 600 4 | stop_line_positions: 5 | - [1148.56, 1184.65] 6 | - [1559.2, 1158.43] 7 | - [2122.14, 1526.79] 8 | - [2175.237, 1795.71] 9 | - [1493.29, 2947.67] 10 | - [821.96, 2905.8] 11 | - [161.76, 2303.82] 12 | - [351.84, 1574.65] 13 | -------------------------------------------------------------------------------- /ros/src/tl_detector/site_traffic_light_config.yaml: -------------------------------------------------------------------------------- 1 | camera_info: 2 | focal_length_x: 1345.200806 3 | focal_length_y: 1353.838257 4 | image_width: 800 5 | image_height: 600 6 | stop_line_positions: 7 | - [8.0, 16.2] 8 | -------------------------------------------------------------------------------- /ros/src/tl_detector/sitesim_traffic_light_config.yaml: -------------------------------------------------------------------------------- 1 | camera_info: 2 | image_width: 800 3 | image_height: 600 4 | stop_line_positions: 5 | - [1148.56, 1184.65] 6 | -------------------------------------------------------------------------------- /ros/src/tl_detector/tl_detector.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import os 4 | import csv 5 | from std_msgs.msg import Int32 6 | from geometry_msgs.msg import PoseStamped, Pose, Quaternion 7 | from styx_msgs.msg import TrafficLightArray, TrafficLight, Waypoint 8 | from styx_msgs.msg import Lane 9 | from sensor_msgs.msg import Image 10 | from cv_bridge import CvBridge 11 | from light_classification.tl_classifier import TLClassifier 12 | import tf 13 | import cv2 14 | import yaml 15 | import math 16 | import numpy as np 17 | 18 | STATE_COUNT_THRESHOLD = 3 19 | CLASSIFIER_ENABLED = True 20 | PUBLISHING_RATE = 12 # Publishing frequency (Hz) 21 | 22 | from PIL import Image as PIL_Image 23 | 24 | class TLDetector(object): 25 | def __init__(self): 26 | rospy.init_node('tl_detector') 27 | self.simulator = True if rospy.get_param('~sim') == 1 else False 28 | 29 | self.pose = None 30 | self.waypoints = None 31 | self.camera_image = None 32 | self.lights = [] 33 | self.bridge = CvBridge() 34 | self.light_classifier = TLClassifier(self.simulator) 35 | self.listener = tf.TransformListener() 36 | self.state = TrafficLight.UNKNOWN 37 | self.last_state = TrafficLight.UNKNOWN 38 | self.last_wp = -1 39 | self.state_count = 0 40 | self.camera_image = None 41 | self.seq = 0 42 | self.count = 0 43 | self.misscount = 0. 44 | self.totcount = 0. 45 | self.sight = 75. # cautionary distance 46 | self.wpAcquired = False 47 | self.lastCrop = None 48 | self.imageAcquired = False 49 | 50 | sub1 = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb) 51 | sub2 = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb) 52 | 53 | ''' 54 | /vehicle/traffic_lights provides you with the location of the traffic light in 3D map space and 55 | helps you acquire an accurate ground truth data source for the traffic light 56 | classifier by sending the current color state of all traffic lights in the 57 | simulator. When testing on the vehicle, the color state will not be available. You'll need to 58 | rely on the position of the light and the camera image to predict it. 59 | ''' 60 | sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_cb) 61 | 62 | config_string = rospy.get_param("/traffic_light_config") 63 | self.config = yaml.load(config_string) 64 | 65 | self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1) 66 | 67 | sub6 = rospy.Subscriber('/image_color', Image, self.image_cb) 68 | #rospy.spin() 69 | # Operations loop to identify red lights in the incoming camera image and publishes 70 | # the index of the waypoint closest to the red light's stop line to /traffic_waypoint 71 | rate = rospy.Rate(PUBLISHING_RATE) 72 | 73 | while not rospy.is_shutdown(): 74 | if self.imageAcquired: 75 | light_wp, state = self.process_traffic_lights() 76 | ''' 77 | Publish upcoming red lights at PUBLISHING_RATE frequency. 78 | Each predicted state has to occur `STATE_COUNT_THRESHOLD` number 79 | of times until we start using it. Otherwise the previous stable 80 | state is used. 81 | ''' 82 | if self.state != state: 83 | self.state_count = 0 84 | self.state = state 85 | elif self.state_count >= STATE_COUNT_THRESHOLD: 86 | self.last_state = self.state 87 | light_wp = light_wp if state == TrafficLight.RED or state == TrafficLight.YELLOW else -1 88 | self.last_wp = light_wp 89 | self.upcoming_red_light_pub.publish(Int32(light_wp)) 90 | else: 91 | self.upcoming_red_light_pub.publish(Int32(self.last_wp)) 92 | self.state_count += 1 93 | rate.sleep() 94 | 95 | def pose_cb(self, msg): 96 | self.pose = msg 97 | 98 | def waypoints_cb(self, msg): 99 | if not self.wpAcquired: 100 | self.wpAcquired = True 101 | self.waypoints = self.filterWaypoints(msg) 102 | if not self.simulator: 103 | self.sight = 16. # adjust cautionary distance for site test 104 | print "cautionary distance", self.sight 105 | print self.waypoints[20].twist.twist.linear.x 106 | 107 | def traffic_cb(self, msg): 108 | self.lights = msg.lights 109 | 110 | def image_cb(self, msg): 111 | """Capture the inbound camera image 112 | Args: 113 | msg (Image): image from car-mounted camera 114 | """ 115 | self.imageAcquired = True 116 | self.camera_image = msg 117 | 118 | def distance(self, pos1, pos2): 119 | return math.sqrt((pos1.position.x - pos2.position.x)**2 + (pos1.position.y - pos2.position.y)**2) 120 | 121 | def get_closest_waypoint(self, pose): 122 | """Identifies the closest path waypoint to the given position 123 | https://en.wikipedia.org/wiki/Closest_pair_of_points_problem 124 | Args: 125 | pose (Pose): position to match a waypoint 126 | Returns: 127 | int: index of the closest waypoint in self.waypoints 128 | """ 129 | index = -1 #Return if waypoints are empty 130 | if self.waypoints is None: 131 | return index 132 | 133 | lowDistance = 0 134 | #rospy.loginfo("len(wp): %d", len(self.waypoints)) 135 | for i in range(len(self.waypoints)): 136 | distance = self.distance(pose, self.waypoints[i].pose.pose) 137 | 138 | if index == -1 or distance < lowDistance: 139 | index = i 140 | lowDistance = distance 141 | 142 | return index 143 | 144 | def get_light_state(self, light): 145 | """Determines the current color of the traffic light 146 | Args: 147 | light (TrafficLight): light to classify 148 | Returns: 149 | int: ID of traffic light color (specified in styx_msgs/TrafficLight) 150 | """ 151 | if(not self.imageAcquired): 152 | self.prev_light_loc = None 153 | return TrafficLight.RED 154 | 155 | # fix camera encoding 156 | if hasattr(self.camera_image, 'encoding'): 157 | if self.camera_image.encoding == '8UC3': 158 | self.camera_image.encoding = "rgb8" 159 | else: 160 | self.camera_image.encoding = 'rgb8' 161 | cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8") 162 | 163 | # get classification (simulator classifies whole image) 164 | if self.simulator: 165 | return self.light_classifier.get_classification(cv_image) 166 | 167 | # get classification (test site detects boxes using SSD and classifies with OpenCV) 168 | # if previous location is not known, scan most of the image 169 | if self.lastCrop is None: 170 | gostop, found, location = self.light_classifier.get_classification( cv_image[0:500, 50:50+700] ) 171 | if found: 172 | # check y extents 173 | if location[0] < 150: 174 | top = 0 175 | bot = 300 176 | else: 177 | top = location[0]-150 178 | if location[0] > 350: 179 | top = 200 180 | bot = 500 181 | else: 182 | bot = location[0]+150 183 | 184 | # check x extents (remember, offset by 50) 185 | if location[1]+50 < 150: 186 | left = 50 187 | right = 350 188 | else: 189 | left = location[1]+50-150 190 | if location[1]+50 > 600: 191 | left = 450 192 | right = 750 193 | else: 194 | right = location[1]+50+150 195 | self.lastCrop = (top,bot, left,right) 196 | #print "first", self.lastCrop 197 | # (no else) TL not found, next cycle will be a complete search again 198 | # otherwise, use last known location as crop starting point 199 | else: 200 | (top,bot, left,right) = self.lastCrop 201 | gostop, found, location = self.light_classifier.get_classification( cv_image[top:bot, left:right] ) 202 | if found: # determine crop for next cycle 203 | # check y extents, offset by top 204 | otop, oleft = top, left 205 | if location[0]+otop < 150: 206 | top = 0 207 | bot = 300 208 | else: 209 | top = location[0]+otop-150 210 | if location[0]+otop > 350: 211 | top = 200 212 | bot = 500 213 | else: 214 | bot = location[0]+otop+150 215 | 216 | # check x extents, offset by left 217 | if location[1]+oleft+50 < 200: 218 | left = 50 219 | right = 350 220 | else: 221 | left = location[1]+oleft+50-150 222 | if location[1]+oleft+50 > 600: 223 | left = 450 224 | right = 750 225 | else: 226 | right = location[1]+oleft+50+150 227 | 228 | self.lastCrop = (top,bot, left,right) 229 | #print "next", self.lastCrop 230 | else: 231 | self.lastCrop = None 232 | #print "none" 233 | return gostop 234 | 235 | def get_nearest_stop_line(self, waypoint_start_index): 236 | stop_line = None 237 | # List of positions that correspond to the line to stop in front of for a given intersection 238 | stop_line_positions = self.config['stop_line_positions'] 239 | last_index = 99999 240 | 241 | for i in range(0, len(stop_line_positions)): 242 | stopline_pose = PoseStamped() 243 | stopline_pose.pose.position.x = float(stop_line_positions[i][0]) 244 | stopline_pose.pose.position.y = float(stop_line_positions[i][1]) 245 | index = self.get_closest_waypoint(stopline_pose.pose) 246 | if index > waypoint_start_index and index < last_index: 247 | last_index = index; 248 | stop_line = stopline_pose 249 | 250 | return stop_line, last_index 251 | 252 | def process_traffic_lights(self): 253 | """Finds closest visible traffic light, if one exists, and determines its 254 | location and color 255 | Returns: 256 | int: index of waypoint closest to the upcoming stop line for a traffic light (-1 if none exists) 257 | int: ID of traffic light color (specified in styx_msgs/TrafficLight) 258 | """ 259 | if(self.pose): 260 | car_position = self.get_closest_waypoint(self.pose.pose) 261 | #rospy.loginfo("car_position: %d", car_position) 262 | if car_position > 0: 263 | stop_pos, stop_waypoint = self.get_nearest_stop_line(car_position) 264 | 265 | if stop_pos is not None: 266 | #rospy.loginfo("stop_pos x: %.2f, stop_waypoint: %d", stop_pos.pose.position.x, stop_waypoint) 267 | state = TrafficLight.UNKNOWN 268 | 269 | # if the traffic light is within sight, then attempt to classify 270 | if self.distance(self.pose.pose, stop_pos.pose) < self.sight: 271 | 272 | if CLASSIFIER_ENABLED: 273 | state = self.get_light_state(None) 274 | if self.lights is not None: 275 | stateTruth = TrafficLight.UNKNOWN 276 | for light in self.lights: 277 | # Get the ground truth from /vehicle/traffic_lights 278 | if self.distance(light.pose.pose, stop_pos.pose) < 30.: 279 | stateTruth = light.state 280 | break 281 | if (((stateTruth is 4) or (stateTruth is 2)) and (state is 0)) or (((stateTruth is 0) or (stateTruth is 1)) and (state is 4)) : 282 | print "Classifier: ", state, " Truth: ", stateTruth 283 | state = stateTruth 284 | #self.saveImage(self.camera_image, stateTruth) 285 | self.misscount += 1. 286 | self.totcount += 1. 287 | #print "mismatch%: ", self.misscount / self.totcount 288 | #self.saveImage(self.camera_image, stateTruth) 289 | else: 290 | for light in self.lights: 291 | # This section uses only /vehicle/traffic_lights 292 | if self.distance(light.pose.pose, stop_pos.pose) < 30.: 293 | state = light.state 294 | #rospy.loginfo("light state: %d, x: %.2f, y: %.2f", state, self.pose.pose.position.x, self.pose.pose.position.y) 295 | #rospy.loginfo("dist tl to stop: %.2f", self.distance(light.pose.pose, stop_pos.pose)) 296 | break 297 | return stop_waypoint, state 298 | #self.waypoints = None 299 | return -1, TrafficLight.UNKNOWN 300 | 301 | def saveImage(self, img, state): 302 | #if self.count%2==0: 303 | # fix camera encoding 304 | if hasattr(img, 'encoding'): 305 | if img.encoding == '8UC3': 306 | img.encoding = "rgb8" 307 | else: 308 | img.encoding = 'rgb8' 309 | img = self.bridge.imgmsg_to_cv2(img, "rgb8") 310 | 311 | image_data = cv2.resize(img, (224,224)) 312 | img= PIL_Image.fromarray(image_data, 'RGB') 313 | #if state == TrafficLight.RED: 314 | # img.save('/home/student/data/red/out'+str(self.seq).zfill(5)+'.png', 'PNG') 315 | # self.seq += 1 316 | if state == TrafficLight.YELLOW: 317 | img.save('/home/student/data/yellow/out'+str(self.seq).zfill(5)+'.png', 'PNG') 318 | self.seq += 1 319 | elif state == TrafficLight.GREEN: 320 | img.save('/home/student/data/green/out'+str(self.seq).zfill(5)+'.png', 'PNG') 321 | self.seq += 1 322 | #else: 323 | # img.save('/home/student/data/unknown/out'+str(self.seq).zfill(5)+'.png', 'PNG') 324 | # self.seq += 1 325 | 326 | def filterWaypoints(self, wp): 327 | if wp.waypoints[0].pose.pose.position.x == 10.4062: 328 | waypoints = [] 329 | path = rospy.get_param('~path') 330 | if not os.path.isfile(path): 331 | return wp.waypoints 332 | with open(path) as wfile: 333 | reader = csv.DictReader(wfile, ['x','y','z','yaw']) 334 | for wp in reader: 335 | p = Waypoint() 336 | p.pose.pose.position.x = float(wp['x']) 337 | p.pose.pose.position.y = float(wp['y']) 338 | p.pose.pose.position.z = float(wp['z']) 339 | q = tf.transformations.quaternion_from_euler(0., 0., float(wp['yaw'])) 340 | p.pose.pose.orientation = Quaternion(*q) 341 | p.twist.twist.linear.x = 2.7777778 342 | waypoints.append(p) 343 | return waypoints 344 | return wp.waypoints 345 | 346 | if __name__ == '__main__': 347 | try: 348 | TLDetector() 349 | except rospy.ROSInterruptException: 350 | rospy.logerr('Could not start traffic node.') 351 | -------------------------------------------------------------------------------- /ros/src/twist_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(twist_controller) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | # add_definitions(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | dbw_mkz_msgs 12 | geometry_msgs 13 | roscpp 14 | rospy 15 | std_msgs 16 | styx_msgs 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | # catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 40 | ## but can be declared for certainty nonetheless: 41 | ## * add a run_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | # add_message_files( 54 | # FILES 55 | # Message1.msg 56 | # Message2.msg 57 | # ) 58 | 59 | ## Generate services in the 'srv' folder 60 | # add_service_files( 61 | # FILES 62 | # Service1.srv 63 | # Service2.srv 64 | # ) 65 | 66 | ## Generate actions in the 'action' folder 67 | # add_action_files( 68 | # FILES 69 | # Action1.action 70 | # Action2.action 71 | # ) 72 | 73 | ## Generate added messages and services with any dependencies listed here 74 | # generate_messages( 75 | # DEPENDENCIES 76 | # dbw_mkz_msgs# geometry_msgs# std_msgs 77 | # ) 78 | 79 | ################################################ 80 | ## Declare ROS dynamic reconfigure parameters ## 81 | ################################################ 82 | 83 | ## To declare and build dynamic reconfigure parameters within this 84 | ## package, follow these steps: 85 | ## * In the file package.xml: 86 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 87 | ## * In this file (CMakeLists.txt): 88 | ## * add "dynamic_reconfigure" to 89 | ## find_package(catkin REQUIRED COMPONENTS ...) 90 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 91 | ## and list every .cfg file to be processed 92 | 93 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 94 | # generate_dynamic_reconfigure_options( 95 | # cfg/DynReconf1.cfg 96 | # cfg/DynReconf2.cfg 97 | # ) 98 | 99 | ################################### 100 | ## catkin specific configuration ## 101 | ################################### 102 | ## The catkin_package macro generates cmake config files for your package 103 | ## Declare things to be passed to dependent projects 104 | ## INCLUDE_DIRS: uncomment this if you package contains header files 105 | ## LIBRARIES: libraries you create in this project that dependent projects also need 106 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 107 | ## DEPENDS: system dependencies of this project that dependent projects also need 108 | catkin_package( 109 | # INCLUDE_DIRS include 110 | # LIBRARIES twist_controller 111 | # CATKIN_DEPENDS dbw_mkz_msgs geometry_msgs roscpp rospy std_msgs 112 | # DEPENDS system_lib 113 | ) 114 | 115 | ########### 116 | ## Build ## 117 | ########### 118 | 119 | ## Specify additional locations of header files 120 | ## Your package locations should be listed before other locations 121 | # include_directories(include) 122 | include_directories( 123 | ${catkin_INCLUDE_DIRS} 124 | ) 125 | 126 | ## Declare a C++ library 127 | # add_library(${PROJECT_NAME} 128 | # src/${PROJECT_NAME}/twist_controller.cpp 129 | # ) 130 | 131 | ## Add cmake target dependencies of the library 132 | ## as an example, code may need to be generated before libraries 133 | ## either from message generation or dynamic reconfigure 134 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Declare a C++ executable 137 | ## With catkin_make all packages are built within a single CMake context 138 | ## The recommended prefix ensures that target names across packages don't collide 139 | # add_executable(${PROJECT_NAME}_node src/twist_controller_node.cpp) 140 | 141 | ## Rename C++ executable without prefix 142 | ## The above recommended prefix causes long target names, the following renames the 143 | ## target back to the shorter version for ease of user use 144 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 145 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 146 | 147 | ## Add cmake target dependencies of the executable 148 | ## same as for the library above 149 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 150 | 151 | ## Specify libraries to link a library or executable target against 152 | # target_link_libraries(${PROJECT_NAME}_node 153 | # ${catkin_LIBRARIES} 154 | # ) 155 | 156 | ############# 157 | ## Install ## 158 | ############# 159 | 160 | # all install targets should use catkin DESTINATION variables 161 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 162 | 163 | ## Mark executable scripts (Python etc.) for installation 164 | ## in contrast to setup.py, you can choose the destination 165 | # install(PROGRAMS 166 | # scripts/my_python_script 167 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark executables and/or libraries for installation 171 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_twist_controller.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /ros/src/twist_controller/dbw_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from std_msgs.msg import Bool 5 | from dbw_mkz_msgs.msg import ThrottleCmd, SteeringCmd, BrakeCmd, SteeringReport 6 | from geometry_msgs.msg import TwistStamped 7 | from lowpass import LowPassFilter 8 | 9 | #DEBUG JWD 10 | #from geometry_msgs.msg import PoseStamped 11 | from styx_msgs.msg import Lane 12 | #ENDEBUG 13 | 14 | import math 15 | 16 | from twist_controller import Controller 17 | 18 | ''' 19 | You can build this node only after you have built (or partially built) the `waypoint_updater` node. 20 | 21 | You will subscribe to `/twist_cmd` message which provides the proposed linear and angular velocities. 22 | You can subscribe to any other message that you find important or refer to the document for list 23 | of messages subscribed to by the reference implementation of this node. 24 | 25 | One thing to keep in mind while building this node and the `twist_controller` class is the status 26 | of `dbw_enabled`. While in the simulator, its enabled all the time, in the real car, that will 27 | not be the case. This may cause your PID controller to accumulate error because the car could 28 | temporarily be driven by a human instead of your controller. 29 | 30 | We have provided two launch files with this node. Vehicle specific values (like vehicle_mass, 31 | wheel_base) etc should not be altered in these files. 32 | 33 | We have also provided some reference implementations for PID controller and other utility classes. 34 | You are free to use them or build your own. 35 | 36 | Once you have the proposed throttle, brake, and steer values, publish it on the various publishers 37 | that we have created in the `__init__` function. 38 | 39 | ''' 40 | 41 | class DBWNode(object): 42 | def __init__(self): 43 | rospy.init_node('dbw_node') 44 | 45 | self.vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) 46 | fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) 47 | self.brake_deadband = rospy.get_param('~brake_deadband', .1) 48 | decel_limit = rospy.get_param('~decel_limit', -5) 49 | accel_limit = rospy.get_param('~accel_limit', 1.) 50 | self.wheel_radius = rospy.get_param('~wheel_radius', 0.2413) 51 | wheel_base = rospy.get_param('~wheel_base', 2.8498) 52 | steer_ratio = rospy.get_param('~steer_ratio', 14.8) 53 | max_lat_accel = rospy.get_param('~max_lat_accel', 3.) 54 | max_steer_angle = rospy.get_param('~max_steer_angle', 8.) 55 | 56 | self.steer_pub = rospy.Publisher('/vehicle/steering_cmd', 57 | SteeringCmd, queue_size=1) 58 | self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd', 59 | ThrottleCmd, queue_size=1) 60 | self.brake_pub = rospy.Publisher('/vehicle/brake_cmd', 61 | BrakeCmd, queue_size=1) 62 | 63 | parms = { 64 | 'wheel_base' : wheel_base, 65 | 'steer_ratio' : steer_ratio, 66 | 'min_velocity' : 0., 67 | 'max_lat_accel' : max_lat_accel, 68 | 'max_steer_angle' : max_steer_angle, 69 | 'decel_limit' : decel_limit, 70 | 'accel_limit' : accel_limit, 71 | 'deadband' : self.brake_deadband 72 | } 73 | self.controller = Controller(**parms) 74 | 75 | self.current_command = None 76 | self.current_velocity = None 77 | self.dbw_enabled = False 78 | 79 | rospy.Subscriber('/twist_cmd', TwistStamped, self.callback_twist_cmd) 80 | rospy.Subscriber('/current_velocity', TwistStamped, self.callback_current_velocity) 81 | rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.callback_dbw_enabled) 82 | 83 | self.loop() 84 | 85 | # Get predicted throttle, brake, and steering using `controller` object 86 | def loop(self): 87 | rate = rospy.Rate(50) # 50Hz 88 | while not rospy.is_shutdown(): 89 | if (self.current_command is not None) and (self.current_velocity is not None): 90 | # get the current velocity, target velocity, and target angle and pass into control 91 | linear_target = self.current_command.twist.linear.x; 92 | angular_target = self.current_command.twist.angular.z; 93 | linear_current = self.current_velocity.twist.linear.x; 94 | angular_current = self.current_velocity.twist.angular.z; 95 | #rospy.loginfo("linearc: %.2f, lineart: %.2f, angularc: %.2f, angulart: %.2f", linear_current, linear_target, angular_current, angular_target) 96 | throttle, brake, steering = self.controller.control(linear_target, angular_target, linear_current) 97 | 98 | # publish the control commands if dbw is enabled 99 | if self.dbw_enabled is True: 100 | #rospy.loginfo("v: %.2f, vtarg: %.2f, thr: %.2f, b: %.2f, s: %.2f", linear_current, linear_target, throttle, brake, steering) 101 | self.publish(throttle, brake, steering) 102 | else: 103 | self.controller.reset() 104 | rate.sleep() 105 | 106 | def publish(self, throttle, brake, steer): 107 | tcmd = ThrottleCmd() 108 | if throttle > 0.0: 109 | tcmd.enable = True 110 | tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT 111 | tcmd.pedal_cmd = throttle 112 | self.throttle_pub.publish(tcmd) 113 | 114 | scmd = SteeringCmd() 115 | scmd.enable = True 116 | scmd.steering_wheel_angle_cmd = steer 117 | self.steer_pub.publish(scmd) 118 | 119 | bcmd = BrakeCmd() 120 | bcmd.enable = True 121 | 122 | bcmd.pedal_cmd_type = BrakeCmd.CMD_PERCENT 123 | if self.brake_deadband > 0.1: 124 | brake *= 1000 # multiplier for sim 125 | bcmd.pedal_cmd = brake 126 | 127 | # alternate torque braking 128 | #bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE 129 | #if brake > 0. and throttle == 0.: 130 | #bcmd.pedal_cmd = brake * self.vehicle_mass * self.wheel_radius 131 | # bcmd.pedal_cmd = BrakeCmd.TORQUE_MAX * brake # 3412. 132 | self.brake_pub.publish(bcmd) 133 | 134 | def callback_twist_cmd(self, msg): 135 | self.current_command = msg 136 | 137 | def callback_current_velocity(self, msg): 138 | self.current_velocity = msg 139 | 140 | def callback_dbw_enabled(self, msg): 141 | rospy.loginfo("dbw_enabled: %s", msg) 142 | self.dbw_enabled = msg.data 143 | 144 | if __name__ == '__main__': 145 | DBWNode() 146 | -------------------------------------------------------------------------------- /ros/src/twist_controller/dbw_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import os 4 | import csv 5 | 6 | import rospy 7 | from std_msgs.msg import Bool 8 | from dbw_mkz_msgs.msg import ThrottleCmd, SteeringCmd, BrakeCmd, SteeringReport 9 | 10 | 11 | ''' 12 | You can use this file to test your DBW code against a bag recorded with a reference implementation. 13 | The bag can be found at https://drive.google.com/open?id=0B2_h37bMVw3iT0ZEdlF4N01QbHc. 14 | 15 | This file will produce 3 csv files which you can process to figure out how your DBW node is 16 | performing on various commands. 17 | 18 | `/actual/*` are commands from the recorded bag while `/vehicle/*` are the output of your node. 19 | 20 | ''' 21 | 22 | 23 | class DBWTestNode(object): 24 | def __init__(self): 25 | rospy.init_node('dbw_test_node') 26 | 27 | rospy.Subscriber('/vehicle/steering_cmd', SteeringCmd, self.steer_cb) 28 | rospy.Subscriber('/vehicle/throttle_cmd', ThrottleCmd, self.throttle_cb) 29 | rospy.Subscriber('/vehicle/brake_cmd', BrakeCmd, self.brake_cb) 30 | 31 | rospy.Subscriber('/actual/steering_cmd', SteeringCmd, self.actual_steer_cb) 32 | rospy.Subscriber('/actual/throttle_cmd', ThrottleCmd, self.actual_throttle_cb) 33 | rospy.Subscriber('/actual/brake_cmd', BrakeCmd, self.actual_brake_cb) 34 | 35 | rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb) 36 | 37 | self.steer = self.throttle = self.brake = None 38 | 39 | self.steer_data = [] 40 | self.throttle_data = [] 41 | self.brake_data = [] 42 | 43 | self.dbw_enabled = False 44 | 45 | base_path = os.path.dirname(os.path.abspath(__file__)) 46 | self.steerfile = os.path.join(base_path, 'steers.csv') 47 | self.throttlefile = os.path.join(base_path, 'throttles.csv') 48 | self.brakefile = os.path.join(base_path, 'brakes.csv') 49 | 50 | self.loop() 51 | 52 | def loop(self): 53 | rate = rospy.Rate(10) # 10Hz 54 | while not rospy.is_shutdown(): 55 | rate.sleep() 56 | fieldnames = ['actual', 'proposed'] 57 | 58 | with open(self.steerfile, 'w') as csvfile: 59 | writer = csv.DictWriter(csvfile, fieldnames=fieldnames) 60 | writer.writeheader() 61 | writer.writerows(self.steer_data) 62 | 63 | with open(self.throttlefile, 'w') as csvfile: 64 | writer = csv.DictWriter(csvfile, fieldnames=fieldnames) 65 | writer.writeheader() 66 | writer.writerows(self.throttle_data) 67 | 68 | with open(self.brakefile, 'w') as csvfile: 69 | writer = csv.DictWriter(csvfile, fieldnames=fieldnames) 70 | writer.writeheader() 71 | writer.writerows(self.brake_data) 72 | 73 | def dbw_enabled_cb(self, msg): 74 | self.dbw_enabled = msg.data 75 | 76 | def steer_cb(self, msg): 77 | self.steer = msg.steering_wheel_angle_cmd 78 | 79 | def throttle_cb(self, msg): 80 | self.throttle = msg.pedal_cmd 81 | 82 | def brake_cb(self, msg): 83 | self.brake = msg.pedal_cmd 84 | 85 | def actual_steer_cb(self, msg): 86 | if self.dbw_enabled and self.steer is not None: 87 | self.steer_data.append({'actual': msg.steering_wheel_angle_cmd, 88 | 'proposed': self.steer}) 89 | self.steer = None 90 | 91 | def actual_throttle_cb(self, msg): 92 | if self.dbw_enabled and self.throttle is not None: 93 | self.throttle_data.append({'actual': msg.pedal_cmd, 94 | 'proposed': self.throttle}) 95 | self.throttle = None 96 | 97 | def actual_brake_cb(self, msg): 98 | if self.dbw_enabled and self.brake is not None: 99 | self.brake_data.append({'actual': msg.pedal_cmd, 100 | 'proposed': self.brake}) 101 | self.brake = None 102 | 103 | 104 | if __name__ == '__main__': 105 | DBWTestNode() 106 | -------------------------------------------------------------------------------- /ros/src/twist_controller/launch/dbw.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /ros/src/twist_controller/launch/dbw_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /ros/src/twist_controller/launch/dbw_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /ros/src/twist_controller/lowpass.py: -------------------------------------------------------------------------------- 1 | 2 | class LowPassFilter(object): 3 | def __init__(self, tau, ts): 4 | self.a = 1. / (tau / ts + 1.) 5 | self.b = tau / ts / (tau / ts + 1.); 6 | 7 | self.last_val = 0. 8 | self.ready = False 9 | 10 | def get(self): 11 | return self.last_val 12 | 13 | def filt(self, val): 14 | if self.ready: 15 | val = self.a * val + self.b * self.last_val 16 | else: 17 | self.ready = True 18 | 19 | self.last_val = val 20 | return val 21 | -------------------------------------------------------------------------------- /ros/src/twist_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | twist_controller 4 | 0.0.0 5 | The twist_controller package 6 | 7 | 8 | 9 | 10 | yousuf 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | dbw_mkz_msgs 44 | geometry_msgs 45 | roscpp 46 | rospy 47 | std_msgs 48 | dbw_mkz_msgs 49 | geometry_msgs 50 | roscpp 51 | rospy 52 | std_msgs 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /ros/src/twist_controller/pid.py: -------------------------------------------------------------------------------- 1 | 2 | MIN_NUM = float('-inf') 3 | MAX_NUM = float('inf') 4 | 5 | 6 | class PID(object): 7 | def __init__(self, kp, ki, kd, mn=MIN_NUM, mx=MAX_NUM): 8 | self.kp = kp 9 | self.ki = ki 10 | self.kd = kd 11 | self.min = mn 12 | self.max = mx 13 | 14 | self.int_val = self.last_int_val = self.last_error = 0. 15 | 16 | def reset(self): 17 | self.int_val = 0.0 18 | self.last_int_val = 0.0 19 | 20 | def step(self, error, sample_time): 21 | self.last_int_val = self.int_val 22 | 23 | integral = self.int_val + error * sample_time; 24 | derivative = (error - self.last_error) / sample_time; 25 | 26 | y = self.kp * error + self.ki * self.int_val + self.kd * derivative; 27 | val = max(self.min, min(y, self.max)) 28 | 29 | if val > self.max: 30 | val = self.max 31 | elif val < self.min: 32 | val = self.min 33 | else: 34 | self.int_val = integral 35 | self.last_error = error 36 | 37 | return val 38 | -------------------------------------------------------------------------------- /ros/src/twist_controller/twist_controller.py: -------------------------------------------------------------------------------- 1 | from yaw_controller import YawController 2 | from pid import PID 3 | from lowpass import LowPassFilter 4 | import math 5 | 6 | GAS_DENSITY = 2.858 7 | ONE_MPH = 0.44704 8 | 9 | 10 | class Controller(object): 11 | def __init__(self, *args, **kwargs): 12 | wheel_base = kwargs['wheel_base'] 13 | self.steer_ratio = kwargs['steer_ratio'] 14 | min_velocity = kwargs['min_velocity'] 15 | max_lat_accel = kwargs['max_lat_accel'] 16 | max_steer_angle = kwargs['max_steer_angle'] 17 | self.decel_limit = kwargs['decel_limit'] 18 | self.accel_limit = kwargs['accel_limit'] 19 | self.deadband = kwargs['deadband'] 20 | 21 | self.yawController = YawController(wheel_base, self.steer_ratio, min_velocity, max_lat_accel, max_steer_angle) 22 | self.velocity_pid = PID(0.5, 0., 0., self.decel_limit, self.accel_limit) 23 | self.lowpassFilt = LowPassFilter(0.07, 0.02) 24 | self.topVelocity = 0. 25 | 26 | def control(self, linear_velocity_target, angular_velocity_target, linear_velocity_current): 27 | 28 | # Throttle xor brake 29 | brake = 0. 30 | throttle_correction = self.velocity_pid.step(linear_velocity_target-linear_velocity_current, 0.02) 31 | if throttle_correction > 0.: 32 | throttle = throttle_correction 33 | throttle = self.lowpassFilt.filt(throttle) 34 | elif throttle_correction < -self.deadband: 35 | throttle = 0. 36 | brake = -throttle_correction 37 | else: 38 | throttle = 0. 39 | # hold brake while stopped at red light, until light changes 40 | if (linear_velocity_target <= 0.01) and (brake < self.deadband): 41 | brake = self.deadband 42 | 43 | # Steering 44 | # Use yawController for simulator 45 | if self.deadband>0.1: 46 | if linear_velocity_target > self.topVelocity: # mitigate rapid turning 47 | self.topVelocity = linear_velocity_target 48 | if linear_velocity_current > 0.05: 49 | steering = self.yawController.get_steering(self.topVelocity, angular_velocity_target, linear_velocity_current) 50 | else: 51 | steering = 0. 52 | # ...and alternate approach (to match reference) on the test site: 53 | else: 54 | steering = angular_velocity_target * self.steer_ratio 55 | return throttle, brake, steering 56 | 57 | def reset(self): 58 | self.velocity_pid.reset() 59 | -------------------------------------------------------------------------------- /ros/src/twist_controller/yaw_controller.py: -------------------------------------------------------------------------------- 1 | from math import atan 2 | 3 | class YawController(object): 4 | def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle): 5 | self.wheel_base = wheel_base 6 | self.steer_ratio = steer_ratio 7 | self.min_speed = min_speed 8 | self.max_lat_accel = max_lat_accel 9 | 10 | self.min_angle = -max_steer_angle 11 | self.max_angle = max_steer_angle 12 | 13 | 14 | def get_angle(self, radius): 15 | angle = atan(self.wheel_base / radius) * self.steer_ratio 16 | return max(self.min_angle, min(self.max_angle, angle)) 17 | 18 | def get_steering(self, linear_velocity, angular_velocity, current_velocity): 19 | angular_velocity = current_velocity * angular_velocity / linear_velocity if abs(linear_velocity) > 0. else 0. 20 | 21 | if abs(current_velocity) > 0.1: 22 | max_yaw_rate = abs(self.max_lat_accel / current_velocity); 23 | angular_velocity = max(-max_yaw_rate, min(max_yaw_rate, angular_velocity)) 24 | 25 | return self.get_angle(max(current_velocity, self.min_speed) / angular_velocity) if abs(angular_velocity) > 0. else 0.0; 26 | -------------------------------------------------------------------------------- /ros/src/waypoint_follower/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(waypoint_follower) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | # add_definitions(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | pcl_ros 14 | pcl_conversions 15 | sensor_msgs 16 | styx_msgs 17 | tf 18 | ) 19 | 20 | ## System dependencies are found with CMake's conventions 21 | # find_package(Boost REQUIRED COMPONENTS system) 22 | 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | # catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 38 | ## * add a build_depend tag for "message_generation" 39 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 40 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 41 | ## but can be declared for certainty nonetheless: 42 | ## * add a run_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | # add_message_files( 55 | # FILES 56 | # Message1.msg 57 | # Message2.msg 58 | # ) 59 | 60 | ## Generate services in the 'srv' folder 61 | # add_service_files( 62 | # FILES 63 | # Service1.srv 64 | # Service2.srv 65 | # ) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ################################### 75 | ## catkin specific configuration ## 76 | ################################### 77 | catkin_package( 78 | INCLUDE_DIRS include 79 | LIBRARIES libwaypoint_follower 80 | CATKIN_DEPENDS roscpp std_msgs tf geometry_msgs styx_msgs 81 | ) 82 | 83 | ########### 84 | ## Build ## 85 | ########### 86 | 87 | SET(CMAKE_CXX_FLAGS "-std=c++11 -O2 -g -Wall ${CMAKE_CXX_FLAGS}") 88 | 89 | include_directories( 90 | include 91 | ${catkin_INCLUDE_DIRS} 92 | ) 93 | 94 | add_library(libwaypoint_follower lib/libwaypoint_follower.cpp) 95 | add_dependencies(libwaypoint_follower 96 | styx_msgs_generate_messages_cpp) 97 | 98 | add_executable(pure_pursuit src/pure_pursuit.cpp src/pure_pursuit_core.cpp) 99 | target_link_libraries(pure_pursuit libwaypoint_follower ${catkin_LIBRARIES}) 100 | add_dependencies(pure_pursuit 101 | styx_msgs_generate_messages_cpp) 102 | 103 | target_link_libraries(libwaypoint_follower ${catkin_LIBRARIES}) 104 | -------------------------------------------------------------------------------- /ros/src/waypoint_follower/include/libwaypoint_follower.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015, Nagoya University 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 11 | * * Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * * Neither the name of Autoware nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #ifndef _LIB_WAYPOINT_FOLLOWER_H_ 32 | #define _LIB_WAYPOINT_FOLLOWER_H_ 33 | 34 | // C++ header 35 | #include 36 | #include 37 | #include 38 | 39 | // ROS header 40 | #include 41 | #include 42 | #include "styx_msgs/Lane.h" 43 | 44 | class WayPoints 45 | { 46 | protected: 47 | styx_msgs::Lane current_waypoints_; 48 | 49 | public: 50 | void setPath(const styx_msgs::Lane &waypoints) 51 | { 52 | current_waypoints_ = waypoints; 53 | } 54 | int getSize() const; 55 | bool isEmpty() const 56 | { 57 | return current_waypoints_.waypoints.empty(); 58 | }; 59 | double getInterval() const; 60 | geometry_msgs::Point getWaypointPosition(int waypoint) const; 61 | geometry_msgs::Quaternion getWaypointOrientation(int waypoint) const; 62 | geometry_msgs::Pose getWaypointPose(int waypoint) const; 63 | double getWaypointVelocityMPS(int waypoint) const; 64 | styx_msgs::Lane getCurrentWaypoints() const 65 | { 66 | return current_waypoints_; 67 | } 68 | bool isFront(int waypoint, geometry_msgs::Pose current_pose) const; 69 | }; 70 | 71 | // inline function (less than 10 lines ) 72 | inline double kmph2mps(double velocity_kmph) 73 | { 74 | return (velocity_kmph * 1000) / (60 * 60); 75 | } 76 | inline double mps2kmph(double velocity_mps) 77 | { 78 | return (velocity_mps * 60 * 60) / 1000; 79 | } 80 | inline double deg2rad(double deg) 81 | { 82 | return deg * M_PI / 180; 83 | } // convert degree to radian 84 | 85 | tf::Vector3 point2vector(geometry_msgs::Point point); // convert point to vector 86 | geometry_msgs::Point vector2point(tf::Vector3 vector); // convert vector to point 87 | tf::Vector3 rotateUnitVector(tf::Vector3 unit_vector, double degree); // rotate unit vector by degree 88 | geometry_msgs::Point rotatePoint(geometry_msgs::Point point, double degree); // rotate point vector by degree 89 | 90 | double DecelerateVelocity(double distance, double prev_velocity); 91 | geometry_msgs::Point calcRelativeCoordinate(geometry_msgs::Point point, 92 | geometry_msgs::Pose current_pose); // transform point into the coordinate 93 | // of current_pose 94 | geometry_msgs::Point calcAbsoluteCoordinate(geometry_msgs::Point point, 95 | geometry_msgs::Pose current_pose); // transform point into the global 96 | // coordinate 97 | double getPlaneDistance(geometry_msgs::Point target1, 98 | geometry_msgs::Point target2); // get 2 dimentional distance between target 1 and target 2 99 | int getClosestWaypoint(const styx_msgs::Lane ¤t_path, geometry_msgs::Pose current_pose); 100 | bool getLinearEquation(geometry_msgs::Point start, geometry_msgs::Point end, double *a, double *b, double *c); 101 | double getDistanceBetweenLineAndPoint(geometry_msgs::Point point, double sa, double b, double c); 102 | double getRelativeAngle(geometry_msgs::Pose waypoint_pose, geometry_msgs::Pose vehicle_pose); 103 | #endif 104 | -------------------------------------------------------------------------------- /ros/src/waypoint_follower/include/pure_pursuit_core.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015, Nagoya University 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 11 | * * Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * * Neither the name of Autoware nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #ifndef PURE_PURSUIT_CORE_H 32 | #define PURE_PURSUIT_CORE_H 33 | 34 | // ROS includes 35 | #include 36 | #include 37 | #include 38 | // C++ includes 39 | #include 40 | #include "libwaypoint_follower.h" 41 | 42 | enum class Mode 43 | { 44 | waypoint, 45 | dialog 46 | }; 47 | 48 | namespace waypoint_follower 49 | { 50 | class PurePursuit 51 | { 52 | private: 53 | 54 | //constant 55 | const double RADIUS_MAX_; 56 | const double KAPPA_MIN_; 57 | 58 | bool linear_interpolate_; 59 | 60 | // config topic 61 | int param_flag_; // 0 = waypoint, 1 = Dialog 62 | double const_lookahead_distance_; // meter 63 | double initial_velocity_; // km/h 64 | double lookahead_distance_calc_ratio_; 65 | double minimum_lookahead_distance_; // the next waypoint must be outside of this threshold. 66 | double displacement_threshold_; 67 | double relative_angle_threshold_; 68 | 69 | bool waypoint_set_; 70 | bool pose_set_; 71 | bool velocity_set_; 72 | int num_of_next_waypoint_; 73 | geometry_msgs::Point position_of_next_target_; 74 | double lookahead_distance_; 75 | 76 | geometry_msgs::PoseStamped current_pose_; 77 | geometry_msgs::TwistStamped current_velocity_; 78 | WayPoints current_waypoints_; 79 | 80 | double getCmdVelocity(int waypoint) const; 81 | void calcLookaheadDistance(int waypoint); 82 | double calcCurvature(geometry_msgs::Point target) const; 83 | double calcRadius(geometry_msgs::Point target) const; 84 | bool interpolateNextTarget(int next_waypoint, geometry_msgs::Point *next_target) const; 85 | bool verifyFollowing() const; 86 | geometry_msgs::Twist calcTwist(double curvature, double cmd_velocity) const; 87 | void getNextWaypoint(); 88 | geometry_msgs::TwistStamped outputZero() const; 89 | geometry_msgs::TwistStamped outputTwist(geometry_msgs::Twist t) const; 90 | 91 | public: 92 | PurePursuit(bool linear_interpolate_mode) 93 | : RADIUS_MAX_(9e10) 94 | , KAPPA_MIN_(1/RADIUS_MAX_) 95 | , linear_interpolate_(linear_interpolate_mode) 96 | , param_flag_(0) 97 | , const_lookahead_distance_(4.0) 98 | , initial_velocity_(5.0) 99 | , lookahead_distance_calc_ratio_(2.0) 100 | , minimum_lookahead_distance_(6.0) 101 | , displacement_threshold_(0.2) 102 | , relative_angle_threshold_(5.) 103 | , waypoint_set_(false) 104 | , pose_set_(false) 105 | , velocity_set_(false) 106 | , num_of_next_waypoint_(-1) 107 | , lookahead_distance_(0) 108 | { 109 | } 110 | ~PurePursuit() 111 | { 112 | } 113 | 114 | // for ROS 115 | void callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr &msg); 116 | void callbackFromCurrentVelocity(const geometry_msgs::TwistStampedConstPtr &msg); 117 | void callbackFromWayPoints(const styx_msgs::LaneConstPtr &msg); 118 | 119 | // for debug 120 | geometry_msgs::Point getPoseOfNextWaypoint() const 121 | { 122 | return current_waypoints_.getWaypointPosition(num_of_next_waypoint_); 123 | } 124 | geometry_msgs::Point getPoseOfNextTarget() const 125 | { 126 | return position_of_next_target_; 127 | } 128 | geometry_msgs::Pose getCurrentPose() const 129 | { 130 | return current_pose_.pose; 131 | } 132 | 133 | double getLookaheadDistance() const 134 | { 135 | return lookahead_distance_; 136 | } 137 | // processing for ROS 138 | geometry_msgs::TwistStamped go(); 139 | }; 140 | } 141 | 142 | #endif // PURE_PURSUIT_CORE_H 143 | -------------------------------------------------------------------------------- /ros/src/waypoint_follower/launch/pure_pursuit.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /ros/src/waypoint_follower/lib/libwaypoint_follower.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015, Nagoya University 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 11 | * * Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * * Neither the name of Autoware nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include "libwaypoint_follower.h" 32 | 33 | int WayPoints::getSize() const 34 | { 35 | if (current_waypoints_.waypoints.empty()) 36 | return 0; 37 | else 38 | return current_waypoints_.waypoints.size(); 39 | } 40 | 41 | double WayPoints::getInterval() const 42 | { 43 | if (current_waypoints_.waypoints.empty()) 44 | return 0; 45 | 46 | // interval between 2 waypoints 47 | tf::Vector3 v1(current_waypoints_.waypoints[0].pose.pose.position.x, 48 | current_waypoints_.waypoints[0].pose.pose.position.y, 0); 49 | 50 | tf::Vector3 v2(current_waypoints_.waypoints[1].pose.pose.position.x, 51 | current_waypoints_.waypoints[1].pose.pose.position.y, 0); 52 | return tf::tfDistance(v1, v2); 53 | } 54 | 55 | geometry_msgs::Point WayPoints::getWaypointPosition(int waypoint) const 56 | { 57 | geometry_msgs::Point p; 58 | if (waypoint > getSize() - 1 || waypoint < 0) 59 | return p; 60 | 61 | p = current_waypoints_.waypoints[waypoint].pose.pose.position; 62 | return p; 63 | } 64 | 65 | geometry_msgs::Quaternion WayPoints::getWaypointOrientation(int waypoint) const 66 | { 67 | geometry_msgs::Quaternion q; 68 | if (waypoint > getSize() - 1 || waypoint < 0) 69 | return q; 70 | 71 | q = current_waypoints_.waypoints[waypoint].pose.pose.orientation; 72 | return q; 73 | } 74 | 75 | geometry_msgs::Pose WayPoints::getWaypointPose(int waypoint) const 76 | { 77 | geometry_msgs::Pose pose; 78 | if (waypoint > getSize() - 1 || waypoint < 0) 79 | return pose; 80 | 81 | pose = current_waypoints_.waypoints[waypoint].pose.pose; 82 | return pose; 83 | } 84 | 85 | double WayPoints::getWaypointVelocityMPS(int waypoint) const 86 | { 87 | if (waypoint > getSize() - 1 || waypoint < 0) 88 | return 0; 89 | 90 | return current_waypoints_.waypoints[waypoint].twist.twist.linear.x; 91 | } 92 | 93 | bool WayPoints::isFront(int waypoint, geometry_msgs::Pose current_pose) const 94 | { 95 | double x = calcRelativeCoordinate(current_waypoints_.waypoints[waypoint].pose.pose.position, current_pose).x; 96 | if (x < 0) 97 | return false; 98 | else 99 | return true; 100 | } 101 | 102 | double DecelerateVelocity(double distance, double prev_velocity) 103 | { 104 | double decel_ms = 1.0; // m/s 105 | double decel_velocity_ms = sqrt(2 * decel_ms * distance); 106 | 107 | std::cout << "velocity/prev_velocity :" << decel_velocity_ms << "/" << prev_velocity << std::endl; 108 | if (decel_velocity_ms < prev_velocity) 109 | { 110 | return decel_velocity_ms; 111 | } 112 | else 113 | { 114 | return prev_velocity; 115 | } 116 | } 117 | 118 | // calculation relative coordinate of point from current_pose frame 119 | geometry_msgs::Point calcRelativeCoordinate(geometry_msgs::Point point_msg, geometry_msgs::Pose current_pose) 120 | { 121 | tf::Transform inverse; 122 | tf::poseMsgToTF(current_pose, inverse); 123 | tf::Transform transform = inverse.inverse(); 124 | 125 | tf::Point p; 126 | pointMsgToTF(point_msg, p); 127 | tf::Point tf_p = transform * p; 128 | geometry_msgs::Point tf_point_msg; 129 | pointTFToMsg(tf_p, tf_point_msg); 130 | 131 | return tf_point_msg; 132 | } 133 | 134 | // calculation absolute coordinate of point on current_pose frame 135 | geometry_msgs::Point calcAbsoluteCoordinate(geometry_msgs::Point point_msg, geometry_msgs::Pose current_pose) 136 | { 137 | tf::Transform inverse; 138 | tf::poseMsgToTF(current_pose, inverse); 139 | 140 | tf::Point p; 141 | pointMsgToTF(point_msg, p); 142 | tf::Point tf_p = inverse * p; 143 | geometry_msgs::Point tf_point_msg; 144 | pointTFToMsg(tf_p, tf_point_msg); 145 | return tf_point_msg; 146 | } 147 | 148 | // distance between target 1 and target2 in 2-D 149 | double getPlaneDistance(geometry_msgs::Point target1, geometry_msgs::Point target2) 150 | { 151 | tf::Vector3 v1 = point2vector(target1); 152 | v1.setZ(0); 153 | tf::Vector3 v2 = point2vector(target2); 154 | v2.setZ(0); 155 | return tf::tfDistance(v1, v2); 156 | } 157 | 158 | double getRelativeAngle(geometry_msgs::Pose waypoint_pose, geometry_msgs::Pose vehicle_pose) 159 | { 160 | geometry_msgs::Point relative_p1 = calcRelativeCoordinate(waypoint_pose.position, vehicle_pose); 161 | geometry_msgs::Point p2; 162 | p2.x = 1.0; 163 | geometry_msgs::Point relative_p2 = calcRelativeCoordinate(calcAbsoluteCoordinate(p2, waypoint_pose), vehicle_pose); 164 | tf::Vector3 relative_waypoint_v(relative_p2.x - relative_p1.x, relative_p2.y - relative_p1.y, 165 | relative_p2.z - relative_p1.z); 166 | relative_waypoint_v.normalize(); 167 | tf::Vector3 relative_pose_v(1, 0, 0); 168 | double angle = relative_pose_v.angle(relative_waypoint_v) * 180 / M_PI; 169 | // ROS_INFO("angle : %lf",angle); 170 | 171 | return angle; 172 | } 173 | 174 | // get closest waypoint from current pose 175 | int getClosestWaypoint(const styx_msgs::Lane ¤t_path, geometry_msgs::Pose current_pose) 176 | { 177 | WayPoints wp; 178 | wp.setPath(current_path); 179 | 180 | if (wp.isEmpty()) 181 | return -1; 182 | 183 | // search closest candidate within a certain meter 184 | double search_distance = 5.0; 185 | std::vector waypoint_candidates; 186 | for (int i = 1; i < wp.getSize(); i++) 187 | { 188 | if (getPlaneDistance(wp.getWaypointPosition(i), current_pose.position) > search_distance) 189 | continue; 190 | 191 | if (!wp.isFront(i, current_pose)) 192 | continue; 193 | 194 | double angle_threshold = 90; 195 | if (getRelativeAngle(wp.getWaypointPose(i), current_pose) > angle_threshold) 196 | continue; 197 | 198 | waypoint_candidates.push_back(i); 199 | } 200 | 201 | // get closest waypoint from candidates 202 | if (!waypoint_candidates.empty()) 203 | { 204 | int waypoint_min = -1; 205 | double distance_min = DBL_MAX; 206 | for (auto el : waypoint_candidates) 207 | { 208 | // ROS_INFO("closest_candidates : %d",el); 209 | double d = getPlaneDistance(wp.getWaypointPosition(el), current_pose.position); 210 | if (d < distance_min) 211 | { 212 | waypoint_min = el; 213 | distance_min = d; 214 | } 215 | } 216 | return waypoint_min; 217 | } 218 | else 219 | { 220 | ROS_INFO("no candidate. search closest waypoint from all waypoints..."); 221 | // if there is no candidate... 222 | int waypoint_min = -1; 223 | double distance_min = DBL_MAX; 224 | for (int i = 1; i < wp.getSize(); i++) 225 | { 226 | if (!wp.isFront(i, current_pose)) 227 | continue; 228 | 229 | // if (!wp.isValid(i, current_pose)) 230 | // continue; 231 | 232 | double d = getPlaneDistance(wp.getWaypointPosition(i), current_pose.position); 233 | if (d < distance_min) 234 | { 235 | waypoint_min = i; 236 | distance_min = d; 237 | } 238 | } 239 | return waypoint_min; 240 | } 241 | } 242 | 243 | // let the linear equation be "ax + by + c = 0" 244 | // if there are two points (x1,y1) , (x2,y2), a = "y2-y1, b = "(-1) * x2 - x1" ,c = "(-1) * (y2-y1)x1 + (x2-x1)y1" 245 | bool getLinearEquation(geometry_msgs::Point start, geometry_msgs::Point end, double *a, double *b, double *c) 246 | { 247 | //(x1, y1) = (start.x, star.y), (x2, y2) = (end.x, end.y) 248 | double sub_x = fabs(start.x - end.x); 249 | double sub_y = fabs(start.y - end.y); 250 | double error = pow(10, -5); // 0.00001 251 | 252 | if (sub_x < error && sub_y < error) 253 | { 254 | ROS_INFO("two points are the same point!!"); 255 | return false; 256 | } 257 | 258 | *a = end.y - start.y; 259 | *b = (-1) * (end.x - start.x); 260 | *c = (-1) * (end.y - start.y) * start.x + (end.x - start.x) * start.y; 261 | 262 | return true; 263 | } 264 | double getDistanceBetweenLineAndPoint(geometry_msgs::Point point, double a, double b, double c) 265 | { 266 | double d = fabs(a * point.x + b * point.y + c) / sqrt(pow(a, 2) + pow(b, 2)); 267 | 268 | return d; 269 | } 270 | 271 | tf::Vector3 point2vector(geometry_msgs::Point point) 272 | { 273 | tf::Vector3 vector(point.x, point.y, point.z); 274 | return vector; 275 | } 276 | 277 | geometry_msgs::Point vector2point(tf::Vector3 vector) 278 | { 279 | geometry_msgs::Point point; 280 | point.x = vector.getX(); 281 | point.y = vector.getY(); 282 | point.z = vector.getZ(); 283 | return point; 284 | } 285 | 286 | tf::Vector3 rotateUnitVector(tf::Vector3 unit_vector, double degree) 287 | { 288 | tf::Vector3 w1(cos(deg2rad(degree)) * unit_vector.getX() - sin(deg2rad(degree)) * unit_vector.getY(), 289 | sin(deg2rad(degree)) * unit_vector.getX() + cos(deg2rad(degree)) * unit_vector.getY(), 0); 290 | tf::Vector3 unit_w1 = w1.normalize(); 291 | 292 | return unit_w1; 293 | } 294 | 295 | geometry_msgs::Point rotatePoint(geometry_msgs::Point point, double degree) 296 | { 297 | geometry_msgs::Point rotate; 298 | rotate.x = cos(deg2rad(degree)) * point.x - sin(deg2rad(degree)) * point.y; 299 | rotate.y = sin(deg2rad(degree)) * point.x + cos(deg2rad(degree)) * point.y; 300 | 301 | return rotate; 302 | } 303 | -------------------------------------------------------------------------------- /ros/src/waypoint_follower/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | waypoint_follower 4 | 0.0.0 5 | The waypoint_follower package 6 | 7 | 8 | 9 | 10 | yousuf 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | geometry_msgs 44 | roscpp 45 | rospy 46 | std_msgs 47 | styx_msgs 48 | tf 49 | geometry_msgs 50 | roscpp 51 | rospy 52 | std_msgs 53 | styx_msgs 54 | tf 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /ros/src/waypoint_follower/src/pure_pursuit.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015, Nagoya University 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 11 | * * Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * * Neither the name of Autoware nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include "pure_pursuit_core.h" 32 | 33 | constexpr int LOOP_RATE = 30; //processing frequency 34 | const int PUB_SUB_QUEUE_SIZE = 1; 35 | // To mitigate simulator lag, see 36 | // https://github.com/udacity/self-driving-car-sim/issues/53 37 | 38 | int main(int argc, char **argv) 39 | { 40 | 41 | 42 | // set up ros 43 | ros::init(argc, argv, "pure_pursuit"); 44 | 45 | ros::NodeHandle nh; 46 | ros::NodeHandle private_nh("~"); 47 | 48 | bool linear_interpolate_mode; 49 | private_nh.param("linear_interpolate_mode", linear_interpolate_mode, bool(true)); 50 | ROS_INFO_STREAM("linear_interpolate_mode : " << linear_interpolate_mode); 51 | 52 | waypoint_follower::PurePursuit pp(linear_interpolate_mode); 53 | 54 | ROS_INFO("set publisher..."); 55 | // publish topic 56 | ros::Publisher cmd_velocity_publisher = nh.advertise("twist_cmd", PUB_SUB_QUEUE_SIZE); 57 | 58 | ROS_INFO("set subscriber..."); 59 | // subscribe topic 60 | ros::Subscriber waypoint_subscriber = 61 | nh.subscribe("final_waypoints", PUB_SUB_QUEUE_SIZE, &waypoint_follower::PurePursuit::callbackFromWayPoints, &pp); 62 | ros::Subscriber ndt_subscriber = 63 | nh.subscribe("current_pose", PUB_SUB_QUEUE_SIZE, &waypoint_follower::PurePursuit::callbackFromCurrentPose, &pp); 64 | ros::Subscriber est_twist_subscriber = 65 | nh.subscribe("current_velocity", PUB_SUB_QUEUE_SIZE, &waypoint_follower::PurePursuit::callbackFromCurrentVelocity, &pp); 66 | 67 | ROS_INFO("pure pursuit start"); 68 | ros::Rate loop_rate(LOOP_RATE); 69 | while (ros::ok()) 70 | { 71 | ros::spinOnce(); 72 | cmd_velocity_publisher.publish(pp.go()); 73 | loop_rate.sleep(); 74 | } 75 | 76 | return 0; 77 | } 78 | -------------------------------------------------------------------------------- /ros/src/waypoint_follower/src/pure_pursuit_core.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015, Nagoya University 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 11 | * * Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * * Neither the name of Autoware nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include "ros/ros.h" 32 | #include "pure_pursuit_core.h" 33 | 34 | namespace waypoint_follower 35 | { 36 | 37 | void PurePursuit::callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr &msg) 38 | { 39 | current_pose_.header = msg->header; 40 | current_pose_.pose = msg->pose; 41 | pose_set_ = true; 42 | }//processing frequency 43 | 44 | void PurePursuit::callbackFromCurrentVelocity(const geometry_msgs::TwistStampedConstPtr &msg) 45 | { 46 | current_velocity_ = *msg; 47 | velocity_set_ = true; 48 | } 49 | 50 | void PurePursuit::callbackFromWayPoints(const styx_msgs::LaneConstPtr &msg) 51 | { 52 | current_waypoints_.setPath(*msg); 53 | waypoint_set_ = true; 54 | // ROS_INFO_STREAM("waypoint subscribed"); 55 | } 56 | 57 | double PurePursuit::getCmdVelocity(int waypoint) const 58 | { 59 | if (current_waypoints_.isEmpty()) 60 | { 61 | ROS_INFO_STREAM("waypoint : not loaded path"); 62 | return 0; 63 | } 64 | 65 | double velocity = current_waypoints_.getWaypointVelocityMPS(waypoint); 66 | // ROS_INFO_STREAM("waypoint : " << mps2kmph(velocity) << " km/h ( " << velocity << "m/s )"); 67 | return velocity; 68 | } 69 | 70 | void PurePursuit::calcLookaheadDistance(int waypoint) 71 | { 72 | double current_velocity_mps = current_velocity_.twist.linear.x; 73 | double maximum_lookahead_distance = current_velocity_mps * 10; 74 | double ld = current_velocity_mps * lookahead_distance_calc_ratio_; 75 | 76 | lookahead_distance_ = ld < minimum_lookahead_distance_ ? minimum_lookahead_distance_ 77 | : ld > maximum_lookahead_distance ? maximum_lookahead_distance 78 | : ld ; 79 | 80 | //ROS_INFO("lookahead distance: %f",lookahead_distance_); 81 | 82 | return ; 83 | } 84 | 85 | double PurePursuit::calcCurvature(geometry_msgs::Point target) const 86 | { 87 | double kappa; 88 | double denominator = pow(getPlaneDistance(target, current_pose_.pose.position), 2); 89 | double numerator = 2 * calcRelativeCoordinate(target, current_pose_.pose).y; 90 | 91 | if (denominator != 0) 92 | kappa = numerator / denominator; 93 | else 94 | { 95 | if(numerator > 0) 96 | kappa = KAPPA_MIN_; 97 | else 98 | kappa = -KAPPA_MIN_; 99 | } 100 | //ROS_INFO_STREAM("kappa :" << kappa); 101 | return kappa; 102 | } 103 | 104 | // linear interpolation of next target 105 | bool PurePursuit::interpolateNextTarget(int next_waypoint, geometry_msgs::Point *next_target) const 106 | { 107 | constexpr double ERROR = pow(10, -5); // 0.00001 108 | 109 | int path_size = static_cast(current_waypoints_.getSize()); 110 | if (next_waypoint == path_size - 1) 111 | { 112 | *next_target = current_waypoints_.getWaypointPosition(next_waypoint); 113 | return true; 114 | } 115 | double search_radius = lookahead_distance_; 116 | geometry_msgs::Point zero_p; 117 | geometry_msgs::Point end = current_waypoints_.getWaypointPosition(next_waypoint); 118 | geometry_msgs::Point start = current_waypoints_.getWaypointPosition(next_waypoint - 1); 119 | 120 | // let the linear equation be "ax + by + c = 0" 121 | // if there are two points (x1,y1) , (x2,y2), a = "y2-y1, b = "(-1) * x2 - x1" ,c = "(-1) * (y2-y1)x1 + (x2-x1)y1" 122 | double a = 0; 123 | double b = 0; 124 | double c = 0; 125 | double get_linear_flag = getLinearEquation(start, end, &a, &b, &c); 126 | if (!get_linear_flag) 127 | return false; 128 | 129 | // let the center of circle be "(x0,y0)", in my code , the center of circle is vehicle position 130 | // the distance "d" between the foot of a perpendicular line and the center of circle is ... 131 | // | a * x0 + b * y0 + c | 132 | // d = ------------------------------- 133 | // √( a~2 + b~2) 134 | double d = getDistanceBetweenLineAndPoint(current_pose_.pose.position, a, b, c); 135 | 136 | // ROS_INFO("a : %lf ", a); 137 | // ROS_INFO("b : %lf ", b); 138 | // ROS_INFO("c : %lf ", c); 139 | // ROS_INFO("distance : %lf ", d); 140 | 141 | if (d > search_radius) 142 | return false; 143 | 144 | // unit vector of point 'start' to point 'end' 145 | tf::Vector3 v((end.x - start.x), (end.y - start.y), 0); 146 | tf::Vector3 unit_v = v.normalize(); 147 | 148 | // normal unit vectors of v 149 | tf::Vector3 unit_w1 = rotateUnitVector(unit_v, 90); // rotate to counter clockwise 90 degree 150 | tf::Vector3 unit_w2 = rotateUnitVector(unit_v, -90); // rotate to counter clockwise 90 degree 151 | 152 | // the foot of a perpendicular line 153 | geometry_msgs::Point h1; 154 | h1.x = current_pose_.pose.position.x + d * unit_w1.getX(); 155 | h1.y = current_pose_.pose.position.y + d * unit_w1.getY(); 156 | h1.z = current_pose_.pose.position.z; 157 | 158 | geometry_msgs::Point h2; 159 | h2.x = current_pose_.pose.position.x + d * unit_w2.getX(); 160 | h2.y = current_pose_.pose.position.y + d * unit_w2.getY(); 161 | h2.z = current_pose_.pose.position.z; 162 | 163 | // ROS_INFO("error : %lf", error); 164 | // ROS_INFO("whether h1 on line : %lf", h1.y - (slope * h1.x + intercept)); 165 | // ROS_INFO("whether h2 on line : %lf", h2.y - (slope * h2.x + intercept)); 166 | 167 | // check which of two foot of a perpendicular line is on the line equation 168 | geometry_msgs::Point h; 169 | if (fabs(a * h1.x + b * h1.y + c) < ERROR) 170 | { 171 | h = h1; 172 | // ROS_INFO("use h1"); 173 | } 174 | else if (fabs(a * h2.x + b * h2.y + c) < ERROR) 175 | { 176 | // ROS_INFO("use h2"); 177 | h = h2; 178 | } 179 | else 180 | { 181 | return false; 182 | } 183 | 184 | // get intersection[s] 185 | // if there is a intersection 186 | if (d == search_radius) 187 | { 188 | *next_target = h; 189 | return true; 190 | } 191 | else 192 | { 193 | // if there are two intersection 194 | // get intersection in front of vehicle 195 | double s = sqrt(pow(search_radius, 2) - pow(d, 2)); 196 | geometry_msgs::Point target1; 197 | target1.x = h.x + s * unit_v.getX(); 198 | target1.y = h.y + s * unit_v.getY(); 199 | target1.z = current_pose_.pose.position.z; 200 | 201 | geometry_msgs::Point target2; 202 | target2.x = h.x - s * unit_v.getX(); 203 | target2.y = h.y - s * unit_v.getY(); 204 | target2.z = current_pose_.pose.position.z; 205 | 206 | // ROS_INFO("target1 : ( %lf , %lf , %lf)", target1.x, target1.y, target1.z); 207 | // ROS_INFO("target2 : ( %lf , %lf , %lf)", target2.x, target2.y, target2.z); 208 | //displayLinePoint(a, b, c, target1, target2, h); // debug tool 209 | 210 | // check intersection is between end and start 211 | double interval = getPlaneDistance(end, start); 212 | if (getPlaneDistance(target1, end) < interval) 213 | { 214 | // ROS_INFO("result : target1"); 215 | *next_target = target1; 216 | return true; 217 | } 218 | else if (getPlaneDistance(target2, end) < interval) 219 | { 220 | // ROS_INFO("result : target2"); 221 | *next_target = target2; 222 | return true; 223 | } 224 | else 225 | { 226 | // ROS_INFO("result : false "); 227 | return false; 228 | } 229 | } 230 | } 231 | 232 | bool PurePursuit::verifyFollowing() const 233 | { 234 | double a = 0; 235 | double b = 0; 236 | double c = 0; 237 | getLinearEquation(current_waypoints_.getWaypointPosition(1), current_waypoints_.getWaypointPosition(2), &a, &b, &c); 238 | double displacement = getDistanceBetweenLineAndPoint(current_pose_.pose.position, a, b, c); 239 | double relative_angle = getRelativeAngle(current_waypoints_.getWaypointPose(1), current_pose_.pose); 240 | //ROS_ERROR("side diff : %lf , angle diff : %lf",displacement,relative_angle); 241 | if (displacement < displacement_threshold_ && relative_angle < relative_angle_threshold_) 242 | { 243 | // ROS_INFO("Following : True"); 244 | return true; 245 | } 246 | else 247 | { 248 | // ROS_INFO("Following : False"); 249 | return false; 250 | } 251 | } 252 | geometry_msgs::Twist PurePursuit::calcTwist(double curvature, double cmd_velocity) const 253 | { 254 | // verify whether vehicle is following the path 255 | bool following_flag = verifyFollowing(); 256 | static double prev_angular_velocity = 0; 257 | 258 | geometry_msgs::Twist twist; 259 | twist.linear.x = cmd_velocity; 260 | if (!following_flag) 261 | { 262 | //ROS_ERROR_STREAM("Not following"); 263 | twist.angular.z = current_velocity_.twist.linear.x * curvature; 264 | } 265 | else 266 | { 267 | twist.angular.z = prev_angular_velocity; 268 | } 269 | 270 | prev_angular_velocity = twist.angular.z; 271 | return twist; 272 | } 273 | 274 | void PurePursuit::getNextWaypoint() 275 | { 276 | int path_size = static_cast(current_waypoints_.getSize()); 277 | 278 | // if waypoints are not given, do nothing. 279 | if (path_size == 0) 280 | { 281 | num_of_next_waypoint_ = -1; 282 | return; 283 | } 284 | 285 | // look for the next waypoint. 286 | for (int i = 0; i < path_size; i++) 287 | { 288 | // if search waypoint is the last 289 | if (i == (path_size - 1)) 290 | { 291 | ROS_INFO("search waypoint is the last"); 292 | num_of_next_waypoint_ = i; 293 | return; 294 | } 295 | 296 | // if there exists an effective waypoint 297 | if (getPlaneDistance(current_waypoints_.getWaypointPosition(i), current_pose_.pose.position) > lookahead_distance_) 298 | { 299 | num_of_next_waypoint_ = i; 300 | //ROS_INFO_STREAM("wp = " << i << " dist = " << getPlaneDistance(current_waypoints_.getWaypointPosition(i), current_pose_.pose.position) ); 301 | //ROS_INFO("wp: %d, dist: %lf, x: %lf, y: %lf", i, getPlaneDistance(current_waypoints_.getWaypointPosition(i), current_pose_.pose.position), current_pose_.pose.position.x, current_pose_.pose.position.y ); 302 | // PROBLEM: returns a zero if the vehicle has traveled beyond the lookahead distance from the beginning of the waypoint list!!! 303 | return; 304 | } 305 | } 306 | 307 | // if this program reaches here , it means we lost the waypoint! 308 | num_of_next_waypoint_ = -1; 309 | return; 310 | } 311 | 312 | geometry_msgs::TwistStamped PurePursuit::outputZero() const 313 | { 314 | geometry_msgs::TwistStamped twist; 315 | twist.twist.linear.x = 0; 316 | twist.twist.angular.z = 0; 317 | twist.header.stamp = ros::Time::now(); 318 | return twist; 319 | } 320 | geometry_msgs::TwistStamped PurePursuit::outputTwist(geometry_msgs::Twist t) const 321 | { 322 | double g_lateral_accel_limit = 5.0; 323 | double ERROR = 1e-8; 324 | 325 | geometry_msgs::TwistStamped twist; 326 | twist.twist = t; 327 | twist.header.stamp = ros::Time::now(); 328 | 329 | double v = t.linear.x; 330 | double omega = t.angular.z; 331 | 332 | if(fabs(omega) < ERROR){ 333 | 334 | return twist; 335 | } 336 | 337 | double max_v = g_lateral_accel_limit / omega; 338 | 339 | 340 | double a = v * omega; 341 | //ROS_INFO("lateral accel = %lf", a); 342 | 343 | twist.twist.linear.x = fabs(a) > g_lateral_accel_limit ? max_v 344 | : v; 345 | twist.twist.angular.z = omega; 346 | 347 | return twist; 348 | } 349 | 350 | geometry_msgs::TwistStamped PurePursuit::go() 351 | { 352 | if(!pose_set_ || !waypoint_set_ || !velocity_set_){ 353 | if(!pose_set_) { 354 | ROS_WARN("position is missing"); 355 | } 356 | if(!waypoint_set_) { 357 | ROS_WARN("waypoint is missing"); 358 | } 359 | if(!velocity_set_) { 360 | ROS_WARN("velocity is missing"); 361 | } 362 | return outputZero(); 363 | } 364 | 365 | bool interpolate_flag = false; 366 | 367 | calcLookaheadDistance(1); 368 | // search next waypoint 369 | getNextWaypoint(); 370 | if (num_of_next_waypoint_ == -1) 371 | { 372 | ROS_WARN("lost next waypoint"); 373 | return outputZero(); 374 | } 375 | //ROS_ERROR_STREAM("next waypoint = " << num_of_next_waypoint_); 376 | 377 | // if g_linear_interpolate_mode is false or next waypoint is first or last 378 | if (!linear_interpolate_ || num_of_next_waypoint_ == 0 || 379 | num_of_next_waypoint_ == (static_cast(current_waypoints_.getSize() - 1))) 380 | { 381 | position_of_next_target_ = current_waypoints_.getWaypointPosition(num_of_next_waypoint_); 382 | return outputTwist(calcTwist(calcCurvature(position_of_next_target_), getCmdVelocity(0))); 383 | } 384 | 385 | // linear interpolation and calculate angular velocity 386 | interpolate_flag = interpolateNextTarget(num_of_next_waypoint_, &position_of_next_target_); 387 | 388 | if (!interpolate_flag) 389 | { 390 | ROS_ERROR_STREAM("lost target! "); 391 | return outputZero(); 392 | } 393 | 394 | // ROS_INFO("next_target : ( %lf , %lf , %lf)", next_target.x, next_target.y,next_target.z); 395 | 396 | return outputTwist(calcTwist(calcCurvature(position_of_next_target_), getCmdVelocity(0))); 397 | 398 | // ROS_INFO("linear : %lf, angular : %lf",twist.twist.linear.x,twist.twist.angular.z); 399 | 400 | #ifdef LOG 401 | std::ofstream ofs("/tmp/pure_pursuit.log", std::ios::app); 402 | ofs << _current_waypoints.getWaypointPosition(next_waypoint).x << " " 403 | << _current_waypoints.getWaypointPosition(next_waypoint).y << " " << next_target.x << " " << next_target.y 404 | << std::endl; 405 | #endif 406 | } 407 | } 408 | -------------------------------------------------------------------------------- /ros/src/waypoint_loader/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(waypoint_loader) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | # add_definitions(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | geometry_msgs 12 | roscpp 13 | rospy 14 | sensor_msgs 15 | std_msgs 16 | styx_msgs 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | # catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 40 | ## but can be declared for certainty nonetheless: 41 | ## * add a run_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | # add_message_files( 54 | # FILES 55 | # Message1.msg 56 | # Message2.msg 57 | # ) 58 | 59 | ## Generate services in the 'srv' folder 60 | # add_service_files( 61 | # FILES 62 | # Service1.srv 63 | # Service2.srv 64 | # ) 65 | 66 | ## Generate actions in the 'action' folder 67 | # add_action_files( 68 | # FILES 69 | # Action1.action 70 | # Action2.action 71 | # ) 72 | 73 | ## Generate added messages and services with any dependencies listed here 74 | # generate_messages( 75 | # DEPENDENCIES 76 | # geometry_msgs# sensor_msgs# std_msgs# styx_msgs 77 | # ) 78 | 79 | ################################################ 80 | ## Declare ROS dynamic reconfigure parameters ## 81 | ################################################ 82 | 83 | ## To declare and build dynamic reconfigure parameters within this 84 | ## package, follow these steps: 85 | ## * In the file package.xml: 86 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 87 | ## * In this file (CMakeLists.txt): 88 | ## * add "dynamic_reconfigure" to 89 | ## find_package(catkin REQUIRED COMPONENTS ...) 90 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 91 | ## and list every .cfg file to be processed 92 | 93 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 94 | # generate_dynamic_reconfigure_options( 95 | # cfg/DynReconf1.cfg 96 | # cfg/DynReconf2.cfg 97 | # ) 98 | 99 | ################################### 100 | ## catkin specific configuration ## 101 | ################################### 102 | ## The catkin_package macro generates cmake config files for your package 103 | ## Declare things to be passed to dependent projects 104 | ## INCLUDE_DIRS: uncomment this if you package contains header files 105 | ## LIBRARIES: libraries you create in this project that dependent projects also need 106 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 107 | ## DEPENDS: system dependencies of this project that dependent projects also need 108 | catkin_package( 109 | # INCLUDE_DIRS include 110 | # LIBRARIES waypoint_loader 111 | # CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs std_msgs styx_msgs 112 | # DEPENDS system_lib 113 | ) 114 | 115 | ########### 116 | ## Build ## 117 | ########### 118 | 119 | ## Specify additional locations of header files 120 | ## Your package locations should be listed before other locations 121 | # include_directories(include) 122 | include_directories( 123 | ${catkin_INCLUDE_DIRS} 124 | ) 125 | 126 | ## Declare a C++ library 127 | # add_library(${PROJECT_NAME} 128 | # src/${PROJECT_NAME}/waypoint_loader.cpp 129 | # ) 130 | 131 | ## Add cmake target dependencies of the library 132 | ## as an example, code may need to be generated before libraries 133 | ## either from message generation or dynamic reconfigure 134 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Declare a C++ executable 137 | ## With catkin_make all packages are built within a single CMake context 138 | ## The recommended prefix ensures that target names across packages don't collide 139 | # add_executable(${PROJECT_NAME}_node src/waypoint_loader_node.cpp) 140 | 141 | ## Rename C++ executable without prefix 142 | ## The above recommended prefix causes long target names, the following renames the 143 | ## target back to the shorter version for ease of user use 144 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 145 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 146 | 147 | ## Add cmake target dependencies of the executable 148 | ## same as for the library above 149 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 150 | 151 | ## Specify libraries to link a library or executable target against 152 | # target_link_libraries(${PROJECT_NAME}_node 153 | # ${catkin_LIBRARIES} 154 | # ) 155 | 156 | ############# 157 | ## Install ## 158 | ############# 159 | 160 | # all install targets should use catkin DESTINATION variables 161 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 162 | 163 | ## Mark executable scripts (Python etc.) for installation 164 | ## in contrast to setup.py, you can choose the destination 165 | # install(PROGRAMS 166 | # scripts/my_python_script 167 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark executables and/or libraries for installation 171 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_waypoint_loader.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /ros/src/waypoint_loader/launch/waypoint_loader.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /ros/src/waypoint_loader/launch/waypoint_loader_site.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /ros/src/waypoint_loader/launch/waypoint_loader_sitesim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /ros/src/waypoint_loader/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | waypoint_loader 4 | 0.0.0 5 | The waypoint_loader package 6 | 7 | 8 | 9 | 10 | yousuf 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | geometry_msgs 44 | roscpp 45 | rospy 46 | sensor_msgs 47 | std_msgs 48 | styx_msgs 49 | geometry_msgs 50 | roscpp 51 | rospy 52 | sensor_msgs 53 | std_msgs 54 | styx_msgs 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /ros/src/waypoint_loader/waypoint_loader.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import os 4 | import csv 5 | import math 6 | 7 | from geometry_msgs.msg import Quaternion 8 | 9 | from styx_msgs.msg import Lane, Waypoint 10 | 11 | import tf 12 | import rospy 13 | 14 | CSV_HEADER = ['x', 'y', 'z', 'yaw'] 15 | MAX_DECEL = 1.0 16 | 17 | 18 | class WaypointLoader(object): 19 | 20 | def __init__(self): 21 | rospy.init_node('waypoint_loader') # , log_level=rospy.DEBUG 22 | 23 | self.pub = rospy.Publisher('/base_waypoints', Lane, queue_size=1, latch=True) 24 | 25 | self.velocity = self.kmph2mps(rospy.get_param('~velocity')) 26 | self.new_waypoint_loader(rospy.get_param('~path')) 27 | rospy.spin() 28 | 29 | def new_waypoint_loader(self, path): 30 | if os.path.isfile(path): 31 | waypoints = self.load_waypoints(path) 32 | self.publish(waypoints) 33 | rospy.loginfo('Waypoint Loded') 34 | else: 35 | rospy.logerr('%s is not a file', path) 36 | 37 | def quaternion_from_yaw(self, yaw): 38 | return tf.transformations.quaternion_from_euler(0., 0., yaw) 39 | 40 | def kmph2mps(self, velocity_kmph): 41 | return (velocity_kmph * 1000.) / (60. * 60.) 42 | 43 | def load_waypoints(self, fname): 44 | waypoints = [] 45 | with open(fname) as wfile: 46 | reader = csv.DictReader(wfile, CSV_HEADER) 47 | for wp in reader: 48 | p = Waypoint() 49 | p.pose.pose.position.x = float(wp['x']) 50 | p.pose.pose.position.y = float(wp['y']) 51 | p.pose.pose.position.z = float(wp['z']) 52 | q = self.quaternion_from_yaw(float(wp['yaw'])) 53 | p.pose.pose.orientation = Quaternion(*q) 54 | p.twist.twist.linear.x = float(self.velocity) 55 | 56 | waypoints.append(p) 57 | return self.decelerate(waypoints) 58 | 59 | def distance(self, p1, p2): 60 | x, y, z = p1.x - p2.x, p1.y - p2.y, p1.z - p2.z 61 | return math.sqrt(x*x + y*y + z*z) 62 | 63 | def decelerate(self, waypoints): 64 | last = waypoints[-1] 65 | last.twist.twist.linear.x = 0. 66 | for wp in waypoints[:-1][::-1]: 67 | dist = self.distance(wp.pose.pose.position, last.pose.pose.position) 68 | vel = math.sqrt(2 * MAX_DECEL * dist) 69 | if vel < 1.: 70 | vel = 0. 71 | wp.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x) 72 | return waypoints 73 | 74 | def publish(self, waypoints): 75 | lane = Lane() 76 | lane.header.frame_id = '/world' 77 | lane.header.stamp = rospy.Time(0) 78 | lane.waypoints = waypoints 79 | self.pub.publish(lane) 80 | 81 | 82 | if __name__ == '__main__': 83 | try: 84 | WaypointLoader() 85 | except rospy.ROSInterruptException: 86 | rospy.logerr('Could not start waypoint node.') 87 | -------------------------------------------------------------------------------- /ros/src/waypoint_updater/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(waypoint_updater) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | # add_definitions(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | geometry_msgs 12 | roscpp 13 | rospy 14 | sensor_msgs 15 | std_msgs 16 | styx_msgs 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | # catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 40 | ## but can be declared for certainty nonetheless: 41 | ## * add a run_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | # add_message_files( 54 | # FILES 55 | # Message1.msg 56 | # Message2.msg 57 | # ) 58 | 59 | ## Generate services in the 'srv' folder 60 | # add_service_files( 61 | # FILES 62 | # Service1.srv 63 | # Service2.srv 64 | # ) 65 | 66 | ## Generate actions in the 'action' folder 67 | # add_action_files( 68 | # FILES 69 | # Action1.action 70 | # Action2.action 71 | # ) 72 | 73 | ## Generate added messages and services with any dependencies listed here 74 | # generate_messages( 75 | # DEPENDENCIES 76 | # geometry_msgs# sensor_msgs# std_msgs# styx_msgs 77 | # ) 78 | 79 | ################################################ 80 | ## Declare ROS dynamic reconfigure parameters ## 81 | ################################################ 82 | 83 | ## To declare and build dynamic reconfigure parameters within this 84 | ## package, follow these steps: 85 | ## * In the file package.xml: 86 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 87 | ## * In this file (CMakeLists.txt): 88 | ## * add "dynamic_reconfigure" to 89 | ## find_package(catkin REQUIRED COMPONENTS ...) 90 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 91 | ## and list every .cfg file to be processed 92 | 93 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 94 | # generate_dynamic_reconfigure_options( 95 | # cfg/DynReconf1.cfg 96 | # cfg/DynReconf2.cfg 97 | # ) 98 | 99 | ################################### 100 | ## catkin specific configuration ## 101 | ################################### 102 | ## The catkin_package macro generates cmake config files for your package 103 | ## Declare things to be passed to dependent projects 104 | ## INCLUDE_DIRS: uncomment this if you package contains header files 105 | ## LIBRARIES: libraries you create in this project that dependent projects also need 106 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 107 | ## DEPENDS: system dependencies of this project that dependent projects also need 108 | catkin_package( 109 | # INCLUDE_DIRS include 110 | # LIBRARIES waypoint_updater 111 | # CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs std_msgs styx_msgs styx_utils 112 | # DEPENDS system_lib 113 | ) 114 | 115 | ########### 116 | ## Build ## 117 | ########### 118 | 119 | ## Specify additional locations of header files 120 | ## Your package locations should be listed before other locations 121 | # include_directories(include) 122 | include_directories( 123 | ${catkin_INCLUDE_DIRS} 124 | ) 125 | 126 | ## Declare a C++ library 127 | # add_library(${PROJECT_NAME} 128 | # src/${PROJECT_NAME}/waypoint_updater.cpp 129 | # ) 130 | 131 | ## Add cmake target dependencies of the library 132 | ## as an example, code may need to be generated before libraries 133 | ## either from message generation or dynamic reconfigure 134 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Declare a C++ executable 137 | ## With catkin_make all packages are built within a single CMake context 138 | ## The recommended prefix ensures that target names across packages don't collide 139 | # add_executable(${PROJECT_NAME}_node src/waypoint_updater_node.cpp) 140 | 141 | ## Rename C++ executable without prefix 142 | ## The above recommended prefix causes long target names, the following renames the 143 | ## target back to the shorter version for ease of user use 144 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 145 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 146 | 147 | ## Add cmake target dependencies of the executable 148 | ## same as for the library above 149 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 150 | 151 | ## Specify libraries to link a library or executable target against 152 | # target_link_libraries(${PROJECT_NAME}_node 153 | # ${catkin_LIBRARIES} 154 | # ) 155 | 156 | ############# 157 | ## Install ## 158 | ############# 159 | 160 | # all install targets should use catkin DESTINATION variables 161 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 162 | 163 | ## Mark executable scripts (Python etc.) for installation 164 | ## in contrast to setup.py, you can choose the destination 165 | # install(PROGRAMS 166 | # scripts/my_python_script 167 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark executables and/or libraries for installation 171 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_waypoint_updater.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /ros/src/waypoint_updater/launch/waypoint_updater.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /ros/src/waypoint_updater/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | waypoint_updater 4 | 0.0.0 5 | The waypoint_updater package 6 | 7 | 8 | 9 | 10 | yousuf 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | geometry_msgs 44 | roscpp 45 | rospy 46 | sensor_msgs 47 | std_msgs 48 | styx_msgs 49 | geometry_msgs 50 | roscpp 51 | rospy 52 | sensor_msgs 53 | std_msgs 54 | styx_msgs 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /ros/src/waypoint_updater/waypoint_updater.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import os 4 | import csv 5 | import rospy 6 | from geometry_msgs.msg import PoseStamped, Quaternion 7 | from styx_msgs.msg import Lane, Waypoint 8 | from std_msgs.msg import Int32 9 | 10 | import math 11 | import tf 12 | 13 | ''' 14 | This node will publish waypoints from the car's current position to some `x` distance ahead. 15 | 16 | As mentioned in the doc, you should ideally first implement a version which does not care 17 | about traffic lights or obstacles. 18 | 19 | Once you have created dbw_node, you will update this node to use the status of traffic lights too. 20 | 21 | Please note that our simulator also provides the exact location of traffic lights and their 22 | current status in `/vehicle/traffic_lights` message. You can use this message to build this node 23 | as well as to verify your TL classifier. 24 | 25 | ''' 26 | 27 | PUBLISHING_RATE = 2 # Publishing frequency (Hz) 28 | 29 | class WaypointUpdater(object): 30 | def __init__(self): 31 | rospy.init_node('waypoint_updater') 32 | 33 | # Member variables 34 | self.current_pose = None # current coords of vehicle 35 | self.base_waypoints = None # list of base waypoints 36 | self.queue_wp = None # waypoints to publish 37 | self.next_waypoint = None # index of next waypoint 38 | self.stop_waypoint = None # stop line index for the nearest light 39 | self.next_basewp = None # the next waypoint index to retrieve from base 40 | self.destination = None # the final waypoint in the list 41 | self.num_base_wp = 0 # the number of points in the base list 42 | self.msg_seq_num = 0 # sequence number of published message 43 | self.velocity_drop = 62. # distance to begin reducing velocity 44 | self.VELOCITY_MAX = 2.777 # mps Carla max of 10 km/h (updated by waypoints_cb) 45 | self.LOOKAHEAD_WPS = 20 # Number of waypoints we will publish. 46 | self.prev_state = None # previous traffic light state 47 | self.halt = False # shut down 48 | self.replan = True # when a light changes, update velocity 49 | self.loop = True # loop around the test site (updated by waypoints_cb) 50 | 51 | rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb) 52 | rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb) 53 | self.traffic_sub = rospy.Subscriber('/traffic_waypoint', Int32, self.traffic_cb) 54 | self.final_waypoints_pub = rospy.Publisher('final_waypoints', Lane, queue_size=1) 55 | 56 | # Operations loop and publishing of /final_waypoints 57 | rate = rospy.Rate(PUBLISHING_RATE) 58 | while not rospy.is_shutdown(): 59 | self.update() 60 | if self.queue_wp is not None: 61 | self.publish(self.queue_wp) 62 | rate.sleep() 63 | 64 | def update(self): 65 | if (self.base_waypoints is None) or (self.current_pose is None): 66 | return 67 | 68 | # find the closest next waypoint in the queue 69 | self.next_waypoint = self.get_next_waypoint(self.queue_wp) 70 | 71 | # if the queue is empty or next waypoint is not found in the queue 72 | if self.next_waypoint is None: 73 | # find the closest waypoint in the base waypoints 74 | next_wp = self.get_next_waypoint(self.base_waypoints) 75 | # initialize queue 76 | wp_idx = [idx % self.num_base_wp for idx in range(next_wp, next_wp + self.LOOKAHEAD_WPS)] 77 | self.queue_wp = [self.base_waypoints[wp] for wp in wp_idx] 78 | self.next_waypoint = 0 79 | self.next_basewp = (next_wp + self.LOOKAHEAD_WPS) % self.num_base_wp 80 | rospy.loginfo("Queue initialized with %d waypoints from base_waypoints", self.LOOKAHEAD_WPS) 81 | 82 | # manage queue 83 | if self.next_waypoint is not 0: 84 | # dequeue until head=next_waypoint 85 | del self.queue_wp[:self.next_waypoint] 86 | '''rospy.loginfo("Dequeued %d waypoints from queue", self.next_waypoint)''' 87 | # enqueue until length is LOOKAHEAD_WPS 88 | for i in range(self.next_waypoint): 89 | self.queue_wp.append(self.base_waypoints[self.next_basewp]) 90 | self.update_waypoint_velocity(self.LOOKAHEAD_WPS-self.next_waypoint+i) 91 | '''rospy.loginfo("Enqueing waypoint %d to queue", self.next_basewp)''' 92 | 93 | self.next_basewp += 1 # TODO: modulo calc 94 | if self.next_basewp == len(self.base_waypoints): # handle end of track 95 | self.next_basewp = 0 # wrap around to the beginning 96 | '''rospy.loginfo("Queue has %d items", len(self.queue_wp))''' 97 | if self.replan: 98 | self.update_velocities() 99 | self.replan = False 100 | 101 | def update_velocities(self): 102 | # update all velocities in the queue 103 | # if red in range of 4m (site) or 62m (sim) decel 104 | 105 | # for all waypoints in the queue 106 | for idx in range(len(self.queue_wp)): 107 | self.update_waypoint_velocity(idx) 108 | 109 | def update_waypoint_velocity(self, index): 110 | # By default, set the velocity to maximum 111 | self.queue_wp[index].twist.twist.linear.x = self.VELOCITY_MAX 112 | # unless we are in a halt state 113 | if not self.loop and self.halt: 114 | self.queue_wp[index].twist.twist.linear.x = 0. 115 | # otherwise... 116 | elif self.stop_waypoint is not None: 117 | # get the distance to the red light 118 | sidx = self.stop_waypoint 119 | distance_to_stopline = self.distance2(self.queue_wp[index].pose.pose.position, self.base_waypoints[sidx].pose.pose.position) 120 | # and calculate the ratio to reduce the velocity 121 | if distance_to_stopline <= 0: # set velocity at or beyond stop line to zero 122 | self.queue_wp[index].twist.twist.linear.x = 0. 123 | elif distance_to_stopline < self.velocity_drop: 124 | ratio = distance_to_stopline / self.velocity_drop 125 | if distance_to_stopline <= 2.0: 126 | ratio = 0. 127 | self.queue_wp[index].twist.twist.linear.x = self.VELOCITY_MAX * ratio 128 | 129 | def publish(self, wp_list): 130 | msg = Lane() 131 | msg.waypoints = wp_list 132 | msg.header.frame_id = '/world' 133 | msg.header.stamp = rospy.Time.now() 134 | msg.header.seq = self.msg_seq_num 135 | self.msg_seq_num += 1 136 | self.final_waypoints_pub.publish(msg) 137 | 138 | # Flexible to work with queue or base waypoint list 139 | def get_next_waypoint(self, waypoint_list): 140 | nearest_idx = None 141 | if waypoint_list is None: 142 | return nearest_idx 143 | num_points = len(waypoint_list) 144 | lowest_dist = 9999999.9 145 | iterations = 0 146 | for idx, wp in enumerate(waypoint_list): 147 | iterations += 1 148 | delta = self.distance2(wp.pose.pose.position, self.current_pose.position) 149 | if delta < lowest_dist: 150 | lowest_dist = delta 151 | nearest_idx = idx 152 | if lowest_dist < 1.: 153 | break 154 | nearest_wp = waypoint_list[nearest_idx] 155 | if not self.is_waypoint_positive(nearest_wp.pose.pose, self.current_pose): # Is it in front? 156 | #nearest_idx = (nearest_idx + 1) % num_points 157 | nearest_idx += 1 158 | return nearest_idx 159 | 160 | def is_waypoint_positive(self, pose1, pose2): 161 | dx = pose1.position.x - pose2.position.x # convert to vehicle coords 162 | dy = pose1.position.y - pose2.position.y 163 | o = pose2.orientation 164 | _,_,t = tf.transformations.euler_from_quaternion([o.x, o.y, o.z, o.w]) 165 | lx = math.cos(-t)*dx - math.sin(-t)*dy 166 | return lx > 0.0 167 | 168 | # Distance between two points: p1 and p2 are position data structures 169 | def distance2(self, p1, p2): 170 | x, y = p1.x - p2.x, p1.y - p2.y 171 | return math.sqrt(x*x + y*y) 172 | 173 | def pose_cb(self, msg): 174 | self.current_pose = msg.pose 175 | 176 | # Check for proximity to destination 177 | if not self.loop and (self.destination is not None): 178 | sidx = self.destination 179 | distance_to_destination = self.distance2(self.current_pose.position, self.base_waypoints[sidx].pose.pose.position) 180 | # and if we are within stopping distance... 181 | if distance_to_destination < self.velocity_drop: 182 | self.traffic_sub.unregister() # unsubscribe from traffic light messages 183 | self.stop_waypoint = sidx # set up an imaginary red traffic light 184 | self.halt = True 185 | rospy.loginfo("Destination waypoint acquired...begin slowing") 186 | self.replan = True 187 | self.destination = None 188 | 189 | # The following callback is latched (called once) 190 | def waypoints_cb(self, waypoints): 191 | self.base_waypoints = self.filterWaypoints(waypoints) 192 | self.num_base_wp = len(self.base_waypoints) # the number of points in the base list 193 | self.destination = self.num_base_wp - 1 194 | 195 | # Acquire the default velocity from the waypoint loader 196 | self.VELOCITY_MAX = self.base_waypoints[self.num_base_wp/2].twist.twist.linear.x 197 | rospy.loginfo("Velocity max is: %.2f", self.VELOCITY_MAX) 198 | 199 | # If simulator, use longer queue and halt at destination 200 | if self.VELOCITY_MAX > 3.0: 201 | self.LOOKAHEAD_WPS = 100 202 | self.loop = False # comment this line to loop in simulator 203 | 204 | # Compute a safe stopping distance 205 | self.velocity_drop = self.VELOCITY_MAX * self.VELOCITY_MAX / 2. 206 | rospy.loginfo("Stopping distance is: %.2f", self.velocity_drop) 207 | 208 | #debug: set a closer destination waypoint to test end-of-track-halt condition 209 | #self.destination = 577 210 | 211 | def filterWaypoints(self, wp): 212 | if wp.waypoints[0].pose.pose.position.x == 10.4062: 213 | waypoints = [] 214 | path = rospy.get_param('~path') 215 | if not os.path.isfile(path): 216 | return wp.waypoints 217 | with open(path) as wfile: 218 | reader = csv.DictReader(wfile, ['x','y','z','yaw']) 219 | for wp in reader: 220 | p = Waypoint() 221 | p.pose.pose.position.x = float(wp['x']) 222 | p.pose.pose.position.y = float(wp['y']) 223 | p.pose.pose.position.z = float(wp['z']) 224 | q = tf.transformations.quaternion_from_euler(0., 0., float(wp['yaw'])) 225 | p.pose.pose.orientation = Quaternion(*q) 226 | p.twist.twist.linear.x = 2.7777778 227 | waypoints.append(p) 228 | rospy.loginfo("Corrected waypoints loaded") 229 | return waypoints 230 | return wp.waypoints 231 | 232 | def traffic_cb(self, msg): 233 | #self.stop_waypoint = msg.data if msg.data >= 0 else None 234 | # Halt before the stop line, otherwise we enter the intersection 235 | self.stop_waypoint = msg.data-3 if msg.data >= 0 else None 236 | 237 | # traffic light transitions cause replanning in update loop 238 | if self.stop_waypoint != self.prev_state: 239 | self.replan = True 240 | self.prev_state = self.stop_waypoint 241 | 242 | def obstacle_cb(self, msg): 243 | # TODO: Callback for /obstacle_waypoint message. We will implement it later 244 | pass 245 | 246 | def get_waypoint_velocity(self, waypoint): 247 | return waypoint.twist.twist.linear.x 248 | 249 | def set_waypoint_velocity(self, waypoints, waypoint, velocity): 250 | waypoints[waypoint].twist.twist.linear.x = velocity 251 | 252 | if __name__ == '__main__': 253 | try: 254 | WaypointUpdater() 255 | except rospy.ROSInterruptException: 256 | rospy.logerr('Could not start waypoint updater node.') 257 | --------------------------------------------------------------------------------