├── Import └── agents │ ├── __init__.py │ ├── tools │ ├── __init__.py │ └── misc.py │ ├── navigation │ ├── __init__.py │ ├── __pycache__ │ │ ├── __init__.cpython-38.pyc │ │ ├── basic_agent.cpython-38.pyc │ │ └── behavior_agent.cpython-38.pyc │ ├── behavior_types.py │ ├── controller.py │ ├── basic_agent.py │ ├── local_planner.py │ ├── behavior_agent.py │ └── global_route_planner.py │ └── __pycache__ │ └── __init__.cpython-38.pyc ├── demo.png ├── Town07.txt ├── Route_Town07.txt ├── LICENSE ├── readme.md ├── capture_multiple_sensors.py ├── route_capture_multiple_sensors.py └── manual_control.py /Import/agents/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /Import/agents/tools/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /Import/agents/navigation/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zxhou/Capda_Carla/HEAD/demo.png -------------------------------------------------------------------------------- /Import/agents/__pycache__/__init__.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zxhou/Capda_Carla/HEAD/Import/agents/__pycache__/__init__.cpython-38.pyc -------------------------------------------------------------------------------- /Import/agents/navigation/__pycache__/__init__.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zxhou/Capda_Carla/HEAD/Import/agents/navigation/__pycache__/__init__.cpython-38.pyc -------------------------------------------------------------------------------- /Import/agents/navigation/__pycache__/basic_agent.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zxhou/Capda_Carla/HEAD/Import/agents/navigation/__pycache__/basic_agent.cpython-38.pyc -------------------------------------------------------------------------------- /Import/agents/navigation/__pycache__/behavior_agent.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zxhou/Capda_Carla/HEAD/Import/agents/navigation/__pycache__/behavior_agent.cpython-38.pyc -------------------------------------------------------------------------------- /Town07.txt: -------------------------------------------------------------------------------- 1 | Location.x, Location.y, Location.z, Rotation.pitch, Rotation.yaw, Rotation.roll, target.x, target.y, target.z 2 | -41.825108 -6.604221 0.600000 0.000000 -90.161217 0.000000 -18.385923 130.210068 0.600000 -------------------------------------------------------------------------------- /Route_Town07.txt: -------------------------------------------------------------------------------- 1 | Location.x, Location.y, Location.z, Rotation.pitch, Rotation.yaw, Rotation.roll, target.x, target.y, target.z 2 | -41.825108 -6.604221 0.600000 0.000000 -90.161217 0.000000 -18.385923 130.210068 0.600000 -------------------------------------------------------------------------------- /Import/agents/navigation/behavior_types.py: -------------------------------------------------------------------------------- 1 | # This work is licensed under the terms of the MIT license. 2 | # For a copy, see . 3 | 4 | """ This module contains the different parameters sets for each behavior. """ 5 | 6 | 7 | class Cautious(object): 8 | """Class for Cautious agent.""" 9 | max_speed = 40 10 | speed_lim_dist = 6 11 | speed_decrease = 12 12 | safety_time = 3 13 | min_proximity_threshold = 12 14 | braking_distance = 6 15 | tailgate_counter = 0 16 | 17 | 18 | class Normal(object): 19 | """Class for Normal agent.""" 20 | max_speed = 50 21 | speed_lim_dist = 3 22 | speed_decrease = 10 23 | safety_time = 3 24 | min_proximity_threshold = 10 25 | braking_distance = 5 26 | tailgate_counter = 0 27 | 28 | 29 | class Aggressive(object): 30 | """Class for Aggressive agent.""" 31 | max_speed = 70 32 | speed_lim_dist = 1 33 | speed_decrease = 8 34 | safety_time = 3 35 | min_proximity_threshold = 8 36 | braking_distance = 4 37 | tailgate_counter = -1 38 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Zhixing Hou 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | ### Code for Capturing multiple sensor data on Carla platform 2 | ___ 3 | 4 | We provide an example code to capture data for multiple sensors in synchronous mode on [Carla](https://carla.org/) platform. 5 | 6 | CARLA is an open-source autonomous driving simulator. The simulation platform supports flexible specification of sensor suites, environmental conditions, full control of all static and dynamic actors, maps generation and much more. 7 | 8 | ![](./demo.png) 9 | 10 | #### Features 11 | * route than can be adjusted 12 | * synchronous mode 13 | * lidar, camera, gnss and IMU 14 | * save the data to the disk 15 | * display mode 16 | * follow ego vehicle in bird's-eye view 17 | * ignore the traffic lights 18 | 19 | 20 | #### Testing system 21 | * Intel i7 gen 9th 22 | * 16 GB RAM memory 23 | * NVIDIA RTX 2080Ti 24 | * Ubuntu 20.04 25 | 26 | #### Dependencies 27 | 1. CarlaUE 4.26 28 | 2. Carla simulator 0.9.12 29 | 3. Numpy 30 | 4. OpenCV 31 | 5. matplotlib 32 | 6. Pillow 33 | 34 | #### Run 35 | * fixed route, you can change the route in the route file (e.g.Route_Town07.txt) 36 | ``` 37 | python3 route_capture_multiple_sensors.py --fix_route 38 | ``` 39 | * random route 40 | ``` 41 | python3 route_capture_multiple_sensors.py 42 | ``` 43 | * random roam on the map 44 | ``` 45 | python3 capture_multiple_sensors.py 46 | ``` 47 | 48 | -------------------------------------------------------------------------------- /Import/agents/tools/misc.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2018 Intel Labs. 4 | # authors: German Ros (german.ros@intel.com) 5 | # 6 | # This work is licensed under the terms of the MIT license. 7 | # For a copy, see . 8 | 9 | """ Module with auxiliary functions. """ 10 | 11 | import math 12 | import numpy as np 13 | import carla 14 | 15 | def draw_waypoints(world, waypoints, z=0.5): 16 | """ 17 | Draw a list of waypoints at a certain height given in z. 18 | 19 | :param world: carla.world object 20 | :param waypoints: list or iterable container with the waypoints to draw 21 | :param z: height in meters 22 | """ 23 | for wpt in waypoints: 24 | wpt_t = wpt.transform 25 | begin = wpt_t.location + carla.Location(z=z) 26 | angle = math.radians(wpt_t.rotation.yaw) 27 | end = begin + carla.Location(x=math.cos(angle), y=math.sin(angle)) 28 | world.debug.draw_arrow(begin, end, arrow_size=0.3, life_time=1.0) 29 | 30 | 31 | def get_speed(vehicle): 32 | """ 33 | Compute speed of a vehicle in Km/h. 34 | 35 | :param vehicle: the vehicle for which speed is calculated 36 | :return: speed as a float in Km/h 37 | """ 38 | vel = vehicle.get_velocity() 39 | 40 | return 3.6 * math.sqrt(vel.x ** 2 + vel.y ** 2 + vel.z ** 2) 41 | 42 | def get_trafficlight_trigger_location(traffic_light): 43 | """ 44 | Calculates the yaw of the waypoint that represents the trigger volume of the traffic light 45 | """ 46 | def rotate_point(point, radians): 47 | """ 48 | rotate a given point by a given angle 49 | """ 50 | rotated_x = math.cos(radians) * point.x - math.sin(radians) * point.y 51 | rotated_y = math.sin(radians) * point.x - math.cos(radians) * point.y 52 | 53 | return carla.Vector3D(rotated_x, rotated_y, point.z) 54 | 55 | base_transform = traffic_light.get_transform() 56 | base_rot = base_transform.rotation.yaw 57 | area_loc = base_transform.transform(traffic_light.trigger_volume.location) 58 | area_ext = traffic_light.trigger_volume.extent 59 | 60 | point = rotate_point(carla.Vector3D(0, 0, area_ext.z), math.radians(base_rot)) 61 | point_location = area_loc + carla.Location(x=point.x, y=point.y) 62 | 63 | return carla.Location(point_location.x, point_location.y, point_location.z) 64 | 65 | 66 | def is_within_distance(target_transform, reference_transform, max_distance, angle_interval=None): 67 | """ 68 | Check if a location is both within a certain distance from a reference object. 69 | By using 'angle_interval', the angle between the location and reference transform 70 | will also be tkaen into account, being 0 a location in front and 180, one behind. 71 | 72 | :param target_transform: location of the target object 73 | :param reference_transform: location of the reference object 74 | :param max_distance: maximum allowed distance 75 | :param angle_interval: only locations between [min, max] angles will be considered. This isn't checked by default. 76 | :return: boolean 77 | """ 78 | target_vector = np.array([ 79 | target_transform.location.x - reference_transform.location.x, 80 | target_transform.location.y - reference_transform.location.y 81 | ]) 82 | norm_target = np.linalg.norm(target_vector) 83 | 84 | # If the vector is too short, we can simply stop here 85 | if norm_target < 0.001: 86 | return True 87 | 88 | # Further than the max distance 89 | if norm_target > max_distance: 90 | return False 91 | 92 | # We don't care about the angle, nothing else to check 93 | if not angle_interval: 94 | return True 95 | 96 | min_angle = angle_interval[0] 97 | max_angle = angle_interval[1] 98 | 99 | fwd = reference_transform.get_forward_vector() 100 | forward_vector = np.array([fwd.x, fwd.y]) 101 | angle = math.degrees(math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1., 1.))) 102 | 103 | return min_angle < angle < max_angle 104 | 105 | 106 | def compute_magnitude_angle(target_location, current_location, orientation): 107 | """ 108 | Compute relative angle and distance between a target_location and a current_location 109 | 110 | :param target_location: location of the target object 111 | :param current_location: location of the reference object 112 | :param orientation: orientation of the reference object 113 | :return: a tuple composed by the distance to the object and the angle between both objects 114 | """ 115 | target_vector = np.array([target_location.x - current_location.x, target_location.y - current_location.y]) 116 | norm_target = np.linalg.norm(target_vector) 117 | 118 | forward_vector = np.array([math.cos(math.radians(orientation)), math.sin(math.radians(orientation))]) 119 | d_angle = math.degrees(math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1., 1.))) 120 | 121 | return (norm_target, d_angle) 122 | 123 | 124 | def distance_vehicle(waypoint, vehicle_transform): 125 | """ 126 | Returns the 2D distance from a waypoint to a vehicle 127 | 128 | :param waypoint: actual waypoint 129 | :param vehicle_transform: transform of the target vehicle 130 | """ 131 | loc = vehicle_transform.location 132 | x = waypoint.transform.location.x - loc.x 133 | y = waypoint.transform.location.y - loc.y 134 | 135 | return math.sqrt(x * x + y * y) 136 | 137 | 138 | def vector(location_1, location_2): 139 | """ 140 | Returns the unit vector from location_1 to location_2 141 | 142 | :param location_1, location_2: carla.Location objects 143 | """ 144 | x = location_2.x - location_1.x 145 | y = location_2.y - location_1.y 146 | z = location_2.z - location_1.z 147 | norm = np.linalg.norm([x, y, z]) + np.finfo(float).eps 148 | 149 | return [x / norm, y / norm, z / norm] 150 | 151 | 152 | def compute_distance(location_1, location_2): 153 | """ 154 | Euclidean distance between 3D points 155 | 156 | :param location_1, location_2: 3D points 157 | """ 158 | x = location_2.x - location_1.x 159 | y = location_2.y - location_1.y 160 | z = location_2.z - location_1.z 161 | norm = np.linalg.norm([x, y, z]) + np.finfo(float).eps 162 | return norm 163 | 164 | 165 | def positive(num): 166 | """ 167 | Return the given number if positive, else 0 168 | 169 | :param num: value to check 170 | """ 171 | return num if num > 0.0 else 0.0 172 | -------------------------------------------------------------------------------- /Import/agents/navigation/controller.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) # Copyright (c) 2018-2020 CVC. 2 | # 3 | # This work is licensed under the terms of the MIT license. 4 | # For a copy, see . 5 | 6 | """ This module contains PID controllers to perform lateral and longitudinal control. """ 7 | 8 | from collections import deque 9 | import math 10 | import numpy as np 11 | import carla 12 | from agents.tools.misc import get_speed 13 | 14 | 15 | class VehiclePIDController(): 16 | """ 17 | VehiclePIDController is the combination of two PID controllers 18 | (lateral and longitudinal) to perform the 19 | low level control a vehicle from client side 20 | """ 21 | 22 | 23 | def __init__(self, vehicle, args_lateral, args_longitudinal, offset=0, max_throttle=0.75, max_brake=0.3, 24 | max_steering=0.8): 25 | """ 26 | Constructor method. 27 | 28 | :param vehicle: actor to apply to local planner logic onto 29 | :param args_lateral: dictionary of arguments to set the lateral PID controller 30 | using the following semantics: 31 | K_P -- Proportional term 32 | K_D -- Differential term 33 | K_I -- Integral term 34 | :param args_longitudinal: dictionary of arguments to set the longitudinal 35 | PID controller using the following semantics: 36 | K_P -- Proportional term 37 | K_D -- Differential term 38 | K_I -- Integral term 39 | :param offset: If different than zero, the vehicle will drive displaced from the center line. 40 | Positive values imply a right offset while negative ones mean a left one. Numbers high enough 41 | to cause the vehicle to drive through other lanes might break the controller. 42 | """ 43 | 44 | self.max_brake = max_brake 45 | self.max_throt = max_throttle 46 | self.max_steer = max_steering 47 | 48 | self._vehicle = vehicle 49 | self._world = self._vehicle.get_world() 50 | self.past_steering = self._vehicle.get_control().steer 51 | self._lon_controller = PIDLongitudinalController(self._vehicle, **args_longitudinal) 52 | self._lat_controller = PIDLateralController(self._vehicle, offset, **args_lateral) 53 | 54 | def run_step(self, target_speed, waypoint): 55 | """ 56 | Execute one step of control invoking both lateral and longitudinal 57 | PID controllers to reach a target waypoint 58 | at a given target_speed. 59 | 60 | :param target_speed: desired vehicle speed 61 | :param waypoint: target location encoded as a waypoint 62 | :return: distance (in meters) to the waypoint 63 | """ 64 | 65 | acceleration = self._lon_controller.run_step(target_speed) 66 | current_steering = self._lat_controller.run_step(waypoint) 67 | control = carla.VehicleControl() 68 | if acceleration >= 0.0: 69 | control.throttle = min(acceleration, self.max_throt) 70 | control.brake = 0.0 71 | else: 72 | control.throttle = 0.0 73 | control.brake = min(abs(acceleration), self.max_brake) 74 | 75 | # Steering regulation: changes cannot happen abruptly, can't steer too much. 76 | 77 | if current_steering > self.past_steering + 0.1: 78 | current_steering = self.past_steering + 0.1 79 | elif current_steering < self.past_steering - 0.1: 80 | current_steering = self.past_steering - 0.1 81 | 82 | if current_steering >= 0: 83 | steering = min(self.max_steer, current_steering) 84 | else: 85 | steering = max(-self.max_steer, current_steering) 86 | 87 | control.steer = steering 88 | control.hand_brake = False 89 | control.manual_gear_shift = False 90 | self.past_steering = steering 91 | 92 | return control 93 | 94 | 95 | def change_longitudinal_PID(self, args_longitudinal): 96 | """Changes the parameters of the PIDLongitudinalController""" 97 | self._lon_controller.change_parameters(**args_longitudinal) 98 | 99 | def change_lateral_PID(self, args_lateral): 100 | """Changes the parameters of the PIDLongitudinalController""" 101 | self._lon_controller.change_parameters(**args_lateral) 102 | 103 | 104 | class PIDLongitudinalController(): 105 | """ 106 | PIDLongitudinalController implements longitudinal control using a PID. 107 | """ 108 | 109 | def __init__(self, vehicle, K_P=1.0, K_I=0.0, K_D=0.0, dt=0.03): 110 | """ 111 | Constructor method. 112 | 113 | :param vehicle: actor to apply to local planner logic onto 114 | :param K_P: Proportional term 115 | :param K_D: Differential term 116 | :param K_I: Integral term 117 | :param dt: time differential in seconds 118 | """ 119 | self._vehicle = vehicle 120 | self._k_p = K_P 121 | self._k_i = K_I 122 | self._k_d = K_D 123 | self._dt = dt 124 | self._error_buffer = deque(maxlen=10) 125 | 126 | def run_step(self, target_speed, debug=False): 127 | """ 128 | Execute one step of longitudinal control to reach a given target speed. 129 | 130 | :param target_speed: target speed in Km/h 131 | :param debug: boolean for debugging 132 | :return: throttle control 133 | """ 134 | current_speed = get_speed(self._vehicle) 135 | 136 | if debug: 137 | print('Current speed = {}'.format(current_speed)) 138 | 139 | return self._pid_control(target_speed, current_speed) 140 | 141 | def _pid_control(self, target_speed, current_speed): 142 | """ 143 | Estimate the throttle/brake of the vehicle based on the PID equations 144 | 145 | :param target_speed: target speed in Km/h 146 | :param current_speed: current speed of the vehicle in Km/h 147 | :return: throttle/brake control 148 | """ 149 | 150 | error = target_speed - current_speed 151 | self._error_buffer.append(error) 152 | 153 | if len(self._error_buffer) >= 2: 154 | _de = (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt 155 | _ie = sum(self._error_buffer) * self._dt 156 | else: 157 | _de = 0.0 158 | _ie = 0.0 159 | 160 | return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) 161 | 162 | def change_parameters(self, K_P, K_I, K_D, dt): 163 | """Changes the PID parameters""" 164 | self._k_p = K_P 165 | self._k_i = K_I 166 | self._k_d = K_D 167 | self._dt = dt 168 | 169 | 170 | class PIDLateralController(): 171 | """ 172 | PIDLateralController implements lateral control using a PID. 173 | """ 174 | 175 | def __init__(self, vehicle, offset=0, K_P=1.0, K_I=0.0, K_D=0.0, dt=0.03): 176 | """ 177 | Constructor method. 178 | 179 | :param vehicle: actor to apply to local planner logic onto 180 | :param offset: distance to the center line. If might cause issues if the value 181 | is large enough to make the vehicle invade other lanes. 182 | :param K_P: Proportional term 183 | :param K_D: Differential term 184 | :param K_I: Integral term 185 | :param dt: time differential in seconds 186 | """ 187 | self._vehicle = vehicle 188 | self._k_p = K_P 189 | self._k_i = K_I 190 | self._k_d = K_D 191 | self._dt = dt 192 | self._offset = offset 193 | self._e_buffer = deque(maxlen=10) 194 | 195 | def run_step(self, waypoint): 196 | """ 197 | Execute one step of lateral control to steer 198 | the vehicle towards a certain waypoin. 199 | 200 | :param waypoint: target waypoint 201 | :return: steering control in the range [-1, 1] where: 202 | -1 maximum steering to left 203 | +1 maximum steering to right 204 | """ 205 | return self._pid_control(waypoint, self._vehicle.get_transform()) 206 | 207 | def _pid_control(self, waypoint, vehicle_transform): 208 | """ 209 | Estimate the steering angle of the vehicle based on the PID equations 210 | 211 | :param waypoint: target waypoint 212 | :param vehicle_transform: current transform of the vehicle 213 | :return: steering control in the range [-1, 1] 214 | """ 215 | # Get the ego's location and forward vector 216 | ego_loc = vehicle_transform.location 217 | v_vec = vehicle_transform.get_forward_vector() 218 | v_vec = np.array([v_vec.x, v_vec.y, 0.0]) 219 | 220 | # Get the vector vehicle-target_wp 221 | if self._offset != 0: 222 | # Displace the wp to the side 223 | w_tran = waypoint.transform 224 | r_vec = w_tran.get_right_vector() 225 | w_loc = w_tran.location + carla.Location(x=self._offset*r_vec.x, 226 | y=self._offset*r_vec.y) 227 | else: 228 | w_loc = waypoint.transform.location 229 | 230 | w_vec = np.array([w_loc.x - ego_loc.x, 231 | w_loc.y - ego_loc.y, 232 | 0.0]) 233 | 234 | wv_linalg = np.linalg.norm(w_vec) * np.linalg.norm(v_vec) 235 | if wv_linalg == 0: 236 | _dot = 1 237 | else: 238 | _dot = math.acos(np.clip(np.dot(w_vec, v_vec) / (wv_linalg), -1.0, 1.0)) 239 | _cross = np.cross(v_vec, w_vec) 240 | if _cross[2] < 0: 241 | _dot *= -1.0 242 | 243 | self._e_buffer.append(_dot) 244 | if len(self._e_buffer) >= 2: 245 | _de = (self._e_buffer[-1] - self._e_buffer[-2]) / self._dt 246 | _ie = sum(self._e_buffer) * self._dt 247 | else: 248 | _de = 0.0 249 | _ie = 0.0 250 | 251 | return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) 252 | 253 | def change_parameters(self, K_P, K_I, K_D, dt): 254 | """Changes the PID parameters""" 255 | self._k_p = K_P 256 | self._k_i = K_I 257 | self._k_d = K_D 258 | self._dt = dt -------------------------------------------------------------------------------- /Import/agents/navigation/basic_agent.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) # Copyright (c) 2018-2020 CVC. 2 | # 3 | # This work is licensed under the terms of the MIT license. 4 | # For a copy, see . 5 | 6 | """ 7 | This module implements an agent that roams around a track following random 8 | waypoints and avoiding other vehicles. The agent also responds to traffic lights. 9 | It can also make use of the global route planner to follow a specifed route 10 | """ 11 | 12 | import carla 13 | from enum import Enum 14 | 15 | from agents.navigation.local_planner import LocalPlanner 16 | from agents.navigation.global_route_planner import GlobalRoutePlanner 17 | from agents.tools.misc import get_speed, is_within_distance, get_trafficlight_trigger_location 18 | 19 | 20 | class BasicAgent(object): 21 | """ 22 | BasicAgent implements an agent that navigates the scene. 23 | This agent respects traffic lights and other vehicles, but ignores stop signs. 24 | It has several functions available to specify the route that the agent must follow, 25 | as well as to change its parameters in case a different driving mode is desired. 26 | """ 27 | 28 | def __init__(self, vehicle, target_speed=20, opt_dict={}): 29 | """ 30 | Initialization the agent paramters, the local and the global planner. 31 | 32 | :param vehicle: actor to apply to agent logic onto 33 | :param target_speed: speed (in Km/h) at which the vehicle will move 34 | :param opt_dict: dictionary in case some of its parameters want to be changed. 35 | This also applies to parameters related to the LocalPlanner. 36 | """ 37 | self._vehicle = vehicle 38 | self._world = self._vehicle.get_world() 39 | self._map = self._world.get_map() 40 | self._last_traffic_light = None 41 | 42 | # Base parameters 43 | self._ignore_traffic_lights = False 44 | self._ignore_stop_signs = False 45 | self._ignore_vehicles = False 46 | self._target_speed = target_speed 47 | self._sampling_resolution = 2.0 48 | self._base_tlight_threshold = 5.0 # meters 49 | self._base_vehicle_threshold = 5.0 # meters 50 | self._max_brake = 0.5 51 | 52 | # Change parameters according to the dictionary 53 | opt_dict['target_speed'] = target_speed 54 | if 'ignore_traffic_lights' in opt_dict: 55 | self._ignore_traffic_lights = opt_dict['ignore_traffic_lights'] 56 | if 'ignore_stop_signs' in opt_dict: 57 | self._ignore_stop_signs = opt_dict['ignore_stop_signs'] 58 | if 'ignore_vehicles' in opt_dict: 59 | self._ignore_vehicles = opt_dict['ignore_vehicles'] 60 | if 'sampling_resolution' in opt_dict: 61 | self._sampling_resolution = opt_dict['sampling_resolution'] 62 | if 'base_tlight_threshold' in opt_dict: 63 | self._base_tlight_threshold = opt_dict['base_tlight_threshold'] 64 | if 'base_vehicle_threshold' in opt_dict: 65 | self._base_vehicle_threshold = opt_dict['base_vehicle_threshold'] 66 | if 'max_brake' in opt_dict: 67 | self._max_steering = opt_dict['max_brake'] 68 | 69 | # Initialize the planners 70 | self._local_planner = LocalPlanner(self._vehicle, opt_dict=opt_dict) 71 | self._global_planner = GlobalRoutePlanner(self._map, self._sampling_resolution) 72 | 73 | def add_emergency_stop(self, control): 74 | """ 75 | Overwrites the throttle a brake values of a control to perform an emergency stop. 76 | The steering is kept the same to avoid going out of the lane when stopping during turns 77 | 78 | :param speed (carl.VehicleControl): control to be modified 79 | """ 80 | control.throttle = 0.0 81 | control.brake = self._max_brake 82 | control.hand_brake = False 83 | return control 84 | 85 | def set_target_speed(self, speed): 86 | """ 87 | Changes the target speed of the agent 88 | :param speed (float): target speed in Km/h 89 | """ 90 | self._local_planner.set_speed(speed) 91 | 92 | def follow_speed_limits(self, value=True): 93 | """ 94 | If active, the agent will dynamically change the target speed according to the speed limits 95 | 96 | :param value (bool): whether or not to activate this behavior 97 | """ 98 | self._local_planner.follow_speed_limits(value) 99 | 100 | def get_local_planner(self): 101 | """Get method for protected member local planner""" 102 | return self._local_planner 103 | 104 | def get_global_planner(self): 105 | """Get method for protected member local planner""" 106 | return self._global_planner 107 | 108 | def set_destination(self, end_location, start_location=None): 109 | """ 110 | This method creates a list of waypoints between a starting and ending location, 111 | based on the route returned by the global router, and adds it to the local planner. 112 | If no starting location is passed, the vehicle local planner's target location is chosen, 113 | which corresponds (by default), to a location about 5 meters in front of the vehicle. 114 | 115 | :param end_location (carla.Location): final location of the route 116 | :param start_location (carla.Location): starting location of the route 117 | """ 118 | if not start_location: 119 | start_location = self._local_planner.target_waypoint.transform.location 120 | clean_queue = True 121 | else: 122 | start_location = self._vehicle.get_location() 123 | clean_queue = False 124 | 125 | start_waypoint = self._map.get_waypoint(start_location) 126 | end_waypoint = self._map.get_waypoint(end_location) 127 | 128 | route_trace = self.trace_route(start_waypoint, end_waypoint) 129 | self._local_planner.set_global_plan(route_trace, clean_queue=clean_queue) 130 | 131 | def set_global_plan(self, plan, stop_waypoint_creation=True, clean_queue=True): 132 | """ 133 | Adds a specific plan to the agent. 134 | 135 | :param plan: list of [carla.Waypoint, RoadOption] representing the route to be followed 136 | :param stop_waypoint_creation: stops the automatic random creation of waypoints 137 | :param clean_queue: resets the current agent's plan 138 | """ 139 | self._local_planner.set_global_plan( 140 | plan, 141 | stop_waypoint_creation=stop_waypoint_creation, 142 | clean_queue=clean_queue 143 | ) 144 | 145 | def trace_route(self, start_waypoint, end_waypoint): 146 | """ 147 | Calculates the shortest route between a starting and ending waypoint. 148 | 149 | :param start_waypoint (carla.Waypoint): initial waypoint 150 | :param end_waypoint (carla.Waypoint): final waypoint 151 | """ 152 | start_location = start_waypoint.transform.location 153 | end_location = end_waypoint.transform.location 154 | return self._global_planner.trace_route(start_location, end_location) 155 | 156 | def run_step(self): 157 | """Execute one step of navigation.""" 158 | hazard_detected = False 159 | 160 | # Retrieve all relevant actors 161 | actor_list = self._world.get_actors() 162 | vehicle_list = actor_list.filter("*vehicle*") 163 | lights_list = actor_list.filter("*traffic_light*") 164 | 165 | vehicle_speed = get_speed(self._vehicle) / 3.6 166 | 167 | # Check for possible vehicle obstacles 168 | max_vehicle_distance = self._base_vehicle_threshold + vehicle_speed 169 | affected_by_vehicle, _ = self._vehicle_obstacle_detected(vehicle_list, max_vehicle_distance) 170 | if affected_by_vehicle: 171 | hazard_detected = True 172 | 173 | # Check if the vehicle is affected by a red traffic light 174 | max_tlight_distance = self._base_tlight_threshold + vehicle_speed 175 | affected_by_tlight, _ = self._affected_by_traffic_light(lights_list, max_tlight_distance) 176 | if affected_by_tlight: 177 | hazard_detected = True 178 | 179 | control = self._local_planner.run_step() 180 | if hazard_detected: 181 | control = self.add_emergency_stop(control) 182 | 183 | return control 184 | 185 | def done(self): 186 | """Check whether the agent has reached its destination.""" 187 | return self._local_planner.done() 188 | 189 | def ignore_traffic_lights(self, active=True): 190 | """(De)activates the checks for traffic lights""" 191 | self._ignore_traffic_lights = active 192 | 193 | def ignore_stop_signs(self, active=True): 194 | """(De)activates the checks for stop signs""" 195 | self._ignore_stop_signs = active 196 | 197 | def ignore_vehicles(self, active=True): 198 | """(De)activates the checks for stop signs""" 199 | self._ignore_vehicles = active 200 | 201 | def _affected_by_traffic_light(self, lights_list=None, max_distance=None): 202 | """ 203 | Method to check if there is a red light affecting the vehicle. 204 | 205 | :param lights_list (list of carla.TrafficLight): list containing TrafficLight objects. 206 | If None, all traffic lights in the scene are used 207 | :param max_distance (float): max distance for traffic lights to be considered relevant. 208 | If None, the base threshold value is used 209 | """ 210 | if self._ignore_traffic_lights: 211 | return (False, None) 212 | 213 | if not lights_list: 214 | lights_list = self._world.get_actors().filter("*traffic_light*") 215 | 216 | if not max_distance: 217 | max_distance = self._base_tlight_threshold 218 | 219 | if self._last_traffic_light: 220 | if self._last_traffic_light.state != carla.TrafficLightState.Red: 221 | self._last_traffic_light = None 222 | else: 223 | return (True, self._last_traffic_light) 224 | 225 | ego_vehicle_location = self._vehicle.get_location() 226 | ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location) 227 | 228 | for traffic_light in lights_list: 229 | object_location = get_trafficlight_trigger_location(traffic_light) 230 | object_waypoint = self._map.get_waypoint(object_location) 231 | 232 | if object_waypoint.road_id != ego_vehicle_waypoint.road_id: 233 | continue 234 | 235 | ve_dir = ego_vehicle_waypoint.transform.get_forward_vector() 236 | wp_dir = object_waypoint.transform.get_forward_vector() 237 | dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z 238 | 239 | if dot_ve_wp < 0: 240 | continue 241 | 242 | if traffic_light.state != carla.TrafficLightState.Red: 243 | continue 244 | 245 | if is_within_distance(object_waypoint.transform, self._vehicle.get_transform(), max_distance, [0, 90]): 246 | self._last_traffic_light = traffic_light 247 | return (True, traffic_light) 248 | 249 | return (False, None) 250 | 251 | def _vehicle_obstacle_detected(self, vehicle_list=None, max_distance=None): 252 | """ 253 | Method to check if there is a vehicle in front of the agent blocking its path. 254 | 255 | :param vehicle_list (list of carla.Vehicle): list contatining vehicle objects. 256 | If None, all vehicle in the scene are used 257 | :param max_distance: max freespace to check for obstacles. 258 | If None, the base threshold value is used 259 | """ 260 | if self._ignore_vehicles: 261 | return (False, None) 262 | 263 | if not vehicle_list: 264 | vehicle_list = self._world.get_actors().filter("*vehicle*") 265 | 266 | if not max_distance: 267 | max_distance = self._base_vehicle_threshold 268 | 269 | ego_transform = self._vehicle.get_transform() 270 | ego_wpt = self._map.get_waypoint(self._vehicle.get_location()) 271 | 272 | # Get the transform of the front of the ego 273 | ego_forward_vector = ego_transform.get_forward_vector() 274 | ego_extent = self._vehicle.bounding_box.extent.x 275 | ego_front_transform = ego_transform 276 | ego_front_transform.location += carla.Location( 277 | x=ego_extent * ego_forward_vector.x, 278 | y=ego_extent * ego_forward_vector.y, 279 | ) 280 | 281 | for target_vehicle in vehicle_list: 282 | target_transform = target_vehicle.get_transform() 283 | target_wpt = self._map.get_waypoint(target_transform.location) 284 | if target_wpt.road_id != ego_wpt.road_id or target_wpt.lane_id != ego_wpt.lane_id: 285 | next_wpt = self._local_planner.get_incoming_waypoint_and_direction(steps=3)[0] 286 | if not next_wpt: 287 | continue 288 | if target_wpt.road_id != next_wpt.road_id or target_wpt.lane_id != next_wpt.lane_id: 289 | continue 290 | 291 | target_forward_vector = target_transform.get_forward_vector() 292 | target_extent = target_vehicle.bounding_box.extent.x 293 | target_rear_transform = target_transform 294 | target_rear_transform.location -= carla.Location( 295 | x=target_extent * target_forward_vector.x, 296 | y=target_extent * target_forward_vector.y, 297 | ) 298 | 299 | if is_within_distance(target_rear_transform, ego_front_transform, max_distance, [0, 90]): 300 | return (True, target_vehicle) 301 | return (False, None) 302 | -------------------------------------------------------------------------------- /Import/agents/navigation/local_planner.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) # Copyright (c) 2018-2020 CVC. 2 | # 3 | # This work is licensed under the terms of the MIT license. 4 | # For a copy, see . 5 | 6 | """ This module contains a local planner to perform low-level waypoint following based on PID controllers. """ 7 | 8 | from enum import Enum 9 | from collections import deque 10 | import random 11 | 12 | import carla 13 | from agents.navigation.controller import VehiclePIDController 14 | from agents.tools.misc import draw_waypoints, get_speed 15 | 16 | 17 | class RoadOption(Enum): 18 | """ 19 | RoadOption represents the possible topological configurations when moving from a segment of lane to other. 20 | 21 | """ 22 | VOID = -1 23 | LEFT = 1 24 | RIGHT = 2 25 | STRAIGHT = 3 26 | LANEFOLLOW = 4 27 | CHANGELANELEFT = 5 28 | CHANGELANERIGHT = 6 29 | 30 | 31 | class LocalPlanner(object): 32 | """ 33 | LocalPlanner implements the basic behavior of following a 34 | trajectory of waypoints that is generated on-the-fly. 35 | 36 | The low-level motion of the vehicle is computed by using two PID controllers, 37 | one is used for the lateral control and the other for the longitudinal control (cruise speed). 38 | 39 | When multiple paths are available (intersections) this local planner makes a random choice, 40 | unless a given global plan has already been specified. 41 | """ 42 | 43 | def __init__(self, vehicle, opt_dict={}): 44 | """ 45 | :param vehicle: actor to apply to local planner logic onto 46 | :param opt_dict: dictionary of arguments with different parameters: 47 | dt: time between simulation steps 48 | target_speed: desired cruise speed in Km/h 49 | sampling_radius: distance between the waypoints part of the plan 50 | lateral_control_dict: values of the lateral PID controller 51 | longitudinal_control_dict: values of the longitudinal PID controller 52 | max_throttle: maximum throttle applied to the vehicle 53 | max_brake: maximum brake applied to the vehicle 54 | max_steering: maximum steering applied to the vehicle 55 | offset: distance between the route waypoints and the center of the lane 56 | """ 57 | self._vehicle = vehicle 58 | self._world = self._vehicle.get_world() 59 | self._map = self._world.get_map() 60 | 61 | self._vehicle_controller = None 62 | self.target_waypoint = None 63 | self.target_road_option = None 64 | 65 | self._waypoints_queue = deque(maxlen=10000) 66 | self._min_waypoint_queue_length = 100 67 | self._stop_waypoint_creation = False 68 | 69 | # Base parameters 70 | self._dt = 1.0 / 20.0 71 | self._target_speed = 20.0 # Km/h 72 | self._sampling_radius = 2.0 73 | self._args_lateral_dict = {'K_P': 1.95, 'K_I': 0.05, 'K_D': 0.2, 'dt': self._dt} 74 | self._args_longitudinal_dict = {'K_P': 1.0, 'K_I': 0.05, 'K_D': 0, 'dt': self._dt} 75 | self._max_throt = 0.75 76 | self._max_brake = 0.3 77 | self._max_steer = 0.8 78 | self._offset = 0 79 | self._base_min_distance = 3.0 80 | self._follow_speed_limits = False 81 | 82 | # Overload parameters 83 | if opt_dict: 84 | if 'dt' in opt_dict: 85 | self._dt = opt_dict['dt'] 86 | if 'target_speed' in opt_dict: 87 | self._target_speed = opt_dict['target_speed'] 88 | if 'sampling_radius' in opt_dict: 89 | self._sampling_radius = opt_dict['sampling_radius'] 90 | if 'lateral_control_dict' in opt_dict: 91 | self._args_lateral_dict = opt_dict['lateral_control_dict'] 92 | if 'longitudinal_control_dict' in opt_dict: 93 | self._args_longitudinal_dict = opt_dict['longitudinal_control_dict'] 94 | if 'max_throttle' in opt_dict: 95 | self._max_throt = opt_dict['max_throttle'] 96 | if 'max_brake' in opt_dict: 97 | self._max_brake = opt_dict['max_brake'] 98 | if 'max_steering' in opt_dict: 99 | self._max_steer = opt_dict['max_steering'] 100 | if 'offset' in opt_dict: 101 | self._offset = opt_dict['offset'] 102 | if 'base_min_distance' in opt_dict: 103 | self._base_min_distance = opt_dict['base_min_distance'] 104 | if 'follow_speed_limits' in opt_dict: 105 | self._follow_speed_limits = opt_dict['follow_speed_limits'] 106 | 107 | # initializing controller 108 | self._init_controller() 109 | 110 | def reset_vehicle(self): 111 | """Reset the ego-vehicle""" 112 | self._vehicle = None 113 | 114 | def _init_controller(self): 115 | """Controller initialization""" 116 | self._vehicle_controller = VehiclePIDController(self._vehicle, 117 | args_lateral=self._args_lateral_dict, 118 | args_longitudinal=self._args_longitudinal_dict, 119 | offset=self._offset, 120 | max_throttle=self._max_throt, 121 | max_brake=self._max_brake, 122 | max_steering=self._max_steer) 123 | 124 | # Compute the current vehicle waypoint 125 | current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) 126 | self.target_waypoint, self.target_road_option = (current_waypoint, RoadOption.LANEFOLLOW) 127 | self._waypoints_queue.append((self.target_waypoint, self.target_road_option)) 128 | 129 | def set_speed(self, speed): 130 | """ 131 | Changes the target speed 132 | 133 | :param speed: new target speed in Km/h 134 | :return: 135 | """ 136 | if self._follow_speed_limits: 137 | print("WARNING: The max speed is currently set to follow the speed limits. " 138 | "Use 'follow_speed_limits' to deactivate this") 139 | self._target_speed = speed 140 | 141 | def follow_speed_limits(self, value=True): 142 | """ 143 | Activates a flag that makes the max speed dynamically vary according to the spped limits 144 | 145 | :param value: bool 146 | :return: 147 | """ 148 | self._follow_speed_limits = value 149 | 150 | def _compute_next_waypoints(self, k=1): 151 | """ 152 | Add new waypoints to the trajectory queue. 153 | 154 | :param k: how many waypoints to compute 155 | :return: 156 | """ 157 | # check we do not overflow the queue 158 | available_entries = self._waypoints_queue.maxlen - len(self._waypoints_queue) 159 | k = min(available_entries, k) 160 | 161 | for _ in range(k): 162 | last_waypoint = self._waypoints_queue[-1][0] 163 | next_waypoints = list(last_waypoint.next(self._sampling_radius)) 164 | 165 | if len(next_waypoints) == 0: 166 | break 167 | elif len(next_waypoints) == 1: 168 | # only one option available ==> lanefollowing 169 | next_waypoint = next_waypoints[0] 170 | road_option = RoadOption.LANEFOLLOW 171 | else: 172 | # random choice between the possible options 173 | road_options_list = _retrieve_options( 174 | next_waypoints, last_waypoint) 175 | road_option = random.choice(road_options_list) 176 | next_waypoint = next_waypoints[road_options_list.index( 177 | road_option)] 178 | 179 | self._waypoints_queue.append((next_waypoint, road_option)) 180 | 181 | def set_global_plan(self, current_plan, stop_waypoint_creation=True, clean_queue=True): 182 | """ 183 | Adds a new plan to the local planner. A plan must be a list of [carla.Waypoint, RoadOption] pairs 184 | If 'clean_queue`, erases the previous plan, and if not, it is added to the old one 185 | The 'stop_waypoint_creation' flag avoids creating more random waypoints 186 | 187 | :param current_plan: list of (carla.Waypoint, RoadOption) 188 | :param stop_waypoint_creation: bool 189 | :param ceal_queue: bool 190 | :return: 191 | """ 192 | if clean_queue: 193 | self._waypoints_queue.clear() 194 | 195 | # Remake the waypoints queue if the new plan has a higher length than the queue 196 | new_plan_length = len(current_plan) + len(self._waypoints_queue) 197 | if new_plan_length > self._waypoints_queue.maxlen: 198 | new_waypoint_queue = deque(maxlen=new_plan_length) 199 | for wp in self._waypoints_queue: 200 | new_waypoint_queue.append(wp) 201 | self._waypoints_queue = new_waypoint_queue 202 | 203 | for elem in current_plan: 204 | self._waypoints_queue.append(elem) 205 | 206 | self._stop_waypoint_creation = stop_waypoint_creation 207 | 208 | def run_step(self, debug=False): 209 | """ 210 | Execute one step of local planning which involves running the longitudinal and lateral PID controllers to 211 | follow the waypoints trajectory. 212 | 213 | :param debug: boolean flag to activate waypoints debugging 214 | :return: control to be applied 215 | """ 216 | if self._follow_speed_limits: 217 | self._target_speed = self._vehicle.get_speed_limit() 218 | 219 | # Add more waypoints too few in the horizon 220 | if not self._stop_waypoint_creation and len(self._waypoints_queue) < self._min_waypoint_queue_length: 221 | self._compute_next_waypoints(k=self._min_waypoint_queue_length) 222 | 223 | # Purge the queue of obsolete waypoints 224 | veh_location = self._vehicle.get_location() 225 | vehicle_speed = get_speed(self._vehicle) / 3.6 226 | self._min_distance = self._base_min_distance + 0.5 *vehicle_speed 227 | 228 | num_waypoint_removed = 0 229 | for waypoint, _ in self._waypoints_queue: 230 | 231 | if len(self._waypoints_queue) - num_waypoint_removed == 1: 232 | min_distance = 1 # Don't remove the last waypoint until very close by 233 | else: 234 | min_distance = self._min_distance 235 | 236 | if veh_location.distance(waypoint.transform.location) < min_distance: 237 | num_waypoint_removed += 1 238 | else: 239 | break 240 | 241 | if num_waypoint_removed > 0: 242 | for _ in range(num_waypoint_removed): 243 | self._waypoints_queue.popleft() 244 | 245 | # Get the target waypoint and move using the PID controllers. Stop if no target waypoint 246 | if len(self._waypoints_queue) == 0: 247 | control = carla.VehicleControl() 248 | control.steer = 0.0 249 | control.throttle = 0.0 250 | control.brake = 1.0 251 | control.hand_brake = False 252 | control.manual_gear_shift = False 253 | else: 254 | self.target_waypoint, self.target_road_option = self._waypoints_queue[0] 255 | control = self._vehicle_controller.run_step(self._target_speed, self.target_waypoint) 256 | 257 | if debug: 258 | draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], 1.0) 259 | 260 | return control 261 | 262 | def get_incoming_waypoint_and_direction(self, steps=3): 263 | """ 264 | Returns direction and waypoint at a distance ahead defined by the user. 265 | 266 | :param steps: number of steps to get the incoming waypoint. 267 | """ 268 | if len(self._waypoints_queue) > steps: 269 | return self._waypoints_queue[steps] 270 | 271 | else: 272 | try: 273 | wpt, direction = self._waypoints_queue[-1] 274 | return wpt, direction 275 | except IndexError as i: 276 | return None, RoadOption.VOID 277 | 278 | def done(self): 279 | """ 280 | Returns whether or not the planner has finished 281 | 282 | :return: boolean 283 | """ 284 | return len(self._waypoints_queue) == 0 285 | 286 | 287 | def _retrieve_options(list_waypoints, current_waypoint): 288 | """ 289 | Compute the type of connection between the current active waypoint and the multiple waypoints present in 290 | list_waypoints. The result is encoded as a list of RoadOption enums. 291 | 292 | :param list_waypoints: list with the possible target waypoints in case of multiple options 293 | :param current_waypoint: current active waypoint 294 | :return: list of RoadOption enums representing the type of connection from the active waypoint to each 295 | candidate in list_waypoints 296 | """ 297 | options = [] 298 | for next_waypoint in list_waypoints: 299 | # this is needed because something we are linking to 300 | # the beggining of an intersection, therefore the 301 | # variation in angle is small 302 | next_next_waypoint = next_waypoint.next(3.0)[0] 303 | link = _compute_connection(current_waypoint, next_next_waypoint) 304 | options.append(link) 305 | 306 | return options 307 | 308 | 309 | def _compute_connection(current_waypoint, next_waypoint, threshold=35): 310 | """ 311 | Compute the type of topological connection between an active waypoint (current_waypoint) and a target waypoint 312 | (next_waypoint). 313 | 314 | :param current_waypoint: active waypoint 315 | :param next_waypoint: target waypoint 316 | :return: the type of topological connection encoded as a RoadOption enum: 317 | RoadOption.STRAIGHT 318 | RoadOption.LEFT 319 | RoadOption.RIGHT 320 | """ 321 | n = next_waypoint.transform.rotation.yaw 322 | n = n % 360.0 323 | 324 | c = current_waypoint.transform.rotation.yaw 325 | c = c % 360.0 326 | 327 | diff_angle = (n - c) % 180.0 328 | if diff_angle < threshold or diff_angle > (180 - threshold): 329 | return RoadOption.STRAIGHT 330 | elif diff_angle > 90.0: 331 | return RoadOption.LEFT 332 | else: 333 | return RoadOption.RIGHT 334 | -------------------------------------------------------------------------------- /Import/agents/navigation/behavior_agent.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) # Copyright (c) 2018-2020 CVC. 2 | # 3 | # This work is licensed under the terms of the MIT license. 4 | # For a copy, see . 5 | 6 | 7 | """ This module implements an agent that roams around a track following random 8 | waypoints and avoiding other vehicles. The agent also responds to traffic lights, 9 | traffic signs, and has different possible configurations. """ 10 | 11 | import random 12 | import numpy as np 13 | import carla 14 | from agents.navigation.basic_agent import BasicAgent 15 | from agents.navigation.local_planner import RoadOption 16 | from agents.navigation.behavior_types import Cautious, Aggressive, Normal 17 | 18 | from agents.tools.misc import get_speed, positive, is_within_distance, compute_distance 19 | 20 | class BehaviorAgent(BasicAgent): 21 | """ 22 | BehaviorAgent implements an agent that navigates scenes to reach a given 23 | target destination, by computing the shortest possible path to it. 24 | This agent can correctly follow traffic signs, speed limitations, 25 | traffic lights, while also taking into account nearby vehicles. Lane changing 26 | decisions can be taken by analyzing the surrounding environment such as tailgating avoidance. 27 | Adding to these are possible behaviors, the agent can also keep safety distance 28 | from a car in front of it by tracking the instantaneous time to collision 29 | and keeping it in a certain range. Finally, different sets of behaviors 30 | are encoded in the agent, from cautious to a more aggressive ones. 31 | """ 32 | 33 | def __init__(self, vehicle, behavior='normal'): 34 | """ 35 | Constructor method. 36 | 37 | :param vehicle: actor to apply to local planner logic onto 38 | :param ignore_traffic_light: boolean to ignore any traffic light 39 | :param behavior: type of agent to apply 40 | """ 41 | 42 | super(BehaviorAgent, self).__init__(vehicle) 43 | self._look_ahead_steps = 0 44 | 45 | # Vehicle information 46 | self._speed = 0 47 | self._speed_limit = 0 48 | self._direction = None 49 | self._incoming_direction = None 50 | self._incoming_waypoint = None 51 | self._min_speed = 5 52 | self._behavior = None 53 | self._sampling_resolution = 4.5 54 | 55 | # Parameters for agent behavior 56 | if behavior == 'cautious': 57 | self._behavior = Cautious() 58 | 59 | elif behavior == 'normal': 60 | self._behavior = Normal() 61 | 62 | elif behavior == 'aggressive': 63 | self._behavior = Aggressive() 64 | 65 | def _update_information(self): 66 | """ 67 | This method updates the information regarding the ego 68 | vehicle based on the surrounding world. 69 | """ 70 | self._speed = get_speed(self._vehicle) 71 | self._speed_limit = self._vehicle.get_speed_limit() 72 | self._local_planner.set_speed(self._speed_limit) 73 | self._direction = self._local_planner.target_road_option 74 | if self._direction is None: 75 | self._direction = RoadOption.LANEFOLLOW 76 | 77 | self._look_ahead_steps = int((self._speed_limit) / 10) 78 | 79 | self._incoming_waypoint, self._incoming_direction = self._local_planner.get_incoming_waypoint_and_direction( 80 | steps=self._look_ahead_steps) 81 | if self._incoming_direction is None: 82 | self._incoming_direction = RoadOption.LANEFOLLOW 83 | 84 | def _vehicle_obstacle_detected(self, vehicle_list, proximity_th, up_angle_th, low_angle_th=0, lane_offset=0): 85 | """ 86 | Check if a given vehicle is an obstacle in our way. To this end we take 87 | into account the road and lane the target vehicle is on and run a 88 | geometry test to check if the target vehicle is under a certain distance 89 | in front of our ego vehicle. We also check the next waypoint, just to be 90 | sure there's not a sudden road id change. 91 | 92 | WARNING: This method is an approximation that could fail for very large 93 | vehicles, which center is actually on a different lane but their 94 | extension falls within the ego vehicle lane. Also, make sure to remove 95 | the ego vehicle from the list. Lane offset is set to +1 for right lanes 96 | and -1 for left lanes, but this has to be inverted if lane values are 97 | negative. 98 | 99 | :param vehicle_list: list of potential obstacle to check 100 | :param proximity_th: threshold for the agent to be alerted of 101 | a possible collision 102 | :param up_angle_th: upper threshold for angle 103 | :param low_angle_th: lower threshold for angle 104 | :param lane_offset: for right and left lane changes 105 | :return: a tuple given by (bool_flag, vehicle, distance), where: 106 | - bool_flag is True if there is a vehicle ahead blocking us 107 | and False otherwise 108 | - vehicle is the blocker object itself 109 | - distance is the meters separating the two vehicles 110 | """ 111 | ego_transform = self._vehicle.get_transform() 112 | ego_location = ego_transform.location 113 | ego_wpt = self._map.get_waypoint(ego_location) 114 | 115 | # Get the right offset 116 | if ego_wpt.lane_id < 0 and lane_offset != 0: 117 | lane_offset *= -1 118 | 119 | for target_vehicle in vehicle_list: 120 | 121 | target_transform = target_vehicle.get_transform() 122 | target_location = target_transform.location 123 | # If the object is not in our next or current lane it's not an obstacle 124 | 125 | target_wpt = self._map.get_waypoint(target_location) 126 | if target_wpt.road_id != ego_wpt.road_id or \ 127 | target_wpt.lane_id != ego_wpt.lane_id + lane_offset: 128 | next_wpt = self._local_planner.get_incoming_waypoint_and_direction(steps=5)[0] 129 | if target_wpt.road_id != next_wpt.road_id or \ 130 | target_wpt.lane_id != next_wpt.lane_id + lane_offset: 131 | continue 132 | 133 | if is_within_distance(target_transform, ego_transform, 134 | proximity_th, [low_angle_th, up_angle_th]): 135 | 136 | return (True, target_vehicle, compute_distance(target_location, ego_location)) 137 | 138 | return (False, None, -1) 139 | 140 | def traffic_light_manager(self): 141 | """ 142 | This method is in charge of behaviors for red lights. 143 | """ 144 | actor_list = self._world.get_actors() 145 | lights_list = actor_list.filter("*traffic_light*") 146 | affected, _ = self._affected_by_traffic_light(lights_list) 147 | 148 | return affected 149 | 150 | def _tailgating(self, waypoint, vehicle_list): 151 | """ 152 | This method is in charge of tailgating behaviors. 153 | 154 | :param location: current location of the agent 155 | :param waypoint: current waypoint of the agent 156 | :param vehicle_list: list of all the nearby vehicles 157 | """ 158 | 159 | left_turn = waypoint.left_lane_marking.lane_change 160 | right_turn = waypoint.right_lane_marking.lane_change 161 | 162 | left_wpt = waypoint.get_left_lane() 163 | right_wpt = waypoint.get_right_lane() 164 | 165 | behind_vehicle_state, behind_vehicle, _ = self._vehicle_obstacle_detected(vehicle_list, max( 166 | self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, low_angle_th=160) 167 | if behind_vehicle_state and self._speed < get_speed(behind_vehicle): 168 | if (right_turn == carla.LaneChange.Right or right_turn == 169 | carla.LaneChange.Both) and waypoint.lane_id * right_wpt.lane_id > 0 and right_wpt.lane_type == carla.LaneType.Driving: 170 | new_vehicle_state, _, _ = self._vehicle_obstacle_detected(vehicle_list, max( 171 | self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=1) 172 | if not new_vehicle_state: 173 | print("Tailgating, moving to the right!") 174 | end_waypoint = self._local_planner.target_waypoint 175 | self._behavior.tailgate_counter = 200 176 | self.set_destination(end_waypoint.transform.location, 177 | right_wpt.transform.location) 178 | elif left_turn == carla.LaneChange.Left and waypoint.lane_id * left_wpt.lane_id > 0 and left_wpt.lane_type == carla.LaneType.Driving: 179 | new_vehicle_state, _, _ = self._vehicle_obstacle_detected(vehicle_list, max( 180 | self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=-1) 181 | if not new_vehicle_state: 182 | print("Tailgating, moving to the left!") 183 | end_waypoint = self._local_planner.target_waypoint 184 | self._behavior.tailgate_counter = 200 185 | self.set_destination(end_waypoint.transform.location, 186 | left_wpt.transform.location) 187 | 188 | def collision_and_car_avoid_manager(self, waypoint): 189 | """ 190 | This module is in charge of warning in case of a collision 191 | and managing possible tailgating chances. 192 | 193 | :param location: current location of the agent 194 | :param waypoint: current waypoint of the agent 195 | :return vehicle_state: True if there is a vehicle nearby, False if not 196 | :return vehicle: nearby vehicle 197 | :return distance: distance to nearby vehicle 198 | """ 199 | 200 | vehicle_list = self._world.get_actors().filter("*vehicle*") 201 | def dist(v): return v.get_location().distance(waypoint.transform.location) 202 | vehicle_list = [v for v in vehicle_list if dist(v) < 45 and v.id != self._vehicle.id] 203 | 204 | if self._direction == RoadOption.CHANGELANELEFT: 205 | vehicle_state, vehicle, distance = self._vehicle_obstacle_detected( 206 | vehicle_list, max( 207 | self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=-1) 208 | elif self._direction == RoadOption.CHANGELANERIGHT: 209 | vehicle_state, vehicle, distance = self._vehicle_obstacle_detected( 210 | vehicle_list, max( 211 | self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=1) 212 | else: 213 | vehicle_state, vehicle, distance = self._vehicle_obstacle_detected( 214 | vehicle_list, max( 215 | self._behavior.min_proximity_threshold, self._speed_limit / 3), up_angle_th=30) 216 | 217 | # Check for tailgating 218 | if not vehicle_state and self._direction == RoadOption.LANEFOLLOW \ 219 | and not waypoint.is_junction and self._speed > 10 \ 220 | and self._behavior.tailgate_counter == 0: 221 | self._tailgating(waypoint, vehicle_list) 222 | 223 | return vehicle_state, vehicle, distance 224 | 225 | def pedestrian_avoid_manager(self, waypoint): 226 | """ 227 | This module is in charge of warning in case of a collision 228 | with any pedestrian. 229 | 230 | :param location: current location of the agent 231 | :param waypoint: current waypoint of the agent 232 | :return vehicle_state: True if there is a walker nearby, False if not 233 | :return vehicle: nearby walker 234 | :return distance: distance to nearby walker 235 | """ 236 | 237 | walker_list = self._world.get_actors().filter("*walker.pedestrian*") 238 | def dist(w): return w.get_location().distance(waypoint.transform.location) 239 | walker_list = [w for w in walker_list if dist(w) < 10] 240 | 241 | if self._direction == RoadOption.CHANGELANELEFT: 242 | walker_state, walker, distance = self._vehicle_obstacle_detected(walker_list, max( 243 | self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=90, lane_offset=-1) 244 | elif self._direction == RoadOption.CHANGELANERIGHT: 245 | walker_state, walker, distance = self._vehicle_obstacle_detected(walker_list, max( 246 | self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=90, lane_offset=1) 247 | else: 248 | walker_state, walker, distance = self._vehicle_obstacle_detected(walker_list, max( 249 | self._behavior.min_proximity_threshold, self._speed_limit / 3), up_angle_th=60) 250 | 251 | return walker_state, walker, distance 252 | 253 | def car_following_manager(self, vehicle, distance, debug=False): 254 | """ 255 | Module in charge of car-following behaviors when there's 256 | someone in front of us. 257 | 258 | :param vehicle: car to follow 259 | :param distance: distance from vehicle 260 | :param debug: boolean for debugging 261 | :return control: carla.VehicleControl 262 | """ 263 | 264 | vehicle_speed = get_speed(vehicle) 265 | delta_v = max(1, (self._speed - vehicle_speed) / 3.6) 266 | ttc = distance / delta_v if delta_v != 0 else distance / np.nextafter(0., 1.) 267 | 268 | # Under safety time distance, slow down. 269 | if self._behavior.safety_time > ttc > 0.0: 270 | target_speed = min([ 271 | positive(vehicle_speed - self._behavior.speed_decrease), 272 | self._behavior.max_speed, 273 | self._speed_limit - self._behavior.speed_lim_dist]) 274 | self._local_planner.set_speed(target_speed) 275 | control = self._local_planner.run_step(debug=debug) 276 | 277 | # Actual safety distance area, try to follow the speed of the vehicle in front. 278 | elif 2 * self._behavior.safety_time > ttc >= self._behavior.safety_time: 279 | target_speed = min([ 280 | max(self._min_speed, vehicle_speed), 281 | self._behavior.max_speed, 282 | self._speed_limit - self._behavior.speed_lim_dist]) 283 | self._local_planner.set_speed(target_speed) 284 | control = self._local_planner.run_step(debug=debug) 285 | 286 | # Normal behavior. 287 | else: 288 | target_speed = min([ 289 | self._behavior.max_speed, 290 | self._speed_limit - self._behavior.speed_lim_dist]) 291 | self._local_planner.set_speed(target_speed) 292 | control = self._local_planner.run_step(debug=debug) 293 | 294 | return control 295 | 296 | def run_step(self, args, debug=False): 297 | """ 298 | Execute one step of navigation. 299 | 300 | :param debug: boolean for debugging 301 | :return control: carla.VehicleControl 302 | """ 303 | self._update_information() 304 | 305 | control = None 306 | if self._behavior.tailgate_counter > 0: 307 | self._behavior.tailgate_counter -= 1 308 | 309 | ego_vehicle_loc = self._vehicle.get_location() 310 | ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc) 311 | 312 | # 1: Red lights and stops behavior 313 | if args.enable_traffic_light: 314 | if self.traffic_light_manager(): 315 | return self.emergency_stop() 316 | 317 | # 2.1: Pedestrian avoidance behaviors 318 | walker_state, walker, w_distance = self.pedestrian_avoid_manager(ego_vehicle_wp) 319 | 320 | if walker_state: 321 | # Distance is computed from the center of the two cars, 322 | # we use bounding boxes to calculate the actual distance 323 | distance = w_distance - max( 324 | walker.bounding_box.extent.y, walker.bounding_box.extent.x) - max( 325 | self._vehicle.bounding_box.extent.y, self._vehicle.bounding_box.extent.x) 326 | 327 | # Emergency brake if the car is very close. 328 | if distance < self._behavior.braking_distance: 329 | return self.emergency_stop() 330 | 331 | # 2.2: Car following behaviors 332 | vehicle_state, vehicle, distance = self.collision_and_car_avoid_manager(ego_vehicle_wp) 333 | 334 | if vehicle_state: 335 | # Distance is computed from the center of the two cars, 336 | # we use bounding boxes to calculate the actual distance 337 | distance = distance - max( 338 | vehicle.bounding_box.extent.y, vehicle.bounding_box.extent.x) - max( 339 | self._vehicle.bounding_box.extent.y, self._vehicle.bounding_box.extent.x) 340 | 341 | # Emergency brake if the car is very close. 342 | if distance < self._behavior.braking_distance: 343 | return self.emergency_stop() 344 | else: 345 | control = self.car_following_manager(vehicle, distance) 346 | 347 | # 3: Intersection behavior 348 | elif self._incoming_waypoint.is_junction and (self._incoming_direction in [RoadOption.LEFT, RoadOption.RIGHT]): 349 | target_speed = min([ 350 | self._behavior.max_speed, 351 | self._speed_limit - 5]) 352 | self._local_planner.set_speed(target_speed) 353 | control = self._local_planner.run_step(debug=debug) 354 | 355 | # 4: Normal behavior 356 | else: 357 | target_speed = min([ 358 | self._behavior.max_speed, 359 | self._speed_limit - self._behavior.speed_lim_dist]) 360 | self._local_planner.set_speed(target_speed) 361 | control = self._local_planner.run_step(debug=debug) 362 | 363 | return control 364 | 365 | def emergency_stop(self): 366 | """ 367 | Overwrites the throttle a brake values of a control to perform an emergency stop. 368 | The steering is kept the same to avoid going out of the lane when stopping during turns 369 | 370 | :param speed (carl.VehicleControl): control to be modified 371 | """ 372 | control = carla.VehicleControl() 373 | control.throttle = 0.0 374 | control.brake = self._max_brake 375 | control.hand_brake = False 376 | return control -------------------------------------------------------------------------------- /Import/agents/navigation/global_route_planner.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) # Copyright (c) 2018-2020 CVC. 2 | # 3 | # This work is licensed under the terms of the MIT license. 4 | # For a copy, see . 5 | 6 | 7 | """ 8 | This module provides GlobalRoutePlanner implementation. 9 | """ 10 | 11 | import math 12 | import numpy as np 13 | import networkx as nx 14 | 15 | import carla 16 | from agents.navigation.local_planner import RoadOption 17 | from agents.tools.misc import vector 18 | 19 | class GlobalRoutePlanner(object): 20 | """ 21 | This class provides a very high level route plan. 22 | """ 23 | 24 | def __init__(self, wmap, sampling_resolution): 25 | self._sampling_resolution = sampling_resolution 26 | self._wmap = wmap 27 | self._topology = None 28 | self._graph = None 29 | self._id_map = None 30 | self._road_id_to_edge = None 31 | 32 | self._intersection_end_node = -1 33 | self._previous_decision = RoadOption.VOID 34 | 35 | # Build the graph 36 | self._build_topology() 37 | self._build_graph() 38 | self._find_loose_ends() 39 | self._lane_change_link() 40 | 41 | def trace_route(self, origin, destination): 42 | """ 43 | This method returns list of (carla.Waypoint, RoadOption) 44 | from origin to destination 45 | """ 46 | route_trace = [] 47 | route = self._path_search(origin, destination) 48 | current_waypoint = self._wmap.get_waypoint(origin) 49 | destination_waypoint = self._wmap.get_waypoint(destination) 50 | 51 | for i in range(len(route) - 1): 52 | road_option = self._turn_decision(i, route) 53 | edge = self._graph.edges[route[i], route[i+1]] 54 | path = [] 55 | 56 | if edge['type'] != RoadOption.LANEFOLLOW and edge['type'] != RoadOption.VOID: 57 | route_trace.append((current_waypoint, road_option)) 58 | exit_wp = edge['exit_waypoint'] 59 | n1, n2 = self._road_id_to_edge[exit_wp.road_id][exit_wp.section_id][exit_wp.lane_id] 60 | next_edge = self._graph.edges[n1, n2] 61 | if next_edge['path']: 62 | closest_index = self._find_closest_in_list(current_waypoint, next_edge['path']) 63 | closest_index = min(len(next_edge['path'])-1, closest_index+5) 64 | current_waypoint = next_edge['path'][closest_index] 65 | else: 66 | current_waypoint = next_edge['exit_waypoint'] 67 | route_trace.append((current_waypoint, road_option)) 68 | 69 | else: 70 | path = path + [edge['entry_waypoint']] + edge['path'] + [edge['exit_waypoint']] 71 | closest_index = self._find_closest_in_list(current_waypoint, path) 72 | for waypoint in path[closest_index:]: 73 | current_waypoint = waypoint 74 | route_trace.append((current_waypoint, road_option)) 75 | if len(route)-i <= 2 and waypoint.transform.location.distance(destination) < 2*self._sampling_resolution: 76 | break 77 | elif len(route)-i <= 2 and current_waypoint.road_id == destination_waypoint.road_id and current_waypoint.section_id == destination_waypoint.section_id and current_waypoint.lane_id == destination_waypoint.lane_id: 78 | destination_index = self._find_closest_in_list(destination_waypoint, path) 79 | if closest_index > destination_index: 80 | break 81 | 82 | return route_trace 83 | 84 | def _build_topology(self): 85 | """ 86 | This function retrieves topology from the server as a list of 87 | road segments as pairs of waypoint objects, and processes the 88 | topology into a list of dictionary objects with the following attributes 89 | 90 | - entry (carla.Waypoint): waypoint of entry point of road segment 91 | - entryxyz (tuple): (x,y,z) of entry point of road segment 92 | - exit (carla.Waypoint): waypoint of exit point of road segment 93 | - exitxyz (tuple): (x,y,z) of exit point of road segment 94 | - path (list of carla.Waypoint): list of waypoints between entry to exit, separated by the resolution 95 | """ 96 | self._topology = [] 97 | # Retrieving waypoints to construct a detailed topology 98 | for segment in self._wmap.get_topology(): 99 | wp1, wp2 = segment[0], segment[1] 100 | l1, l2 = wp1.transform.location, wp2.transform.location 101 | # Rounding off to avoid floating point imprecision 102 | x1, y1, z1, x2, y2, z2 = np.round([l1.x, l1.y, l1.z, l2.x, l2.y, l2.z], 0) 103 | wp1.transform.location, wp2.transform.location = l1, l2 104 | seg_dict = dict() 105 | seg_dict['entry'], seg_dict['exit'] = wp1, wp2 106 | seg_dict['entryxyz'], seg_dict['exitxyz'] = (x1, y1, z1), (x2, y2, z2) 107 | seg_dict['path'] = [] 108 | endloc = wp2.transform.location 109 | if wp1.transform.location.distance(endloc) > self._sampling_resolution: 110 | w = wp1.next(self._sampling_resolution)[0] 111 | while w.transform.location.distance(endloc) > self._sampling_resolution: 112 | seg_dict['path'].append(w) 113 | w = w.next(self._sampling_resolution)[0] 114 | else: 115 | seg_dict['path'].append(wp1.next(self._sampling_resolution)[0]) 116 | self._topology.append(seg_dict) 117 | 118 | def _build_graph(self): 119 | """ 120 | This function builds a networkx graph representation of topology, creating several class attributes: 121 | - graph (networkx.DiGraph): networkx graph representing the world map, with: 122 | Node properties: 123 | vertex: (x,y,z) position in world map 124 | Edge properties: 125 | entry_vector: unit vector along tangent at entry point 126 | exit_vector: unit vector along tangent at exit point 127 | net_vector: unit vector of the chord from entry to exit 128 | intersection: boolean indicating if the edge belongs to an intersection 129 | - id_map (dictionary): mapping from (x,y,z) to node id 130 | - road_id_to_edge (dictionary): map from road id to edge in the graph 131 | """ 132 | 133 | self._graph = nx.DiGraph() 134 | self._id_map = dict() # Map with structure {(x,y,z): id, ... } 135 | self._road_id_to_edge = dict() # Map with structure {road_id: {lane_id: edge, ... }, ... } 136 | 137 | for segment in self._topology: 138 | entry_xyz, exit_xyz = segment['entryxyz'], segment['exitxyz'] 139 | path = segment['path'] 140 | entry_wp, exit_wp = segment['entry'], segment['exit'] 141 | intersection = entry_wp.is_junction 142 | road_id, section_id, lane_id = entry_wp.road_id, entry_wp.section_id, entry_wp.lane_id 143 | 144 | for vertex in entry_xyz, exit_xyz: 145 | # Adding unique nodes and populating id_map 146 | if vertex not in self._id_map: 147 | new_id = len(self._id_map) 148 | self._id_map[vertex] = new_id 149 | self._graph.add_node(new_id, vertex=vertex) 150 | n1 = self._id_map[entry_xyz] 151 | n2 = self._id_map[exit_xyz] 152 | if road_id not in self._road_id_to_edge: 153 | self._road_id_to_edge[road_id] = dict() 154 | if section_id not in self._road_id_to_edge[road_id]: 155 | self._road_id_to_edge[road_id][section_id] = dict() 156 | self._road_id_to_edge[road_id][section_id][lane_id] = (n1, n2) 157 | 158 | entry_carla_vector = entry_wp.transform.rotation.get_forward_vector() 159 | exit_carla_vector = exit_wp.transform.rotation.get_forward_vector() 160 | 161 | # Adding edge with attributes 162 | self._graph.add_edge( 163 | n1, n2, 164 | length=len(path) + 1, path=path, 165 | entry_waypoint=entry_wp, exit_waypoint=exit_wp, 166 | entry_vector=np.array( 167 | [entry_carla_vector.x, entry_carla_vector.y, entry_carla_vector.z]), 168 | exit_vector=np.array( 169 | [exit_carla_vector.x, exit_carla_vector.y, exit_carla_vector.z]), 170 | net_vector=vector(entry_wp.transform.location, exit_wp.transform.location), 171 | intersection=intersection, type=RoadOption.LANEFOLLOW) 172 | 173 | def _find_loose_ends(self): 174 | """ 175 | This method finds road segments that have an unconnected end, and 176 | adds them to the internal graph representation 177 | """ 178 | count_loose_ends = 0 179 | hop_resolution = self._sampling_resolution 180 | for segment in self._topology: 181 | end_wp = segment['exit'] 182 | exit_xyz = segment['exitxyz'] 183 | road_id, section_id, lane_id = end_wp.road_id, end_wp.section_id, end_wp.lane_id 184 | if road_id in self._road_id_to_edge \ 185 | and section_id in self._road_id_to_edge[road_id] \ 186 | and lane_id in self._road_id_to_edge[road_id][section_id]: 187 | pass 188 | else: 189 | count_loose_ends += 1 190 | if road_id not in self._road_id_to_edge: 191 | self._road_id_to_edge[road_id] = dict() 192 | if section_id not in self._road_id_to_edge[road_id]: 193 | self._road_id_to_edge[road_id][section_id] = dict() 194 | n1 = self._id_map[exit_xyz] 195 | n2 = -1*count_loose_ends 196 | self._road_id_to_edge[road_id][section_id][lane_id] = (n1, n2) 197 | next_wp = end_wp.next(hop_resolution) 198 | path = [] 199 | while next_wp is not None and next_wp \ 200 | and next_wp[0].road_id == road_id \ 201 | and next_wp[0].section_id == section_id \ 202 | and next_wp[0].lane_id == lane_id: 203 | path.append(next_wp[0]) 204 | next_wp = next_wp[0].next(hop_resolution) 205 | if path: 206 | n2_xyz = (path[-1].transform.location.x, 207 | path[-1].transform.location.y, 208 | path[-1].transform.location.z) 209 | self._graph.add_node(n2, vertex=n2_xyz) 210 | self._graph.add_edge( 211 | n1, n2, 212 | length=len(path) + 1, path=path, 213 | entry_waypoint=end_wp, exit_waypoint=path[-1], 214 | entry_vector=None, exit_vector=None, net_vector=None, 215 | intersection=end_wp.is_junction, type=RoadOption.LANEFOLLOW) 216 | 217 | def _lane_change_link(self): 218 | """ 219 | This method places zero cost links in the topology graph 220 | representing availability of lane changes. 221 | """ 222 | 223 | for segment in self._topology: 224 | left_found, right_found = False, False 225 | 226 | for waypoint in segment['path']: 227 | if not segment['entry'].is_junction: 228 | next_waypoint, next_road_option, next_segment = None, None, None 229 | 230 | if waypoint.right_lane_marking.lane_change & carla.LaneChange.Right and not right_found: 231 | next_waypoint = waypoint.get_right_lane() 232 | if next_waypoint is not None \ 233 | and next_waypoint.lane_type == carla.LaneType.Driving \ 234 | and waypoint.road_id == next_waypoint.road_id: 235 | next_road_option = RoadOption.CHANGELANERIGHT 236 | next_segment = self._localize(next_waypoint.transform.location) 237 | if next_segment is not None: 238 | self._graph.add_edge( 239 | self._id_map[segment['entryxyz']], next_segment[0], entry_waypoint=waypoint, 240 | exit_waypoint=next_waypoint, intersection=False, exit_vector=None, 241 | path=[], length=0, type=next_road_option, change_waypoint=next_waypoint) 242 | right_found = True 243 | if waypoint.left_lane_marking.lane_change & carla.LaneChange.Left and not left_found: 244 | next_waypoint = waypoint.get_left_lane() 245 | if next_waypoint is not None \ 246 | and next_waypoint.lane_type == carla.LaneType.Driving \ 247 | and waypoint.road_id == next_waypoint.road_id: 248 | next_road_option = RoadOption.CHANGELANELEFT 249 | next_segment = self._localize(next_waypoint.transform.location) 250 | if next_segment is not None: 251 | self._graph.add_edge( 252 | self._id_map[segment['entryxyz']], next_segment[0], entry_waypoint=waypoint, 253 | exit_waypoint=next_waypoint, intersection=False, exit_vector=None, 254 | path=[], length=0, type=next_road_option, change_waypoint=next_waypoint) 255 | left_found = True 256 | if left_found and right_found: 257 | break 258 | 259 | def _localize(self, location): 260 | """ 261 | This function finds the road segment that a given location 262 | is part of, returning the edge it belongs to 263 | """ 264 | waypoint = self._wmap.get_waypoint(location) 265 | edge = None 266 | try: 267 | edge = self._road_id_to_edge[waypoint.road_id][waypoint.section_id][waypoint.lane_id] 268 | except KeyError: 269 | pass 270 | return edge 271 | 272 | def _distance_heuristic(self, n1, n2): 273 | """ 274 | Distance heuristic calculator for path searching 275 | in self._graph 276 | """ 277 | l1 = np.array(self._graph.nodes[n1]['vertex']) 278 | l2 = np.array(self._graph.nodes[n2]['vertex']) 279 | return np.linalg.norm(l1-l2) 280 | 281 | def _path_search(self, origin, destination): 282 | """ 283 | This function finds the shortest path connecting origin and destination 284 | using A* search with distance heuristic. 285 | origin : carla.Location object of start position 286 | destination : carla.Location object of of end position 287 | return : path as list of node ids (as int) of the graph self._graph 288 | connecting origin and destination 289 | """ 290 | start, end = self._localize(origin), self._localize(destination) 291 | 292 | route = nx.astar_path( 293 | self._graph, source=start[0], target=end[0], 294 | heuristic=self._distance_heuristic, weight='length') 295 | route.append(end[1]) 296 | return route 297 | 298 | def _successive_last_intersection_edge(self, index, route): 299 | """ 300 | This method returns the last successive intersection edge 301 | from a starting index on the route. 302 | This helps moving past tiny intersection edges to calculate 303 | proper turn decisions. 304 | """ 305 | 306 | last_intersection_edge = None 307 | last_node = None 308 | for node1, node2 in [(route[i], route[i+1]) for i in range(index, len(route)-1)]: 309 | candidate_edge = self._graph.edges[node1, node2] 310 | if node1 == route[index]: 311 | last_intersection_edge = candidate_edge 312 | if candidate_edge['type'] == RoadOption.LANEFOLLOW and candidate_edge['intersection']: 313 | last_intersection_edge = candidate_edge 314 | last_node = node2 315 | else: 316 | break 317 | 318 | return last_node, last_intersection_edge 319 | 320 | def _turn_decision(self, index, route, threshold=math.radians(35)): 321 | """ 322 | This method returns the turn decision (RoadOption) for pair of edges 323 | around current index of route list 324 | """ 325 | 326 | decision = None 327 | previous_node = route[index-1] 328 | current_node = route[index] 329 | next_node = route[index+1] 330 | next_edge = self._graph.edges[current_node, next_node] 331 | if index > 0: 332 | if self._previous_decision != RoadOption.VOID \ 333 | and self._intersection_end_node > 0 \ 334 | and self._intersection_end_node != previous_node \ 335 | and next_edge['type'] == RoadOption.LANEFOLLOW \ 336 | and next_edge['intersection']: 337 | decision = self._previous_decision 338 | else: 339 | self._intersection_end_node = -1 340 | current_edge = self._graph.edges[previous_node, current_node] 341 | calculate_turn = current_edge['type'] == RoadOption.LANEFOLLOW and not current_edge[ 342 | 'intersection'] and next_edge['type'] == RoadOption.LANEFOLLOW and next_edge['intersection'] 343 | if calculate_turn: 344 | last_node, tail_edge = self._successive_last_intersection_edge(index, route) 345 | self._intersection_end_node = last_node 346 | if tail_edge is not None: 347 | next_edge = tail_edge 348 | cv, nv = current_edge['exit_vector'], next_edge['exit_vector'] 349 | if cv is None or nv is None: 350 | return next_edge['type'] 351 | cross_list = [] 352 | for neighbor in self._graph.successors(current_node): 353 | select_edge = self._graph.edges[current_node, neighbor] 354 | if select_edge['type'] == RoadOption.LANEFOLLOW: 355 | if neighbor != route[index+1]: 356 | sv = select_edge['net_vector'] 357 | cross_list.append(np.cross(cv, sv)[2]) 358 | next_cross = np.cross(cv, nv)[2] 359 | deviation = math.acos(np.clip( 360 | np.dot(cv, nv)/(np.linalg.norm(cv)*np.linalg.norm(nv)), -1.0, 1.0)) 361 | if not cross_list: 362 | cross_list.append(0) 363 | if deviation < threshold: 364 | decision = RoadOption.STRAIGHT 365 | elif cross_list and next_cross < min(cross_list): 366 | decision = RoadOption.LEFT 367 | elif cross_list and next_cross > max(cross_list): 368 | decision = RoadOption.RIGHT 369 | elif next_cross < 0: 370 | decision = RoadOption.LEFT 371 | elif next_cross > 0: 372 | decision = RoadOption.RIGHT 373 | else: 374 | decision = next_edge['type'] 375 | 376 | else: 377 | decision = next_edge['type'] 378 | 379 | self._previous_decision = decision 380 | return decision 381 | 382 | def _find_closest_in_list(self, current_waypoint, waypoint_list): 383 | min_distance = float('inf') 384 | closest_index = -1 385 | for i, waypoint in enumerate(waypoint_list): 386 | distance = waypoint.transform.location.distance( 387 | current_waypoint.transform.location) 388 | if distance < min_distance: 389 | min_distance = distance 390 | closest_index = i 391 | 392 | return closest_index 393 | -------------------------------------------------------------------------------- /capture_multiple_sensors.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma de 4 | # Barcelona (UAB). 5 | # 6 | # This work is licensed under the terms of the MIT license. 7 | # For a copy, see . 8 | 9 | 10 | """ 11 | We provide an example code to capture data for multiple sensors in synchronous mode on [Carla](https://carla.org/) platform. 12 | """ 13 | 14 | import glob 15 | import os 16 | import sys 17 | import argparse 18 | import random 19 | import time 20 | from datetime import datetime 21 | import numpy as np 22 | from PIL import Image 23 | from matplotlib import cm 24 | import open3d as o3d 25 | import cv2 26 | from queue import Queue 27 | from queue import Empty 28 | 29 | try: 30 | sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( 31 | sys.version_info.major, 32 | sys.version_info.minor, 33 | 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) 34 | except IndexError: 35 | pass 36 | 37 | import carla 38 | 39 | 40 | VIRIDIS = np.array(cm.get_cmap('plasma').colors) 41 | VID_RANGE = np.linspace(0.0, 1.0, VIRIDIS.shape[0]) 42 | LABEL_COLORS = np.array([ 43 | (255, 255, 255), # None 44 | (70, 70, 70), # Building 45 | (100, 40, 40), # Fences 46 | (55, 90, 80), # Other 47 | (220, 20, 60), # Pedestrian 48 | (153, 153, 153), # Pole 49 | (157, 234, 50), # RoadLines 50 | (128, 64, 128), # Road 51 | (244, 35, 232), # Sidewalk 52 | (107, 142, 35), # Vegetation 53 | (0, 0, 142), # Vehicle 54 | (102, 102, 156), # Wall 55 | (220, 220, 0), # TrafficSign 56 | (70, 130, 180), # Sky 57 | (81, 0, 81), # Ground 58 | (150, 100, 100), # Bridge 59 | (230, 150, 140), # RailTrack 60 | (180, 165, 180), # GuardRail 61 | (250, 170, 30), # TrafficLight 62 | (110, 190, 160), # Static 63 | (170, 120, 50), # Dynamic 64 | (45, 60, 150), # Water 65 | (145, 170, 100), # Terrain 66 | ]) / 255.0 # normalize each channel [0-1] since is what Open3D uses 67 | 68 | 69 | def parser(): 70 | argparser = argparse.ArgumentParser( 71 | description=__doc__) 72 | argparser.add_argument( 73 | '--host', 74 | metavar='H', 75 | default='localhost', 76 | help='IP of the host CARLA Simulator (default: localhost)') 77 | argparser.add_argument( 78 | '-p', '--port', 79 | metavar='P', 80 | default=2000, 81 | type=int, 82 | help='TCP port of CARLA Simulator (default: 2000)') 83 | argparser.add_argument( 84 | '--no-rendering', 85 | action='store_true', 86 | help='use the no-rendering mode which will provide some extra' 87 | ' performance but you will lose the articulated objects in the' 88 | ' lidar, such as pedestrians') 89 | argparser.add_argument( 90 | '--semantic', 91 | action='store_true', 92 | help='use the semantic lidar instead, which provides ground truth' 93 | ' information') 94 | argparser.add_argument( 95 | '--no-noise', 96 | action='store_true', 97 | help='remove the drop off and noise from the normal (non-semantic) lidar') 98 | argparser.add_argument( 99 | '--no-autopilot', 100 | action='store_false', 101 | help='disables the autopilot so the vehicle will remain stopped') 102 | argparser.add_argument( 103 | '--show-axis', 104 | action='store_true', 105 | help='show the cartesian coordinates axis') 106 | argparser.add_argument( 107 | '--filter', 108 | metavar='PATTERN', 109 | default='model3', 110 | help='actor filter (default: "vehicle.*")') 111 | argparser.add_argument( 112 | '--upper-fov', 113 | default=15.0, 114 | type=float, 115 | help='lidar\'s upper field of view in degrees (default: 15.0)') 116 | argparser.add_argument( 117 | '--lower-fov', 118 | default=-25.0, 119 | type=float, 120 | help='lidar\'s lower field of view in degrees (default: -25.0)') 121 | argparser.add_argument( 122 | '--channels', 123 | default=64.0, 124 | type=float, 125 | help='lidar\'s channel count (default: 64)') 126 | argparser.add_argument( 127 | '--range', 128 | default=100.0, 129 | type=float, 130 | help='lidar\'s maximum range in meters (default: 100.0)') 131 | argparser.add_argument( 132 | '--points-per-second', 133 | default=200000, 134 | type=int, 135 | help='lidar\'s points per second (default: 500000)') 136 | argparser.add_argument( 137 | '-x', 138 | default=0.0, 139 | type=float, 140 | help='offset in the sensor position in the X-axis in meters (default: 0.0)') 141 | argparser.add_argument( 142 | '-y', 143 | default=0.0, 144 | type=float, 145 | help='offset in the sensor position in the Y-axis in meters (default: 0.0)') 146 | argparser.add_argument( 147 | '-z', 148 | default=0.0, 149 | type=float, 150 | help='offset in the sensor position in the Z-axis in meters (default: 0.0)') 151 | ''' 152 | argparser.add_argument( 153 | '--im-width', 154 | default=1920, 155 | type=int, 156 | help='image width (default: 1920)' 157 | ) 158 | argparser.add_argument( 159 | '--im-height', 160 | default=1080, 161 | type=int, 162 | help='image height (default: 1080)' 163 | ) 164 | ''' 165 | argparser.add_argument( 166 | '--not_save', 167 | action='store_true', 168 | help='disables to save the data to the disk') 169 | argparser.add_argument( 170 | '--set_start_end', 171 | action='store_true', 172 | help='set start and destination position') 173 | 174 | return argparser.parse_args() 175 | 176 | 177 | # Sensor callback. 178 | # This is where you receive the sensor data and 179 | # process it as you liked and the important part is that, 180 | # at the end, it should include an element into the sensor queue. 181 | def sensor_callback(sensor_data, sensor_queue, sensor_name, args): 182 | # Do stuff with the sensor_data data like save it to disk 183 | # Then you just need to add to the queue 184 | 185 | if 'camera' in sensor_name: 186 | array = np.frombuffer(sensor_data.raw_data, dtype=np.dtype("uint8")) 187 | array = np.reshape(array, (600, 800, 4)) 188 | array = array[:, :, :3] 189 | array = array[:, :, ::-1] 190 | # array = pygame.surfarray.make_surface(array.swapaxes(0, 1)) 191 | im = Image.fromarray(array) 192 | if not args.not_save: 193 | outputImgPath="../output/img/" 194 | filename = datetime.now().strftime("%Y-%m-%d-%H-%M-%S-%f") 195 | if not os.path.exists(outputImgPath): 196 | os.makedirs(outputImgPath) 197 | im.save(outputImgPath+str(filename)+'.jpg') 198 | # sensor_data.save_to_disk(os.path.join('../outputs/output_synchronized', '%06d.png' % sensor_data.frame)) 199 | 200 | if 'lidar' in sensor_name: 201 | """Prepares a point cloud with intensity 202 | colors ready to be consumed by Open3D""" 203 | data = np.copy(np.frombuffer(sensor_data.raw_data, dtype=np.dtype('f4'))) 204 | data = np.reshape(data, (int(data.shape[0] / 4), 4)) 205 | if not args.not_save: 206 | outputLidarPath="../output/lidar/" 207 | filename = datetime.now().strftime("%Y-%m-%d-%H-%M-%S-%f") 208 | if not os.path.exists(outputLidarPath): 209 | os.makedirs(outputLidarPath) 210 | np.savetxt(outputLidarPath+str(filename)+'.txt', data) 211 | 212 | # Isolate the intensity and compute a color for it 213 | intensity = data[:, -1] 214 | intensity_col = 1.0 - np.log(intensity) / np.log(np.exp(-0.004 * 100)) 215 | int_color = np.c_[ 216 | np.interp(intensity_col, VID_RANGE, VIRIDIS[:, 0]), 217 | np.interp(intensity_col, VID_RANGE, VIRIDIS[:, 1]), 218 | np.interp(intensity_col, VID_RANGE, VIRIDIS[:, 2])] 219 | 220 | # Isolate the 3D data 221 | points = data[:, :-1] 222 | 223 | # We're negating the y to correclty visualize a world that matches 224 | # what we see in Unreal since Open3D uses a right-handed coordinate system 225 | points[:, :1] = -points[:, :1] 226 | 227 | # # An example of converting points from sensor to vehicle space if we had 228 | # # a carla.Transform variable named "tran": 229 | # points = np.append(points, np.ones((points.shape[0], 1)), axis=1) 230 | # points = np.dot(tran.get_matrix(), points.T).T 231 | # points = points[:, :-1] 232 | 233 | vis_points = o3d.utility.Vector3dVector(points) 234 | vis_colors = o3d.utility.Vector3dVector(int_color) 235 | 236 | 237 | 238 | if 'gnss' in sensor_name: 239 | data = np.array([sensor_data.transform.location.x, sensor_data.transform.location.y, sensor_data.transform.location.z, \ 240 | sensor_data.transform.rotation.pitch, sensor_data.transform.rotation.yaw, sensor_data.transform.rotation.roll, \ 241 | sensor_data.latitude, sensor_data.longitude, sensor_data.altitude]) 242 | if not args.not_save: 243 | outputGnssPath = '../output/gnss/' 244 | filename = datetime.now().strftime("%Y-%m-%d-%H-%M-%S-%f") 245 | if not os.path.exists(outputGnssPath): 246 | os.makedirs(outputGnssPath) 247 | np.savetxt(outputGnssPath+str(filename)+'.txt', data) 248 | if 'imu' in sensor_name: 249 | #print(sensor_data.accelerometer, sensor_data.gyroscope, sensor_data.compass) 250 | data = np.array([sensor_data.accelerometer.x, sensor_data.accelerometer.y, sensor_data.accelerometer.z, \ 251 | sensor_data.gyroscope.x, sensor_data.gyroscope.y, sensor_data.gyroscope.z,sensor_data.compass]) 252 | if not args.not_save: 253 | outputIMUPath = '../output/imu/' 254 | filename = datetime.now().strftime("%Y-%m-%d-%H-%M-%S-%f") 255 | if not os.path.exists(outputIMUPath): 256 | os.makedirs(outputIMUPath) 257 | np.savetxt(outputIMUPath+str(filename)+'.txt', data) 258 | 259 | if 'camera' in sensor_name: 260 | sensor_queue.put((sensor_data.frame, sensor_name, array)) 261 | elif 'lidar' in sensor_name: 262 | sensor_queue.put((sensor_data.frame, sensor_name, vis_points, vis_colors)) 263 | else: 264 | sensor_queue.put((sensor_data.frame, sensor_name)) 265 | 266 | def lidarDisplayWin(): 267 | vis = o3d.visualization.Visualizer() 268 | vis.create_window( 269 | window_name='Carla Lidar', 270 | width=960, 271 | height=540, 272 | left=480, 273 | top=270) 274 | vis.get_render_option().background_color = [0.05, 0.05, 0.05] 275 | vis.get_render_option().point_size = 1 276 | vis.get_render_option().show_coordinate_frame = True 277 | 278 | # if args.show_axis: 279 | # add_open3d_axis(vis) 280 | 281 | return vis 282 | 283 | def set_start_end_pos(world): 284 | 285 | '''TODO: specify positions as the start and destination''' 286 | 287 | all_optional_position = world.get_map().get_spawn_points() 288 | spawn_position = random.choice(all_optional_position) 289 | random.shuffle(all_optional_position) 290 | if all_optional_position[0] != spawn_position: 291 | destination = all_optional_position[0] 292 | else: 293 | destination = all_optional_position[1] 294 | return spawn_position, destination 295 | 296 | 297 | 298 | def main(): 299 | # We start creating the client 300 | 301 | args = parser() 302 | #args.width, args.height = [int(x) for x in args.res.split('x')] 303 | client = carla.Client(args.host, args.port) 304 | client.set_timeout(2.0) 305 | world = client.get_world() 306 | 307 | try: 308 | # We need to save the settings to be able to recover them at the end 309 | # of the script to leave the server in the same state that we found it. 310 | original_settings = world.get_settings() 311 | settings = world.get_settings() 312 | traffic_manager = client.get_trafficmanager(8000) 313 | traffic_manager.set_synchronous_mode(True) # hzx: In synchronous mode, autopilot depends on the traffic manager 314 | 315 | # We set CARLA syncronous mode 316 | delta = 0.05 317 | settings.fixed_delta_seconds = delta # hzx: set fixed time-step, one tick = 50ms, i.e. 20fps 318 | settings.synchronous_mode = True # hzx: In synchronous mode, server will wait the client to finish its current work, then update 319 | settings.no_rendering_mode = args.no_rendering 320 | world.apply_settings(settings) 321 | 322 | 323 | # hzx: create ego vehicle 324 | blueprint_library = world.get_blueprint_library() # hzx: create blueprint object 325 | vehicle_bp = blueprint_library.filter(args.filter)[0] # hzx: get the vehicle(model3) 326 | if args.set_start_end: 327 | vehicle_transform, vehicle_destination = set_start_end_pos(world) 328 | else: 329 | vehicle_transform = random.choice(world.get_map().get_spawn_points()) # hzx: generate the spawn position randomly 330 | #print('start position:', vehicle_transform) 331 | vehicle = world.spawn_actor(vehicle_bp, vehicle_transform) # hzx: generate the ego vehicle 332 | vehicle.set_autopilot(args.no_autopilot) # hzx: default:True, run autopilot 333 | traffic_manager.ignore_lights_percentage(vehicle, 100) # hzx: ignore the traffic ligths 334 | 335 | # hzx: add spectator for monitoring better 336 | spectator = world.get_spectator() 337 | #spec_transform = vehicle.get_transform() 338 | #spectator.set_transform(carla.Transform(spec_transform.location + carla.Location(z=20), carla.Rotation(pitch=-90))) 339 | 340 | # We create the sensor queue in which we keep track of the information 341 | # already received. This structure is thread safe and can be 342 | # accessed by all the sensors callback concurrently without problem. 343 | sensor_queue = Queue() 344 | 345 | # Bluepints for the sensors 346 | blueprint_library = world.get_blueprint_library() 347 | cam_bp = blueprint_library.find('sensor.camera.rgb') 348 | #camera_bp.set_attribute('image_size_x', str(1920)) # hzx: image width 349 | #camera_bp.set_attribute('image_size_y', str(1080)) # hzx: image height 350 | #camera_bp.set_attribute('fov', '90') # hzx: image field of view 351 | #camera_bp.set_attribute('sensor_tick', str(0.04)) # hzx: simulation seconds between sensor captures (ticks) 352 | 353 | lidar_bp = blueprint_library.find('sensor.lidar.ray_cast') 354 | 355 | # hzx: Blueprints for the gnss 356 | gnss_bp = blueprint_library.find('sensor.other.gnss') 357 | 358 | # hzx: Blueprints for the IMU 359 | imu_bp = blueprint_library.find('sensor.other.imu') 360 | 361 | 362 | 363 | user_offset = carla.Location(args.x, args.y, args.z) 364 | 365 | # We create all the sensors and keep them in a list for convenience. 366 | sensor_list = [] 367 | 368 | # camera relative position related to the vehicle 369 | #camera_transform = carla.Transform(carla.Location(0, 0, 2)) 370 | camera_transform = carla.Transform(carla.Location(x=-0.5, z=1.8) + user_offset) 371 | 372 | cam01 = world.spawn_actor(cam_bp, camera_transform, attach_to=vehicle) 373 | # set the callback function 374 | cam01.listen(lambda data: sensor_callback(data, sensor_queue, "camera01", args)) 375 | sensor_list.append(cam01) 376 | 377 | #lidar_bp.set_attribute('points_per_second', '100000') 378 | lidar_bp.set_attribute('upper_fov', str(args.upper_fov)) 379 | lidar_bp.set_attribute('lower_fov', str(args.lower_fov)) 380 | lidar_bp.set_attribute('channels', str(args.channels)) 381 | lidar_bp.set_attribute('range', str(args.range)) 382 | lidar_bp.set_attribute('rotation_frequency', str(1.0 / delta)) 383 | lidar_bp.set_attribute('points_per_second', str(args.points_per_second)) 384 | 385 | lidar01_transform = carla.Transform(carla.Location(x=-0.5, z=1.8) + user_offset) 386 | lidar01 = world.spawn_actor(lidar_bp, lidar01_transform, attach_to=vehicle) 387 | lidar01.listen(lambda data: sensor_callback(data, sensor_queue, "lidar01", args)) 388 | sensor_list.append(lidar01) 389 | 390 | gnss_transform = carla.Transform(carla.Location(x=-0.5, z=1.8) + user_offset) 391 | gnss = world.spawn_actor(gnss_bp, gnss_transform, attach_to=vehicle) 392 | gnss.listen(lambda data: sensor_callback(data, sensor_queue, "gnss", args)) 393 | sensor_list.append(gnss) 394 | 395 | imu_transform = carla.Transform(carla.Location(x=-0.5, z=1.8) + user_offset) 396 | imu = world.spawn_actor(imu_bp, imu_transform, attach_to=vehicle) 397 | imu.listen(lambda data: sensor_callback(data, sensor_queue, "imu", args)) 398 | sensor_list.append(imu) 399 | 400 | print('display the lidar and camera image') 401 | # hzx: lidar display window 402 | point_list = o3d.geometry.PointCloud() 403 | vis = lidarDisplayWin() 404 | # hzx: image display window 405 | cv2.namedWindow("front_cam", 0) 406 | #cv2.resizeWindow('front_cam', 400, 300) 407 | 408 | frame = 0 409 | # Main loop 410 | while True: 411 | # Tick the server 412 | world.tick() 413 | w_frame = world.get_snapshot().frame 414 | print("\nWorld's frame: %d" % w_frame) 415 | 416 | # hzx: set the sectator to follow the ego vehicle 417 | spec_transform = vehicle.get_transform() 418 | spectator.set_transform(carla.Transform(spec_transform.location + carla.Location(z=20), carla.Rotation(pitch=-90))) 419 | 420 | # Now, we wait to the sensors data to be received. 421 | # As the queue is blocking, we will wait in the queue.get() methods 422 | # until all the information is processed and we continue with the next frame. 423 | # We include a timeout of 1.0 s (in the get method) and if some information is 424 | # not received in this time we continue. 425 | try: 426 | for _ in range(len(sensor_list)): 427 | s_frame = sensor_queue.get(True, 1.0) # hzx: neccessary for synchronous mode 428 | print(" Frame: %d Sensor: %s" % (s_frame[0], s_frame[1])) 429 | if 'camera' in s_frame[1]: 430 | im_dis = s_frame[2][:, :, ::-1] 431 | cv2.imshow("front_cam",im_dis) 432 | cv2.waitKey(1) 433 | 434 | if 'lidar' in s_frame[1]: 435 | point_list.points = s_frame[2] 436 | point_list.colors = s_frame[3] 437 | if frame == 0: 438 | vis.add_geometry(point_list) 439 | vis.update_geometry(point_list) 440 | 441 | vis.poll_events() 442 | vis.update_renderer() 443 | # # This can fix Open3D jittering issues: 444 | time.sleep(0.001) 445 | frame += 1 446 | 447 | 448 | 449 | except Empty: 450 | print("Some of the sensor information is missed") 451 | # hzx: for other exceptions, break the loop and release the resources. 452 | except: 453 | print('other exception') 454 | break 455 | 456 | finally: 457 | world.apply_settings(original_settings) # hzx: back to original setting, otherwise, the world will crash as it can't find the synchronous client 458 | vehicle.destroy() 459 | for sensor in sensor_list: 460 | sensor.destroy() 461 | vis.destroy_window() 462 | cv2.destroyAllWindows() 463 | 464 | 465 | if __name__ == "__main__": 466 | try: 467 | main() 468 | except KeyboardInterrupt: 469 | print(' - Exited by user.') 470 | 471 | 472 | -------------------------------------------------------------------------------- /route_capture_multiple_sensors.py: -------------------------------------------------------------------------------- 1 | 2 | #!/usr/bin/env python 3 | 4 | # Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma de 5 | # Barcelona (UAB). 6 | # 7 | # This work is licensed under the terms of the MIT license. 8 | # For a copy, see . 9 | 10 | 11 | """ 12 | We provide an example code to capture data for multiple sensors in synchronous mode on [Carla](https://carla.org/) platform. 13 | """ 14 | 15 | import glob 16 | import os 17 | import sys 18 | import argparse 19 | import random 20 | import time 21 | from datetime import datetime 22 | import numpy as np 23 | from PIL import Image 24 | from matplotlib import cm 25 | import open3d as o3d 26 | import cv2 27 | from queue import Queue 28 | from queue import Empty 29 | 30 | try: 31 | sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( 32 | sys.version_info.major, 33 | sys.version_info.minor, 34 | 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) 35 | except IndexError: 36 | pass 37 | 38 | import carla 39 | from Import.agents.navigation.behavior_agent import BehaviorAgent 40 | 41 | VIRIDIS = np.array(cm.get_cmap('plasma').colors) 42 | VID_RANGE = np.linspace(0.0, 1.0, VIRIDIS.shape[0]) 43 | LABEL_COLORS = np.array([ 44 | (255, 255, 255), # None 45 | (70, 70, 70), # Building 46 | (100, 40, 40), # Fences 47 | (55, 90, 80), # Other 48 | (220, 20, 60), # Pedestrian 49 | (153, 153, 153), # Pole 50 | (157, 234, 50), # RoadLines 51 | (128, 64, 128), # Road 52 | (244, 35, 232), # Sidewalk 53 | (107, 142, 35), # Vegetation 54 | (0, 0, 142), # Vehicle 55 | (102, 102, 156), # Wall 56 | (220, 220, 0), # TrafficSign 57 | (70, 130, 180), # Sky 58 | (81, 0, 81), # Ground 59 | (150, 100, 100), # Bridge 60 | (230, 150, 140), # RailTrack 61 | (180, 165, 180), # GuardRail 62 | (250, 170, 30), # TrafficLight 63 | (110, 190, 160), # Static 64 | (170, 120, 50), # Dynamic 65 | (45, 60, 150), # Water 66 | (145, 170, 100), # Terrain 67 | ]) / 255.0 # normalize each channel [0-1] since is what Open3D uses 68 | 69 | 70 | def parser(): 71 | argparser = argparse.ArgumentParser( 72 | description=__doc__) 73 | argparser.add_argument( 74 | '--host', 75 | metavar='H', 76 | default='localhost', 77 | help='IP of the host CARLA Simulator (default: localhost)') 78 | argparser.add_argument( 79 | '-p', '--port', 80 | metavar='P', 81 | default=2000, 82 | type=int, 83 | help='TCP port of CARLA Simulator (default: 2000)') 84 | argparser.add_argument( 85 | '--no-rendering', 86 | action='store_true', 87 | help='use the no-rendering mode which will provide some extra' 88 | ' performance but you will lose the articulated objects in the' 89 | ' lidar, such as pedestrians') 90 | argparser.add_argument( 91 | '--semantic', 92 | action='store_true', 93 | help='use the semantic lidar instead, which provides ground truth' 94 | ' information') 95 | argparser.add_argument( 96 | '--no-noise', 97 | action='store_true', 98 | help='remove the drop off and noise from the normal (non-semantic) lidar') 99 | argparser.add_argument( 100 | '--no-autopilot', 101 | action='store_false', 102 | help='disables the autopilot so the vehicle will remain stopped') 103 | argparser.add_argument( 104 | '--show-axis', 105 | action='store_true', 106 | help='show the cartesian coordinates axis') 107 | argparser.add_argument( 108 | '--filter', 109 | metavar='PATTERN', 110 | default='vehicle.lincoln.mkz_2020', 111 | help='actor filter (default: "vehicle.*")') 112 | argparser.add_argument( 113 | '--upper-fov', 114 | default=15.0, 115 | type=float, 116 | help='lidar\'s upper field of view in degrees (default: 15.0)') 117 | argparser.add_argument( 118 | '--lower-fov', 119 | default=-25.0, 120 | type=float, 121 | help='lidar\'s lower field of view in degrees (default: -25.0)') 122 | argparser.add_argument( 123 | '--channels', 124 | default=64.0, 125 | type=float, 126 | help='lidar\'s channel count (default: 64)') 127 | argparser.add_argument( 128 | '--range', 129 | default=100.0, 130 | type=float, 131 | help='lidar\'s maximum range in meters (default: 100.0)') 132 | argparser.add_argument( 133 | '--points-per-second', 134 | default=200000, 135 | type=int, 136 | help='lidar\'s points per second (default: 500000)') 137 | argparser.add_argument( 138 | '-x', 139 | default=0.0, 140 | type=float, 141 | help='offset in the sensor position in the X-axis in meters (default: 0.0)') 142 | argparser.add_argument( 143 | '-y', 144 | default=0.0, 145 | type=float, 146 | help='offset in the sensor position in the Y-axis in meters (default: 0.0)') 147 | argparser.add_argument( 148 | '-z', 149 | default=0.0, 150 | type=float, 151 | help='offset in the sensor position in the Z-axis in meters (default: 0.0)') 152 | argparser.add_argument( 153 | '--not_save', 154 | action='store_true', 155 | help='disables to save the data to the disk') 156 | argparser.add_argument( 157 | '--route', 158 | help='Set the route file (default: ./Route_Town07.txt)', 159 | default='./Route_Town07.txt', 160 | type=str) 161 | argparser.add_argument( 162 | '--fix_route', 163 | help='specify the start and target points (default: False)', 164 | action='store_true') 165 | argparser.add_argument( 166 | '--enable_traffic_light', 167 | help='enable_traffic_light (default: False)', 168 | action='store_true') 169 | 170 | return argparser.parse_args() 171 | 172 | 173 | # Sensor callback. 174 | # This is where you receive the sensor data and 175 | # process it as you liked and the important part is that, 176 | # at the end, it should include an element into the sensor queue. 177 | def sensor_callback(sensor_data, sensor_queue, sensor_name, args): 178 | # Do stuff with the sensor_data data like save it to disk 179 | # Then you just need to add to the queue 180 | 181 | if 'camera' in sensor_name: 182 | array = np.frombuffer(sensor_data.raw_data, dtype=np.dtype("uint8")) 183 | array = np.reshape(array, (600, 800, 4)) 184 | array = array[:, :, :3] 185 | array = array[:, :, ::-1] 186 | # array = pygame.surfarray.make_surface(array.swapaxes(0, 1)) 187 | im = Image.fromarray(array) 188 | if not args.not_save: 189 | outputImgPath="../output/img/" 190 | filename = datetime.now().strftime("%Y-%m-%d-%H-%M-%S-%f") 191 | if not os.path.exists(outputImgPath): 192 | os.makedirs(outputImgPath) 193 | im.save(outputImgPath+str(filename)+'.jpg') 194 | # sensor_data.save_to_disk(os.path.join('../outputs/output_synchronized', '%06d.png' % sensor_data.frame)) 195 | 196 | if 'lidar' in sensor_name: 197 | """Prepares a point cloud with intensity 198 | colors ready to be consumed by Open3D""" 199 | data = np.copy(np.frombuffer(sensor_data.raw_data, dtype=np.dtype('f4'))) 200 | data = np.reshape(data, (int(data.shape[0] / 4), 4)) 201 | if not args.not_save: 202 | outputLidarPath="../output/lidar/" 203 | filename = datetime.now().strftime("%Y-%m-%d-%H-%M-%S-%f") 204 | if not os.path.exists(outputLidarPath): 205 | os.makedirs(outputLidarPath) 206 | np.savetxt(outputLidarPath+str(filename)+'.txt', data) 207 | 208 | # Isolate the intensity and compute a color for it 209 | intensity = data[:, -1] 210 | intensity_col = 1.0 - np.log(intensity) / np.log(np.exp(-0.004 * 100)) 211 | int_color = np.c_[ 212 | np.interp(intensity_col, VID_RANGE, VIRIDIS[:, 0]), 213 | np.interp(intensity_col, VID_RANGE, VIRIDIS[:, 1]), 214 | np.interp(intensity_col, VID_RANGE, VIRIDIS[:, 2])] 215 | 216 | # Isolate the 3D data 217 | points = data[:, :-1] 218 | 219 | # We're negating the y to correclty visualize a world that matches 220 | # what we see in Unreal since Open3D uses a right-handed coordinate system 221 | points[:, :1] = -points[:, :1] 222 | 223 | # # An example of converting points from sensor to vehicle space if we had 224 | # # a carla.Transform variable named "tran": 225 | # points = np.append(points, np.ones((points.shape[0], 1)), axis=1) 226 | # points = np.dot(tran.get_matrix(), points.T).T 227 | # points = points[:, :-1] 228 | 229 | vis_points = o3d.utility.Vector3dVector(points) 230 | vis_colors = o3d.utility.Vector3dVector(int_color) 231 | 232 | 233 | 234 | if 'gnss' in sensor_name: 235 | data = np.array([sensor_data.transform.location.x, sensor_data.transform.location.y, sensor_data.transform.location.z, \ 236 | sensor_data.transform.rotation.pitch, sensor_data.transform.rotation.yaw, sensor_data.transform.rotation.roll, \ 237 | sensor_data.latitude, sensor_data.longitude, sensor_data.altitude]) 238 | if not args.not_save: 239 | outputGnssPath = '../output/gnss/' 240 | filename = datetime.now().strftime("%Y-%m-%d-%H-%M-%S-%f") 241 | if not os.path.exists(outputGnssPath): 242 | os.makedirs(outputGnssPath) 243 | np.savetxt(outputGnssPath+str(filename)+'.txt', data) 244 | if 'imu' in sensor_name: 245 | #print(sensor_data.accelerometer, sensor_data.gyroscope, sensor_data.compass) 246 | data = np.array([sensor_data.accelerometer.x, sensor_data.accelerometer.y, sensor_data.accelerometer.z, \ 247 | sensor_data.gyroscope.x, sensor_data.gyroscope.y, sensor_data.gyroscope.z,sensor_data.compass]) 248 | if not args.not_save: 249 | outputIMUPath = '../output/imu/' 250 | filename = datetime.now().strftime("%Y-%m-%d-%H-%M-%S-%f") 251 | if not os.path.exists(outputIMUPath): 252 | os.makedirs(outputIMUPath) 253 | np.savetxt(outputIMUPath+str(filename)+'.txt', data) 254 | 255 | if 'camera' in sensor_name: 256 | sensor_queue.put((sensor_data.frame, sensor_name, array)) 257 | elif 'lidar' in sensor_name: 258 | sensor_queue.put((sensor_data.frame, sensor_name, vis_points, vis_colors)) 259 | else: 260 | sensor_queue.put((sensor_data.frame, sensor_name)) 261 | 262 | def lidarDisplayWin(): 263 | vis = o3d.visualization.Visualizer() 264 | vis.create_window( 265 | window_name='Carla Lidar', 266 | width=960, 267 | height=540, 268 | left=480, 269 | top=270) 270 | vis.get_render_option().background_color = [0.05, 0.05, 0.05] 271 | vis.get_render_option().point_size = 1 272 | vis.get_render_option().show_coordinate_frame = True 273 | 274 | # if args.show_axis: 275 | # add_open3d_axis(vis) 276 | 277 | return vis 278 | 279 | def set_start_end_pos(world, args, mode = 'random'): 280 | if mode == 'random': 281 | all_optional_position = world.get_map().get_spawn_points() 282 | spawn_point = random.choice(all_optional_position) 283 | random.shuffle(all_optional_position) 284 | if all_optional_position[0] != spawn_point: 285 | destination = all_optional_position[0] 286 | else: 287 | destination = all_optional_position[1] 288 | elif mode == 'manual': 289 | f = open(args.route, 'r') 290 | data = f.readlines() 291 | data = data[1:] 292 | line_split = [float(i) for i in data[0].split(" ") if i != ""] 293 | spawn_point = carla.Transform(carla.Location(line_split[0], line_split[1], line_split[2]), \ 294 | carla.Rotation(line_split[3], line_split[4], line_split[5])) 295 | destination = carla.Location(line_split[6], line_split[7], line_split[8]) 296 | return spawn_point, destination 297 | 298 | def modify_vehicle_physics(actor): 299 | #If actor is not a vehicle, we cannot use the physics control 300 | try: 301 | physics_control = actor.get_physics_control() 302 | physics_control.use_sweep_wheel_collision = True 303 | actor.apply_physics_control(physics_control) 304 | except Exception: 305 | pass 306 | 307 | def main(): 308 | # We start creating the client 309 | 310 | args = parser() 311 | 312 | client = carla.Client(args.host, args.port) 313 | client.set_timeout(2.0) 314 | world = client.get_world() 315 | 316 | try: 317 | # We need to save the settings to be able to recover them at the end 318 | # of the script to leave the server in the same state that we found it. 319 | original_settings = world.get_settings() 320 | settings = world.get_settings() 321 | traffic_manager = client.get_trafficmanager(8000) 322 | traffic_manager.set_synchronous_mode(True) # hzx: In synchronous mode, autopilot depends on the traffic manager 323 | 324 | # We set CARLA syncronous mode 325 | delta = 0.05 326 | settings.fixed_delta_seconds = delta # hzx: set fixed time-step, one tick = 50ms, i.e. 20fps 327 | settings.synchronous_mode = True # hzx: In synchronous mode, server will wait the client to finish its current work, then update 328 | settings.no_rendering_mode = args.no_rendering 329 | world.apply_settings(settings) 330 | 331 | 332 | # hzx: create ego vehicle 333 | blueprint_library = world.get_blueprint_library() # hzx: create blueprint object 334 | vehicle_bp = blueprint_library.filter(args.filter)[0] # hzx: get the vehicle 335 | if args.fix_route: 336 | vehicle_transform, vehicle_destination = set_start_end_pos(world, args, mode='manual') 337 | else: 338 | # hzx: generate the spawn position and destination randomly 339 | vehicle_transform, vehicle_destination = set_start_end_pos(world, args, mode='random') 340 | #print('start position:', vehicle_transform) 341 | vehicle = world.spawn_actor(vehicle_bp, vehicle_transform) # hzx: generate the ego vehicle 342 | #vehicle = world.try_spawn_actor(vehicle_bp, vehicle_transform) 343 | 344 | #modify_vehicle_physics(vehicle) 345 | #vehicle.set_autopilot(True) # hzx: default:True, run autopilot 346 | #traffic_manager.ignore_lights_percentage(vehicle, 100) # hzx: ignore the traffic ligths 347 | 348 | # hzx: add spectator for monitoring better 349 | spectator = world.get_spectator() 350 | ori_spec_tran = spectator.get_transform() 351 | #spec_transform = vehicle.get_transform() 352 | #spectator.set_transform(carla.Transform(spec_transform.location + carla.Location(z=20), carla.Rotation(pitch=-90))) 353 | 354 | # We create the sensor queue in which we keep track of the information 355 | # already received. This structure is thread safe and can be 356 | # accessed by all the sensors callback concurrently without problem. 357 | sensor_queue = Queue() 358 | 359 | # Bluepints for the sensors 360 | blueprint_library = world.get_blueprint_library() 361 | cam_bp = blueprint_library.find('sensor.camera.rgb') 362 | #camera_bp.set_attribute('image_size_x', str(1920)) # hzx: image width 363 | #camera_bp.set_attribute('image_size_y', str(1080)) # hzx: image height 364 | #camera_bp.set_attribute('fov', '90') # hzx: image field of view 365 | #camera_bp.set_attribute('sensor_tick', str(0.04)) # hzx: simulation seconds between sensor captures (ticks) 366 | 367 | lidar_bp = blueprint_library.find('sensor.lidar.ray_cast') 368 | 369 | # hzx: Blueprints for the gnss 370 | gnss_bp = blueprint_library.find('sensor.other.gnss') 371 | 372 | # hzx: Blueprints for the IMU 373 | imu_bp = blueprint_library.find('sensor.other.imu') 374 | 375 | user_offset = carla.Location(args.x, args.y, args.z) 376 | 377 | # We create all the sensors and keep them in a list for convenience. 378 | sensor_list = [] 379 | 380 | # camera relative position related to the vehicle 381 | #camera_transform = carla.Transform(carla.Location(0, 0, 2)) 382 | camera_transform = carla.Transform(carla.Location(x=-0.5, z=1.8) + user_offset) 383 | 384 | cam01 = world.spawn_actor(cam_bp, camera_transform, attach_to=vehicle) 385 | # set the callback function 386 | cam01.listen(lambda data: sensor_callback(data, sensor_queue, "camera01", args)) 387 | sensor_list.append(cam01) 388 | 389 | #lidar_bp.set_attribute('points_per_second', '100000') 390 | lidar_bp.set_attribute('upper_fov', str(args.upper_fov)) 391 | lidar_bp.set_attribute('lower_fov', str(args.lower_fov)) 392 | lidar_bp.set_attribute('channels', str(args.channels)) 393 | lidar_bp.set_attribute('range', str(args.range)) 394 | lidar_bp.set_attribute('rotation_frequency', str(1.0 / delta)) 395 | lidar_bp.set_attribute('points_per_second', str(args.points_per_second)) 396 | 397 | lidar01_transform = carla.Transform(carla.Location(x=-0.5, z=1.8) + user_offset) 398 | lidar01 = world.spawn_actor(lidar_bp, lidar01_transform, attach_to=vehicle) 399 | lidar01.listen(lambda data: sensor_callback(data, sensor_queue, "lidar01", args)) 400 | sensor_list.append(lidar01) 401 | 402 | gnss_transform = carla.Transform(carla.Location(x=-0.5, z=1.8) + user_offset) 403 | gnss = world.spawn_actor(gnss_bp, gnss_transform, attach_to=vehicle) 404 | gnss.listen(lambda data: sensor_callback(data, sensor_queue, "gnss", args)) 405 | sensor_list.append(gnss) 406 | 407 | imu_transform = carla.Transform(carla.Location(x=-0.5, z=1.8) + user_offset) 408 | imu = world.spawn_actor(imu_bp, imu_transform, attach_to=vehicle) 409 | imu.listen(lambda data: sensor_callback(data, sensor_queue, "imu", args)) 410 | sensor_list.append(imu) 411 | 412 | 413 | world.tick() 414 | # hzx: create agent behavior 415 | agent = BehaviorAgent(vehicle, behavior='normal') 416 | agent.set_destination(vehicle_destination, agent._vehicle.get_location()) 417 | #print('start:', agent._vehicle.get_location(), 'destination:', vehicle_destination.location) 418 | 419 | print('display the lidar and camera image') 420 | # hzx: lidar display window 421 | point_list = o3d.geometry.PointCloud() 422 | vis = lidarDisplayWin() 423 | # hzx: image display window 424 | cv2.namedWindow("front_cam", 0) 425 | #cv2.resizeWindow('front_cam', 400, 300) 426 | 427 | frame = 0 428 | # Main loop 429 | while True: 430 | # Tick the server 431 | #agent._update_information() 432 | 433 | world.tick() # we need to tick the world once to let the client update the spawn position 434 | w_frame = world.get_snapshot().frame 435 | #print("\nWorld's frame: %d" % w_frame) 436 | 437 | #print('length waypoints queue:', len(agent._local_planner._waypoints_queue)) 438 | #if len(agent._local_planner._waypoints_queue) < 1: 439 | 440 | 441 | #control = agent.run_step() 442 | #vehicle.apply_control(control) 443 | if agent.done(): 444 | print('Arrive at the target point!') 445 | break 446 | 447 | speed_limit = vehicle.get_speed_limit() 448 | agent.get_local_planner().set_speed(speed_limit) 449 | 450 | control = agent.run_step(args, debug=False) 451 | #control.manual_gear_shift = False 452 | vehicle.apply_control(control) 453 | 454 | # hzx: set the sectator to follow the ego vehicle 455 | spec_transform = vehicle.get_transform() 456 | spectator.set_transform(carla.Transform(spec_transform.location + carla.Location(z=40), carla.Rotation(pitch=-90))) 457 | #print(vehicle_destination.location, spec_transform.location) 458 | 459 | # Now, we wait to the sensors data to be received. 460 | # As the queue is blocking, we will wait in the queue.get() methods 461 | # until all the information is processed and we continue with the next frame. 462 | # We include a timeout of 1.0 s (in the get method) and if some information is 463 | # not received in this time we continue. 464 | try: 465 | 466 | for _ in range(len(sensor_list)): 467 | s_frame = sensor_queue.get(True, 1.0) # hzx: neccessary for synchronous mode 468 | #print(" Frame: %d Sensor: %s" % (s_frame[0], s_frame[1])) 469 | if 'camera' in s_frame[1]: 470 | im_dis = s_frame[2][:, :, ::-1] 471 | cv2.imshow("front_cam",im_dis) 472 | cv2.waitKey(1) 473 | 474 | if 'lidar' in s_frame[1]: 475 | point_list.points = s_frame[2] 476 | point_list.colors = s_frame[3] 477 | if frame == 0: 478 | vis.add_geometry(point_list) 479 | vis.update_geometry(point_list) 480 | 481 | vis.poll_events() 482 | vis.update_renderer() 483 | # # This can fix Open3D jittering issues: 484 | time.sleep(0.001) 485 | 486 | frame += 1 487 | 488 | 489 | 490 | except Empty: 491 | print("Some of the sensor information is missed") 492 | # hzx: for other exceptions, break the loop and release the resources. 493 | except: 494 | print('other exception') 495 | break 496 | 497 | finally: 498 | world.apply_settings(original_settings) # hzx: back to original setting, otherwise, the world will crash as it can't find the synchronous client 499 | vehicle.destroy() 500 | spectator.set_transform(ori_spec_tran) 501 | for sensor in sensor_list: 502 | sensor.destroy() 503 | vis.destroy_window() 504 | cv2.destroyAllWindows() 505 | 506 | 507 | if __name__ == "__main__": 508 | try: 509 | main() 510 | except KeyboardInterrupt: 511 | print(' - Exited by user.') 512 | -------------------------------------------------------------------------------- /manual_control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de 4 | # Barcelona (UAB). 5 | # 6 | # This work is licensed under the terms of the MIT license. 7 | # For a copy, see . 8 | 9 | # Allows controlling a vehicle with a keyboard. For a simpler and more 10 | # documented example, please take a look at tutorial.py. 11 | 12 | """ 13 | Welcome to CARLA manual control. 14 | 15 | Use ARROWS or WASD keys for control. 16 | 17 | W : throttle 18 | S : brake 19 | A/D : steer left/right 20 | Q : toggle reverse 21 | Space : hand-brake 22 | P : toggle autopilot 23 | M : toggle manual transmission 24 | ,/. : gear up/down 25 | CTRL + W : toggle constant velocity mode at 60 km/h 26 | 27 | L : toggle next light type 28 | SHIFT + L : toggle high beam 29 | Z/X : toggle right/left blinker 30 | I : toggle interior light 31 | 32 | TAB : change sensor position 33 | ` or N : next sensor 34 | [1-9] : change to sensor [1-9] 35 | G : toggle radar visualization 36 | C : change weather (Shift+C reverse) 37 | Backspace : change vehicle 38 | 39 | V : Select next map layer (Shift+V reverse) 40 | B : Load current selected map layer (Shift+B to unload) 41 | 42 | R : toggle recording images to disk 43 | T : toggle vehicle's telemetry 44 | 45 | CTRL + R : toggle recording of simulation (replacing any previous) 46 | CTRL + P : start replaying last recorded simulation 47 | CTRL + + : increments the start time of the replay by 1 second (+SHIFT = 10 seconds) 48 | CTRL + - : decrements the start time of the replay by 1 second (+SHIFT = 10 seconds) 49 | 50 | F1 : toggle HUD 51 | H/? : toggle help 52 | ESC : quit 53 | """ 54 | 55 | from __future__ import print_function 56 | 57 | 58 | # ============================================================================== 59 | # -- find carla module --------------------------------------------------------- 60 | # ============================================================================== 61 | 62 | 63 | import glob 64 | import os 65 | import sys 66 | 67 | try: 68 | sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( 69 | sys.version_info.major, 70 | sys.version_info.minor, 71 | 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) 72 | except IndexError: 73 | pass 74 | 75 | 76 | # ============================================================================== 77 | # -- imports ------------------------------------------------------------------- 78 | # ============================================================================== 79 | 80 | 81 | import carla 82 | 83 | from carla import ColorConverter as cc 84 | 85 | import argparse 86 | import collections 87 | import datetime 88 | import logging 89 | import math 90 | import random 91 | import re 92 | import weakref 93 | 94 | try: 95 | import pygame 96 | from pygame.locals import KMOD_CTRL 97 | from pygame.locals import KMOD_SHIFT 98 | from pygame.locals import K_0 99 | from pygame.locals import K_9 100 | from pygame.locals import K_BACKQUOTE 101 | from pygame.locals import K_BACKSPACE 102 | from pygame.locals import K_COMMA 103 | from pygame.locals import K_DOWN 104 | from pygame.locals import K_ESCAPE 105 | from pygame.locals import K_F1 106 | from pygame.locals import K_LEFT 107 | from pygame.locals import K_PERIOD 108 | from pygame.locals import K_RIGHT 109 | from pygame.locals import K_SLASH 110 | from pygame.locals import K_SPACE 111 | from pygame.locals import K_TAB 112 | from pygame.locals import K_UP 113 | from pygame.locals import K_a 114 | from pygame.locals import K_b 115 | from pygame.locals import K_c 116 | from pygame.locals import K_d 117 | from pygame.locals import K_g 118 | from pygame.locals import K_h 119 | from pygame.locals import K_i 120 | from pygame.locals import K_l 121 | from pygame.locals import K_m 122 | from pygame.locals import K_n 123 | from pygame.locals import K_p 124 | from pygame.locals import K_q 125 | from pygame.locals import K_r 126 | from pygame.locals import K_s 127 | from pygame.locals import K_t 128 | from pygame.locals import K_v 129 | from pygame.locals import K_w 130 | from pygame.locals import K_x 131 | from pygame.locals import K_z 132 | from pygame.locals import K_MINUS 133 | from pygame.locals import K_EQUALS 134 | except ImportError: 135 | raise RuntimeError('cannot import pygame, make sure pygame package is installed') 136 | 137 | try: 138 | import numpy as np 139 | except ImportError: 140 | raise RuntimeError('cannot import numpy, make sure numpy package is installed') 141 | 142 | 143 | # ============================================================================== 144 | # -- Global functions ---------------------------------------------------------- 145 | # ============================================================================== 146 | 147 | 148 | def find_weather_presets(): 149 | rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') 150 | name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x)) 151 | presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] 152 | return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] 153 | 154 | 155 | def get_actor_display_name(actor, truncate=250): 156 | name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) 157 | return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name 158 | 159 | def get_actor_blueprints(world, filter, generation): 160 | bps = world.get_blueprint_library().filter(filter) 161 | 162 | if generation.lower() == "all": 163 | return bps 164 | 165 | # If the filter returns only one bp, we assume that this one needed 166 | # and therefore, we ignore the generation 167 | if len(bps) == 1: 168 | return bps 169 | 170 | try: 171 | int_generation = int(generation) 172 | # Check if generation is in available generations 173 | if int_generation in [1, 2]: 174 | bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] 175 | return bps 176 | else: 177 | print(" Warning! Actor Generation is not valid. No actor will be spawned.") 178 | return [] 179 | except: 180 | print(" Warning! Actor Generation is not valid. No actor will be spawned.") 181 | return [] 182 | 183 | 184 | # ============================================================================== 185 | # -- World --------------------------------------------------------------------- 186 | # ============================================================================== 187 | 188 | 189 | class World(object): 190 | def __init__(self, carla_world, hud, args): 191 | self.world = carla_world 192 | self.sync = args.sync 193 | self.actor_role_name = args.rolename 194 | try: 195 | self.map = self.world.get_map() 196 | except RuntimeError as error: 197 | print('RuntimeError: {}'.format(error)) 198 | print(' The server could not send the OpenDRIVE (.xodr) file:') 199 | print(' Make sure it exists, has the same name of your town, and is correct.') 200 | sys.exit(1) 201 | self.hud = hud 202 | self.player = None 203 | self.collision_sensor = None 204 | self.lane_invasion_sensor = None 205 | self.gnss_sensor = None 206 | self.imu_sensor = None 207 | self.radar_sensor = None 208 | self.camera_manager = None 209 | self._weather_presets = find_weather_presets() 210 | self._weather_index = 0 211 | self._actor_filter = args.filter 212 | self._actor_generation = args.generation 213 | self._gamma = args.gamma 214 | self.restart() 215 | self.world.on_tick(hud.on_world_tick) 216 | self.recording_enabled = False 217 | self.recording_start = 0 218 | self.constant_velocity_enabled = False 219 | self.show_vehicle_telemetry = False 220 | self.current_map_layer = 0 221 | self.map_layer_names = [ 222 | carla.MapLayer.NONE, 223 | carla.MapLayer.Buildings, 224 | carla.MapLayer.Decals, 225 | carla.MapLayer.Foliage, 226 | carla.MapLayer.Ground, 227 | carla.MapLayer.ParkedVehicles, 228 | carla.MapLayer.Particles, 229 | carla.MapLayer.Props, 230 | carla.MapLayer.StreetLights, 231 | carla.MapLayer.Walls, 232 | carla.MapLayer.All 233 | ] 234 | 235 | def restart(self): 236 | self.player_max_speed = 1.589 237 | self.player_max_speed_fast = 3.713 238 | # Keep same camera config if the camera manager exists. 239 | cam_index = self.camera_manager.index if self.camera_manager is not None else 0 240 | cam_pos_index = self.camera_manager.transform_index if self.camera_manager is not None else 0 241 | # Get a random blueprint. 242 | blueprint = random.choice(get_actor_blueprints(self.world, self._actor_filter, self._actor_generation)) 243 | blueprint.set_attribute('role_name', self.actor_role_name) 244 | if blueprint.has_attribute('color'): 245 | color = random.choice(blueprint.get_attribute('color').recommended_values) 246 | blueprint.set_attribute('color', color) 247 | if blueprint.has_attribute('driver_id'): 248 | driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) 249 | blueprint.set_attribute('driver_id', driver_id) 250 | if blueprint.has_attribute('is_invincible'): 251 | blueprint.set_attribute('is_invincible', 'true') 252 | # set the max speed 253 | if blueprint.has_attribute('speed'): 254 | self.player_max_speed = float(blueprint.get_attribute('speed').recommended_values[1]) 255 | self.player_max_speed_fast = float(blueprint.get_attribute('speed').recommended_values[2]) 256 | 257 | # Spawn the player. 258 | if self.player is not None: 259 | spawn_point = self.player.get_transform() 260 | spawn_point.location.z += 2.0 261 | spawn_point.rotation.roll = 0.0 262 | spawn_point.rotation.pitch = 0.0 263 | self.destroy() 264 | self.player = self.world.try_spawn_actor(blueprint, spawn_point) 265 | self.modify_vehicle_physics(self.player) 266 | while self.player is None: 267 | if not self.map.get_spawn_points(): 268 | print('There are no spawn points available in your map/town.') 269 | print('Please add some Vehicle Spawn Point to your UE4 scene.') 270 | sys.exit(1) 271 | spawn_points = self.map.get_spawn_points() 272 | spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() 273 | self.player = self.world.try_spawn_actor(blueprint, spawn_point) 274 | self.modify_vehicle_physics(self.player) 275 | # Set up the sensors. 276 | self.collision_sensor = CollisionSensor(self.player, self.hud) 277 | self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) 278 | self.gnss_sensor = GnssSensor(self.player) 279 | self.imu_sensor = IMUSensor(self.player) 280 | self.camera_manager = CameraManager(self.player, self.hud, self._gamma) 281 | self.camera_manager.transform_index = cam_pos_index 282 | self.camera_manager.set_sensor(cam_index, notify=False) 283 | actor_type = get_actor_display_name(self.player) 284 | self.hud.notification(actor_type) 285 | 286 | if self.sync: 287 | self.world.tick() 288 | else: 289 | self.world.wait_for_tick() 290 | 291 | def next_weather(self, reverse=False): 292 | self._weather_index += -1 if reverse else 1 293 | self._weather_index %= len(self._weather_presets) 294 | preset = self._weather_presets[self._weather_index] 295 | self.hud.notification('Weather: %s' % preset[1]) 296 | self.player.get_world().set_weather(preset[0]) 297 | 298 | def next_map_layer(self, reverse=False): 299 | self.current_map_layer += -1 if reverse else 1 300 | self.current_map_layer %= len(self.map_layer_names) 301 | selected = self.map_layer_names[self.current_map_layer] 302 | self.hud.notification('LayerMap selected: %s' % selected) 303 | 304 | def load_map_layer(self, unload=False): 305 | selected = self.map_layer_names[self.current_map_layer] 306 | if unload: 307 | self.hud.notification('Unloading map layer: %s' % selected) 308 | self.world.unload_map_layer(selected) 309 | else: 310 | self.hud.notification('Loading map layer: %s' % selected) 311 | self.world.load_map_layer(selected) 312 | 313 | def toggle_radar(self): 314 | if self.radar_sensor is None: 315 | self.radar_sensor = RadarSensor(self.player) 316 | elif self.radar_sensor.sensor is not None: 317 | self.radar_sensor.sensor.destroy() 318 | self.radar_sensor = None 319 | 320 | def modify_vehicle_physics(self, actor): 321 | #If actor is not a vehicle, we cannot use the physics control 322 | try: 323 | physics_control = actor.get_physics_control() 324 | physics_control.use_sweep_wheel_collision = True 325 | actor.apply_physics_control(physics_control) 326 | except Exception: 327 | pass 328 | 329 | def tick(self, clock): 330 | self.hud.tick(self, clock) 331 | 332 | def render(self, display): 333 | self.camera_manager.render(display) 334 | self.hud.render(display) 335 | 336 | def destroy_sensors(self): 337 | self.camera_manager.sensor.destroy() 338 | self.camera_manager.sensor = None 339 | self.camera_manager.index = None 340 | 341 | def destroy(self): 342 | if self.radar_sensor is not None: 343 | self.toggle_radar() 344 | sensors = [ 345 | self.camera_manager.sensor, 346 | self.collision_sensor.sensor, 347 | self.lane_invasion_sensor.sensor, 348 | self.gnss_sensor.sensor, 349 | self.imu_sensor.sensor] 350 | for sensor in sensors: 351 | if sensor is not None: 352 | sensor.stop() 353 | sensor.destroy() 354 | if self.player is not None: 355 | self.player.destroy() 356 | 357 | 358 | # ============================================================================== 359 | # -- KeyboardControl ----------------------------------------------------------- 360 | # ============================================================================== 361 | 362 | 363 | class KeyboardControl(object): 364 | """Class that handles keyboard input.""" 365 | def __init__(self, world, start_in_autopilot): 366 | self._autopilot_enabled = start_in_autopilot 367 | if isinstance(world.player, carla.Vehicle): 368 | self._control = carla.VehicleControl() 369 | self._lights = carla.VehicleLightState.NONE 370 | world.player.set_autopilot(self._autopilot_enabled) 371 | world.player.set_light_state(self._lights) 372 | elif isinstance(world.player, carla.Walker): 373 | self._control = carla.WalkerControl() 374 | self._autopilot_enabled = False 375 | self._rotation = world.player.get_transform().rotation 376 | else: 377 | raise NotImplementedError("Actor type not supported") 378 | self._steer_cache = 0.0 379 | world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) 380 | 381 | def parse_events(self, client, world, clock, sync_mode): 382 | if isinstance(self._control, carla.VehicleControl): 383 | current_lights = self._lights 384 | for event in pygame.event.get(): 385 | if event.type == pygame.QUIT: 386 | return True 387 | elif event.type == pygame.KEYUP: 388 | if self._is_quit_shortcut(event.key): 389 | return True 390 | elif event.key == K_BACKSPACE: 391 | if self._autopilot_enabled: 392 | world.player.set_autopilot(False) 393 | world.restart() 394 | world.player.set_autopilot(True) 395 | else: 396 | world.restart() 397 | elif event.key == K_F1: 398 | world.hud.toggle_info() 399 | elif event.key == K_v and pygame.key.get_mods() & KMOD_SHIFT: 400 | world.next_map_layer(reverse=True) 401 | elif event.key == K_v: 402 | world.next_map_layer() 403 | elif event.key == K_b and pygame.key.get_mods() & KMOD_SHIFT: 404 | world.load_map_layer(unload=True) 405 | elif event.key == K_b: 406 | world.load_map_layer() 407 | elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT): 408 | world.hud.help.toggle() 409 | elif event.key == K_TAB: 410 | world.camera_manager.toggle_camera() 411 | elif event.key == K_c and pygame.key.get_mods() & KMOD_SHIFT: 412 | world.next_weather(reverse=True) 413 | elif event.key == K_c: 414 | world.next_weather() 415 | elif event.key == K_g: 416 | world.toggle_radar() 417 | elif event.key == K_BACKQUOTE: 418 | world.camera_manager.next_sensor() 419 | elif event.key == K_n: 420 | world.camera_manager.next_sensor() 421 | elif event.key == K_w and (pygame.key.get_mods() & KMOD_CTRL): 422 | if world.constant_velocity_enabled: 423 | world.player.disable_constant_velocity() 424 | world.constant_velocity_enabled = False 425 | world.hud.notification("Disabled Constant Velocity Mode") 426 | else: 427 | world.player.enable_constant_velocity(carla.Vector3D(17, 0, 0)) 428 | world.constant_velocity_enabled = True 429 | world.hud.notification("Enabled Constant Velocity Mode at 60 km/h") 430 | elif event.key == K_t: 431 | if world.show_vehicle_telemetry: 432 | world.player.show_debug_telemetry(False) 433 | world.show_vehicle_telemetry = False 434 | world.hud.notification("Disabled Vehicle Telemetry") 435 | else: 436 | try: 437 | world.player.show_debug_telemetry(True) 438 | world.show_vehicle_telemetry = True 439 | world.hud.notification("Enabled Vehicle Telemetry") 440 | except Exception: 441 | pass 442 | elif event.key > K_0 and event.key <= K_9: 443 | index_ctrl = 0 444 | if pygame.key.get_mods() & KMOD_CTRL: 445 | index_ctrl = 9 446 | world.camera_manager.set_sensor(event.key - 1 - K_0 + index_ctrl) 447 | elif event.key == K_r and not (pygame.key.get_mods() & KMOD_CTRL): 448 | world.camera_manager.toggle_recording() 449 | elif event.key == K_r and (pygame.key.get_mods() & KMOD_CTRL): 450 | if (world.recording_enabled): 451 | client.stop_recorder() 452 | world.recording_enabled = False 453 | world.hud.notification("Recorder is OFF") 454 | else: 455 | client.start_recorder("manual_recording.rec") 456 | world.recording_enabled = True 457 | world.hud.notification("Recorder is ON") 458 | elif event.key == K_p and (pygame.key.get_mods() & KMOD_CTRL): 459 | # stop recorder 460 | client.stop_recorder() 461 | world.recording_enabled = False 462 | # work around to fix camera at start of replaying 463 | current_index = world.camera_manager.index 464 | world.destroy_sensors() 465 | # disable autopilot 466 | self._autopilot_enabled = False 467 | world.player.set_autopilot(self._autopilot_enabled) 468 | world.hud.notification("Replaying file 'manual_recording.rec'") 469 | # replayer 470 | client.replay_file("manual_recording.rec", world.recording_start, 0, 0) 471 | world.camera_manager.set_sensor(current_index) 472 | elif event.key == K_MINUS and (pygame.key.get_mods() & KMOD_CTRL): 473 | if pygame.key.get_mods() & KMOD_SHIFT: 474 | world.recording_start -= 10 475 | else: 476 | world.recording_start -= 1 477 | world.hud.notification("Recording start time is %d" % (world.recording_start)) 478 | elif event.key == K_EQUALS and (pygame.key.get_mods() & KMOD_CTRL): 479 | if pygame.key.get_mods() & KMOD_SHIFT: 480 | world.recording_start += 10 481 | else: 482 | world.recording_start += 1 483 | world.hud.notification("Recording start time is %d" % (world.recording_start)) 484 | if isinstance(self._control, carla.VehicleControl): 485 | if event.key == K_q: 486 | self._control.gear = 1 if self._control.reverse else -1 487 | elif event.key == K_m: 488 | self._control.manual_gear_shift = not self._control.manual_gear_shift 489 | self._control.gear = world.player.get_control().gear 490 | world.hud.notification('%s Transmission' % 491 | ('Manual' if self._control.manual_gear_shift else 'Automatic')) 492 | elif self._control.manual_gear_shift and event.key == K_COMMA: 493 | self._control.gear = max(-1, self._control.gear - 1) 494 | elif self._control.manual_gear_shift and event.key == K_PERIOD: 495 | self._control.gear = self._control.gear + 1 496 | elif event.key == K_p and not pygame.key.get_mods() & KMOD_CTRL: 497 | if not self._autopilot_enabled and not sync_mode: 498 | print("WARNING: You are currently in asynchronous mode and could " 499 | "experience some issues with the traffic simulation") 500 | self._autopilot_enabled = not self._autopilot_enabled 501 | world.player.set_autopilot(self._autopilot_enabled) 502 | world.hud.notification( 503 | 'Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) 504 | elif event.key == K_l and pygame.key.get_mods() & KMOD_CTRL: 505 | current_lights ^= carla.VehicleLightState.Special1 506 | elif event.key == K_l and pygame.key.get_mods() & KMOD_SHIFT: 507 | current_lights ^= carla.VehicleLightState.HighBeam 508 | elif event.key == K_l: 509 | # Use 'L' key to switch between lights: 510 | # closed -> position -> low beam -> fog 511 | if not self._lights & carla.VehicleLightState.Position: 512 | world.hud.notification("Position lights") 513 | current_lights |= carla.VehicleLightState.Position 514 | else: 515 | world.hud.notification("Low beam lights") 516 | current_lights |= carla.VehicleLightState.LowBeam 517 | if self._lights & carla.VehicleLightState.LowBeam: 518 | world.hud.notification("Fog lights") 519 | current_lights |= carla.VehicleLightState.Fog 520 | if self._lights & carla.VehicleLightState.Fog: 521 | world.hud.notification("Lights off") 522 | current_lights ^= carla.VehicleLightState.Position 523 | current_lights ^= carla.VehicleLightState.LowBeam 524 | current_lights ^= carla.VehicleLightState.Fog 525 | elif event.key == K_i: 526 | current_lights ^= carla.VehicleLightState.Interior 527 | elif event.key == K_z: 528 | current_lights ^= carla.VehicleLightState.LeftBlinker 529 | elif event.key == K_x: 530 | current_lights ^= carla.VehicleLightState.RightBlinker 531 | 532 | if not self._autopilot_enabled: 533 | if isinstance(self._control, carla.VehicleControl): 534 | self._parse_vehicle_keys(pygame.key.get_pressed(), clock.get_time()) 535 | self._control.reverse = self._control.gear < 0 536 | # Set automatic control-related vehicle lights 537 | if self._control.brake: 538 | current_lights |= carla.VehicleLightState.Brake 539 | else: # Remove the Brake flag 540 | current_lights &= ~carla.VehicleLightState.Brake 541 | if self._control.reverse: 542 | current_lights |= carla.VehicleLightState.Reverse 543 | else: # Remove the Reverse flag 544 | current_lights &= ~carla.VehicleLightState.Reverse 545 | if current_lights != self._lights: # Change the light state only if necessary 546 | self._lights = current_lights 547 | world.player.set_light_state(carla.VehicleLightState(self._lights)) 548 | elif isinstance(self._control, carla.WalkerControl): 549 | self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time(), world) 550 | world.player.apply_control(self._control) 551 | 552 | def _parse_vehicle_keys(self, keys, milliseconds): 553 | if keys[K_UP] or keys[K_w]: 554 | self._control.throttle = min(self._control.throttle + 0.01, 1.00) 555 | else: 556 | self._control.throttle = 0.0 557 | 558 | if keys[K_DOWN] or keys[K_s]: 559 | self._control.brake = min(self._control.brake + 0.2, 1) 560 | else: 561 | self._control.brake = 0 562 | 563 | steer_increment = 5e-4 * milliseconds 564 | if keys[K_LEFT] or keys[K_a]: 565 | if self._steer_cache > 0: 566 | self._steer_cache = 0 567 | else: 568 | self._steer_cache -= steer_increment 569 | elif keys[K_RIGHT] or keys[K_d]: 570 | if self._steer_cache < 0: 571 | self._steer_cache = 0 572 | else: 573 | self._steer_cache += steer_increment 574 | else: 575 | self._steer_cache = 0.0 576 | self._steer_cache = min(0.7, max(-0.7, self._steer_cache)) 577 | self._control.steer = round(self._steer_cache, 1) 578 | self._control.hand_brake = keys[K_SPACE] 579 | 580 | def _parse_walker_keys(self, keys, milliseconds, world): 581 | self._control.speed = 0.0 582 | if keys[K_DOWN] or keys[K_s]: 583 | self._control.speed = 0.0 584 | if keys[K_LEFT] or keys[K_a]: 585 | self._control.speed = .01 586 | self._rotation.yaw -= 0.08 * milliseconds 587 | if keys[K_RIGHT] or keys[K_d]: 588 | self._control.speed = .01 589 | self._rotation.yaw += 0.08 * milliseconds 590 | if keys[K_UP] or keys[K_w]: 591 | self._control.speed = world.player_max_speed_fast if pygame.key.get_mods() & KMOD_SHIFT else world.player_max_speed 592 | self._control.jump = keys[K_SPACE] 593 | self._rotation.yaw = round(self._rotation.yaw, 1) 594 | self._control.direction = self._rotation.get_forward_vector() 595 | 596 | @staticmethod 597 | def _is_quit_shortcut(key): 598 | return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) 599 | 600 | 601 | # ============================================================================== 602 | # -- HUD ----------------------------------------------------------------------- 603 | # ============================================================================== 604 | 605 | 606 | class HUD(object): 607 | def __init__(self, width, height): 608 | self.dim = (width, height) 609 | font = pygame.font.Font(pygame.font.get_default_font(), 20) 610 | font_name = 'courier' if os.name == 'nt' else 'mono' 611 | fonts = [x for x in pygame.font.get_fonts() if font_name in x] 612 | default_font = 'ubuntumono' 613 | mono = default_font if default_font in fonts else fonts[0] 614 | mono = pygame.font.match_font(mono) 615 | self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) 616 | self._notifications = FadingText(font, (width, 40), (0, height - 40)) 617 | self.help = HelpText(pygame.font.Font(mono, 16), width, height) 618 | self.server_fps = 0 619 | self.frame = 0 620 | self.simulation_time = 0 621 | self._show_info = True 622 | self._info_text = [] 623 | self._server_clock = pygame.time.Clock() 624 | 625 | def on_world_tick(self, timestamp): 626 | self._server_clock.tick() 627 | self.server_fps = self._server_clock.get_fps() 628 | self.frame = timestamp.frame 629 | self.simulation_time = timestamp.elapsed_seconds 630 | 631 | def tick(self, world, clock): 632 | self._notifications.tick(world, clock) 633 | if not self._show_info: 634 | return 635 | t = world.player.get_transform() 636 | v = world.player.get_velocity() 637 | c = world.player.get_control() 638 | compass = world.imu_sensor.compass 639 | heading = 'N' if compass > 270.5 or compass < 89.5 else '' 640 | heading += 'S' if 90.5 < compass < 269.5 else '' 641 | heading += 'E' if 0.5 < compass < 179.5 else '' 642 | heading += 'W' if 180.5 < compass < 359.5 else '' 643 | colhist = world.collision_sensor.get_collision_history() 644 | collision = [colhist[x + self.frame - 200] for x in range(0, 200)] 645 | max_col = max(1.0, max(collision)) 646 | collision = [x / max_col for x in collision] 647 | vehicles = world.world.get_actors().filter('vehicle.*') 648 | self._info_text = [ 649 | 'Server: % 16.0f FPS' % self.server_fps, 650 | 'Client: % 16.0f FPS' % clock.get_fps(), 651 | '', 652 | 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), 653 | 'Map: % 20s' % world.map.name.split('/')[-1], 654 | 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), 655 | '', 656 | 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)), 657 | u'Compass:% 17.0f\N{DEGREE SIGN} % 2s' % (compass, heading), 658 | 'Accelero: (%5.1f,%5.1f,%5.1f)' % (world.imu_sensor.accelerometer), 659 | 'Gyroscop: (%5.1f,%5.1f,%5.1f)' % (world.imu_sensor.gyroscope), 660 | 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (t.location.x, t.location.y)), 661 | 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), 662 | 'Height: % 18.0f m' % t.location.z, 663 | ''] 664 | if isinstance(c, carla.VehicleControl): 665 | self._info_text += [ 666 | ('Throttle:', c.throttle, 0.0, 1.0), 667 | ('Steer:', c.steer, -1.0, 1.0), 668 | ('Brake:', c.brake, 0.0, 1.0), 669 | ('Reverse:', c.reverse), 670 | ('Hand brake:', c.hand_brake), 671 | ('Manual:', c.manual_gear_shift), 672 | 'Gear: %s' % {-1: 'R', 0: 'N'}.get(c.gear, c.gear)] 673 | elif isinstance(c, carla.WalkerControl): 674 | self._info_text += [ 675 | ('Speed:', c.speed, 0.0, 5.556), 676 | ('Jump:', c.jump)] 677 | self._info_text += [ 678 | '', 679 | 'Collision:', 680 | collision, 681 | '', 682 | 'Number of vehicles: % 8d' % len(vehicles)] 683 | if len(vehicles) > 1: 684 | self._info_text += ['Nearby vehicles:'] 685 | distance = lambda l: math.sqrt((l.x - t.location.x)**2 + (l.y - t.location.y)**2 + (l.z - t.location.z)**2) 686 | vehicles = [(distance(x.get_location()), x) for x in vehicles if x.id != world.player.id] 687 | for d, vehicle in sorted(vehicles, key=lambda vehicles: vehicles[0]): 688 | if d > 200.0: 689 | break 690 | vehicle_type = get_actor_display_name(vehicle, truncate=22) 691 | self._info_text.append('% 4dm %s' % (d, vehicle_type)) 692 | 693 | def toggle_info(self): 694 | self._show_info = not self._show_info 695 | 696 | def notification(self, text, seconds=2.0): 697 | self._notifications.set_text(text, seconds=seconds) 698 | 699 | def error(self, text): 700 | self._notifications.set_text('Error: %s' % text, (255, 0, 0)) 701 | 702 | def render(self, display): 703 | if self._show_info: 704 | info_surface = pygame.Surface((220, self.dim[1])) 705 | info_surface.set_alpha(100) 706 | display.blit(info_surface, (0, 0)) 707 | v_offset = 4 708 | bar_h_offset = 100 709 | bar_width = 106 710 | for item in self._info_text: 711 | if v_offset + 18 > self.dim[1]: 712 | break 713 | if isinstance(item, list): 714 | if len(item) > 1: 715 | points = [(x + 8, v_offset + 8 + (1.0 - y) * 30) for x, y in enumerate(item)] 716 | pygame.draw.lines(display, (255, 136, 0), False, points, 2) 717 | item = None 718 | v_offset += 18 719 | elif isinstance(item, tuple): 720 | if isinstance(item[1], bool): 721 | rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) 722 | pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) 723 | else: 724 | rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) 725 | pygame.draw.rect(display, (255, 255, 255), rect_border, 1) 726 | f = (item[1] - item[2]) / (item[3] - item[2]) 727 | if item[2] < 0.0: 728 | rect = pygame.Rect((bar_h_offset + f * (bar_width - 6), v_offset + 8), (6, 6)) 729 | else: 730 | rect = pygame.Rect((bar_h_offset, v_offset + 8), (f * bar_width, 6)) 731 | pygame.draw.rect(display, (255, 255, 255), rect) 732 | item = item[0] 733 | if item: # At this point has to be a str. 734 | surface = self._font_mono.render(item, True, (255, 255, 255)) 735 | display.blit(surface, (8, v_offset)) 736 | v_offset += 18 737 | self._notifications.render(display) 738 | self.help.render(display) 739 | 740 | 741 | # ============================================================================== 742 | # -- FadingText ---------------------------------------------------------------- 743 | # ============================================================================== 744 | 745 | 746 | class FadingText(object): 747 | def __init__(self, font, dim, pos): 748 | self.font = font 749 | self.dim = dim 750 | self.pos = pos 751 | self.seconds_left = 0 752 | self.surface = pygame.Surface(self.dim) 753 | 754 | def set_text(self, text, color=(255, 255, 255), seconds=2.0): 755 | text_texture = self.font.render(text, True, color) 756 | self.surface = pygame.Surface(self.dim) 757 | self.seconds_left = seconds 758 | self.surface.fill((0, 0, 0, 0)) 759 | self.surface.blit(text_texture, (10, 11)) 760 | 761 | def tick(self, _, clock): 762 | delta_seconds = 1e-3 * clock.get_time() 763 | self.seconds_left = max(0.0, self.seconds_left - delta_seconds) 764 | self.surface.set_alpha(500.0 * self.seconds_left) 765 | 766 | def render(self, display): 767 | display.blit(self.surface, self.pos) 768 | 769 | 770 | # ============================================================================== 771 | # -- HelpText ------------------------------------------------------------------ 772 | # ============================================================================== 773 | 774 | 775 | class HelpText(object): 776 | """Helper class to handle text output using pygame""" 777 | def __init__(self, font, width, height): 778 | lines = __doc__.split('\n') 779 | self.font = font 780 | self.line_space = 18 781 | self.dim = (780, len(lines) * self.line_space + 12) 782 | self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) 783 | self.seconds_left = 0 784 | self.surface = pygame.Surface(self.dim) 785 | self.surface.fill((0, 0, 0, 0)) 786 | for n, line in enumerate(lines): 787 | text_texture = self.font.render(line, True, (255, 255, 255)) 788 | self.surface.blit(text_texture, (22, n * self.line_space)) 789 | self._render = False 790 | self.surface.set_alpha(220) 791 | 792 | def toggle(self): 793 | self._render = not self._render 794 | 795 | def render(self, display): 796 | if self._render: 797 | display.blit(self.surface, self.pos) 798 | 799 | 800 | # ============================================================================== 801 | # -- CollisionSensor ----------------------------------------------------------- 802 | # ============================================================================== 803 | 804 | 805 | class CollisionSensor(object): 806 | def __init__(self, parent_actor, hud): 807 | self.sensor = None 808 | self.history = [] 809 | self._parent = parent_actor 810 | self.hud = hud 811 | world = self._parent.get_world() 812 | bp = world.get_blueprint_library().find('sensor.other.collision') 813 | self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) 814 | # We need to pass the lambda a weak reference to self to avoid circular 815 | # reference. 816 | weak_self = weakref.ref(self) 817 | self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) 818 | 819 | def get_collision_history(self): 820 | history = collections.defaultdict(int) 821 | for frame, intensity in self.history: 822 | history[frame] += intensity 823 | return history 824 | 825 | @staticmethod 826 | def _on_collision(weak_self, event): 827 | self = weak_self() 828 | if not self: 829 | return 830 | actor_type = get_actor_display_name(event.other_actor) 831 | self.hud.notification('Collision with %r' % actor_type) 832 | impulse = event.normal_impulse 833 | intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) 834 | self.history.append((event.frame, intensity)) 835 | if len(self.history) > 4000: 836 | self.history.pop(0) 837 | 838 | 839 | # ============================================================================== 840 | # -- LaneInvasionSensor -------------------------------------------------------- 841 | # ============================================================================== 842 | 843 | 844 | class LaneInvasionSensor(object): 845 | def __init__(self, parent_actor, hud): 846 | self.sensor = None 847 | 848 | # If the spawn object is not a vehicle, we cannot use the Lane Invasion Sensor 849 | if parent_actor.type_id.startswith("vehicle."): 850 | self._parent = parent_actor 851 | self.hud = hud 852 | world = self._parent.get_world() 853 | bp = world.get_blueprint_library().find('sensor.other.lane_invasion') 854 | self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) 855 | # We need to pass the lambda a weak reference to self to avoid circular 856 | # reference. 857 | weak_self = weakref.ref(self) 858 | self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) 859 | 860 | @staticmethod 861 | def _on_invasion(weak_self, event): 862 | self = weak_self() 863 | if not self: 864 | return 865 | lane_types = set(x.type for x in event.crossed_lane_markings) 866 | text = ['%r' % str(x).split()[-1] for x in lane_types] 867 | self.hud.notification('Crossed line %s' % ' and '.join(text)) 868 | 869 | 870 | # ============================================================================== 871 | # -- GnssSensor ---------------------------------------------------------------- 872 | # ============================================================================== 873 | 874 | 875 | class GnssSensor(object): 876 | def __init__(self, parent_actor): 877 | self.sensor = None 878 | self._parent = parent_actor 879 | self.lat = 0.0 880 | self.lon = 0.0 881 | world = self._parent.get_world() 882 | bp = world.get_blueprint_library().find('sensor.other.gnss') 883 | self.sensor = world.spawn_actor(bp, carla.Transform(carla.Location(x=1.0, z=2.8)), attach_to=self._parent) 884 | # We need to pass the lambda a weak reference to self to avoid circular 885 | # reference. 886 | weak_self = weakref.ref(self) 887 | self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) 888 | 889 | @staticmethod 890 | def _on_gnss_event(weak_self, event): 891 | self = weak_self() 892 | if not self: 893 | return 894 | self.lat = event.latitude 895 | self.lon = event.longitude 896 | 897 | 898 | # ============================================================================== 899 | # -- IMUSensor ----------------------------------------------------------------- 900 | # ============================================================================== 901 | 902 | 903 | class IMUSensor(object): 904 | def __init__(self, parent_actor): 905 | self.sensor = None 906 | self._parent = parent_actor 907 | self.accelerometer = (0.0, 0.0, 0.0) 908 | self.gyroscope = (0.0, 0.0, 0.0) 909 | self.compass = 0.0 910 | world = self._parent.get_world() 911 | bp = world.get_blueprint_library().find('sensor.other.imu') 912 | self.sensor = world.spawn_actor( 913 | bp, carla.Transform(), attach_to=self._parent) 914 | # We need to pass the lambda a weak reference to self to avoid circular 915 | # reference. 916 | weak_self = weakref.ref(self) 917 | self.sensor.listen( 918 | lambda sensor_data: IMUSensor._IMU_callback(weak_self, sensor_data)) 919 | 920 | @staticmethod 921 | def _IMU_callback(weak_self, sensor_data): 922 | self = weak_self() 923 | if not self: 924 | return 925 | limits = (-99.9, 99.9) 926 | self.accelerometer = ( 927 | max(limits[0], min(limits[1], sensor_data.accelerometer.x)), 928 | max(limits[0], min(limits[1], sensor_data.accelerometer.y)), 929 | max(limits[0], min(limits[1], sensor_data.accelerometer.z))) 930 | self.gyroscope = ( 931 | max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.x))), 932 | max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.y))), 933 | max(limits[0], min(limits[1], math.degrees(sensor_data.gyroscope.z)))) 934 | self.compass = math.degrees(sensor_data.compass) 935 | 936 | 937 | # ============================================================================== 938 | # -- RadarSensor --------------------------------------------------------------- 939 | # ============================================================================== 940 | 941 | 942 | class RadarSensor(object): 943 | def __init__(self, parent_actor): 944 | self.sensor = None 945 | self._parent = parent_actor 946 | bound_x = 0.5 + self._parent.bounding_box.extent.x 947 | bound_y = 0.5 + self._parent.bounding_box.extent.y 948 | bound_z = 0.5 + self._parent.bounding_box.extent.z 949 | 950 | self.velocity_range = 7.5 # m/s 951 | world = self._parent.get_world() 952 | self.debug = world.debug 953 | bp = world.get_blueprint_library().find('sensor.other.radar') 954 | bp.set_attribute('horizontal_fov', str(35)) 955 | bp.set_attribute('vertical_fov', str(20)) 956 | self.sensor = world.spawn_actor( 957 | bp, 958 | carla.Transform( 959 | carla.Location(x=bound_x + 0.05, z=bound_z+0.05), 960 | carla.Rotation(pitch=5)), 961 | attach_to=self._parent) 962 | # We need a weak reference to self to avoid circular reference. 963 | weak_self = weakref.ref(self) 964 | self.sensor.listen( 965 | lambda radar_data: RadarSensor._Radar_callback(weak_self, radar_data)) 966 | 967 | @staticmethod 968 | def _Radar_callback(weak_self, radar_data): 969 | self = weak_self() 970 | if not self: 971 | return 972 | # To get a numpy [[vel, altitude, azimuth, depth],...[,,,]]: 973 | # points = np.frombuffer(radar_data.raw_data, dtype=np.dtype('f4')) 974 | # points = np.reshape(points, (len(radar_data), 4)) 975 | 976 | current_rot = radar_data.transform.rotation 977 | for detect in radar_data: 978 | azi = math.degrees(detect.azimuth) 979 | alt = math.degrees(detect.altitude) 980 | # The 0.25 adjusts a bit the distance so the dots can 981 | # be properly seen 982 | fw_vec = carla.Vector3D(x=detect.depth - 0.25) 983 | carla.Transform( 984 | carla.Location(), 985 | carla.Rotation( 986 | pitch=current_rot.pitch + alt, 987 | yaw=current_rot.yaw + azi, 988 | roll=current_rot.roll)).transform(fw_vec) 989 | 990 | def clamp(min_v, max_v, value): 991 | return max(min_v, min(value, max_v)) 992 | 993 | norm_velocity = detect.velocity / self.velocity_range # range [-1, 1] 994 | r = int(clamp(0.0, 1.0, 1.0 - norm_velocity) * 255.0) 995 | g = int(clamp(0.0, 1.0, 1.0 - abs(norm_velocity)) * 255.0) 996 | b = int(abs(clamp(- 1.0, 0.0, - 1.0 - norm_velocity)) * 255.0) 997 | self.debug.draw_point( 998 | radar_data.transform.location + fw_vec, 999 | size=0.075, 1000 | life_time=0.06, 1001 | persistent_lines=False, 1002 | color=carla.Color(r, g, b)) 1003 | 1004 | # ============================================================================== 1005 | # -- CameraManager ------------------------------------------------------------- 1006 | # ============================================================================== 1007 | 1008 | 1009 | class CameraManager(object): 1010 | def __init__(self, parent_actor, hud, gamma_correction): 1011 | self.sensor = None 1012 | self.surface = None 1013 | self._parent = parent_actor 1014 | self.hud = hud 1015 | self.recording = False 1016 | bound_x = 0.5 + self._parent.bounding_box.extent.x 1017 | bound_y = 0.5 + self._parent.bounding_box.extent.y 1018 | bound_z = 0.5 + self._parent.bounding_box.extent.z 1019 | Attachment = carla.AttachmentType 1020 | 1021 | if not self._parent.type_id.startswith("walker.pedestrian"): 1022 | self._camera_transforms = [ 1023 | (carla.Transform(carla.Location(x=-2.0*bound_x, y=+0.0*bound_y, z=2.0*bound_z), carla.Rotation(pitch=8.0)), Attachment.SpringArm), 1024 | (carla.Transform(carla.Location(x=+0.8*bound_x, y=+0.0*bound_y, z=1.3*bound_z)), Attachment.Rigid), 1025 | (carla.Transform(carla.Location(x=+1.9*bound_x, y=+1.0*bound_y, z=1.2*bound_z)), Attachment.SpringArm), 1026 | (carla.Transform(carla.Location(x=-2.8*bound_x, y=+0.0*bound_y, z=4.6*bound_z), carla.Rotation(pitch=6.0)), Attachment.SpringArm), 1027 | (carla.Transform(carla.Location(x=-1.0, y=-1.0*bound_y, z=0.4*bound_z)), Attachment.Rigid)] 1028 | else: 1029 | self._camera_transforms = [ 1030 | (carla.Transform(carla.Location(x=-2.5, z=0.0), carla.Rotation(pitch=-8.0)), Attachment.SpringArm), 1031 | (carla.Transform(carla.Location(x=1.6, z=1.7)), Attachment.Rigid), 1032 | (carla.Transform(carla.Location(x=2.5, y=0.5, z=0.0), carla.Rotation(pitch=-8.0)), Attachment.SpringArm), 1033 | (carla.Transform(carla.Location(x=-4.0, z=2.0), carla.Rotation(pitch=6.0)), Attachment.SpringArm), 1034 | (carla.Transform(carla.Location(x=0, y=-2.5, z=-0.0), carla.Rotation(yaw=90.0)), Attachment.Rigid)] 1035 | 1036 | self.transform_index = 1 1037 | self.sensors = [ 1038 | ['sensor.camera.rgb', cc.Raw, 'Camera RGB', {}], 1039 | ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)', {}], 1040 | ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)', {}], 1041 | ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)', {}], 1042 | ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)', {}], 1043 | ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, 1044 | 'Camera Semantic Segmentation (CityScapes Palette)', {}], 1045 | ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)', {'range': '50'}], 1046 | ['sensor.camera.dvs', cc.Raw, 'Dynamic Vision Sensor', {}], 1047 | ['sensor.camera.rgb', cc.Raw, 'Camera RGB Distorted', 1048 | {'lens_circle_multiplier': '3.0', 1049 | 'lens_circle_falloff': '3.0', 1050 | 'chromatic_aberration_intensity': '0.5', 1051 | 'chromatic_aberration_offset': '0'}], 1052 | ['sensor.camera.optical_flow', cc.Raw, 'Optical Flow', {}], 1053 | ] 1054 | world = self._parent.get_world() 1055 | bp_library = world.get_blueprint_library() 1056 | for item in self.sensors: 1057 | bp = bp_library.find(item[0]) 1058 | if item[0].startswith('sensor.camera'): 1059 | bp.set_attribute('image_size_x', str(hud.dim[0])) 1060 | bp.set_attribute('image_size_y', str(hud.dim[1])) 1061 | if bp.has_attribute('gamma'): 1062 | bp.set_attribute('gamma', str(gamma_correction)) 1063 | for attr_name, attr_value in item[3].items(): 1064 | bp.set_attribute(attr_name, attr_value) 1065 | elif item[0].startswith('sensor.lidar'): 1066 | self.lidar_range = 50 1067 | 1068 | for attr_name, attr_value in item[3].items(): 1069 | bp.set_attribute(attr_name, attr_value) 1070 | if attr_name == 'range': 1071 | self.lidar_range = float(attr_value) 1072 | 1073 | item.append(bp) 1074 | self.index = None 1075 | 1076 | def toggle_camera(self): 1077 | self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) 1078 | self.set_sensor(self.index, notify=False, force_respawn=True) 1079 | 1080 | def set_sensor(self, index, notify=True, force_respawn=False): 1081 | index = index % len(self.sensors) 1082 | needs_respawn = True if self.index is None else \ 1083 | (force_respawn or (self.sensors[index][2] != self.sensors[self.index][2])) 1084 | if needs_respawn: 1085 | if self.sensor is not None: 1086 | self.sensor.destroy() 1087 | self.surface = None 1088 | self.sensor = self._parent.get_world().spawn_actor( 1089 | self.sensors[index][-1], 1090 | self._camera_transforms[self.transform_index][0], 1091 | attach_to=self._parent, 1092 | attachment_type=self._camera_transforms[self.transform_index][1]) 1093 | # We need to pass the lambda a weak reference to self to avoid 1094 | # circular reference. 1095 | weak_self = weakref.ref(self) 1096 | self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) 1097 | if notify: 1098 | self.hud.notification(self.sensors[index][2]) 1099 | self.index = index 1100 | 1101 | def next_sensor(self): 1102 | self.set_sensor(self.index + 1) 1103 | 1104 | def toggle_recording(self): 1105 | self.recording = not self.recording 1106 | self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) 1107 | 1108 | def render(self, display): 1109 | if self.surface is not None: 1110 | display.blit(self.surface, (0, 0)) 1111 | 1112 | @staticmethod 1113 | def _parse_image(weak_self, image): 1114 | self = weak_self() 1115 | if not self: 1116 | return 1117 | if self.sensors[self.index][0].startswith('sensor.lidar'): 1118 | points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) 1119 | points = np.reshape(points, (int(points.shape[0] / 4), 4)) 1120 | lidar_data = np.array(points[:, :2]) 1121 | lidar_data *= min(self.hud.dim) / (2.0 * self.lidar_range) 1122 | lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) 1123 | lidar_data = np.fabs(lidar_data) # pylint: disable=E1111 1124 | lidar_data = lidar_data.astype(np.int32) 1125 | lidar_data = np.reshape(lidar_data, (-1, 2)) 1126 | lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) 1127 | lidar_img = np.zeros((lidar_img_size), dtype=np.uint8) 1128 | lidar_img[tuple(lidar_data.T)] = (255, 255, 255) 1129 | self.surface = pygame.surfarray.make_surface(lidar_img) 1130 | elif self.sensors[self.index][0].startswith('sensor.camera.dvs'): 1131 | # Example of converting the raw_data from a carla.DVSEventArray 1132 | # sensor into a NumPy array and using it as an image 1133 | dvs_events = np.frombuffer(image.raw_data, dtype=np.dtype([ 1134 | ('x', np.uint16), ('y', np.uint16), ('t', np.int64), ('pol', np.bool)])) 1135 | dvs_img = np.zeros((image.height, image.width, 3), dtype=np.uint8) 1136 | # Blue is positive, red is negative 1137 | dvs_img[dvs_events[:]['y'], dvs_events[:]['x'], dvs_events[:]['pol'] * 2] = 255 1138 | self.surface = pygame.surfarray.make_surface(dvs_img.swapaxes(0, 1)) 1139 | elif self.sensors[self.index][0].startswith('sensor.camera.optical_flow'): 1140 | image = image.get_color_coded_flow() 1141 | array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) 1142 | array = np.reshape(array, (image.height, image.width, 4)) 1143 | array = array[:, :, :3] 1144 | array = array[:, :, ::-1] 1145 | self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) 1146 | else: 1147 | image.convert(self.sensors[self.index][1]) 1148 | array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) 1149 | array = np.reshape(array, (image.height, image.width, 4)) 1150 | array = array[:, :, :3] 1151 | array = array[:, :, ::-1] 1152 | self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) 1153 | if self.recording: 1154 | image.save_to_disk('_out/%08d' % image.frame) 1155 | 1156 | 1157 | # ============================================================================== 1158 | # -- game_loop() --------------------------------------------------------------- 1159 | # ============================================================================== 1160 | 1161 | 1162 | def game_loop(args): 1163 | pygame.init() 1164 | pygame.font.init() 1165 | world = None 1166 | original_settings = None 1167 | 1168 | try: 1169 | client = carla.Client(args.host, args.port) 1170 | client.set_timeout(20.0) 1171 | 1172 | sim_world = client.get_world() 1173 | if args.sync: 1174 | original_settings = sim_world.get_settings() 1175 | settings = sim_world.get_settings() 1176 | if not settings.synchronous_mode: 1177 | settings.synchronous_mode = True 1178 | settings.fixed_delta_seconds = 0.05 1179 | sim_world.apply_settings(settings) 1180 | 1181 | traffic_manager = client.get_trafficmanager() 1182 | traffic_manager.set_synchronous_mode(True) 1183 | 1184 | if args.autopilot and not sim_world.get_settings().synchronous_mode: 1185 | print("WARNING: You are currently in asynchronous mode and could " 1186 | "experience some issues with the traffic simulation") 1187 | 1188 | display = pygame.display.set_mode( 1189 | (args.width, args.height), 1190 | pygame.HWSURFACE | pygame.DOUBLEBUF) 1191 | display.fill((0,0,0)) 1192 | pygame.display.flip() 1193 | 1194 | hud = HUD(args.width, args.height) 1195 | world = World(sim_world, hud, args) 1196 | controller = KeyboardControl(world, args.autopilot) 1197 | 1198 | if args.sync: 1199 | sim_world.tick() 1200 | else: 1201 | sim_world.wait_for_tick() 1202 | 1203 | clock = pygame.time.Clock() 1204 | while True: 1205 | if args.sync: 1206 | sim_world.tick() 1207 | clock.tick_busy_loop(60) 1208 | if controller.parse_events(client, world, clock, args.sync): 1209 | return 1210 | world.tick(clock) 1211 | world.render(display) 1212 | pygame.display.flip() 1213 | 1214 | finally: 1215 | 1216 | if original_settings: 1217 | sim_world.apply_settings(original_settings) 1218 | 1219 | if (world and world.recording_enabled): 1220 | client.stop_recorder() 1221 | 1222 | if world is not None: 1223 | world.destroy() 1224 | 1225 | pygame.quit() 1226 | 1227 | 1228 | # ============================================================================== 1229 | # -- main() -------------------------------------------------------------------- 1230 | # ============================================================================== 1231 | 1232 | 1233 | def main(): 1234 | argparser = argparse.ArgumentParser( 1235 | description='CARLA Manual Control Client') 1236 | argparser.add_argument( 1237 | '-v', '--verbose', 1238 | action='store_true', 1239 | dest='debug', 1240 | help='print debug information') 1241 | argparser.add_argument( 1242 | '--host', 1243 | metavar='H', 1244 | default='127.0.0.1', 1245 | help='IP of the host server (default: 127.0.0.1)') 1246 | argparser.add_argument( 1247 | '-p', '--port', 1248 | metavar='P', 1249 | default=2000, 1250 | type=int, 1251 | help='TCP port to listen to (default: 2000)') 1252 | argparser.add_argument( 1253 | '-a', '--autopilot', 1254 | action='store_true', 1255 | help='enable autopilot') 1256 | argparser.add_argument( 1257 | '--res', 1258 | metavar='WIDTHxHEIGHT', 1259 | default='1280x720', 1260 | help='window resolution (default: 1280x720)') 1261 | argparser.add_argument( 1262 | '--filter', 1263 | metavar='PATTERN', 1264 | default='vehicle.*', 1265 | help='actor filter (default: "vehicle.*")') 1266 | argparser.add_argument( 1267 | '--generation', 1268 | metavar='G', 1269 | default='2', 1270 | help='restrict to certain actor generation (values: "1","2","All" - default: "2")') 1271 | argparser.add_argument( 1272 | '--rolename', 1273 | metavar='NAME', 1274 | default='hero', 1275 | help='actor role name (default: "hero")') 1276 | argparser.add_argument( 1277 | '--gamma', 1278 | default=2.2, 1279 | type=float, 1280 | help='Gamma correction of the camera (default: 2.2)') 1281 | argparser.add_argument( 1282 | '--sync', 1283 | action='store_true', 1284 | help='Activate synchronous mode execution') 1285 | args = argparser.parse_args() 1286 | 1287 | args.width, args.height = [int(x) for x in args.res.split('x')] 1288 | 1289 | log_level = logging.DEBUG if args.debug else logging.INFO 1290 | logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) 1291 | 1292 | logging.info('listening to server %s:%s', args.host, args.port) 1293 | 1294 | print(__doc__) 1295 | 1296 | try: 1297 | 1298 | game_loop(args) 1299 | 1300 | except KeyboardInterrupt: 1301 | print('\nCancelled by user. Bye!') 1302 | 1303 | 1304 | if __name__ == '__main__': 1305 | 1306 | main() 1307 | --------------------------------------------------------------------------------