├── ros ├── src │ ├── tl_detector │ │ ├── light_classification │ │ │ ├── __init__.py │ │ │ ├── weights │ │ │ │ └── 01.weights │ │ │ └── tl_classifier.py │ │ ├── launch │ │ │ ├── tl_detector.launch │ │ │ └── tl_detector_site.launch │ │ ├── site_traffic_light_config.yaml │ │ ├── sim_traffic_light_config.yaml │ │ ├── light_publisher.py │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ └── tl_detector.py │ ├── CMakeLists.txt │ ├── styx_msgs │ │ ├── msg │ │ │ ├── Lane.msg │ │ │ ├── TrafficLightArray.msg │ │ │ ├── Waypoint.msg │ │ │ └── TrafficLight.msg │ │ ├── package.xml │ │ └── CMakeLists.txt │ ├── waypoint_updater │ │ ├── launch │ │ │ └── waypoint_updater.launch │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ └── waypoint_updater.py │ ├── styx │ │ ├── launch │ │ │ └── server.launch │ │ ├── unity_simulator_launcher.sh │ │ ├── conf.py │ │ ├── server.py │ │ ├── package.xml │ │ ├── bridge.py │ │ └── CMakeLists.txt │ ├── camera_info_publisher │ │ ├── launch │ │ │ └── camera_info_publisher.launch │ │ ├── package.xml │ │ ├── yaml_to_camera_info_publisher.py │ │ └── CMakeLists.txt │ ├── waypoint_loader │ │ ├── launch │ │ │ ├── waypoint_loader.launch │ │ │ └── waypoint_loader_site.launch │ │ ├── package.xml │ │ ├── waypoint_loader.py │ │ └── CMakeLists.txt │ ├── waypoint_follower │ │ ├── launch │ │ │ └── pure_pursuit.launch │ │ ├── package.xml │ │ ├── src │ │ │ ├── pure_pursuit.cpp │ │ │ └── pure_pursuit_core.cpp │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ ├── libwaypoint_follower.h │ │ │ └── pure_pursuit_core.h │ │ └── lib │ │ │ └── libwaypoint_follower.cpp │ └── twist_controller │ │ ├── launch │ │ ├── dbw_test.launch │ │ ├── dbw_sim.launch │ │ └── dbw.launch │ │ ├── lowpass.py │ │ ├── yaw_controller.py │ │ ├── pid.py │ │ ├── package.xml │ │ ├── twist_controller.py │ │ ├── dbw_test.py │ │ ├── CMakeLists.txt │ │ └── dbw_node.py ├── .catkin_workspace └── launch │ ├── styx.launch │ └── site.launch ├── .gitignore ├── imgs ├── unity.png ├── autoware_tf1.png ├── autoware_tf2.png ├── open_simulator.png ├── select_waypoint.png └── autoware_computing.png ├── data ├── maptf.launch ├── grasshopper_calibration.yml └── churchlot_with_cars.csv ├── requirements.txt ├── README.md └── Dockerfile /ros/src/tl_detector/light_classification/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.py[cod] 2 | 3 | build 4 | devel 5 | 6 | profile.tmp 7 | -------------------------------------------------------------------------------- /ros/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/kinetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /ros/src/styx_msgs/msg/Lane.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | Waypoint[] waypoints 3 | -------------------------------------------------------------------------------- /ros/src/styx_msgs/msg/TrafficLightArray.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | TrafficLight[] lights 3 | -------------------------------------------------------------------------------- /imgs/unity.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mimoralea/CarND-Capstone/master/imgs/unity.png -------------------------------------------------------------------------------- /imgs/autoware_tf1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mimoralea/CarND-Capstone/master/imgs/autoware_tf1.png -------------------------------------------------------------------------------- /imgs/autoware_tf2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mimoralea/CarND-Capstone/master/imgs/autoware_tf2.png -------------------------------------------------------------------------------- /imgs/open_simulator.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mimoralea/CarND-Capstone/master/imgs/open_simulator.png -------------------------------------------------------------------------------- /ros/src/styx_msgs/msg/Waypoint.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped pose 2 | geometry_msgs/TwistStamped twist 3 | -------------------------------------------------------------------------------- /imgs/select_waypoint.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mimoralea/CarND-Capstone/master/imgs/select_waypoint.png -------------------------------------------------------------------------------- /imgs/autoware_computing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mimoralea/CarND-Capstone/master/imgs/autoware_computing.png -------------------------------------------------------------------------------- /ros/.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /data/maptf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /ros/src/tl_detector/light_classification/weights/01.weights: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mimoralea/CarND-Capstone/master/ros/src/tl_detector/light_classification/weights/01.weights -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 8 | keras 9 | tensorflow==1.0.0 10 | h5py 11 | -------------------------------------------------------------------------------- /ros/src/tl_detector/launch/tl_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /ros/src/waypoint_updater/launch/waypoint_updater.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Miguel Morales | mimoralea@gmail.com
2 | Thomas Weustenfeld | weustenfeld@me.com
3 | Jessica Yung | jessicayungyc@gmail.com
4 | Maxime Adjigble | maxime.adjigble@gmail.com
5 | -------------------------------------------------------------------------------- /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 | - [20.991, 22.837] 8 | -------------------------------------------------------------------------------- /ros/src/styx/launch/server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /ros/src/camera_info_publisher/launch/camera_info_publisher.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /ros/src/tl_detector/launch/tl_detector_site.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /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_follower/launch/pure_pursuit.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /ros/src/tl_detector/sim_traffic_light_config.yaml: -------------------------------------------------------------------------------- 1 | camera_info: 2 | focal_length_x: 0.97428 3 | focal_length_y: 1.73205 4 | image_width: 800 5 | image_height: 600 6 | stop_line_positions: 7 | - [1148.56, 1184.65] 8 | - [1559.2, 1158.43] 9 | - [2122.14, 1526.79] 10 | - [2175.237, 1795.71] 11 | - [1493.29, 2947.67] 12 | - [821.96, 2905.8] 13 | - [161.76, 2303.82] 14 | - [351.84, 1574.65] 15 | -------------------------------------------------------------------------------- /ros/src/twist_controller/launch/dbw_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /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/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/launch/dbw_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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/src/twist_controller/launch/dbw.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /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/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/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 | # Not sure this anti-windup implementation is working. Basically int_val is ALWAYS updated since min <= val <= max (line 27) 30 | if val > self.max: 31 | val = self.max 32 | elif val < self.min: 33 | val = self.min 34 | else: 35 | self.int_val = integral 36 | self.last_error = error 37 | 38 | return val 39 | -------------------------------------------------------------------------------- /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 | 27 | # socket io 28 | RUN apt-get install -y netbase 29 | 30 | RUN mkdir /capstone 31 | VOLUME ["/capstone"] 32 | VOLUME ["/root/.ros/log/"] 33 | -------------------------------------------------------------------------------- /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/server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import socketio 4 | import eventlet 5 | import eventlet.wsgi 6 | import time 7 | from flask import Flask, render_template 8 | 9 | from bridge import Bridge 10 | from conf import conf 11 | 12 | sio = socketio.Server() 13 | app = Flask(__name__) 14 | msgs = [] 15 | 16 | dbw_enable = False 17 | 18 | @sio.on('connect') 19 | def connect(sid, environ): 20 | print("connect ", sid) 21 | 22 | def send(topic, data): 23 | s = 1 24 | msgs.append((topic, data)) 25 | #sio.emit(topic, data=json.dumps(data), skip_sid=True) 26 | 27 | bridge = Bridge(conf, send) 28 | 29 | @sio.on('telemetry') 30 | def telemetry(sid, data): 31 | global dbw_enable 32 | if data["dbw_enable"] != dbw_enable: 33 | dbw_enable = data["dbw_enable"] 34 | bridge.publish_dbw_status(dbw_enable) 35 | bridge.publish_odometry(data) 36 | for i in range(len(msgs)): 37 | topic, data = msgs.pop(0) 38 | sio.emit(topic, data=data, skip_sid=True) 39 | 40 | @sio.on('control') 41 | def control(sid, data): 42 | bridge.publish_controls(data) 43 | 44 | @sio.on('obstacle') 45 | def obstacle(sid, data): 46 | bridge.publish_obstacles(data) 47 | 48 | @sio.on('lidar') 49 | def obstacle(sid, data): 50 | bridge.publish_lidar(data) 51 | 52 | @sio.on('trafficlights') 53 | def trafficlights(sid, data): 54 | bridge.publish_traffic(data) 55 | 56 | @sio.on('image') 57 | def image(sid, data): 58 | bridge.publish_camera(data) 59 | 60 | if __name__ == '__main__': 61 | 62 | # wrap Flask application with engineio's middleware 63 | app = socketio.Middleware(sio, app) 64 | 65 | # deploy as an eventlet WSGI server 66 | eventlet.wsgi.server(eventlet.listen(('', 4567)), app) 67 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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/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/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/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/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_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_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/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/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/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 | 28 | rospy.spin() 29 | 30 | def new_waypoint_loader(self, path): 31 | if os.path.isfile(path): 32 | waypoints = self.load_waypoints(path) 33 | self.publish(waypoints) 34 | rospy.loginfo('Waypoint Loded') 35 | else: 36 | rospy.logerr('%s is not a file', path) 37 | 38 | def quaternion_from_yaw(self, yaw): 39 | return tf.transformations.quaternion_from_euler(0., 0., yaw) 40 | 41 | def kmph2mps(self, velocity_kmph): 42 | return (velocity_kmph * 1000.) / (60. * 60.) 43 | 44 | def load_waypoints(self, fname): 45 | waypoints = [] 46 | with open(fname) as wfile: 47 | reader = csv.DictReader(wfile, CSV_HEADER) 48 | for wp in reader: 49 | p = Waypoint() 50 | p.pose.pose.position.x = float(wp['x']) 51 | p.pose.pose.position.y = float(wp['y']) 52 | p.pose.pose.position.z = float(wp['z']) 53 | # convert radians back to degrees, see 54 | # [Bugfix] #31 Convert degree to radius in simulator waypoint file 55 | q = self.quaternion_from_yaw(float(wp['yaw']) * (180.0/math.pi)) 56 | p.pose.pose.orientation = Quaternion(*q) 57 | p.twist.twist.linear.x = float(self.velocity) 58 | 59 | waypoints.append(p) 60 | return self.decelerate(waypoints) 61 | 62 | def distance(self, p1, p2): 63 | x, y, z = p1.x - p2.x, p1.y - p2.y, p1.z - p2.z 64 | return math.sqrt(x*x + y*y + z*z) 65 | 66 | def decelerate(self, waypoints): 67 | last = waypoints[-1] 68 | last.twist.twist.linear.x = 0. 69 | for wp in waypoints[:-1][::-1]: 70 | dist = self.distance(wp.pose.pose.position, last.pose.pose.position) 71 | vel = math.sqrt(2 * MAX_DECEL * dist) 72 | if vel < 1.: 73 | vel = 0. 74 | wp.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x) 75 | return waypoints 76 | 77 | def publish(self, waypoints): 78 | lane = Lane() 79 | lane.header.frame_id = '/world' 80 | lane.header.stamp = rospy.Time(0) 81 | lane.waypoints = waypoints 82 | self.pub.publish(lane) 83 | 84 | 85 | if __name__ == '__main__': 86 | try: 87 | WaypointLoader() 88 | except rospy.ROSInterruptException: 89 | rospy.logerr('Could not start waypoint node.') 90 | -------------------------------------------------------------------------------- /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 | 35 | 36 | int main(int argc, char **argv) 37 | { 38 | 39 | 40 | // set up ros 41 | ros::init(argc, argv, "pure_pursuit"); 42 | 43 | ros::NodeHandle nh; 44 | ros::NodeHandle private_nh("~"); 45 | 46 | bool linear_interpolate_mode; 47 | private_nh.param("linear_interpolate_mode", linear_interpolate_mode, bool(true)); 48 | ROS_INFO_STREAM("linear_interpolate_mode : " << linear_interpolate_mode); 49 | 50 | waypoint_follower::PurePursuit pp(linear_interpolate_mode); 51 | 52 | ROS_INFO("set publisher..."); 53 | // publish topic 54 | ros::Publisher cmd_velocity_publisher = nh.advertise("twist_cmd", 10); 55 | 56 | ROS_INFO("set subscriber..."); 57 | // subscribe topic 58 | ros::Subscriber waypoint_subscriber = 59 | nh.subscribe("final_waypoints", 10, &waypoint_follower::PurePursuit::callbackFromWayPoints, &pp); 60 | ros::Subscriber ndt_subscriber = 61 | nh.subscribe("current_pose", 10, &waypoint_follower::PurePursuit::callbackFromCurrentPose, &pp); 62 | ros::Subscriber est_twist_subscriber = 63 | nh.subscribe("current_velocity", 10, &waypoint_follower::PurePursuit::callbackFromCurrentVelocity, &pp); 64 | 65 | ROS_INFO("pure pursuit start"); 66 | ros::Rate loop_rate(LOOP_RATE); 67 | while (ros::ok()) 68 | { 69 | ros::spinOnce(); 70 | cmd_velocity_publisher.publish(pp.go()); 71 | loop_rate.sleep(); 72 | } 73 | 74 | return 0; 75 | } 76 | -------------------------------------------------------------------------------- /ros/src/twist_controller/twist_controller.py: -------------------------------------------------------------------------------- 1 | 2 | import math 3 | import time 4 | import rospy 5 | 6 | GAS_DENSITY = 2.858 7 | ONE_MPH = 0.44704 8 | 9 | 10 | class Controller(object): 11 | def __init__(self, yaw_ctrl, pid_lin_vel, filt_ang_vel): 12 | self.yaw_ctrl = yaw_ctrl 13 | self.pid_lin_vel = pid_lin_vel 14 | self.filt_ang_vel = filt_ang_vel 15 | self.t0 = time.time() 16 | self.dt = 0.0 17 | self.vehicle_mass = None 18 | self.brake_deadband = None 19 | self.wheel_radius = None 20 | self.feed_forward_brake_control = True 21 | 22 | def set_vehicle_parameters(self, vehicle_mass, vehicle_mass_offset, brake_deadband, wheel_radius): 23 | self.vehicle_mass = vehicle_mass + vehicle_mass_offset 24 | self.brake_deadband = brake_deadband 25 | self.wheel_radius = wheel_radius 26 | 27 | def control(self, current_velocity, linear_velocity, angular_velocity): 28 | # Return throttle, brake, steer 29 | 30 | # Check if all required parameters are set 31 | if not ( self.vehicle_mass and self.brake_deadband and self.wheel_radius): 32 | rospy.logerror('vehicle parameters not set') 33 | 34 | #Apply the filter to the angular velocity 35 | angular_velocity = self.filt_ang_vel.filt(angular_velocity) 36 | 37 | #Compute the steering angle 38 | steer = self.yaw_ctrl.get_steering( 39 | linear_velocity, 40 | angular_velocity, 41 | current_velocity) 42 | 43 | #Compute the throttle command 44 | cmd = 0 45 | vel_error = (linear_velocity - current_velocity) 46 | if self.dt: 47 | cmd = self.pid_lin_vel.step(vel_error, self.dt) 48 | 49 | self.dt = time.time() - self.t0 50 | self.t0 += self.dt 51 | 52 | #Apply the brakes if necessary 53 | if cmd > 0: 54 | throttle = cmd 55 | brake = 0.0 56 | else: 57 | # The correct brake torque can be computed using the desired acceleration, 58 | # weight of the vehicle and wheel radius with 59 | # Torque = Length x Force = Length x Mass x Accelecation 60 | throttle = 0.0 61 | 62 | if self.feed_forward_brake_control: 63 | desired_neg_acceleration = vel_error / self.dt 64 | brake = - self.wheel_radius * self.vehicle_mass * desired_neg_acceleration 65 | self.pid_lin_vel.reset() # reset controller to prevent windup 66 | else: 67 | # Alternatively use negative speed control 68 | brake = - self.wheel_radius * self.vehicle_mass * (cmd - self.brake_deadband) 69 | 70 | # If braking effort is small then don't use wheel brakes 71 | if math.fabs(cmd) < self.brake_deadband: 72 | # Car is currently still in the braking deadband, meaning that it is 73 | # not required to brake (the torque by the engine is enough for braking) 74 | brake = 0.0 75 | else: 76 | # The car needs to brake using wheel brakes 77 | pass 78 | 79 | # debug 80 | rospy.logdebug('cmd = %.2f, T = %.2f, B = %.2f, S = %.2f (BAND: %.2f)', cmd, throttle, brake, steer, self.brake_deadband) 81 | 82 | # The correct values for brake can be computed using the desired acceleration, weight of the vehicle, and wheel radius. 83 | return throttle, brake, steer 84 | 85 | def reset(self): 86 | self.pid_lin_vel.reset() 87 | -------------------------------------------------------------------------------- /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/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 in `styx/data` folder. 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/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/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 | 37 | 38 | class Bridge(object): 39 | def __init__(self, conf, server): 40 | rospy.init_node('styx_server') 41 | self.server = server 42 | self.vel = 0. 43 | self.yaw = None 44 | self.angular_vel = 0. 45 | self.bridge = CvBridge() 46 | 47 | self.callbacks = { 48 | '/vehicle/steering_cmd': self.callback_steering, 49 | '/vehicle/throttle_cmd': self.callback_throttle, 50 | '/vehicle/brake_cmd': self.callback_brake, 51 | } 52 | 53 | self.subscribers = [rospy.Subscriber(e.topic, TYPE[e.type], self.callbacks[e.topic]) 54 | for e in conf.subscribers] 55 | 56 | self.publishers = {e.name: rospy.Publisher(e.topic, TYPE[e.type], queue_size=1) 57 | for e in conf.publishers} 58 | 59 | def create_light(self, x, y, z, yaw, state): 60 | light = TrafficLight() 61 | 62 | light.header = Header() 63 | light.header.stamp = rospy.Time.now() 64 | light.header.frame_id = '/world' 65 | 66 | light.pose = self.create_pose(x, y, z, yaw) 67 | light.state = state 68 | 69 | return light 70 | 71 | def create_pose(self, x, y, z, yaw=0.): 72 | pose = PoseStamped() 73 | 74 | pose.header = Header() 75 | pose.header.stamp = rospy.Time.now() 76 | pose.header.frame_id = '/world' 77 | 78 | pose.pose.position.x = x 79 | pose.pose.position.y = y 80 | pose.pose.position.z = z 81 | 82 | q = tf.transformations.quaternion_from_euler(0., 0., math.pi * yaw/180.) 83 | pose.pose.orientation = Quaternion(*q) 84 | 85 | return pose 86 | 87 | def create_float(self, val): 88 | fl = Float() 89 | fl.data = val 90 | return fl 91 | 92 | def create_twist(self, velocity, angular): 93 | tw = TwistStamped() 94 | tw.twist.linear.x = velocity 95 | tw.twist.angular.z = angular 96 | return tw 97 | 98 | def create_steer(self, val): 99 | st = SteeringReport() 100 | st.steering_wheel_angle_cmd = val * math.pi/180. 101 | st.enabled = True 102 | st.speed = self.vel 103 | return st 104 | 105 | def calc_angular(self, yaw): 106 | angular_vel = 0. 107 | if self.yaw is not None: 108 | angular_vel = (yaw - self.yaw)/(rospy.get_time() - self.prev_time) 109 | self.yaw = yaw 110 | self.prev_time = rospy.get_time() 111 | return angular_vel 112 | 113 | def create_point_cloud_message(self, pts): 114 | header = Header() 115 | header.stamp = rospy.Time.now() 116 | header.frame_id = '/world' 117 | cloud_message = pcl2.create_cloud_xyz32(header, pts) 118 | return cloud_message 119 | 120 | def broadcast_transform(self, name, position, orientation): 121 | br = tf.TransformBroadcaster() 122 | br.sendTransform(position, 123 | orientation, 124 | rospy.Time.now(), 125 | name, 126 | "world") 127 | 128 | def publish_odometry(self, data): 129 | pose = self.create_pose(data['x'], data['y'], data['z'], data['yaw']) 130 | 131 | position = (data['x'], data['y'], data['z']) 132 | orientation = tf.transformations.quaternion_from_euler(0, 0, math.pi * data['yaw']/180.) 133 | self.broadcast_transform("base_link", position, orientation) 134 | 135 | self.publishers['current_pose'].publish(pose) 136 | self.vel = data['velocity']* 0.44704 137 | self.angular = self.calc_angular(data['yaw'] * math.pi/180.) 138 | self.publishers['current_velocity'].publish(self.create_twist(self.vel, self.angular)) 139 | 140 | 141 | def publish_controls(self, data): 142 | steering, throttle, brake = data['steering_angle'], data['throttle'], data['brake'] 143 | self.publishers['steering_report'].publish(self.create_steer(steering)) 144 | self.publishers['throttle_report'].publish(self.create_float(throttle)) 145 | self.publishers['brake_report'].publish(self.create_float(brake)) 146 | 147 | def publish_obstacles(self, data): 148 | for obs in data['obstacles']: 149 | pose = self.create_pose(obs[0], obs[1], obs[2]) 150 | self.publishers['obstacle'].publish(pose) 151 | header = Header() 152 | header.stamp = rospy.Time.now() 153 | header.frame_id = '/world' 154 | cloud = pcl2.create_cloud_xyz32(header, data['obstacles']) 155 | self.publishers['obstacle_points'].publish(cloud) 156 | 157 | def publish_lidar(self, data): 158 | self.publishers['lidar'].publish(self.create_point_cloud_message(zip(data['lidar_x'], data['lidar_y'], data['lidar_z']))) 159 | 160 | def publish_traffic(self, data): 161 | x, y, z = data['light_pos_x'], data['light_pos_y'], data['light_pos_z'], 162 | yaw = [math.atan2(dy, dx) for dx, dy in zip(data['light_pos_dx'], data['light_pos_dy'])] 163 | status = data['light_state'] 164 | 165 | lights = TrafficLightArray() 166 | header = Header() 167 | header.stamp = rospy.Time.now() 168 | header.frame_id = '/world' 169 | lights.lights = [self.create_light(*e) for e in zip(x, y, z, yaw, status)] 170 | self.publishers['trafficlights'].publish(lights) 171 | 172 | def publish_dbw_status(self, data): 173 | self.publishers['dbw_status'].publish(Bool(data)) 174 | 175 | def publish_camera(self, data): 176 | imgString = data["image"] 177 | image = PIL_Image.open(BytesIO(base64.b64decode(imgString))) 178 | image_array = np.asarray(image) 179 | 180 | image_message = self.bridge.cv2_to_imgmsg(image_array, encoding="rgb8") 181 | self.publishers['image'].publish(image_message) 182 | 183 | def callback_steering(self, data): 184 | self.server('steer', data={'steering_angle': str(data.steering_wheel_angle_cmd)}) 185 | 186 | def callback_throttle(self, data): 187 | self.server('throttle', data={'throttle': str(data.pedal_cmd)}) 188 | 189 | def callback_brake(self, data): 190 | self.server('brake', data={'brake': str(data.pedal_cmd)}) 191 | -------------------------------------------------------------------------------- /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/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/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/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/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/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/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/twist_controller/dbw_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from std_msgs.msg import Bool, Float64 5 | from dbw_mkz_msgs.msg import ThrottleCmd, SteeringCmd, BrakeCmd, SteeringReport 6 | from geometry_msgs.msg import TwistStamped 7 | from geometry_msgs.msg import PoseStamped 8 | import math 9 | 10 | from twist_controller import Controller 11 | from yaw_controller import YawController 12 | from lowpass import LowPassFilter 13 | from pid import PID 14 | 15 | ''' 16 | You can build this node only after you have built (or partially built) the `waypoint_updater` node. 17 | 18 | You will subscribe to `/twist_cmd` message which provides the proposed linear and angular velocities. 19 | You can subscribe to any other message that you find important or refer to the document for list 20 | of messages subscribed to by the reference implementation of this node. 21 | 22 | One thing to keep in mind while building this node and the `twist_controller` class is the status 23 | of `dbw_enabled`. While in the simulator, its enabled all the time, in the real car, that will 24 | not be the case. This may cause your PID controller to accumulate error because the car could 25 | temporarily be driven by a human instead of your controller. 26 | 27 | We have provided two launch files with this node. Vehicle specific values (like vehicle_mass, 28 | wheel_base) etc should not be altered in these files. 29 | 30 | We have also provided some reference implementations for PID controller and other utility classes. 31 | You are free to use them or build your own. 32 | 33 | Once you have the proposed throttle, brake, and steer values, publish it on the various publishers 34 | that we have created in the `__init__` function. 35 | 36 | ''' 37 | 38 | class DBWNode(object): 39 | def __init__(self): 40 | rospy.init_node('dbw_node') 41 | 42 | vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) 43 | fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) 44 | brake_deadband = rospy.get_param('~brake_deadband', .1) 45 | decel_limit = rospy.get_param('~decel_limit', -5) # 'brake' is a torque 46 | accel_limit = rospy.get_param('~accel_limit', 1.) # 'accel' is a percentage 47 | wheel_radius = rospy.get_param('~wheel_radius', 0.2413) 48 | wheel_base = rospy.get_param('~wheel_base', 2.8498) 49 | steer_ratio = rospy.get_param('~steer_ratio', 14.8) 50 | max_lat_accel = rospy.get_param('~max_lat_accel', 3.) 51 | max_steer_angle = rospy.get_param('~max_steer_angle', 8.) 52 | min_speed = rospy.get_param('~min_speed', 0.1) 53 | Kp = rospy.get_param('~pid_kp', 1.1) 54 | Ki = rospy.get_param('~pid_ki', 0.010) 55 | Kd = rospy.get_param('~pid_kd', 0.005) 56 | pid_cmd_range = rospy.get_param('~pid_cmd_range', 4) 57 | filter_tau = rospy.get_param('~filter_tau', 0.0) 58 | 59 | self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd', 60 | ThrottleCmd, queue_size=1) 61 | self.brake_pub = rospy.Publisher('/vehicle/brake_cmd', 62 | BrakeCmd, queue_size=1) 63 | self.steer_pub = rospy.Publisher('/vehicle/steering_cmd', 64 | SteeringCmd, queue_size=1) 65 | 66 | self.vx_pub = rospy.Publisher('/debug_vx', Float64, queue_size=1) 67 | self.sz_pub = rospy.Publisher('/debug_sz', Float64, queue_size=1) 68 | self.vxd_pub = rospy.Publisher('/debug_vxd', Float64, queue_size=1) 69 | self.szd_pub = rospy.Publisher('/debug_szd', Float64, queue_size=1) 70 | 71 | # def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle): 72 | self.yaw_controller = YawController(wheel_base, steer_ratio, 73 | min_speed, max_lat_accel, 74 | max_steer_angle) 75 | 76 | # def __init__(self, tau, ts): 77 | self.filter = LowPassFilter(filter_tau, 0.8) # hm why 0.8? 78 | 79 | # Pid controller for the target velocity (decel_limit is already negative) 80 | self.pid_vel = PID(Kp, Ki, Kd, decel_limit, accel_limit) 81 | 82 | # write controller 83 | self.controller = Controller(self.yaw_controller, self.pid_vel, self.filter) 84 | 85 | # set controller parameters 86 | vehicle_mass_offset = 25.0 + 70.0 + 30.0 # additional weight (gas, passenger, load) 87 | self.controller.set_vehicle_parameters(vehicle_mass, vehicle_mass_offset, brake_deadband, wheel_radius) 88 | 89 | self.current_velocity = None 90 | self.target_velocity = None 91 | self.dbw_enabled = True # maybe change to False by default 92 | self.pose = None 93 | 94 | rospy.Subscriber("/current_velocity", TwistStamped, self.velocity_cb) 95 | rospy.Subscriber("/twist_cmd", TwistStamped, self.twist_cb) 96 | rospy.Subscriber("/vehicle/dbw_enabled", Bool, self.dbw_cb) 97 | rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb) 98 | 99 | self.loop() 100 | 101 | def velocity_cb(self, msg): 102 | self.current_velocity = msg.twist 103 | self.vx_pub.publish(self.current_velocity.linear.x) 104 | self.sz_pub.publish(self.current_velocity.angular.z) 105 | 106 | def twist_cb(self, msg): 107 | self.target_velocity = msg.twist 108 | self.vxd_pub.publish(self.target_velocity.linear.x) 109 | self.szd_pub.publish(self.target_velocity.angular.z) 110 | 111 | def dbw_cb(self, msg): 112 | self.dbw_enabled = msg.data 113 | rospy.loginfo('received dbw_enabled: %s', str(msg.data)) 114 | 115 | def pose_cb(self, msg): 116 | self.pose = msg.pose 117 | 118 | def loop(self): 119 | rate = rospy.Rate(50) # 50Hz 120 | while not rospy.is_shutdown(): 121 | # TODO: Get predicted throttle, brake, and steering using `twist_controller` 122 | # You should only publish the control commands if dbw is enabled 123 | # throttle, brake, steering = self.controller.control(, 124 | # , 125 | # , 126 | # , 127 | # ) 128 | 129 | if not self.current_velocity: 130 | rospy.logwarn('no current velocity has been set') 131 | rate.sleep() 132 | continue 133 | 134 | if not self.target_velocity: 135 | rospy.logwarn('no target velocity has been set') 136 | rate.sleep() 137 | continue 138 | 139 | if not self.pose: 140 | rospy.logwarn('no pose has been set') 141 | rate.sleep() 142 | continue 143 | 144 | if not self.dbw_enabled: 145 | rospy.logdebug('no driving by wire') 146 | #Reset the PID controller 147 | self.controller.reset() 148 | rate.sleep() 149 | continue 150 | 151 | throttle, brake, steer = self.controller.control( 152 | current_velocity=self.current_velocity.linear.x, 153 | linear_velocity=self.target_velocity.linear.x, 154 | angular_velocity=self.target_velocity.angular.z 155 | ) 156 | 157 | self.publish(throttle, brake, steer) 158 | rate.sleep() 159 | 160 | def publish(self, throttle, brake, steer): 161 | tcmd = ThrottleCmd() 162 | tcmd.enable = True 163 | tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT 164 | tcmd.pedal_cmd = throttle 165 | self.throttle_pub.publish(tcmd) 166 | 167 | scmd = SteeringCmd() 168 | scmd.enable = True 169 | scmd.steering_wheel_angle_cmd = steer 170 | self.steer_pub.publish(scmd) 171 | 172 | bcmd = BrakeCmd() 173 | bcmd.enable = True 174 | bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE 175 | bcmd.pedal_cmd = brake 176 | self.brake_pub.publish(bcmd) 177 | 178 | 179 | if __name__ == '__main__': 180 | DBWNode() 181 | -------------------------------------------------------------------------------- /ros/src/tl_detector/light_classification/tl_classifier.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import h5py 3 | import rospy 4 | import keras.backend as K 5 | import numpy as np 6 | import matplotlib.pyplot as plt 7 | 8 | from styx_msgs.msg import TrafficLight 9 | 10 | import keras 11 | from keras.models import Model 12 | from keras.preprocessing.image import img_to_array 13 | from keras.layers import Input, Activation, Concatenate 14 | from keras.layers import Flatten, Dropout 15 | from keras.layers import MaxPooling2D 16 | from keras.layers.convolutional import Conv2D 17 | from keras.layers import AveragePooling2D 18 | 19 | import keras.backend as K 20 | 21 | K.set_image_dim_ordering('tf') 22 | 23 | 24 | def SqueezeNet(nb_classes=3, inputs=(224, 224, 3)): 25 | """ Keras Implementation of SqueezeNet(arXiv 1602.07360) 26 | @param nb_classes: total number of final categories 27 | Arguments: 28 | inputs -- shape of the input images (channel, cols, rows) 29 | """ 30 | 31 | input_img = Input(shape=inputs) 32 | conv1 = Conv2D( 33 | 96, (7, 7), activation='relu', kernel_initializer='glorot_uniform', 34 | strides=(2, 2), padding='same', name='conv1')(input_img) 35 | maxpool1 = MaxPooling2D( 36 | pool_size=(3, 3), strides=(2, 2), name='maxpool1')(conv1) 37 | 38 | fire2_squeeze = Conv2D( 39 | 16, (1, 1), activation='relu', kernel_initializer='glorot_uniform', 40 | padding='same', name='fire2_squeeze')(maxpool1) 41 | fire2_expand1 = Conv2D( 42 | 64, (1, 1), activation='relu', kernel_initializer='glorot_uniform', 43 | padding='same', name='fire2_expand1')(fire2_squeeze) 44 | fire2_expand2 = Conv2D( 45 | 64, (3, 3), activation='relu', kernel_initializer='glorot_uniform', 46 | padding='same', name='fire2_expand2')(fire2_squeeze) 47 | merge2 = Concatenate()([fire2_expand1, fire2_expand2]) 48 | 49 | fire3_squeeze = Conv2D( 50 | 16, (1, 1), activation='relu', kernel_initializer='glorot_uniform', 51 | padding='same', name='fire3_squeeze')(merge2) 52 | fire3_expand1 = Conv2D( 53 | 64, (1, 1), activation='relu', kernel_initializer='glorot_uniform', 54 | padding='same', name='fire3_expand1')(fire3_squeeze) 55 | fire3_expand2 = Conv2D( 56 | 64, (3, 3), activation='relu', kernel_initializer='glorot_uniform', 57 | padding='same', name='fire3_expand2')(fire3_squeeze) 58 | merge3 = Concatenate()([fire3_expand1, fire3_expand2]) 59 | 60 | fire4_squeeze = Conv2D( 61 | 32, (1, 1), activation='relu', kernel_initializer='glorot_uniform', 62 | padding='same', name='fire4_squeeze')(merge3) 63 | fire4_expand1 = Conv2D( 64 | 128, (1, 1), activation='relu', kernel_initializer='glorot_uniform', 65 | padding='same', name='fire4_expand1')(fire4_squeeze) 66 | fire4_expand2 = Conv2D( 67 | 128, (3, 3), activation='relu', kernel_initializer='glorot_uniform', 68 | padding='same', name='fire4_expand2')(fire4_squeeze) 69 | merge4 = Concatenate()([fire4_expand1, fire4_expand2]) 70 | 71 | maxpool4 = MaxPooling2D( 72 | pool_size=(3, 3), strides=(2, 2), name='maxpool4')(merge4) 73 | 74 | fire5_squeeze = Conv2D( 75 | 32, (1, 1), activation='relu', kernel_initializer='glorot_uniform', 76 | padding='same', name='fire5_squeeze')(maxpool4) 77 | fire5_expand1 = Conv2D( 78 | 128, (1, 1), activation='relu', kernel_initializer='glorot_uniform', 79 | padding='same', name='fire5_expand1')(fire5_squeeze) 80 | fire5_expand2 = Conv2D( 81 | 128, (3, 3), activation='relu', kernel_initializer='glorot_uniform', 82 | padding='same', name='fire5_expand2')(fire5_squeeze) 83 | merge5 = Concatenate()([fire5_expand1, fire5_expand2]) 84 | 85 | fire6_squeeze = Conv2D( 86 | 48, (1, 1), activation='relu', kernel_initializer='glorot_uniform', 87 | padding='same', name='fire6_squeeze')(merge5) 88 | fire6_expand1 = Conv2D( 89 | 192, (1, 1), activation='relu', kernel_initializer='glorot_uniform', 90 | padding='same', name='fire6_expand1')(fire6_squeeze) 91 | fire6_expand2 = Conv2D( 92 | 192, (3, 3), activation='relu', kernel_initializer='glorot_uniform', 93 | padding='same', name='fire6_expand2')(fire6_squeeze) 94 | merge6 = Concatenate()([fire6_expand1, fire6_expand2]) 95 | 96 | fire7_squeeze = Conv2D( 97 | 48, (1, 1), activation='relu', kernel_initializer='glorot_uniform', 98 | padding='same', name='fire7_squeeze')(merge6) 99 | fire7_expand1 = Conv2D( 100 | 192, (1, 1), activation='relu', kernel_initializer='glorot_uniform', 101 | padding='same', name='fire7_expand1')(fire7_squeeze) 102 | fire7_expand2 = Conv2D( 103 | 192, (3, 3), activation='relu', kernel_initializer='glorot_uniform', 104 | padding='same', name='fire7_expand2')(fire7_squeeze) 105 | merge7 = Concatenate()([fire7_expand1, fire7_expand2]) 106 | 107 | fire8_squeeze = Conv2D( 108 | 64, (1, 1), activation='relu', kernel_initializer='glorot_uniform', 109 | padding='same', name='fire8_squeeze')(merge7) 110 | fire8_expand1 = Conv2D( 111 | 256, (1, 1), activation='relu', kernel_initializer='glorot_uniform', 112 | padding='same', name='fire8_expand1')(fire8_squeeze) 113 | fire8_expand2 = Conv2D( 114 | 256, (3, 3), activation='relu', kernel_initializer='glorot_uniform', 115 | padding='same', name='fire8_expand2')(fire8_squeeze) 116 | merge8 = Concatenate()([fire8_expand1, fire8_expand2]) 117 | 118 | maxpool8 = MaxPooling2D( 119 | pool_size=(3, 3), strides=(2, 2), name='maxpool8')(merge8) 120 | 121 | fire9_squeeze = Conv2D( 122 | 64, (1, 1), activation='relu', kernel_initializer='glorot_uniform', 123 | padding='same', name='fire9_squeeze')(maxpool8) 124 | fire9_expand1 = Conv2D( 125 | 256, (1, 1), activation='relu', kernel_initializer='glorot_uniform', 126 | padding='same', name='fire9_expand1')(fire9_squeeze) 127 | fire9_expand2 = Conv2D( 128 | 256, (3, 3), activation='relu', kernel_initializer='glorot_uniform', 129 | padding='same', name='fire9_expand2')(fire9_squeeze) 130 | merge9 = Concatenate()([fire9_expand1, fire9_expand2]) 131 | 132 | fire9_dropout = Dropout(0.5, name='fire9_dropout')(merge9) 133 | conv10 = Conv2D( 134 | nb_classes, (1, 1), kernel_initializer='glorot_uniform', 135 | padding='valid', name='conv10')(fire9_dropout) 136 | 137 | # The size should match the output of conv10 138 | avgpool10 = AveragePooling2D((13, 13), name='avgpool10')(conv10) 139 | flatten = Flatten(name='flatten')(avgpool10) 140 | softmax = Activation("softmax", name='softmax')(flatten) 141 | 142 | return Model(inputs=input_img, outputs=softmax) 143 | 144 | 145 | 146 | class TLClassifier(object): 147 | def __init__(self): 148 | self.model = SqueezeNet() 149 | self.model.load_weights("light_classification/weights/01.weights") 150 | self.model._make_predict_function() 151 | rospy.logdebug("Model loaded.") 152 | 153 | def get_classification(self, image): 154 | """Determines the color of the traffic light in the image 155 | 156 | Args: 157 | image (cv::Mat): image containing the traffic light 158 | 159 | Returns: 160 | int: ID of traffic light color (specified in styx_msgs/TrafficLight) 161 | 162 | """ 163 | d = cv2.cvtColor(image, cv2.COLOR_RGB2BGR) 164 | 165 | image = img_to_array(image) 166 | image /= 255.0 167 | image = np.expand_dims(image, axis=0) 168 | preds = self.model.predict(image)[0] 169 | prob_no, prob_red, prob_green = preds 170 | np.set_printoptions(precision=3, suppress=True) 171 | rospy.logdebug("no, red, green {}".format(preds)) 172 | # neural network 173 | # 0:= No traffic light in driving direction 174 | # 1:= Red traffic light in driving direction 175 | # 2:= Green traffic light in driving direction 176 | 177 | # data type 178 | # uint8 RED=0 179 | # uint8 YELLOW=1 180 | # uint8 GREEN=2 181 | # uint8 UNKNOWN=4 182 | 183 | strn = 'RED' 184 | state = TrafficLight.RED 185 | if prob_no > 0.90: 186 | strn = 'UNKNOWN' 187 | state = TrafficLight.UNKNOWN 188 | if prob_green > 0.90: 189 | strn = 'GREEN' 190 | state = TrafficLight.GREEN 191 | if prob_red > 0.20: 192 | strn = 'RED' 193 | state = TrafficLight.RED 194 | 195 | #cv2.putText(d, strn, (10, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255)) 196 | #cv2.imshow("Camera stream", d) 197 | #cv2.waitKey(1) 198 | 199 | return state 200 | -------------------------------------------------------------------------------- /ros/src/waypoint_updater/waypoint_updater.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from geometry_msgs.msg import PoseStamped 5 | from geometry_msgs.msg import TwistStamped 6 | from geometry_msgs.msg import Pose 7 | from std_msgs.msg import Int32 8 | from styx_msgs.msg import Lane, Waypoint 9 | from operator import itemgetter 10 | 11 | import tf 12 | import math 13 | 14 | ''' 15 | This node will publish waypoints from the car's current position to some `x` distance ahead. 16 | 17 | As mentioned in the doc, you should ideally first implement a version which does not care 18 | about traffic lights or obstacles. 19 | 20 | Once you have created dbw_node, you will update this node to use the status of traffic lights too. 21 | 22 | Please note that our simulator also provides the exact location of traffic lights and their 23 | current status in `/vehicle/traffic_lights` message. You can use this message to build this node 24 | as well as to verify your TL classifier. 25 | ''' 26 | 27 | LOOKAHEAD_WPS = 200 # Number of waypoints we will publish. You can change this number 28 | ONEMPH = 0.44704 # in mps 29 | 30 | class WaypointUpdater(object): 31 | def __init__(self): 32 | rospy.init_node('waypoint_updater') 33 | 34 | rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb) 35 | rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb) 36 | rospy.Subscriber("/current_velocity", TwistStamped, self.velocity_cb) 37 | rospy.Subscriber('/traffic_waypoint', Int32, self.traffic_cb) 38 | rospy.Subscriber('/obstacle_waypoint', PoseStamped, self.obstacle_cb) 39 | 40 | self.final_waypoints_pub = rospy.Publisher('final_waypoints', Lane, queue_size=1) 41 | 42 | # Current pose of the vehicle updated at an unknown rate 43 | self.pose = None 44 | 45 | # Part of the complete waypoints retrived from /waypoints 46 | self.waypoints = None 47 | 48 | #Velocity 49 | self.current_velocity = None 50 | 51 | # Select the base waypoints ahead of the vehicle 52 | self.waypoints_ahead = [] 53 | 54 | #Id of the first waypoint ahead 55 | self.next_waypoint_id = None 56 | 57 | # id of red light waypoint 58 | self.red_waypoint = -1 59 | 60 | # Cycle time for the waypoint updater 61 | self.dt = 0.1 62 | 63 | #Maximum allowed velocity 64 | self.max_vel = 10.0*ONEMPH 65 | 66 | # Minimum distance to the waypoint ahead 67 | self.min_distance_ahead = self.max_vel*self.dt 68 | 69 | #Distance to traffic light waypoint 70 | self.stop_distance_to_tl = 7.0 71 | 72 | #Distance from the stop point where the car starts to decelerate 73 | self.decelerating_distance = self.max_vel*10 74 | 75 | rospy.Timer(rospy.Duration(self.dt), self.process_final_waypoints) 76 | rospy.spin() 77 | 78 | def process_final_waypoints(self, event): 79 | 80 | if not self.pose: 81 | rospy.logwarn('no pose has been set') 82 | return None 83 | 84 | if not self.current_velocity: 85 | rospy.logwarn('no velocity have been set') 86 | return None 87 | 88 | if not self.waypoints: 89 | rospy.logwarn('no waypoints have been set') 90 | return None 91 | 92 | 93 | # get closest waypoint 94 | idx_begin = self.get_closest_waypoint_ahead() 95 | idx_end = idx_begin + LOOKAHEAD_WPS 96 | idx_end = min(idx_end, len(self.waypoints)) 97 | rospy.logdebug("begin {}, end {}, len {}".format(idx_begin, idx_end, len(self.waypoints))) 98 | 99 | wps = [] 100 | epsilon = 1.0 101 | dist_to_red = 0 102 | dist_to_stop = 0 103 | velocity = 0 104 | ptv = 0 105 | for i in range(idx_begin, idx_end): 106 | target_velocity = self.max_vel 107 | 108 | if self.red_waypoint > 0: 109 | dist_to_red = self.distance(i, self.red_waypoint) 110 | dist_to_stop = max(0, dist_to_red - self.stop_distance_to_tl) 111 | velocity = self.get_waypoint_velocity(i) 112 | 113 | # slow down getting closer to intersection 114 | if dist_to_stop > 0 and dist_to_stop < self.decelerating_distance: 115 | target_velocity *= dist_to_red/self.decelerating_distance 116 | 117 | # push down the brakes a bit harder second half 118 | if dist_to_stop > 0 and dist_to_stop < self.decelerating_distance * .5: 119 | target_velocity *= dist_to_stop/self.decelerating_distance 120 | 121 | # don't slow down so it doesn't make sense 122 | target_velocity = max(2., target_velocity) 123 | 124 | # perform the brake motion 125 | if dist_to_stop > 0.5 and dist_to_stop <= 2.: 126 | target_velocity = 1.0 127 | if dist_to_stop > 0 and dist_to_stop <= 0.5: 128 | target_velocity = 0.33 129 | elif dist_to_stop == 0: 130 | target_velocity = 0. 131 | 132 | # accelerate smoothly 133 | swv = self.get_waypoint_velocity(idx_begin) 134 | ptv = swv if i == idx_begin else ptv 135 | cwv = self.get_waypoint_velocity(i) 136 | 137 | rospy.logdebug("{} = start v {}, current v {}, prev v {}, target v {}".format(i, swv, cwv, ptv, target_velocity)) 138 | if swv == 0 and cwv == 0 and ptv == 0: 139 | target_velocity = (0.25 * target_velocity + 0.75 * ptv) 140 | elif ptv < target_velocity: 141 | target_velocity = (0.1 * target_velocity + 0.9 * ptv) 142 | 143 | # make higher velocities just be the highest 144 | if target_velocity > 4.0: 145 | target_velocity = self.max_vel 146 | 147 | # clip around the velocities always greater than min and lesser than max 148 | target_velocity = min(max(0, target_velocity), self.max_vel) 149 | 150 | # save previous waypoint target velocity 151 | ptv = target_velocity 152 | 153 | 154 | rospy.logdebug("waypoint {}, decelerating_dist {}, dist_to_red {}, " 155 | "dist_to_stop {}, velocity {}, target_velocity {}".format( 156 | i, 157 | self.decelerating_distance, 158 | dist_to_red, 159 | dist_to_stop, 160 | velocity, 161 | target_velocity)) 162 | 163 | self.set_waypoint_velocity(i, target_velocity) 164 | wps.append(self.waypoints[i]) 165 | 166 | lane = Lane() 167 | lane.waypoints = wps 168 | self.final_waypoints_pub.publish(lane) 169 | 170 | dr = self.distance(idx_begin, self.red_waypoint) 171 | ds = max(0, dr - self.stop_distance_to_tl) 172 | vd = self.get_waypoint_velocity(idx_begin) 173 | rospy.logdebug("[idx %s ] tl %s ds %s dr %s vd %s ", idx_begin, self.red_waypoint, ds, dr, vd) 174 | 175 | def get_closest_waypoint_ahead(self): 176 | 177 | # get the pose of the vehicle 178 | cx = self.pose.position.x 179 | cy = self.pose.position.y 180 | co = self.pose.orientation 181 | cq = (co.x, co.y, co.z, co.w) 182 | _, _, ct = tf.transformations.euler_from_quaternion(cq) 183 | 184 | closest_wp = (float('inf'), -1) 185 | for wp_i in range(len(self.waypoints)): 186 | wx = self.waypoints[wp_i].pose.pose.position.x 187 | wy = self.waypoints[wp_i].pose.pose.position.y 188 | wa = ((wx - cx) * math.cos(ct) + 189 | (wy - cy) * math.sin(ct)) > self.min_distance_ahead 190 | 191 | if not wa: 192 | continue 193 | 194 | dist = math.sqrt((cx - wx)**2 + (cy - wy)**2) 195 | if dist < closest_wp[0]: 196 | closest_wp = (dist, wp_i) 197 | 198 | return closest_wp[1] 199 | 200 | def pose_cb(self, msg): 201 | self.pose = msg.pose 202 | 203 | def waypoints_cb(self, lane): 204 | self.waypoints = lane.waypoints 205 | 206 | def traffic_cb(self, msg): 207 | self.red_waypoint = msg.data 208 | 209 | def obstacle_cb(self, msg): 210 | # TODO: callback for /obstacle_waypoint message. 211 | pass 212 | 213 | def velocity_cb(self, msg): 214 | self.current_velocity = msg.twist 215 | 216 | def get_waypoint_velocity(self, waypoint): 217 | return self.waypoints[waypoint].twist.twist.linear.x 218 | 219 | def set_waypoint_velocity(self, waypoint, velocity): 220 | self.waypoints[waypoint].twist.twist.linear.x = velocity 221 | 222 | def distance(self, wp1, wp2): 223 | dist = 0 224 | dl = lambda a, b: math.sqrt((a.x-b.x)**2 + (a.y-b.y)**2 + (a.z-b.z)**2) 225 | for i in range(wp1, wp2+1): 226 | dist += dl(self.waypoints[wp1].pose.pose.position, 227 | self.waypoints[i].pose.pose.position) 228 | wp1 = i 229 | return dist 230 | 231 | def distance_from_waypoint(self, waypoint): 232 | """ 233 | Compute the euclidian distance of the waipoint 234 | from the current pose of the vehicle 235 | """ 236 | xw = waypoint.pose.pose.position.x 237 | yw = waypoint.pose.pose.position.y 238 | zw = waypoint.pose.pose.position.z 239 | 240 | xc = self.pose.position.x 241 | yc = self.pose.position.y 242 | zc = self.pose.position.z 243 | 244 | return math.sqrt((xw - xc)*(xw - xc) + 245 | (yw - yc)*(yw - yc) + 246 | (zw - zc)*(zw - zc)) 247 | 248 | 249 | 250 | if __name__ == '__main__': 251 | try: 252 | WaypointUpdater() 253 | except rospy.ROSInterruptException: 254 | rospy.logerr('Could not start waypoint updater node.') 255 | -------------------------------------------------------------------------------- /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/tl_detector/tl_detector.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from std_msgs.msg import Int32 4 | from geometry_msgs.msg import PoseStamped, Pose 5 | from styx_msgs.msg import TrafficLightArray, TrafficLight 6 | from styx_msgs.msg import Lane 7 | from sensor_msgs.msg import Image 8 | from cv_bridge import CvBridge 9 | from light_classification.tl_classifier import TLClassifier 10 | import tf 11 | import cv2 12 | import yaml 13 | import math 14 | import numpy as np 15 | from scipy.misc import imresize 16 | import Queue 17 | 18 | STATE_COUNT_THRESHOLD = 2 19 | 20 | class TLDetector(object): 21 | def __init__(self): 22 | rospy.init_node('tl_detector') 23 | 24 | self.pose = None 25 | self.waypoints = None 26 | self.camera_image = None 27 | self.lights = [] 28 | 29 | # /current_pose can be used to determine the vehicle's location 30 | rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb) 31 | 32 | # /base_waypoints provides a complete list of waypoints for the course 33 | rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb) 34 | 35 | # /image_color provides the image stream from the car's camera 36 | rospy.Subscriber('/image_color', Image, self.image_cb) 37 | 38 | # /traffic_waypoint is the topic to publish to. It expects the index of the 39 | # waypoint for nearest upcoming red light 40 | self.redl_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1) 41 | 42 | ''' 43 | /vehicle/traffic_lights provides you with the location of the traffic light in 3D map space and 44 | helps you acquire an accurate ground truth data source for the traffic light 45 | classifier by sending the current color state of all traffic lights in the 46 | simulator. When testing on the vehicle, the color state will not be available. You'll need to 47 | rely on the position of the light and the camera image to predict it. 48 | ''' 49 | self.gt_sub = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_cb) 50 | self.image_queue = Queue.Queue() 51 | 52 | config_string = rospy.get_param("/traffic_light_config") 53 | self.config = yaml.load(config_string) 54 | 55 | self.bridge = CvBridge() 56 | self.listener = tf.TransformListener() 57 | 58 | self.last_state = TrafficLight.UNKNOWN 59 | self.last_wp = -1 60 | self.state_count = 0 61 | self.light_classifier = None 62 | self.light_classifier = TLClassifier() 63 | 64 | self.loop() 65 | 66 | def pose_cb(self, msg): 67 | self.pose = msg.pose 68 | 69 | def waypoints_cb(self, lane): 70 | self.waypoints = lane.waypoints 71 | 72 | def traffic_cb(self, msg): 73 | self.lights = msg.lights 74 | 75 | def image_cb(self, msg): 76 | """Identifies red lights in the incoming camera image and publishes the index 77 | of the waypoint closest to the red light's stop line to /traffic_waypoint 78 | 79 | Args: 80 | msg (Image): image from car-mounted camera 81 | 82 | """ 83 | while not self.image_queue.empty(): 84 | try: 85 | self.image_queue.get() 86 | except: 87 | pass 88 | self.image_queue.put(msg) 89 | 90 | def get_closest_waypoint(self, pose): 91 | """Identifies the closest path waypoint to the given position 92 | https://en.wikipedia.org/wiki/Closest_pair_of_points_problem 93 | Args: 94 | pose (Pose): position to match a waypoint to 95 | 96 | Returns: 97 | (float, int): tuple of the distance and index of 98 | closest waypoint in self.waypoints 99 | """ 100 | x = pose.position.x 101 | y = pose.position.y 102 | 103 | closest_wp = (float('inf'), -1) 104 | for wp_i in range(len(self.waypoints)): 105 | wx = self.waypoints[wp_i].pose.pose.position.x 106 | wy = self.waypoints[wp_i].pose.pose.position.y 107 | dist = math.sqrt((x - wx)**2 + (y - wy)**2) 108 | if dist < closest_wp[0]: 109 | closest_wp = (dist, wp_i) 110 | 111 | rospy.logdebug("Closest waypoint distance is {}, " 112 | "which is waypoint {}".format(closest_wp[0], closest_wp[1])) 113 | 114 | return closest_wp[1] 115 | 116 | def get_closest_stop_waypoint(self, position): 117 | """Identifies the closest path waypoint to the given position 118 | https://en.wikipedia.org/wiki/Closest_pair_of_points_problem 119 | Args: 120 | pose (Pose): position to match a waypoint to 121 | 122 | Returns: 123 | (float, int): tuple of the distance and index of 124 | closest waypoint in self.waypoints 125 | """ 126 | x = position[0] 127 | y = position[1] 128 | 129 | closest_wp = (float('inf'), -1) 130 | for wp_i in range(len(self.waypoints)): 131 | wx = self.waypoints[wp_i].pose.pose.position.x 132 | wy = self.waypoints[wp_i].pose.pose.position.y 133 | dist = math.sqrt((x - wx)**2 + (y - wy)**2) 134 | if dist < closest_wp[0]: 135 | closest_wp = (dist, wp_i) 136 | 137 | rospy.logdebug("Closest waypoint distance is {}, " 138 | "which is waypoint {}".format(closest_wp[0], closest_wp[1])) 139 | return closest_wp[1] 140 | 141 | def project_to_image_plane(self, point_in_world): 142 | """Project point from 3D world coordinates to 2D camera image location 143 | 144 | Args: 145 | point_in_world (Point): 3D location of a point in the world 146 | 147 | Returns: 148 | x (int): x coordinate of target point in image 149 | y (int): y coordinate of target point in image 150 | 151 | """ 152 | 153 | fx = self.config['camera_info']['focal_length_x'] 154 | fy = self.config['camera_info']['focal_length_y'] 155 | image_width = self.config['camera_info']['image_width'] 156 | image_height = self.config['camera_info']['image_height'] 157 | 158 | # get transform between pose of camera and world frame 159 | trans = None 160 | try: 161 | now = rospy.Time.now() 162 | self.listener.waitForTransform("/base_link", 163 | "/world", now, rospy.Duration(1.0)) 164 | (trans, rot) = self.listener.lookupTransform("/base_link", 165 | "/world", now) 166 | 167 | except (tf.Exception, tf.LookupException, tf.ConnectivityException): 168 | rospy.logerr("Failed to find camera to map transform") 169 | return (0, 0) 170 | 171 | # Use tranform and rotation to calculate 2D position of light in image 172 | trans_m = self.listener.fromTranslationRotation(trans, rot) 173 | pt_world = np.array([[point_in_world.x], 174 | [point_in_world.y], 175 | [point_in_world.z], 176 | [1.0]]) 177 | cam_vec = np.dot(trans_m, pt_world) 178 | x = cam_vec[0][0] 179 | y = cam_vec[1][0] 180 | return (x, y) 181 | 182 | def get_light_state(self, light): 183 | """Determines the current color of the traffic light 184 | 185 | Args: 186 | light (TrafficLight): light to classify 187 | 188 | Returns: 189 | int: ID of traffic light color (specified in styx_msgs/TrafficLight) 190 | 191 | """ 192 | if not light: 193 | self.prev_light_loc = None 194 | rospy.logdebug("No light was passed") 195 | return TrafficLight.UNKNOWN 196 | 197 | # get image from msg into cv2 198 | try: 199 | # get rgb! cv handles different format 200 | cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8") 201 | except: 202 | pass 203 | 204 | #TODO: use light location to zoom in on traffic light in image 205 | x, y = self.project_to_image_plane(light.pose.pose.position) 206 | #rospy.logerr("Project to image plane returned x={}, y={}".format(x, y)) 207 | 208 | # Get classification 209 | image = imresize(cv_image, (224, 224, 3)) 210 | return self.light_classifier.get_classification(image) 211 | 212 | def loop(self): 213 | rate = rospy.Rate(3) 214 | while not rospy.is_shutdown(): 215 | try: 216 | self.camera_image = self.image_queue.get() 217 | except: 218 | rospy.logerr("couldn't get camera image") 219 | rate.sleep() 220 | continue 221 | 222 | if not self.camera_image: 223 | rospy.logerr("camera image was unexpectedly None") 224 | rate.sleep() 225 | continue 226 | 227 | light_wp, state = self.process_traffic_lights() 228 | rospy.logdebug("waypoint {}, state {}".format(light_wp, state)) 229 | 230 | if state != self.last_state: 231 | self.state_count = 0 232 | self.last_state = state 233 | self.redl_pub.publish(Int32(self.last_wp)) 234 | elif self.state_count < STATE_COUNT_THRESHOLD: 235 | self.state_count += 1 236 | self.redl_pub.publish(Int32(self.last_wp)) 237 | else: 238 | light_wp = light_wp if state == TrafficLight.RED else -1 239 | self.last_wp = light_wp 240 | self.redl_pub.publish(Int32(light_wp)) 241 | 242 | self.camera_image = None 243 | rate.sleep() 244 | 245 | 246 | def process_traffic_lights(self): 247 | """Finds closest visible traffic light, if one exists, and determines its 248 | location and color 249 | 250 | Returns: 251 | int: index of waypoint closes to the upcoming stop line for a traffic light (-1 if none exists) 252 | int: ID of traffic light color (specified in styx_msgs/TrafficLight) 253 | 254 | """ 255 | # prevent from running function if values are not set 256 | idx, tl_state = -1, TrafficLight.UNKNOWN 257 | if not self.pose: 258 | rospy.logwarn('no pose has been set') 259 | return idx, tl_state 260 | 261 | if not self.waypoints: 262 | rospy.logwarn('no waypoints have been set') 263 | return idx, tl_state 264 | 265 | if not self.lights: 266 | rospy.logwarn('no lights have been set') 267 | return idx, tl_state 268 | 269 | if not self.light_classifier: 270 | rospy.logwarn('no classifier initialized') 271 | return idx, tl_state 272 | 273 | # get current position and yaw of the car 274 | c_wp = self.get_closest_waypoint(self.pose) 275 | c_x = self.pose.position.x 276 | c_y = self.pose.position.y 277 | c_o = self.pose.orientation 278 | c_q = (c_o.x, c_o.y, c_o.z, c_o.w) 279 | _, _, c_w = tf.transformations.euler_from_quaternion(c_q) 280 | 281 | # find closest light ahead 282 | l_wp = (float('inf'), -1, None) 283 | for i in range(len(self.lights)): 284 | l = self.lights[i] 285 | l_x = l.pose.pose.position.x 286 | l_y = l.pose.pose.position.y 287 | l_o = l.pose.pose.orientation 288 | l_q = (l_o.x, l_o.y, l_o.z, l_o.w) 289 | _, _, l_w = tf.transformations.euler_from_quaternion(l_q) 290 | 291 | # determine if light is ahead 292 | l_ahead = ((l_x - c_x) * math.cos(c_w) + 293 | (l_y - c_y) * math.sin(c_w)) > 0 294 | if not l_ahead: 295 | rospy.logdebug("light not ahead " + str(self.get_closest_waypoint(l.pose.pose))) 296 | continue 297 | rospy.logdebug("light ahead " + str(self.get_closest_waypoint(l.pose.pose))) 298 | 299 | # determine if light is facing car 300 | l_facing_car = l_w * c_w > 0 301 | if not l_facing_car: 302 | rospy.logdebug("light not facing " + str(self.get_closest_waypoint(l.pose.pose))) 303 | continue 304 | rospy.logdebug("light facing " + str(self.get_closest_waypoint(l.pose.pose))) 305 | 306 | # calculate distance and store if closer than current 307 | l_d = math.sqrt((c_x - l_x)**2 + (c_y - l_y)**2) 308 | rospy.logdebug("Store light {} with distance {} and position {}, {}".format(i, l_d, l_x, l_y)) 309 | if l_d < l_wp[0]: 310 | l_wp = l_d, self.get_closest_waypoint(l.pose.pose), l 311 | 312 | # waypoints that correspond to the stopping line positions 313 | stop_line_positions = self.config['stop_line_positions'] 314 | rospy.logdebug("stop line positions ({})".format(stop_line_positions)) 315 | s_wp = (float('inf'), -1) 316 | for stop_line in stop_line_positions: 317 | s_x = stop_line[0] 318 | s_y = stop_line[1] 319 | s_d = math.sqrt((c_x - s_x)**2 + (c_y - s_y)**2) 320 | if s_d < s_wp[0]: 321 | s_wp = (s_d, self.get_closest_stop_waypoint(stop_line)) 322 | 323 | # don't classify lights that are just too far away 324 | rospy.logdebug("closest light is {} far and is {} waypoint".format(l_wp[0], l_wp[1])) 325 | rospy.logdebug("closest stop line is {} far and is {} waypoint".format(s_wp[0], s_wp[1])) 326 | if l_wp[0] > 50: 327 | return idx, tl_state 328 | 329 | idx = s_wp[1] 330 | tl_state = self.get_light_state(l_wp[2]) 331 | rospy.logdebug("light state is " + str(tl_state)) 332 | 333 | return idx, tl_state 334 | 335 | if __name__ == '__main__': 336 | try: 337 | TLDetector() 338 | except rospy.ROSInterruptException: 339 | rospy.logerr('Could not start traffic node.') 340 | -------------------------------------------------------------------------------- /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_ERROR_STREAM("wp = " << i << " dist = " << getPlaneDistance(current_waypoints_.getWaypointPosition(i), current_pose_.pose.position) ); 301 | return; 302 | } 303 | } 304 | 305 | // if this program reaches here , it means we lost the waypoint! 306 | num_of_next_waypoint_ = -1; 307 | return; 308 | } 309 | 310 | geometry_msgs::TwistStamped PurePursuit::outputZero() const 311 | { 312 | geometry_msgs::TwistStamped twist; 313 | twist.twist.linear.x = 0; 314 | twist.twist.angular.z = 0; 315 | twist.header.stamp = ros::Time::now(); 316 | return twist; 317 | } 318 | geometry_msgs::TwistStamped PurePursuit::outputTwist(geometry_msgs::Twist t) const 319 | { 320 | double g_lateral_accel_limit = 5.0; 321 | double ERROR = 1e-8; 322 | 323 | geometry_msgs::TwistStamped twist; 324 | twist.twist = t; 325 | twist.header.stamp = ros::Time::now(); 326 | 327 | double v = t.linear.x; 328 | double omega = t.angular.z; 329 | 330 | if(fabs(omega) < ERROR){ 331 | 332 | return twist; 333 | } 334 | 335 | double max_v = g_lateral_accel_limit / omega; 336 | 337 | 338 | double a = v * omega; 339 | ROS_INFO("lateral accel = %lf", a); 340 | 341 | twist.twist.linear.x = fabs(a) > g_lateral_accel_limit ? max_v 342 | : v; 343 | twist.twist.angular.z = omega; 344 | 345 | return twist; 346 | } 347 | 348 | geometry_msgs::TwistStamped PurePursuit::go() 349 | { 350 | if(!pose_set_ || !waypoint_set_ || !velocity_set_){ 351 | if(!pose_set_) { 352 | ROS_WARN("position is missing"); 353 | } 354 | if(!waypoint_set_) { 355 | ROS_WARN("waypoint is missing"); 356 | } 357 | if(!velocity_set_) { 358 | ROS_WARN("velocity is missing"); 359 | } 360 | return outputZero(); 361 | } 362 | 363 | bool interpolate_flag = false; 364 | 365 | calcLookaheadDistance(1); 366 | // search next waypoint 367 | getNextWaypoint(); 368 | if (num_of_next_waypoint_ == -1) 369 | { 370 | ROS_WARN("lost next waypoint"); 371 | return outputZero(); 372 | } 373 | //ROS_ERROR_STREAM("next waypoint = " << num_of_next_waypoint_); 374 | 375 | // if g_linear_interpolate_mode is false or next waypoint is first or last 376 | if (!linear_interpolate_ || num_of_next_waypoint_ == 0 || 377 | num_of_next_waypoint_ == (static_cast(current_waypoints_.getSize() - 1))) 378 | { 379 | position_of_next_target_ = current_waypoints_.getWaypointPosition(num_of_next_waypoint_); 380 | return outputTwist(calcTwist(calcCurvature(position_of_next_target_), getCmdVelocity(0))); 381 | } 382 | 383 | // linear interpolation and calculate angular velocity 384 | interpolate_flag = interpolateNextTarget(num_of_next_waypoint_, &position_of_next_target_); 385 | 386 | if (!interpolate_flag) 387 | { 388 | ROS_ERROR_STREAM("lost target! "); 389 | return outputZero(); 390 | } 391 | 392 | // ROS_INFO("next_target : ( %lf , %lf , %lf)", next_target.x, next_target.y,next_target.z); 393 | 394 | return outputTwist(calcTwist(calcCurvature(position_of_next_target_), getCmdVelocity(0))); 395 | 396 | // ROS_INFO("linear : %lf, angular : %lf",twist.twist.linear.x,twist.twist.angular.z); 397 | 398 | #ifdef LOG 399 | std::ofstream ofs("/tmp/pure_pursuit.log", std::ios::app); 400 | ofs << _current_waypoints.getWaypointPosition(next_waypoint).x << " " 401 | << _current_waypoints.getWaypointPosition(next_waypoint).y << " " << next_target.x << " " << next_target.y 402 | << std::endl; 403 | #endif 404 | } 405 | } 406 | --------------------------------------------------------------------------------