├── BUILD ├── PythonAPI-Apollo-async ├── .DS_Store ├── .flake8 ├── lgsvl │ ├── .DS_Store │ ├── __init__.py │ ├── agent.py │ ├── controllable.py │ ├── dreamview │ │ ├── __init__.py │ │ └── dreamview.py │ ├── evaluator │ │ ├── __init__.py │ │ └── utils.py │ ├── geometry.py │ ├── remote.py │ ├── sensor.py │ ├── simulator.py │ ├── utils.py │ └── wise │ │ ├── __init__.py │ │ └── wise.py ├── package.xml ├── pyproject.toml ├── requirements.txt ├── resource │ └── lgsvl ├── scripts │ └── run_and_create_coverage_report.sh ├── setup.cfg ├── setup.py └── tests │ ├── __init__.py │ ├── common.py │ ├── test_EGO.py │ ├── test_NPC.py │ ├── test_collisions.py │ ├── test_manual_features.py │ ├── test_peds.py │ ├── test_sensors.py │ ├── test_simulator.py │ └── test_utils.py ├── README.md ├── behavexplor ├── BUILD ├── corpus.py ├── element.py ├── models │ ├── BUILD │ ├── coverage.py │ └── feature.py └── utils.py ├── common ├── BUILD ├── frame.py ├── runner.py ├── scenario.py ├── simulator.py └── utils.py ├── configs ├── s1.yaml ├── s2.yaml ├── s3.yaml └── s4.yaml ├── data ├── SanFrancisco.zip ├── junction │ ├── junction_basic_info.json │ └── junction_routes.json ├── map │ └── SanFrancisco.json └── straight │ ├── straight_basic_info.json │ └── straight_routes.json ├── figs └── MethodOverview.png ├── main.py ├── prepare.sh └── requirements.txt /BUILD: -------------------------------------------------------------------------------- 1 | load("@rules_python//python:defs.bzl", "py_binary", "py_library") 2 | 3 | package(default_visibility = ["//visibility:public"]) 4 | 5 | py_binary( 6 | name = "main", 7 | srcs = ["main.py"], 8 | deps = [ 9 | "//BehAVExplor/behavexplor:corpus", 10 | "//BehAVExplor/behavexplor:utils", 11 | "//BehAVExplor/common:scenario", 12 | "//BehAVExplor/common:runner", 13 | "//BehAVExplor/common:simulator", 14 | ], 15 | ) -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingfeiCheng/BehAVExplor/4124436fe38643196fea675acfcd163d3b369b75/PythonAPI-Apollo-async/.DS_Store -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/.flake8: -------------------------------------------------------------------------------- 1 | [flake8] 2 | max-line-length = 130 3 | max-complexity = 23 4 | 5 | ignore = E701, E501, W503 6 | exclude = 7 | .git 8 | tests/* 9 | examples/* 10 | 11 | per-file-ignores = 12 | lgsvl/__init__.py:F401 13 | lgsvl/dreamview/__init__.py:F401 14 | lgsvl/evaluator/__init__.py:F401 15 | lgsvl/wise/__init__.py:F401 16 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/lgsvl/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingfeiCheng/BehAVExplor/4124436fe38643196fea675acfcd163d3b369b75/PythonAPI-Apollo-async/lgsvl/.DS_Store -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/lgsvl/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2021 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | from .geometry import Vector, BoundingBox, Transform, Quaternion 8 | from .simulator import Simulator, RaycastHit, WeatherState 9 | from .sensor import Sensor, CameraSensor, LidarSensor, ImuSensor 10 | from .agent import ( 11 | AgentType, 12 | AgentState, 13 | VehicleControl, 14 | Vehicle, 15 | EgoVehicle, 16 | NpcVehicle, 17 | Pedestrian, 18 | DriveWaypoint, 19 | WalkWaypoint, 20 | WaypointTrigger, 21 | TriggerEffector, 22 | NPCControl, 23 | ) 24 | from .controllable import Controllable 25 | from .utils import ObjectState 26 | 27 | # Subpackages 28 | import lgsvl.dreamview 29 | import lgsvl.evaluator 30 | import lgsvl.wise 31 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/lgsvl/controllable.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2021 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | from .geometry import Transform 8 | from .utils import accepts, ObjectState 9 | 10 | 11 | class Controllable: 12 | def __init__(self, remote, j): 13 | self.remote = remote 14 | self.uid = j["uid"] 15 | self.type = j["type"] 16 | self.transform = Transform.from_json(j) 17 | self.valid_actions = j["valid_actions"] 18 | self.default_control_policy = j["default_control_policy"] 19 | 20 | @property 21 | def object_state(self): 22 | j = self.remote.command("controllable/object_state/get", {"uid": self.uid}) 23 | return ObjectState.from_json(j) 24 | 25 | @object_state.setter 26 | @accepts(ObjectState) 27 | def object_state(self, object_state): 28 | self.remote.command("controllable/object_state/set", { 29 | "uid": self.uid, 30 | "state": object_state.to_json() 31 | }) 32 | 33 | @property 34 | def current_state(self): 35 | j = self.remote.command("controllable/current_state/get", {"uid": self.uid}) 36 | return j["state"] 37 | 38 | @property 39 | def control_policy(self): 40 | j = self.remote.command("controllable/control_policy/get", {"uid": self.uid}) 41 | return j["control_policy"] 42 | 43 | @accepts((str, list)) 44 | def control(self, control_policy): 45 | self.remote.command("controllable/control_policy/set", { 46 | "uid": self.uid, 47 | "control_policy": control_policy, 48 | }) 49 | 50 | def __eq__(self, other): 51 | return self.uid == other.uid 52 | 53 | def __hash__(self): 54 | return hash(self.uid) 55 | 56 | def __repr__(self): 57 | return str({ 58 | "type": str(self.type), 59 | "transform": str(self.transform), 60 | "valid_actions": str(self.valid_actions), 61 | "default_control_policy": str(self.default_control_policy), 62 | }) 63 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/lgsvl/dreamview/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2020 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | from .dreamview import Connection, CoordType 8 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/lgsvl/dreamview/dreamview.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2020 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | import time 7 | 8 | from websocket import create_connection 9 | from enum import Enum 10 | import json 11 | import lgsvl 12 | import math 13 | import logging 14 | import sys 15 | import os 16 | 17 | log = logging.getLogger(__name__) 18 | from loguru import logger 19 | 20 | 21 | class CoordType(Enum): 22 | Unity = 1 23 | Northing = 2 24 | Latitude = 3 25 | 26 | 27 | class Connection: 28 | def __init__(self, simulator, ego_agent, ip=os.environ.get("LGSVL__AUTOPILOT_0_HOST", "localhost"), port="8888"): 29 | """ 30 | simulator: is an lgsvl.Simulator object 31 | ego_agent: an lgsvl.EgoVehicle object, this is intended to be used with a vehicle equipped with Apollo 5.0 32 | ip: address of the machine where the Apollo stack is running 33 | port: the port number for Dreamview 34 | """ 35 | self.url = "ws://" + ip + ":" + port + "/websocket" 36 | self.sim = simulator 37 | self.ego = ego_agent 38 | self.ws = create_connection(self.url) 39 | self.gps_offset = lgsvl.Vector() 40 | 41 | def set_destination(self, x_long_east, z_lat_north, y=0, coord_type=CoordType.Unity): 42 | """ 43 | This function can accept a variety of Coordinate systems 44 | 45 | If using Unity World Coordinate System: 46 | x_long_east = x 47 | z_lat_north = z 48 | y = y 49 | 50 | If using Latitude/Longitude: 51 | x_long_east = Longitude 52 | z_lat_north = Latitude 53 | 54 | If using Easting/Northing: 55 | x_long_east = Easting 56 | z_lat_north = Northing 57 | """ 58 | current_pos = self.ego.state.transform 59 | current_gps = self.sim.map_to_gps(current_pos) 60 | heading = math.radians(current_gps.orientation) 61 | 62 | # Start position should be the position of the GPS 63 | # Unity returns the position of the center of the vehicle so adjustment is required 64 | northing_adjustment = ( 65 | math.sin(heading) * self.gps_offset.z - math.cos(heading) * self.gps_offset.x 66 | ) 67 | easting_adjustment = ( 68 | math.cos(heading) * self.gps_offset.z + math.sin(heading) * self.gps_offset.x 69 | ) 70 | 71 | if coord_type == CoordType.Unity: 72 | transform = lgsvl.Transform( 73 | lgsvl.Vector(x_long_east, y, z_lat_north), lgsvl.Vector(0, 0, 0) 74 | ) 75 | gps = self.sim.map_to_gps(transform) 76 | dest_x = gps.easting 77 | dest_y = gps.northing 78 | 79 | elif coord_type == CoordType.Northing: 80 | dest_x = x_long_east 81 | dest_y = z_lat_north 82 | 83 | elif coord_type == CoordType.Latitude: 84 | transform = self.sim.map_from_gps(longitude=x_long_east, latitude=z_lat_north) 85 | gps = self.sim.map_to_gps(transform) 86 | dest_x = gps.easting 87 | dest_y = gps.northing 88 | 89 | else: 90 | log.error( 91 | "Please input a valid Coordinate Type: Unity position, Easting/Northing, or Longitude/Latitude" 92 | ) 93 | return 94 | 95 | self.ws.send( 96 | json.dumps( 97 | { 98 | "type": "SendRoutingRequest", 99 | "start": { 100 | "x": current_gps.easting + easting_adjustment, 101 | "y": current_gps.northing + northing_adjustment, 102 | "z": 0, 103 | "heading": heading, 104 | }, 105 | "end": {"x": dest_x, "y": dest_y, "z": 0}, 106 | "waypoint": "[]", 107 | } 108 | ) 109 | ) 110 | 111 | return 112 | 113 | def enable_module(self, module): 114 | """ 115 | module is the name of the Apollo 5.0 module as seen in the "Module Controller" tab of Dreamview 116 | """ 117 | self.ws.send( 118 | json.dumps({"type": "HMIAction", "action": "START_MODULE", "value": module}) 119 | ) 120 | return 121 | 122 | def disable_module(self, module): 123 | """ 124 | module is the name of the Apollo 5.0 module as seen in the "Module Controller" tab of Dreamview 125 | """ 126 | self.ws.send( 127 | json.dumps({"type": "HMIAction", "action": "STOP_MODULE", "value": module}) 128 | ) 129 | return 130 | 131 | def set_hd_map(self, hd_map): 132 | """ 133 | Folders in /apollo/modules/map/data/ are the available HD maps 134 | Map options in Dreamview are the folder names with the following changes: 135 | - underscores (_) are replaced with spaces 136 | - the first letter of each word is capitalized 137 | 138 | hd_map parameter is the modified folder name. 139 | hd_map should match one of the options in the right-most drop down in the top-right corner of Dreamview. 140 | """ 141 | 142 | word_list = [] 143 | for s in hd_map.split('_'): 144 | word_list.append(s[0].upper() + s[1:]) 145 | 146 | mapped_map = ' '.join(word_list) 147 | 148 | self.ws.send( 149 | json.dumps({"type": "HMIAction", "action": "CHANGE_MAP", "value": mapped_map}) 150 | ) 151 | 152 | if not self.get_current_map() == mapped_map: 153 | folder_name = hd_map.replace(" ", "_") 154 | error_message = ( 155 | "HD Map {0} was not set. Verify the files exist in " 156 | "/apollo/modules/map/data/{1} and restart Dreamview -- Aborting..." 157 | ) 158 | log.error( 159 | error_message.format( 160 | mapped_map, folder_name 161 | ) 162 | ) 163 | sys.exit(1) 164 | return 165 | 166 | def set_vehicle(self, vehicle, gps_offset_x=0.0, gps_offset_y=0.0, gps_offset_z=-1.348): 167 | # Lincoln2017MKZ from LGSVL has a GPS offset of 1.348m behind the center of the vehicle, lgsvl.Vector(0.0, 0.0, -1.348) 168 | """ 169 | Folders in /apollo/modules/calibration/data/ are the available vehicle calibrations 170 | Vehicle options in Dreamview are the folder names with the following changes: 171 | - underscores (_) are replaced with spaces 172 | - the first letter of each word is capitalized 173 | 174 | vehicle parameter is the modified folder name. 175 | vehicle should match one of the options in the middle drop down in the top-right corner of Dreamview. 176 | """ 177 | 178 | word_list = [] 179 | for s in vehicle.split('_'): 180 | word_list.append(s[0].upper() + s[1:]) 181 | 182 | mapped_vehicle = ' '.join(word_list) 183 | 184 | self.ws.send( 185 | json.dumps( 186 | {"type": "HMIAction", "action": "CHANGE_VEHICLE", "value": mapped_vehicle} 187 | ) 188 | ) 189 | 190 | self.gps_offset = lgsvl.Vector(gps_offset_x, gps_offset_y, gps_offset_z) 191 | 192 | if not self.get_current_vehicle() == mapped_vehicle: 193 | folder_name = vehicle.replace(" ", "_") 194 | error_message = ( 195 | "Vehicle calibration {0} was not set. Verify the files exist in " 196 | "/apollo/modules/calibration/data/{1} and restart Dreamview -- Aborting..." 197 | ) 198 | log.error( 199 | error_message.format( 200 | mapped_vehicle, folder_name 201 | ) 202 | ) 203 | sys.exit(1) 204 | return 205 | 206 | def set_setup_mode(self, mode): 207 | """ 208 | mode is the name of the Apollo 5.0 mode as seen in the left-most drop down in the top-right corner of Dreamview 209 | """ 210 | self.ws.send( 211 | json.dumps({"type": "HMIAction", "action": "CHANGE_MODE", "value": mode}) 212 | ) 213 | return 214 | 215 | def get_module_status(self): 216 | """ 217 | Returns a dict where the key is the name of the module and value is a bool based on the module's current status 218 | """ 219 | self.reconnect() 220 | data = json.loads( 221 | self.ws.recv() 222 | ) # This first recv() call returns the SimControlStatus in the form '{"enabled":false,"type":"SimControlStatus"}' 223 | while data["type"] != "HMIStatus": 224 | data = json.loads(self.ws.recv()) 225 | 226 | # The second recv() call also contains other information: 227 | # the current map, vehicle, and mode: 228 | # data["data"]["currentMap"], data["data"]["currentVehicle"], data["data"]["currentMode"] 229 | # 230 | # the available maps, vehicles, and modes: 231 | # data["data"]["maps"], data["data"]["vehicles"], data["data"]["modes"] 232 | # 233 | # the status of monitors components: 234 | # data["data"]["monitoredComponents"] 235 | 236 | return data["data"]["modules"] 237 | 238 | def get_current_map(self): 239 | """ 240 | Returns the current HD Map loaded in Dreamview 241 | """ 242 | try: 243 | self.reconnect() 244 | except ConnectionRefusedError as e: 245 | log.error("Not able to get the current HD map loaded in Dreamview.") 246 | log.error("Original exception: " + str(e)) 247 | return None 248 | 249 | data = json.loads(self.ws.recv()) 250 | while data["type"] != "HMIStatus": 251 | data = json.loads(self.ws.recv()) 252 | return data["data"]["currentMap"] 253 | 254 | def get_current_vehicle(self): 255 | """ 256 | Returns the current Vehicle configuration loaded in Dreamview 257 | """ 258 | try: 259 | self.reconnect() 260 | except ConnectionRefusedError as e: 261 | log.error("Not able to get the current vehicle configuration loaded in Dreamview.") 262 | log.error("Original exception: " + str(e)) 263 | return None 264 | 265 | data = json.loads(self.ws.recv()) 266 | while data["type"] != "HMIStatus": 267 | data = json.loads(self.ws.recv()) 268 | return data["data"]["currentVehicle"] 269 | 270 | def reconnect(self): 271 | """ 272 | Closes the websocket connection and re-creates it so that data can be received again 273 | """ 274 | self.ws.close() 275 | self.ws = create_connection(self.url) 276 | return 277 | 278 | def enable_apollo(self, dest_x, dest_z, modules, coord_type=CoordType.Unity): 279 | """ 280 | Enables a list of modules and then sets the destination 281 | """ 282 | for mod in modules: 283 | logger.info("Starting {} module...".format(mod)) 284 | self.enable_module(mod) 285 | 286 | self.set_destination(dest_x, dest_z, coord_type=coord_type) 287 | 288 | def disable_apollo(self): 289 | """ 290 | Disables all Apollo modules 291 | """ 292 | module_status = self.get_module_status() 293 | for module in module_status.keys(): 294 | self.disable_module(module) 295 | 296 | def check_module_status(self, modules): 297 | """ 298 | Checks if all modules in a provided list are enabled 299 | """ 300 | module_status = self.get_module_status() 301 | for module, status in module_status.items(): 302 | if not status and module in modules: 303 | log.warning( 304 | "Warning: Apollo module {} is not running!!!".format(module) 305 | ) 306 | 307 | def setup_apollo(self, dest_x, dest_z, modules, default_timeout=60.0, coord_type=CoordType.Unity): 308 | """ 309 | Starts a list of Apollo modules and sets the destination. Will wait for Control module to send a message before returning. 310 | Control sending a message indicates that all modules are working and Apollo is ready to continue. 311 | """ 312 | initial_state = self.ego.state 313 | mod_status = self.get_module_status() 314 | logger.info(mod_status) 315 | 316 | # If any module is not enabled, Control may still send a message even though Apollo is not ready 317 | # while not all(mod_status[mod] for mod in modules): 318 | # self.disable_apollo() 319 | # time.sleep(0.5) 320 | # mod_status = self.get_module_status() 321 | if not all(mod_status[mod] for mod in modules): 322 | logger.info('disable modules') 323 | self.disable_apollo() 324 | # time.sleep(0.5) 325 | 326 | self.enable_apollo(dest_x, dest_z, modules, coord_type=coord_type) 327 | self.ego.is_control_received = False 328 | 329 | def on_control_received(agent, kind, context): 330 | if kind == "checkControl": 331 | agent.is_control_received = True 332 | logger.info("Control message received") 333 | 334 | self.ego.on_custom(on_control_received) 335 | 336 | try: 337 | timeout = float(os.environ.get("LGSVL__DREAMVIEW__CONTROL_MESSAGE_TIMEOUT_SECS", default_timeout)) 338 | except Exception: 339 | timeout = default_timeout 340 | logger.warning("Invalid LGSVL__DREAMVIEW__CONTROL_MESSAGE_TIMEOUT_SECS, using default {0}s".format(default_timeout)) 341 | 342 | run_time = 2 343 | elapsed = 0 344 | while timeout <= 0.0 or float(elapsed) < timeout: 345 | self.sim.run(run_time) 346 | 347 | if self.ego.is_control_received: 348 | break 349 | 350 | if elapsed > 0 and elapsed % (run_time * 5) == 0: 351 | self.check_module_status(modules) 352 | logger.info("{} seconds have passed but Ego hasn't received any control messages.".format(elapsed)) 353 | logger.info("Please also check if your route has been set correctly in Dreamview.") 354 | 355 | elapsed += run_time 356 | else: 357 | logger.error("No control message from Apollo within {} seconds. Aborting...".format(timeout)) 358 | self.disable_apollo() 359 | raise WaitApolloError() 360 | 361 | self.ego.state = initial_state 362 | 363 | 364 | class WaitApolloError(Exception): 365 | """ 366 | Raised when Apollo control message is not received in time 367 | """ 368 | 369 | pass 370 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/lgsvl/evaluator/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2020 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | from .utils import TestException, right_lane_check, separation, almost_equal, in_parking_zone 8 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/lgsvl/evaluator/utils.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2020 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | import lgsvl 8 | import numpy 9 | 10 | 11 | class TestException(Exception): 12 | pass 13 | 14 | 15 | def right_lane_check(simulator, ego_transform): 16 | egoLane = simulator.map_point_on_lane(ego_transform.position) 17 | right = lgsvl.utils.transform_to_right(ego_transform) 18 | rightLane = simulator.map_point_on_lane(ego_transform.position + 3.6 * right) 19 | 20 | return almost_equal(egoLane.position.x, rightLane.position.x) and \ 21 | almost_equal(egoLane.position.y, rightLane.position.y) and \ 22 | almost_equal(egoLane.position.z, rightLane.position.z) 23 | 24 | 25 | def in_parking_zone(beginning, end, ego_transform): 26 | forward = lgsvl.utils.transform_to_forward(ego_transform) 27 | b2e = ego_transform.position - beginning # Vector from beginning of parking zone to EGO's position 28 | b2e = b2e * (1 / b2e.magnitude()) # Make it a Unit vector to simplify dot product result 29 | e2e = end - ego_transform.position # Vector from EGO's position to end of parking zone 30 | e2e = e2e * (1 / e2e.magnitude()) 31 | return ( 32 | numpy.dot([forward.x, forward.y, forward.z], [b2e.x, b2e.y, b2e.z]) > 0.9 33 | and numpy.dot([forward.x, forward.y, forward.z], [e2e.x, e2e.y, e2e.z]) > 0.9 34 | ) 35 | 36 | 37 | def almost_equal(a, b, diff=0.5): 38 | return abs(a - b) <= diff 39 | 40 | 41 | def separation(V1, V2): 42 | return (V1 - V2).magnitude() 43 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/lgsvl/geometry.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2020 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | from math import sqrt 8 | 9 | 10 | class Vector: 11 | def __init__(self, x=0.0, y=0.0, z=0.0): 12 | self.x = x 13 | self.y = y 14 | self.z = z 15 | 16 | @staticmethod 17 | def from_json(j): 18 | return Vector(j["x"], j["y"], j["z"]) 19 | 20 | def to_json(self): 21 | return {"x": self.x, "y": self.y, "z": self.z} 22 | 23 | def __repr__(self): 24 | return "Vector({}, {}, {})".format(self.x, self.y, self.z) 25 | 26 | def __add__(self, v): 27 | if isinstance(v, Vector): 28 | return Vector(self.x + v.x, self.y + v.y, self.z + v.z) 29 | elif isinstance(v, (int, float)): 30 | return Vector(self.x + v, self.y + v, self.z + v) 31 | else: 32 | raise TypeError("Vector addition only allowed between Vectors and numbers") 33 | 34 | def __sub__(self, v): 35 | if isinstance(v, Vector): 36 | return Vector(self.x - v.x, self.y - v.y, self.z - v.z) 37 | elif isinstance(v, (int, float)): 38 | return Vector(self.x - v, self.y - v, self.z - v) 39 | else: 40 | raise TypeError("Vector subtraction only allowed between Vectors and numbers") 41 | 42 | def __mul__(self, v): 43 | if isinstance(v, Vector): 44 | return Vector(self.x * v.x, self.y * v.y, self.z * v.z) 45 | elif isinstance(v, (int, float)): 46 | return Vector(self.x * v, self.y * v, self.z * v) 47 | else: 48 | raise TypeError("Vector multiplication only allowed between Vectors and numbers") 49 | 50 | def __rmul__(self, v): 51 | return self * v 52 | 53 | def __neg__(self): 54 | return Vector(-self.x, -self.y, -self.z) 55 | 56 | def magnitude(self): 57 | return sqrt(self.x**2 + self.y**2 + self.z**2) 58 | 59 | 60 | class BoundingBox: 61 | def __init__(self, min, max): 62 | self.min = min 63 | self.max = max 64 | 65 | @staticmethod 66 | def from_json(j): 67 | return BoundingBox(Vector.from_json(j["min"]), Vector.from_json(j["max"])) 68 | 69 | def to_json(self): 70 | return {"min": self.min.to_json(), "max": self.max.to_json()} 71 | 72 | def __repr__(self): 73 | return "BoundingBox({}, {})".format(self.min, self.max) 74 | 75 | @property 76 | def center(self): 77 | return Vector( 78 | (self.max.x + self.min.x) * 0.5, 79 | (self.max.y + self.min.y) * 0.5, 80 | (self.max.z + self.min.z) * 0.5, 81 | ) 82 | 83 | @property 84 | def size(self): 85 | return Vector( 86 | self.max.x - self.min.x, 87 | self.max.y - self.min.y, 88 | self.max.z - self.min.z, 89 | ) 90 | 91 | 92 | class Transform: 93 | def __init__(self, position=None, rotation=None): 94 | if position is None: position = Vector() 95 | if rotation is None: rotation = Vector() 96 | self.position = position 97 | self.rotation = rotation 98 | 99 | @staticmethod 100 | def from_json(j): 101 | return Transform(Vector.from_json(j["position"]), Vector.from_json(j["rotation"])) 102 | 103 | def to_json(self): 104 | return {"position": self.position.to_json(), "rotation": self.rotation.to_json()} 105 | 106 | def __repr__(self): 107 | return "Transform(position={}, rotation={})".format(self.position, self.rotation) 108 | 109 | 110 | class Spawn: 111 | def __init__(self, transform=None, destinations=None): 112 | if transform is None: transform = Transform() 113 | if destinations is None: destinations = [] 114 | self.position = transform.position 115 | self.rotation = transform.rotation 116 | self.destinations = destinations 117 | 118 | @staticmethod 119 | def from_json(j): 120 | spawn_point = Transform.from_json(j) 121 | destinations = [] 122 | if "destinations" in j: 123 | for d in j["destinations"]: 124 | destinations.append(Transform.from_json(d)) 125 | 126 | return Spawn(spawn_point, destinations) 127 | 128 | def to_json(self): 129 | return {"position": self.position.to_json(), "rotation": self.rotation.to_json()} 130 | 131 | def __repr__(self): 132 | return "Spawn(position={}, rotation={}, destinations={})".format( 133 | self.position, self.rotation, self.destinations 134 | ) 135 | 136 | 137 | class Quaternion: 138 | def __init__(self, x=0.0, y=0.0, z=0.0, w=0.0): 139 | self.x = x 140 | self.y = y 141 | self.z = z 142 | self.w = w 143 | 144 | @staticmethod 145 | def from_json(j): 146 | return Quaternion(j["x"], j["y"], j["z"], j["w"]) 147 | 148 | def to_json(self): 149 | return {"x": self.x, "y": self.y, "z": self.z, "w": self.w} 150 | 151 | def __repr__(self): 152 | return "Quaternion({}, {}, {}, {})".format(self.x, self.y, self.z, self.w) 153 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/lgsvl/remote.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2020 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | import threading 8 | import websockets 9 | import asyncio 10 | import json 11 | 12 | from multiprocessing import Queue 13 | from loguru import logger 14 | 15 | class Remote(threading.Thread): 16 | 17 | def __init__(self, host, port): 18 | super().__init__(daemon=True) 19 | self.endpoint = "ws://{}:{}".format(host, port) 20 | self.lock = threading.Lock() 21 | self.cv = threading.Condition() 22 | self.data = None 23 | self.sem = threading.Semaphore(0) 24 | self.running = True 25 | self.start() 26 | self.sem.acquire() 27 | 28 | self.sim_data = None 29 | self.state_queue = Queue() 30 | 31 | def run(self): 32 | logger.info('Remote: run') 33 | self.loop = asyncio.new_event_loop() 34 | asyncio.set_event_loop(self.loop) 35 | self.loop.run_until_complete(self.process()) 36 | 37 | def close(self): 38 | asyncio.run_coroutine_threadsafe(self.websocket.close(), self.loop) 39 | self.join() 40 | self.loop.close() 41 | 42 | async def process(self): 43 | self.websocket = await websockets.connect(self.endpoint, compression=None) 44 | self.sem.release() 45 | 46 | while True: 47 | try: 48 | data = await self.websocket.recv() # continuous listen 49 | # logger.info(data) 50 | except Exception as e: 51 | if isinstance(e, websockets.exceptions.ConnectionClosed): 52 | break 53 | with self.cv: 54 | self.data = {"error": str(e)} 55 | self.cv.notify() 56 | break 57 | 58 | with self.cv: 59 | self.data = json.loads(data) # if running -> None, will overwrite last one 60 | if self.data is not None: 61 | # logger.warning(self.data) 62 | if ('result' in self.data.keys()) and (self.data['result'] is not None) and isinstance(self.data['result'], dict) and "events" in self.data['result']: 63 | # logger.error(self.data) 64 | self.sim_data = self.data 65 | # self.data = None 66 | self.cv.notify() 67 | 68 | logger.error('Close websocket') 69 | await self.websocket.close() 70 | 71 | def command(self, name, args={}): 72 | if not self.websocket: 73 | raise Exception("Not connected") 74 | 75 | input_data = json.dumps({"command": name, "arguments": args}) 76 | 77 | if self.data is not None: 78 | self.sim_data = self.data 79 | self.data = None 80 | 81 | asyncio.run_coroutine_threadsafe(self.websocket.send(input_data), self.loop) 82 | 83 | with self.cv: 84 | if name == 'agent/state/get': 85 | self.cv.wait_for(lambda: (self.data is not None and isinstance(self.data, dict)) and ('result' in self.data.keys()) and (self.data['result'] is not None) and ("events" not in self.data['result'])) 86 | if isinstance(self.data['result'], dict) and "event" in self.data['result']: 87 | logger.error(self.data) 88 | else: 89 | self.cv.wait_for(lambda: self.data is not None) 90 | output_data = self.data 91 | self.data = None 92 | 93 | if "error" in output_data: 94 | raise Exception(output_data["error"]) 95 | return output_data["result"] 96 | 97 | def status_monitor(self): 98 | if not self.websocket: 99 | raise Exception("Not connected") 100 | 101 | if self.sim_data is not None: 102 | if "error" in self.sim_data: 103 | raise Exception(self.sim_data["error"]) 104 | return self.sim_data["result"] 105 | else: 106 | self.sim_data = self.data 107 | self.data = None 108 | if self.sim_data is None: 109 | return self.sim_data 110 | else: 111 | if "error" in self.sim_data: 112 | raise Exception(self.sim_data["error"]) 113 | return self.sim_data["result"] 114 | 115 | def command_run(self, name, args={}): 116 | if not self.websocket: 117 | raise Exception("Not connected") 118 | logger.info('[PythonAPI] Start Running') 119 | data = json.dumps({"command": name, "arguments": args}) 120 | asyncio.run_coroutine_threadsafe(self.websocket.send(data), self.loop) 121 | self.sim_data = None 122 | self.data = None 123 | return None -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/lgsvl/sensor.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2020 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | from .geometry import Transform 8 | from .utils import accepts 9 | 10 | from collections import namedtuple 11 | 12 | GpsData = namedtuple("GpsData", "latitude longitude northing easting altitude orientation") 13 | 14 | 15 | class Sensor: 16 | def __init__(self, remote, uid, name): 17 | self.remote = remote 18 | self.uid = uid 19 | self.name = name 20 | 21 | @property 22 | def transform(self): 23 | j = self.remote.command("sensor/transform/get", {"uid": self.uid}) 24 | return Transform.from_json(j) 25 | 26 | @property 27 | def enabled(self): 28 | return self.remote.command("sensor/enabled/get", {"uid": self.uid}) 29 | 30 | @enabled.setter 31 | @accepts(bool) 32 | def enabled(self, value): 33 | self.remote.command("sensor/enabled/set", {"uid": self.uid, "enabled": value}) 34 | 35 | def __eq__(self, other): 36 | return self.uid == other.uid 37 | 38 | def __hash__(self): 39 | return hash(self.uid) 40 | 41 | @staticmethod 42 | def create(remote, j): 43 | if j["type"] == "camera": 44 | return CameraSensor(remote, j) 45 | if j["type"] == "lidar": 46 | return LidarSensor(remote, j) 47 | if j["type"] == "imu": 48 | return ImuSensor(remote, j) 49 | if j["type"] == "gps": 50 | return GpsSensor(remote, j) 51 | if j["type"] == "radar": 52 | return RadarSensor(remote, j) 53 | if j["type"] == "canbus": 54 | return CanBusSensor(remote, j) 55 | if j["type"] == "recorder": 56 | return VideoRecordingSensor(remote, j) 57 | if j["type"] == "analysis": 58 | return AnalysisSensor(remote, j) 59 | raise ValueError("Sensor type '{}' not supported".format(j["type"])) 60 | 61 | 62 | class CameraSensor(Sensor): 63 | def __init__(self, remote, j): 64 | super().__init__(remote, j["uid"], j["name"]) 65 | self.frequency = j["frequency"] 66 | self.width = j["width"] 67 | self.height = j["height"] 68 | self.fov = j["fov"] 69 | self.near_plane = j["near_plane"] 70 | self.far_plane = j["far_plane"] 71 | """ 72 | RGB - 24-bit color image 73 | DEPTH - 8-bit grayscale depth buffer 74 | SEMANTIC - 24-bit color image with semantic segmentation 75 | """ 76 | self.format = j["format"] 77 | 78 | @accepts(str, int, int) 79 | def save(self, path, quality=75, compression=6): 80 | success = self.remote.command("sensor/camera/save", { 81 | "uid": self.uid, 82 | "path": path, 83 | "quality": quality, 84 | "compression": compression, 85 | }) 86 | return success 87 | 88 | 89 | class LidarSensor(Sensor): 90 | def __init__(self, remote, j): 91 | super().__init__(remote, j["uid"], j["name"]) 92 | self.min_distance = j["min_distance"] 93 | self.max_distance = j["max_distance"] 94 | self.rays = j["rays"] 95 | self.rotations = j["rotations"] # rotation frequency, Hz 96 | self.measurements = j["measurements"] # how many measurements each ray does per one rotation 97 | self.fov = j["fov"] 98 | self.angle = j["angle"] 99 | self.compensated = j["compensated"] 100 | 101 | @accepts(str) 102 | def save(self, path): 103 | success = self.remote.command("sensor/lidar/save", { 104 | "uid": self.uid, 105 | "path": path, 106 | }) 107 | return success 108 | 109 | 110 | class ImuSensor(Sensor): 111 | def __init__(self, remote, j): 112 | super().__init__(remote, j["uid"], j["name"]) 113 | 114 | 115 | class GpsSensor(Sensor): 116 | def __init__(self, remote, j): 117 | super().__init__(remote, j["uid"], j["name"]) 118 | self.frequency = j["frequency"] 119 | 120 | @property 121 | def data(self): 122 | j = self.remote.command("sensor/gps/data", {"uid": self.uid}) 123 | return GpsData(j["latitude"], j["longitude"], j["northing"], j["easting"], j["altitude"], j["orientation"]) 124 | 125 | 126 | class RadarSensor(Sensor): 127 | def __init__(self, remote, j): 128 | super().__init__(remote, j["uid"], j["name"]) 129 | 130 | 131 | class CanBusSensor(Sensor): 132 | def __init__(self, remote, j): 133 | super().__init__(remote, j["uid"], j["name"]) 134 | self.frequency = j["frequency"] 135 | 136 | 137 | class VideoRecordingSensor(Sensor): 138 | def __init__(self, remote, j): 139 | super().__init__(remote, j["uid"], j["name"]) 140 | self.width = j["width"] 141 | self.height = j["height"] 142 | self.framerate = j["framerate"] 143 | self.min_distance = j["near_plane"] 144 | self.max_distance = j["far_plane"] 145 | self.fov = j["fov"] 146 | self.quality = j["quality"] 147 | self.bitrate = j["bitrate"] 148 | self.max_bitrate = j["max_bitrate"] 149 | 150 | 151 | class AnalysisSensor(Sensor): 152 | def __init__(self, remote, j): 153 | super().__init__(remote, j["uid"], j["name"]) 154 | self.stucktravelthreshold = j["stucktravelthreshold"] 155 | self.stucktimethreshold = j["stucktimethreshold"] 156 | self.stoplinethreshold = j["stoplinethreshold"] 157 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/lgsvl/simulator.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2021 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | from .remote import Remote 8 | from .agent import Agent, AgentType, AgentState 9 | from .sensor import GpsData 10 | from .geometry import Vector, Transform, Spawn, Quaternion 11 | from .utils import accepts, ObjectState 12 | from .controllable import Controllable 13 | 14 | from enum import Enum 15 | from collections import namedtuple 16 | from environs import Env 17 | from datetime import datetime 18 | import re 19 | 20 | RaycastHit = namedtuple("RaycastHit", "distance point normal") 21 | 22 | WeatherState = namedtuple("WeatherState", "rain fog wetness cloudiness damage") 23 | WeatherState.__new__.__defaults__ = (0,) * len(WeatherState._fields) 24 | 25 | env = Env() 26 | 27 | from loguru import logger 28 | 29 | class Simulator: 30 | 31 | class SimulatorCameraState(Enum): 32 | FREE = 0 33 | FOLLOW = 1 34 | CINEMATIC = 2 35 | DRIVER = 3 36 | 37 | @accepts(str, int) 38 | def __init__(self, address=env.str("LGSVL__SIMULATOR_HOST", "localhost"), port=env.int("LGSVL__SIMULATOR_PORT", 8181)): 39 | if port <= 0 or port > 65535: 40 | raise ValueError("port value is out of range") 41 | self.remote = Remote(address, port) 42 | self.agents = {} 43 | self.callbacks = {} 44 | self.stopped = False 45 | self.stop_flag = False 46 | 47 | def close(self): 48 | self.remote.close() 49 | 50 | @accepts(str, int) 51 | def load(self, scene, seed=None): 52 | self.remote.command("simulator/load_scene", {"scene": scene, "seed": seed}) 53 | self.agents.clear() 54 | self.callbacks.clear() 55 | 56 | @property 57 | def version(self): 58 | return self.remote.command("simulator/version") 59 | 60 | @property 61 | def layers(self): 62 | return self.remote.command("simulator/layers/get") 63 | 64 | @property 65 | def current_scene(self): 66 | return self.remote.command("simulator/current_scene") 67 | 68 | @property 69 | def current_scene_id(self): 70 | return self.remote.command("simulator/current_scene_id") 71 | 72 | @property 73 | def current_frame(self): 74 | return self.remote.command("simulator/current_frame") 75 | 76 | @property 77 | def current_time(self): 78 | return self.remote.command("simulator/current_time") 79 | 80 | @property 81 | def available_agents(self): 82 | return self.remote.command("simulator/available_agents") 83 | 84 | @property 85 | def available_npc_behaviours(self): 86 | return self.remote.command("simulator/npc/available_behaviours") 87 | 88 | @accepts(Transform) 89 | def set_sim_camera(self, transform): 90 | self.remote.command("simulator/camera/set", {"transform": transform.to_json()}) 91 | 92 | @accepts(SimulatorCameraState) 93 | def set_sim_camera_state(self, state): 94 | self.remote.command("simulator/camera/state/set", {"state": state.value}) 95 | 96 | def agents_traversed_waypoints(self, fn): 97 | self._add_callback(None, "agents_traversed_waypoints", fn) 98 | 99 | def reset(self): 100 | self.remote.command("simulator/reset") 101 | self.agents.clear() 102 | self.callbacks.clear() 103 | 104 | def stop(self): 105 | self.stopped = True 106 | 107 | @accepts((int, float), (int, float)) 108 | def run(self, time_limit=0.0, time_scale=None): 109 | self._process("simulator/run", {"time_limit": time_limit, "time_scale": time_scale}) 110 | 111 | @accepts((int, float), (int, float)) 112 | def run_custom(self, time_limit=0.0, time_scale=None): 113 | self.stop_flag = False 114 | logger.info('[PythonAPI] simulator.run_custom') 115 | cmd = "simulator/run" 116 | args = {"time_limit": time_limit, "time_scale": time_scale} 117 | j = self.remote.command_run(cmd, args) 118 | 119 | def check_status(self): 120 | j = self.remote.status_monitor() 121 | if j is None: 122 | return False 123 | if "events" in j: 124 | stop = self._process_events(j["events"]) 125 | logger.warning('events: ' + str(j["events"])) 126 | logger.warning('stop: ' + str(stop)) 127 | return stop 128 | return False 129 | 130 | def _add_callback(self, agent, name, fn): 131 | if agent not in self.callbacks: 132 | self.callbacks[agent] = {} 133 | if name not in self.callbacks[agent]: 134 | self.callbacks[agent][name] = set() 135 | self.callbacks[agent][name].add(fn) 136 | 137 | def _process_events(self, events): 138 | self.stopped = False 139 | stop = False 140 | for ev in events: 141 | if "agent" in ev: 142 | agent = self.agents[ev["agent"]] 143 | if agent in self.callbacks: 144 | callbacks = self.callbacks[agent] 145 | event_type = ev["type"] 146 | if event_type in callbacks: 147 | for fn in callbacks[event_type]: 148 | if event_type == "collision": 149 | stop = True 150 | fn(agent, self.agents.get(ev["other"]), Vector.from_json(ev["contact"]) if ev["contact"] is not None else None) 151 | elif event_type == "waypoint_reached": 152 | fn(agent, ev["index"]) 153 | elif event_type == "stop_line": 154 | fn(agent) 155 | elif event_type == "lane_change": 156 | fn(agent) 157 | elif event_type == "destination_reached": 158 | stop = True 159 | fn(agent) 160 | elif event_type == "custom": 161 | fn(agent, ev["kind"], ev["context"]) 162 | if self.stopped: 163 | return 164 | elif None in self.callbacks: 165 | callbacks = self.callbacks[None] 166 | event_type = ev["type"] 167 | if event_type in callbacks: 168 | for fn in callbacks[event_type]: 169 | if event_type == "agents_traversed_waypoints": 170 | fn() 171 | return stop 172 | 173 | def _process(self, cmd, args): 174 | # logger.warning('[PythonAPI] _process: ' + str(cmd) + ' ' + str(args)) 175 | j = self.remote.command(cmd, args) 176 | while True: 177 | # logger.error('[PythonAPI] j value: ' + str(j)) 178 | if j is None: 179 | return 180 | if "events" in j: 181 | _ = self._process_events(j["events"]) 182 | if self.stopped: 183 | break 184 | j = self.remote.command("simulator/continue") 185 | # logger.error('_process continue j: ' + str(j)) 186 | 187 | @accepts(str, AgentType, (AgentState, type(None)), (Vector, type(None))) 188 | def add_agent(self, name, agent_type, state=None, color=None): 189 | if state is None: state = AgentState() 190 | if color is None: color = Vector(-1, -1, -1) 191 | args = {"name": name, "type": agent_type.value, "state": state.to_json(), "color": color.to_json()} 192 | uid = self.remote.command("simulator/add_agent", args) 193 | agent = Agent.create(self, uid, agent_type) 194 | agent.name = name 195 | self.agents[uid] = agent 196 | return agent 197 | 198 | @accepts(Agent) 199 | def remove_agent(self, agent): 200 | self.remote.command("simulator/agent/remove", {"uid": agent.uid}) 201 | del self.agents[agent.uid] 202 | if agent in self.callbacks: 203 | del self.callbacks[agent] 204 | 205 | @accepts(AgentType) 206 | def add_random_agents(self, agent_type): 207 | args = {"type": agent_type.value} 208 | self.remote.command("simulator/add_random_agents", args) 209 | 210 | def get_agents(self): 211 | return list(self.agents.values()) 212 | 213 | @property 214 | def weather(self): 215 | j = self.remote.command("environment/weather/get") 216 | return WeatherState(j.get("rain", 0), j.get("fog", 0), j.get("wetness", 0), j.get("cloudiness", 0), j.get("damage", 0)) 217 | 218 | @weather.setter 219 | @accepts(WeatherState) 220 | def weather(self, state): 221 | self.remote.command("environment/weather/set", {"rain": state.rain, "fog": state.fog, "wetness": state.wetness, "cloudiness": state.cloudiness, "damage": state.damage}) 222 | 223 | @property 224 | def time_of_day(self): 225 | return self.remote.command("environment/time/get") 226 | 227 | @property 228 | def current_datetime(self): 229 | date_time_str = self.remote.command("simulator/datetime/get") 230 | date_time_arr = list(map(int, re.split('[. :]', date_time_str))) 231 | date_time = datetime( 232 | date_time_arr[2], 233 | date_time_arr[1], 234 | date_time_arr[0], 235 | date_time_arr[3], 236 | date_time_arr[4], 237 | date_time_arr[5] 238 | ) 239 | return date_time 240 | 241 | @accepts((int, float), bool) 242 | def set_time_of_day(self, time, fixed=True): 243 | self.remote.command("environment/time/set", {"time": time, "fixed": fixed}) 244 | 245 | @accepts(datetime, bool) 246 | def set_date_time(self, date_time, fixed=True): 247 | date_time = date_time.__str__() 248 | self.remote.command("environment/datetime/set", {"datetime": date_time, "fixed": fixed}) 249 | 250 | def get_spawn(self): 251 | spawns = self.remote.command("map/spawn/get") 252 | return [Spawn.from_json(spawn) for spawn in spawns] 253 | 254 | @accepts((Transform, Spawn)) 255 | def map_to_gps(self, transform): 256 | j = self.remote.command("map/to_gps", {"transform": transform.to_json()}) 257 | return GpsData(j["latitude"], j["longitude"], j["northing"], j["easting"], j["altitude"], j["orientation"]) 258 | 259 | def map_from_gps(self, latitude=None, longitude=None, northing=None, easting=None, altitude=None, orientation=None): 260 | c = [] 261 | coord = { 262 | "latitude": latitude, 263 | "longitude": longitude, 264 | "northing": northing, 265 | "easting": easting, 266 | "altitude": altitude, 267 | "orientation": orientation 268 | } 269 | c.append(coord) 270 | return self.map_from_gps_batch(c)[0] 271 | 272 | def map_from_gps_batch(self, coords): 273 | # coords dictionary 274 | jarr = [] 275 | 276 | for c in coords: 277 | j = {} 278 | numtype = (int, float) 279 | if ("latitude" in c and c["latitude"] is not None) and ("longitude" in c and c["longitude"] is not None): 280 | if not isinstance(c["latitude"], numtype): raise TypeError("Argument 'latitude' should have '{}' type".format(numtype)) 281 | if not isinstance(c["longitude"], numtype): raise TypeError("Argument 'longitude' should have '{}' type".format(numtype)) 282 | if c["latitude"] < -90 or c["latitude"] > 90: raise ValueError("Latitude is out of range") 283 | if c["longitude"] < -180 or c["longitude"] > 180: raise ValueError("Longitude is out of range") 284 | j["latitude"] = c["latitude"] 285 | j["longitude"] = c["longitude"] 286 | elif ("northing" in c and c["northing"] is not None) and ("easting" in c and c["easting"] is not None): 287 | if not isinstance(c["northing"], numtype): raise TypeError("Argument 'northing' should have '{}' type".format(numtype)) 288 | if not isinstance(c["easting"], numtype): raise TypeError("Argument 'easting' should have '{}' type".format(numtype)) 289 | if c["northing"] < 0 or c["northing"] > 10000000: raise ValueError("Northing is out of range") 290 | if c["easting"] < 160000 or c["easting"] > 834000: raise ValueError("Easting is out of range") 291 | j["northing"] = c["northing"] 292 | j["easting"] = c["easting"] 293 | else: 294 | raise Exception("Either latitude and longitude or northing and easting should be specified") 295 | if "altitude" in c and c["altitude"] is not None: 296 | if not isinstance(c["altitude"], numtype): raise TypeError("Argument 'altitude' should have '{}' type".format(numtype)) 297 | j["altitude"] = c["altitude"] 298 | if "orientation" in c and c["orientation"] is not None: 299 | if not isinstance(c["orientation"], numtype): raise TypeError("Argument 'orientation' should have '{}' type".format(numtype)) 300 | j["orientation"] = c["orientation"] 301 | jarr.append(j) 302 | 303 | jarr = self.remote.command("map/from_gps", jarr) 304 | transforms = [] 305 | for j in jarr: 306 | transforms.append(Transform.from_json(j)) 307 | return transforms 308 | 309 | @accepts(Vector) 310 | def map_point_on_lane(self, point): 311 | j = self.remote.command("map/point_on_lane", {"point": point.to_json()}) 312 | return Transform.from_json(j) 313 | 314 | @accepts(Vector, Quaternion) 315 | def map_from_nav(self, position, orientation): 316 | res = self.remote.command( 317 | "map/from_nav", 318 | { 319 | "position": position.to_json(), 320 | "orientation": orientation.to_json() 321 | } 322 | ) 323 | return Transform.from_json(res) 324 | 325 | @accepts(Transform, Vector) 326 | def set_nav_origin(self, transform, offset=Vector()): 327 | self.remote.command( 328 | "navigation/set_origin", 329 | { 330 | "transform": transform.to_json(), 331 | "offset": offset.to_json(), 332 | } 333 | ) 334 | 335 | def get_nav_origin(self): 336 | res = self.remote.command("navigation/get_origin") 337 | nav_origin = None 338 | if res: 339 | nav_origin = { 340 | "transform": Transform.from_json(res), 341 | "offset": res["offset"] 342 | } 343 | return nav_origin 344 | 345 | @accepts(Vector, Vector, int, float) 346 | def raycast(self, origin, direction, layer_mask=-1, max_distance=float("inf")): 347 | hit = self.remote.command("simulator/raycast", [{ 348 | "origin": origin.to_json(), 349 | "direction": direction.to_json(), 350 | "layer_mask": layer_mask, 351 | "max_distance": max_distance 352 | }]) 353 | if hit[0] is None: 354 | return None 355 | return RaycastHit(hit[0]["distance"], Vector.from_json(hit[0]["point"]), Vector.from_json(hit[0]["normal"])) 356 | 357 | def raycast_batch(self, args): 358 | jarr = [] 359 | for arg in args: 360 | jarr.append({ 361 | "origin": arg["origin"].to_json(), 362 | "direction": arg["direction"].to_json(), 363 | "layer_mask": arg["layer_mask"], 364 | "max_distance": arg["max_distance"] 365 | }) 366 | 367 | hits = self.remote.command("simulator/raycast", jarr) 368 | results = [] 369 | for hit in hits: 370 | if hit is None: 371 | results.append(None) 372 | else: 373 | results.append(RaycastHit(hit["distance"], Vector.from_json(hit["point"]), Vector.from_json(hit["normal"]))) 374 | 375 | return results 376 | 377 | @accepts(str, (ObjectState, type(None))) 378 | def controllable_add(self, name, object_state=None): 379 | if object_state is None: object_state = ObjectState() 380 | args = {"name": name, "state": object_state.to_json()} 381 | j = self.remote.command("simulator/controllable_add", args) 382 | controllable = Controllable(self.remote, j) 383 | controllable.name = name 384 | return controllable 385 | 386 | @accepts(Controllable) 387 | def controllable_remove(self, controllable): 388 | self.remote.command("simulator/controllable_remove", {"uid": controllable.uid}) 389 | del self.controllables[controllable.uid] 390 | 391 | @accepts(str) 392 | def get_controllables(self, control_type=None): 393 | j = self.remote.command("controllable/get/all", { 394 | "type": control_type, 395 | }) 396 | return [Controllable(self.remote, controllable) for controllable in j] 397 | 398 | @accepts(str) 399 | def get_controllable_by_uid(self, uid): 400 | j = self.remote.command("controllable/get", { 401 | "uid": uid, 402 | }) 403 | return Controllable(self.remote, j) 404 | 405 | @accepts(Vector, str) 406 | def get_controllable(self, position, control_type=None): 407 | j = self.remote.command("controllable/get", { 408 | "position": position.to_json(), 409 | "type": control_type, 410 | }) 411 | return Controllable(self.remote, j) 412 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/lgsvl/utils.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2020 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | from .geometry import Vector, Transform 8 | 9 | import math 10 | import inspect 11 | 12 | 13 | def accepts(*types): 14 | def check_accepts(f): 15 | assert len(types) + 1 == f.__code__.co_argcount 16 | 17 | def new_f(*args, **kwargs): 18 | names = inspect.getfullargspec(f)[0] 19 | it = zip(args[1:], types, names[1:]) 20 | for (a, t, n) in it: 21 | if not isinstance(a, t): 22 | raise TypeError("Argument '{}' should have '{}' type".format(n, t)) 23 | return f(*args, **kwargs) 24 | new_f.__name__ = f.__name__ 25 | return new_f 26 | return check_accepts 27 | 28 | 29 | class ObjectState: 30 | def __init__(self, transform=None, velocity=None, angular_velocity=None): 31 | if transform is None: 32 | transform = Transform() 33 | if velocity is None: 34 | velocity = Vector() 35 | if angular_velocity is None: 36 | angular_velocity = Vector() 37 | self.transform = transform 38 | self.velocity = velocity 39 | self.angular_velocity = angular_velocity 40 | 41 | @property 42 | def position(self): 43 | return self.transform.position 44 | 45 | @property 46 | def rotation(self): 47 | return self.transform.rotation 48 | 49 | @property 50 | def speed(self): 51 | return math.sqrt( 52 | self.velocity.x * self.velocity.x 53 | + self.velocity.y * self.velocity.y 54 | + self.velocity.z * self.velocity.z 55 | ) 56 | 57 | @staticmethod 58 | def from_json(j): 59 | return ObjectState( 60 | Transform.from_json(j["transform"]), 61 | Vector.from_json(j["velocity"]), 62 | Vector.from_json(j["angular_velocity"]), 63 | ) 64 | 65 | def to_json(self): 66 | return { 67 | "transform": self.transform.to_json(), 68 | "velocity": self.velocity.to_json(), 69 | "angular_velocity": self.angular_velocity.to_json(), 70 | } 71 | 72 | def __repr__(self): 73 | return str( 74 | { 75 | "transform": str(self.transform), 76 | "velocity": str(self.velocity), 77 | "angular_velocity": str(self.angular_velocity), 78 | } 79 | ) 80 | 81 | 82 | def transform_to_matrix(tr): 83 | px = tr.position.x 84 | py = tr.position.y 85 | pz = tr.position.z 86 | 87 | ax = tr.rotation.x * math.pi / 180.0 88 | ay = tr.rotation.y * math.pi / 180.0 89 | az = tr.rotation.z * math.pi / 180.0 90 | 91 | sx, cx = math.sin(ax), math.cos(ax) 92 | sy, cy = math.sin(ay), math.cos(ay) 93 | sz, cz = math.sin(az), math.cos(az) 94 | 95 | # Unity uses left-handed coordinate system, Rz * Rx * Ry order 96 | return [ 97 | [sx * sy * sz + cy * cz, cx * sz, sx * cy * sz - sy * cz, 0.0], 98 | [sx * sy * cz - cy * sz, cx * cz, sy * sz + sx * cy * cz, 0.0], 99 | [cx * sy, -sx, cx * cy, 0.0], 100 | [px, py, pz, 1.0], 101 | ] 102 | 103 | 104 | def transform_to_forward(tr): 105 | ax = tr.rotation.x * math.pi / 180.0 106 | sx, cx = math.sin(ax), math.cos(ax) 107 | 108 | ay = tr.rotation.y * math.pi / 180.0 109 | sy, cy = math.sin(ay), math.cos(ay) 110 | 111 | return Vector(cx * sy, -sx, cx * cy) 112 | 113 | 114 | def transform_to_up(tr): 115 | ax = tr.rotation.x * math.pi / 180.0 116 | ay = tr.rotation.y * math.pi / 180.0 117 | az = tr.rotation.z * math.pi / 180.0 118 | 119 | sx, cx = math.sin(ax), math.cos(ax) 120 | sy, cy = math.sin(ay), math.cos(ay) 121 | sz, cz = math.sin(az), math.cos(az) 122 | 123 | return Vector(sx * sy * cz - cy * sz, cx * cz, sy * sz + sx * cy * cz) 124 | 125 | 126 | def transform_to_right(tr): 127 | ax = tr.rotation.x * math.pi / 180.0 128 | ay = tr.rotation.y * math.pi / 180.0 129 | az = tr.rotation.z * math.pi / 180.0 130 | 131 | sx, cx = math.sin(ax), math.cos(ax) 132 | sy, cy = math.sin(ay), math.cos(ay) 133 | sz, cz = math.sin(az), math.cos(az) 134 | 135 | return Vector(sx * sy * sz + cy * cz, cx * sz, sx * cy * sz - sy * cz) 136 | 137 | 138 | def vector_dot(a, b): 139 | return a.x * b.x + a.y * b.y + a.z * b.z 140 | 141 | 142 | # this works only with transformation matrices (no scaling, no projection) 143 | def matrix_inverse(m): 144 | x = Vector(m[0][0], m[0][1], m[0][2]) 145 | y = Vector(m[1][0], m[1][1], m[1][2]) 146 | z = Vector(m[2][0], m[2][1], m[2][2]) 147 | v = Vector(m[3][0], m[3][1], m[3][2]) 148 | a = -vector_dot(v, x) 149 | b = -vector_dot(v, y) 150 | c = -vector_dot(v, z) 151 | return [ 152 | [x.x, y.x, z.x, 0.0], 153 | [x.y, y.y, z.y, 0.0], 154 | [x.z, y.z, z.z, 0.0], 155 | [a, b, c, 1.0], 156 | ] 157 | 158 | 159 | def matrix_multiply(a, b): 160 | r = [[0, 0, 0, 0] for t in range(4)] 161 | for i in range(4): 162 | for j in range(4): 163 | for k in range(4): 164 | r[i][j] += a[i][k] * b[k][j] 165 | return r 166 | 167 | 168 | def vector_multiply(v, m): 169 | tmp = [None] * 3 170 | for i in range(3): 171 | tmp[i] = v.x * m[0][i] + v.y * m[1][i] + v.z * m[2][i] + m[3][i] 172 | return Vector(tmp[0], tmp[1], tmp[2]) 173 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/lgsvl/wise/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2021 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | from .wise import DefaultAssets, SimulatorSettings 8 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/lgsvl/wise/wise.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2021 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | 8 | # Settings classes with common variables used in the quickstart and example scripts. 9 | class SimulatorSettings: 10 | # IP address of the endpoint running the Simulator. Default value is "127.0.0.1" 11 | simulator_host = "127.0.0.1" 12 | 13 | # Port used by the Simulator for the Python API connection. Default value is 8181 14 | simulator_port = 8181 15 | 16 | # IP address of the endpoint running the bridge. Default value is "127.0.0.1" 17 | bridge_host = "127.0.0.1" 18 | 19 | # Port used by the Simulator for the bridge connection. Default value is 9090 20 | bridge_port = 9090 21 | 22 | 23 | class DefaultAssets: 24 | # Map loaded in the quickstart scripts. Changing the map may lead to unexpected behaviour in the quickstart scripts. Default value is "BorregasAve". 25 | # https://wise.svlsimulator.com/maps/profile/aae03d2a-b7ca-4a88-9e41-9035287a12cc 26 | map_borregasave = "aae03d2a-b7ca-4a88-9e41-9035287a12cc" 27 | 28 | # Map loaded in the quickstart scripts. Changing the map may lead to unexpected behaviour in the quickstart scripts. Default value is "CubeTown". 29 | # https://wise.svlsimulator.com/maps/profile/06773677-1ce3-492f-9fe2-b3147e126e27 30 | map_cubetown = "06773677-1ce3-492f-9fe2-b3147e126e27" 31 | 32 | # Map loaded in some of the examples. Changing the map may lead to unexpected behaviour in the example scripts. Default value is "SanFrancisco". 33 | # https://wise.svlsimulator.com/maps/profile/5d272540-f689-4355-83c7-03bf11b6865f 34 | map_sanfrancisco = "5d272540-f689-4355-83c7-03bf11b6865f" 35 | 36 | # Map loaded in some of the examples. Changing the map may lead to unexpected behaviour in the example scripts. Default value is "Straight1LanePedestrianCrosswalk". 37 | # https://wise.svlsimulator.com/maps/profile/a3a818b5-c66b-488a-a780-979bd5692db1 38 | map_straight1lanepedestriancrosswalk = "a3a818b5-c66b-488a-a780-979bd5692db1" 39 | 40 | # Map loaded in some of the examples. Changing the map may lead to unexpected behaviour in the example scripts. Default value is "SingleLaneRoad". 41 | # https://wise.svlsimulator.com/maps/profile/a6e2d149-6a18-4b83-9029-4411d7b2e69a 42 | map_singlelaneroad = "a6e2d149-6a18-4b83-9029-4411d7b2e69a" 43 | 44 | # Map loaded in some of the examples. Changing the map may lead to unexpected behaviour in the example scripts. Default value is "Straight1LaneSame". 45 | # https://wise.svlsimulator.com/maps/profile/1e2287cf-c590-4804-bcb1-18b2fd3752d1 46 | map_straight1lanesame = "1e2287cf-c590-4804-bcb1-18b2fd3752d1" 47 | 48 | # Map loaded in some of the examples. Changing the map may lead to unexpected behaviour in the example scripts. Default value is "Straight2LaneSame". 49 | # https://wise.svlsimulator.com/maps/profile/b39d3ef9-21d7-409d-851b-4c90dad80a25 50 | map_straight2lanesame = "b39d3ef9-21d7-409d-851b-4c90dad80a25" 51 | 52 | # Map loaded in some of the examples. Changing the map may lead to unexpected behaviour in the example scripts. Default value is "Straight2LaneSameCurbRightIntersection". 53 | # https://wise.svlsimulator.com/maps/profile/378edc3f-8fce-4596-87dc-7d12fc2ad743 54 | map_straight2lanesamecurbrightintersection = "378edc3f-8fce-4596-87dc-7d12fc2ad743" 55 | 56 | # Map loaded in some of the examples. Changing the map may lead to unexpected behaviour in the example scripts. Default value is "Straight2LaneOpposing". 57 | # https://wise.svlsimulator.com/maps/profile/671868be-44f9-44a1-913c-cb0f29d12634 58 | map_straight2laneopposing = "671868be-44f9-44a1-913c-cb0f29d12634" 59 | 60 | # Map loaded in the quickstart scripts. Changing the map may lead to unexpected behaviour in the quickstart scripts. Default value is "LGSeocho". 61 | # https://wise.svlsimulator.com/maps/profile/26546191-86e8-4b53-9432-1cecbbd95c87 62 | map_lgseocho = "26546191-86e8-4b53-9432-1cecbbd95c87" 63 | 64 | # Ego vehicle that is loaded in most of the quickstart scripts. Default value is "Lincoln2017MKZ" using the "Apollo 5.0" sensor configuration. 65 | # This includes a bridge connection if needed and also bunch of sensors including LIDAR. 66 | # https://wise.svlsimulator.com/vehicles/profile/73805704-1e46-4eb6-b5f9-ec2244d5951e/edit/configuration/47b529db-0593-4908-b3e7-4b24a32a0f70 67 | ego_lincoln2017mkz_apollo5 = "47b529db-0593-4908-b3e7-4b24a32a0f70" 68 | 69 | # Ego vehicle that is loaded in quickstart/tests scripts. Default value is "Jaguar2015XE" using the "Apollo 5.0" sensor configuration. 70 | # https://wise.svlsimulator.com/vehicles/profile/3f4211dc-e5d7-42dc-94c5-c4832b1331bb/edit/configuration/c06d4932-5928-4730-8a91-ba64ac5f1813 71 | ego_jaguar2015xe_apollo5 = "c06d4932-5928-4730-8a91-ba64ac5f1813" 72 | 73 | # Ego vehicle that is loaded in quickstart/tests scripts. Default value is "Jaguar2015XE" using the "Autoware AI" sensor configuration. 74 | # https://wise.svlsimulator.com/vehicles/profile/3f4211dc-e5d7-42dc-94c5-c4832b1331bb/edit/configuration/05cbb194-d095-4a0e-ae66-ff56c331ca83 75 | ego_jaguae2015xe_autowareai = "05cbb194-d095-4a0e-ae66-ff56c331ca83" 76 | 77 | # Ego vehicle that is loaded in most of the NHTSA example scripts. Default value is "Lincoln2017MKZ" using the "Apollo 5.0 (Full Analysis)" sensor configuration. 78 | # This includes bunch of sensors that help analyze test results efficiently. 79 | # https://wise.svlsimulator.com/vehicles/profile/73805704-1e46-4eb6-b5f9-ec2244d5951e/edit/configuration/22656c7b-104b-4e6a-9c70-9955b6582220 80 | ego_lincoln2017mkz_apollo5_full_analysis = "22656c7b-104b-4e6a-9c70-9955b6582220" 81 | 82 | # Ego vehicle that is loaded in most of the NHTSA example scripts. Default value is "Lincoln2017MKZ" using the "Apollo 5.0 (modular testing)" sensor configuration. 83 | # This has sensors for modular testing. 84 | # https://wise.svlsimulator.com/vehicles/profile/73805704-1e46-4eb6-b5f9-ec2244d5951e/edit/configuration/5c7fb3b0-1fd4-4943-8347-f73a05749718 85 | ego_lincoln2017mkz_apollo5_modular = "5c7fb3b0-1fd4-4943-8347-f73a05749718" 86 | 87 | # Ego vehicle that is loaded in most of the Apollo 6.0 scripts. Default value is "Lincoln2017MKZ" using the "Apollo 6.0 (modular testing)" sensor configuration. 88 | # This has sensors for modular testing. 89 | # https://wise.svlsimulator.com/vehicles/profile/73805704-1e46-4eb6-b5f9-ec2244d5951e/edit/configuration/2e9095fa-c9b9-4f3f-8d7d-65fa2bb03921 90 | ego_lincoln2017mkz_apollo6_modular = "2e9095fa-c9b9-4f3f-8d7d-65fa2bb03921" 91 | 92 | # Cloi robot that is loaded in quickstart/tests scripts. Default value is "LGCloi" using the "Navigation2" sensor configuration. 93 | # https://wise.svlsimulator.com/vehicles/profile/20609b67-6dbd-40ad-9b46-e6bc455278ed/edit/configuration/c2207cd4-c8d0-4a12-b5b7-c79ab748becc 94 | ego_lgcloi_navigation2 = "c2207cd4-c8d0-4a12-b5b7-c79ab748becc" 95 | 96 | # Cloi robot that is loaded in quickstart/tests scripts. Default value is "LGCloi" using the "Nav2_Multi_Robot1" sensor configuration. 97 | # https://wise.svlsimulator.com/vehicles/profile/20609b67-6dbd-40ad-9b46-e6bc455278ed/edit/configuration/eee61d18-c6e3-4292-988d-445802aaee97 98 | ego_lgcloi_navigation2_multi_robot1 = "eee61d18-c6e3-4292-988d-445802aaee97" 99 | 100 | # Cloi robot that is loaded in quickstart/tests scripts. Default value is "LGCloi" using the "Nav2_Multi_Robot2" sensor configuration. 101 | # https://wise.svlsimulator.com/vehicles/profile/20609b67-6dbd-40ad-9b46-e6bc455278ed/edit/configuration/f9c5ace0-969a-4ade-8208-87d09d1a53f8 102 | ego_lgcloi_navigation2_multi_robot2 = "f9c5ace0-969a-4ade-8208-87d09d1a53f8" 103 | 104 | # Cloi robot that is loaded in quickstart/tests scripts. Default value is "LGCloi" using the "Nav2_Multi_Robot3" sensor configuration. 105 | # https://wise.svlsimulator.com/vehicles/profile/20609b67-6dbd-40ad-9b46-e6bc455278ed/edit/configuration/cfdb1484-91b7-4f27-b729-e313cc31ed8e 106 | ego_lgcloi_navigation2_multi_robot3 = "cfdb1484-91b7-4f27-b729-e313cc31ed8e" 107 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lgsvl 5 | 2021.1.0 6 | A Python API for SVL Simulator 7 | Hadi Tabatabaee 8 | Other 9 | 10 | python3-environs-pip 11 | python3-numpy 12 | python3-websocket 13 | python3-websockets 14 | 15 | 16 | ament_python 17 | 18 | 19 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | # get_version() in setup.py requires GitPython. 3 | requires = ["setuptools", "wheel", "GitPython==3.1.14"] 4 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/requirements.txt: -------------------------------------------------------------------------------- 1 | 2 | # - PACKAGE >= PKG_VERSION ; python_version >= PYTHON_VERSION 3 | # PKG_VERSION is the first version of PACKAGE that supports PYTHON_VERSION 4 | # - PACKAGE < PKG_VERSION ; python_version < PYTHON_VERSION 5 | # PKG_VERSION is the first version of PACKAGE that no longer supports 6 | # PYTHON_VERSION; add entries for PYTHON_VERSION > "python_requires" of 7 | # lgsvl in setup.py 8 | # - Keep in alphabetical order. 9 | environs >= 8.1.0 ; python_version >= '3.9' 10 | numpy < 1.20 ; python_version < '3.7' 11 | numpy >= 1.20 ; python_version >= '3.7' 12 | websocket-client >= 0.58.0 ; python_version >= '3.8' 13 | websockets < 8.1 ; python_version < '3.6.1' 14 | websockets >= 8.1 ; python_version >= '3.8' 15 | websockets >= 9.0.1 ; python_version >= '3.9' 16 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/resource/lgsvl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingfeiCheng/BehAVExplor/4124436fe38643196fea675acfcd163d3b369b75/PythonAPI-Apollo-async/resource/lgsvl -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/scripts/run_and_create_coverage_report.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | set -eu 4 | 5 | ~/.local/bin/coverage run -m unittest discover -v -c 6 | ~/.local/bin/coverage html --omit "~/.local/*","tests/*" 7 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/lgsvl 3 | [install] 4 | install_scripts=$base/lib/lgsvl 5 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from setuptools import setup 4 | 5 | # Without enabling the user site, "python3 -m pip install --user -e ." fails 6 | # with: 7 | # 8 | # WARNING: The user site-packages directory is disabled. 9 | # error: can't create or remove files in install directory 10 | # ... 11 | # [Errno 13] Permission denied: '/usr/local/lib/python3.6/dist-packages/' 12 | # 13 | # Taken from https://github.com/pypa/pip/issues/7953#issuecomment-645133255. 14 | import site 15 | import sys 16 | site.ENABLE_USER_SITE = "--user" in sys.argv[1:] 17 | 18 | 19 | def get_version(git_tag): 20 | """ 21 | Returns a PEP 440 version identifier 22 | 23 | [+] 24 | 25 | derived from its parameter and the output from: 26 | 27 | git.sh describe --match --tags 28 | 29 | which has the format: 30 | 31 | [--g] 32 | 33 | The public version identifier is formed from . PEP 440 versioning 34 | allows only digits without leading zeros in the release segments of public 35 | version identifiers, so they are dropped along with any "P"-s found in 36 | . And PEP 440 does not allow hyphens in pre-release segments, so 37 | any "-rc"-s are replaced with "rc". Fortunately, this mapping doesn't 38 | introduce any ambiguities when used for the tags that currently exist. 39 | 40 | The optional local version label is formed from the portion of the 41 | "git.sh describe" output which follows with all hyphens converted to 42 | periods (except the first, which is dropped). This segment will be empty if: 43 | - Git is not available, or 44 | - the working tree is not a Git repository, or 45 | - the tag is not present or not on the currrent branch, or 46 | - the HEAD of the current branch is coincident with the tag . 47 | 48 | NB. - Not using https://pypi.org/project/setuptools-git-version/ because it 49 | passes "--long" to "git.sh describe". 50 | - Not using https://pypi.org/project/setuptools-scm/ because it's way 51 | more complicated to configure it to do what is wanted. 52 | """ 53 | 54 | # ASSERT( has no more than one leading "0", no more than one "-rc", 55 | # and no more than one "P", which is expected to be at the end -- but 56 | # this is not checked) 57 | public_version_identifier = git_tag.replace(".0", ".", 1) \ 58 | .replace("P", "", 1) \ 59 | .replace("-rc", "rc", 1) 60 | from os import getenv 61 | try: 62 | from git import Repo 63 | # PWD is the directory from where we invoked "pip install", which is 64 | # the root of the Git repository; when setup() is 65 | # called, pip has changed the current directory to be under /tmp. 66 | repo = Repo(path=getenv('PWD')) 67 | val = repo.git.describe('--match', git_tag, '--tags') 68 | except Exception: 69 | val = "" 70 | 71 | # Remove and convert the first hyphen to a "+": 72 | local_version_label_with_plus = val.replace(git_tag, '', 1) \ 73 | .replace('-', '+', 1) \ 74 | .replace('-', '.') 75 | 76 | return public_version_identifier + local_version_label_with_plus 77 | 78 | 79 | package_name = 'lgsvl' 80 | 81 | setup( 82 | name=package_name, 83 | version=get_version('2021.1'), 84 | data_files=[ 85 | ('share/ament_index/resource_index/packages', 86 | ['resource/' + package_name]), 87 | ('share/' + package_name, ['package.xml']), 88 | ], 89 | description="Python API for SVL Simulator", 90 | author="LGSVL", 91 | author_email="contact@svlsimulator.com", 92 | python_requires=">=3.6.0", 93 | url="https://github.com/lgsvl/PythonAPI", 94 | packages=["lgsvl", "lgsvl.dreamview", "lgsvl.evaluator", "lgsvl.wise"], 95 | install_requires=[ 96 | "environs", 97 | "numpy", 98 | "setuptools", 99 | "websocket-client", 100 | "websockets" 101 | ], 102 | setup_requires=[ 103 | "GitPython==3.1.14" 104 | ], 105 | extras_require={ 106 | "ci": [ 107 | "flake8>=3.7.0" 108 | ], 109 | }, 110 | zip_safe=True, 111 | maintainer='Hadi Tabatabaee', 112 | maintainer_email='hadi.tabatabaee@lge.com', 113 | license='Other', 114 | classifiers=[ 115 | "License :: Other/Proprietary License", 116 | "Programming Language :: Python :: 3", 117 | "Programming Language :: Python :: 3.6", 118 | ], 119 | ) 120 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/tests/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | import unittest 8 | from .test_collisions import TestCollisions 9 | from .test_simulator import TestSimulator 10 | from .test_EGO import TestEGO 11 | from .test_NPC import TestNPC 12 | from .test_sensors import TestSensors 13 | from .test_peds import TestPeds 14 | from .test_utils import TestUtils 15 | 16 | 17 | def load_tests(loader, standard_tests, pattern): 18 | suite = unittest.TestSuite() 19 | suite.addTests(loader.loadTestsFromTestCase(TestNPC)) # must be first 20 | suite.addTests(loader.loadTestsFromTestCase(TestCollisions)) 21 | suite.addTests(loader.loadTestsFromTestCase(TestEGO)) 22 | suite.addTests(loader.loadTestsFromTestCase(TestSensors)) 23 | suite.addTests(loader.loadTestsFromTestCase(TestPeds)) 24 | suite.addTests(loader.loadTestsFromTestCase(TestUtils)) 25 | suite.addTests(loader.loadTestsFromTestCase(TestSimulator)) #must be last 26 | return suite 27 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/tests/common.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2021 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | import signal 8 | import lgsvl 9 | import os 10 | 11 | class TestTimeout(Exception): 12 | pass 13 | 14 | 15 | class TestException(Exception): 16 | pass 17 | 18 | 19 | class SimConnection: 20 | def __init__(self, seconds=30, scene=lgsvl.wise.DefaultAssets.map_borregasave, error_message=None, load_scene=True): 21 | if error_message is None: 22 | error_message = 'test timed out after {}s.'.format(seconds) 23 | self.seconds = seconds 24 | self.error_message = error_message 25 | self.scene = scene 26 | self.load_scene = load_scene 27 | 28 | def handle_timeout(self, signum, frame): 29 | raise TestTimeout(self.error_message) 30 | 31 | def __enter__(self): 32 | signal.signal(signal.SIGALRM, self.handle_timeout) 33 | signal.alarm(self.seconds) 34 | 35 | self.sim = lgsvl.Simulator(os.environ.get("LGSVL__SIMULATOR_HOST", "127.0.0.1"), 8181) 36 | if self.load_scene: 37 | if self.sim.current_scene == self.scene: 38 | self.sim.reset() 39 | signal.alarm(self.seconds - 20) 40 | else: 41 | self.sim.load(self.scene) 42 | return self.sim 43 | 44 | def __exit__(self, exc_type, exc_val, exc_tb): 45 | signal.alarm(0) 46 | # agents = self.sim.get_agents() 47 | # for a in agents: 48 | # self.sim.remove_agent(a) 49 | self.sim.close() 50 | 51 | 52 | def spawnState(sim, index=0): 53 | state = lgsvl.AgentState() 54 | state.transform = sim.get_spawn()[index] 55 | return state 56 | 57 | 58 | def cmEqual(self, a, b, msg): # Test vectors within 1cm 59 | self.assertAlmostEqual(a.x, b.x, 2, msg) 60 | self.assertAlmostEqual(a.y, b.y, 2, msg) 61 | self.assertAlmostEqual(a.z, b.z, 2, msg) 62 | 63 | 64 | def mEqual(self, a, b, msg): # Test vectors within 1cm 65 | self.assertAlmostEqual(a.x, b.x, delta=1.5, msg=msg) 66 | self.assertAlmostEqual(a.y, b.y, delta=1.5, msg=msg) 67 | self.assertAlmostEqual(a.z, b.z, delta=1.5, msg=msg) 68 | 69 | 70 | def notAlmostEqual(a,b): 71 | return round(a-b,7) != 0 72 | 73 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/tests/test_EGO.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2021 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | import unittest 8 | import math 9 | 10 | import lgsvl 11 | from .common import SimConnection, spawnState, cmEqual 12 | 13 | 14 | # TODO add tests for bridge connection 15 | 16 | 17 | class TestEGO(unittest.TestCase): 18 | def test_agent_name(self): # Check if EGO Apollo is created 19 | with SimConnection() as sim: 20 | agent = self.create_EGO(sim) 21 | 22 | self.assertEqual(agent.name, lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5) 23 | 24 | def test_different_spawns(self): # Check if EGO is spawned in the spawn positions 25 | with SimConnection() as sim: 26 | spawns = sim.get_spawn() 27 | agent = self.create_EGO(sim) 28 | cmEqual(self, agent.state.position, spawns[0].position, "Spawn Position 0") 29 | cmEqual(self, agent.state.rotation, spawns[0].rotation, "Spawn Rotation 0") 30 | 31 | state = spawnState(sim) 32 | state.transform = spawns[1] 33 | agent2 = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 34 | 35 | cmEqual(self, agent2.state.position, spawns[1].position, "Spawn Position 1") 36 | cmEqual(self, agent2.state.rotation, spawns[1].rotation, "Spawn Rotation 1") 37 | 38 | def test_agent_velocity(self): # Check EGO velocity 39 | with SimConnection(60) as sim: 40 | state = spawnState(sim) 41 | agent = self.create_EGO(sim) 42 | cmEqual(self, agent.state.velocity, state.velocity, "0 Velocity") 43 | 44 | sim.reset() 45 | right = lgsvl.utils.transform_to_right(state.transform) 46 | state.velocity = -50 * right 47 | agent = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 48 | 49 | cmEqual(self, agent.state.velocity, state.velocity, "50 Velocity") 50 | 51 | def test_ego_different_directions(self): # Check that the xyz velocities equate to xyz changes in position 52 | with SimConnection(60) as sim: 53 | state = spawnState(sim) 54 | forward = lgsvl.utils.transform_to_forward(state.transform) 55 | up = lgsvl.utils.transform_to_up(state.transform) 56 | right = lgsvl.utils.transform_to_right(state.transform) 57 | 58 | state.velocity = -10 * right 59 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 60 | sim.run(.5) 61 | target = state.position - 3 * right 62 | self.assertLess((ego.state.position - target).magnitude(), 1) 63 | sim.remove_agent(ego) 64 | 65 | state.velocity = 10 * up 66 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 67 | sim.run(.5) 68 | target = state.position + 3 * up 69 | self.assertLess((ego.state.position - target).magnitude(), 1) 70 | sim.remove_agent(ego) 71 | 72 | state.velocity = 10 * forward 73 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 74 | sim.run(.5) 75 | target = state.position + 4 * forward 76 | self.assertLess((ego.state.position - target).magnitude(), 1) 77 | 78 | def test_speed(self): # check that speed returns a reasonable number 79 | with SimConnection() as sim: 80 | state = spawnState(sim) 81 | forward = lgsvl.utils.transform_to_forward(state.transform) 82 | up = lgsvl.utils.transform_to_up(state.transform) 83 | right = lgsvl.utils.transform_to_right(state.transform) 84 | state.velocity = -10 * right + 10 * up + 10 * forward 85 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 86 | self.assertAlmostEqual(ego.state.speed, math.sqrt(300), places=3) 87 | 88 | @unittest.skip("No highway in currents maps") 89 | def test_rotation_on_highway_ramp(self): # Check that vehicle is rotated when spawned on the highway ramp 90 | with SimConnection() as sim: 91 | state = spawnState(sim) 92 | state.transform.position = lgsvl.Vector(469.6401, 15.67488, 100.4229) 93 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 94 | self.assertAlmostEqual(ego.state.rotation.z, state.rotation.z) 95 | sim.run(0.5) 96 | self.assertAlmostEqual(ego.state.rotation.z, 356, delta=0.5) 97 | 98 | def test_ego_steering(self): # Check that a steering command can be given to an EGO vehicle, and the car turns 99 | with SimConnection() as sim: 100 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 101 | control = lgsvl.VehicleControl() 102 | control.throttle = 0.3 103 | control.steering = -1.0 104 | ego.apply_control(control, True) 105 | initialRotation = ego.state.rotation 106 | sim.run(1) 107 | finalRotation = ego.state.rotation 108 | self.assertNotAlmostEqual(initialRotation.y, finalRotation.y) 109 | 110 | def test_ego_throttle(self): # Check that a throttle command can be given to an EGO vehicle, and the car accelerates 111 | with SimConnection() as sim: 112 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 113 | control = lgsvl.VehicleControl() 114 | control.throttle = 0.5 115 | ego.apply_control(control, True) 116 | initialSpeed = ego.state.speed 117 | sim.run(2) 118 | finalSpeed = ego.state.speed 119 | self.assertGreater(finalSpeed, initialSpeed) 120 | 121 | def test_ego_braking(self): # Check that a brake command can be given to an EGO vehicle, and the car stops sooner than without brakes 122 | with SimConnection(60) as sim: 123 | state = spawnState(sim) 124 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 125 | control = lgsvl.VehicleControl() 126 | control.throttle = 1 127 | ego.apply_control(control, True) 128 | sim.run(1) 129 | control = lgsvl.VehicleControl() 130 | ego.apply_control(control,True) 131 | sim.run(3) 132 | noBrakeDistance = (ego.state.position - state.position).magnitude() 133 | 134 | sim.reset() 135 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 136 | control = lgsvl.VehicleControl() 137 | control.throttle = 1 138 | ego.apply_control(control, True) 139 | sim.run(1) 140 | control = lgsvl.VehicleControl() 141 | control.braking = 1 142 | ego.apply_control(control, True) 143 | sim.run(3) 144 | brakeDistance = (ego.state.position - state.position).magnitude() 145 | self.assertLess(brakeDistance, noBrakeDistance) 146 | 147 | def test_ego_handbrake(self): # Check that the handbrake can be enable on an EGO vehicle, and the car stops sooner than without brakes 148 | with SimConnection(60) as sim: 149 | state = spawnState(sim) 150 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 151 | control = lgsvl.VehicleControl() 152 | control.throttle = 1 153 | ego.apply_control(control, True) 154 | sim.run(1) 155 | control = lgsvl.VehicleControl() 156 | ego.apply_control(control,True) 157 | sim.run(3) 158 | noBrakeDistance = (ego.state.position - state.position).magnitude() 159 | 160 | sim.reset() 161 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 162 | control = lgsvl.VehicleControl() 163 | control.throttle = 1 164 | ego.apply_control(control, True) 165 | sim.run(1) 166 | control = lgsvl.VehicleControl() 167 | control.handbrake = True 168 | ego.apply_control(control, True) 169 | sim.run(3) 170 | brakeDistance = (ego.state.position - state.position).magnitude() 171 | self.assertLess(brakeDistance, noBrakeDistance) 172 | 173 | def test_ego_reverse(self): # Check that the gear can be changed in an EGO vehicle, and the car moves in reverse 174 | with SimConnection() as sim: 175 | state = spawnState(sim) 176 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 177 | control = lgsvl.VehicleControl() 178 | control.throttle = 0.5 179 | control.reverse = True 180 | ego.apply_control(control, True) 181 | sim.run(2) 182 | forward = lgsvl.utils.transform_to_forward(state.transform) 183 | target = state.position - 3.5 * forward 184 | diff = ego.state.position - target 185 | self.assertLess((diff).magnitude(), 1) 186 | 187 | def test_not_sticky_control(self): # Check that the a non sticky control is removed 188 | with SimConnection(60) as sim: 189 | state = spawnState(sim) 190 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 191 | control = lgsvl.VehicleControl() 192 | control.throttle = 1 193 | ego.apply_control(control, True) 194 | sim.run(3) 195 | stickySpeed = ego.state.speed 196 | 197 | sim.reset() 198 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 199 | control = lgsvl.VehicleControl() 200 | control.throttle = 1 201 | ego.apply_control(control, True) 202 | sim.run(1) 203 | ego.apply_control(control,False) 204 | sim.run(2) 205 | finalSpeed = ego.state.speed 206 | self.assertGreater(stickySpeed, finalSpeed) 207 | 208 | def test_vary_throttle(self): # Check that different throttle values accelerate differently 209 | with SimConnection(40) as sim: 210 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 211 | control = lgsvl.VehicleControl() 212 | control.throttle = 0.5 213 | ego.apply_control(control, True) 214 | sim.run(1) 215 | initialSpeed = ego.state.speed 216 | 217 | sim.reset() 218 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 219 | control = lgsvl.VehicleControl() 220 | control.throttle = 0.1 221 | ego.apply_control(control, True) 222 | sim.run(1) 223 | self.assertLess(ego.state.speed, initialSpeed) 224 | 225 | def test_vary_steering(self): # Check that different steering values turn the car differently 226 | with SimConnection(40) as sim: 227 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 228 | control = lgsvl.VehicleControl() 229 | control.throttle = 0.5 230 | control.steering = -0.8 231 | ego.apply_control(control, True) 232 | sim.run(1) 233 | initialAngle = ego.state.rotation.y 234 | 235 | sim.reset() 236 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 237 | control = lgsvl.VehicleControl() 238 | control.throttle = 0.5 239 | control.steering = -0.3 240 | ego.apply_control(control, True) 241 | sim.run(1) 242 | self.assertGreater(ego.state.rotation.y, initialAngle) 243 | 244 | def test_bounding_box_size(self): # Check that the bounding box is calculated properly and is reasonable 245 | with SimConnection() as sim: 246 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 247 | bBox = ego.bounding_box 248 | self.assertAlmostEqual(bBox.size.x, abs(bBox.max.x-bBox.min.x)) 249 | self.assertAlmostEqual(bBox.size.y, abs(bBox.max.y-bBox.min.y)) 250 | self.assertAlmostEqual(bBox.size.z, abs(bBox.max.z-bBox.min.z)) 251 | self.assertLess(bBox.size.x, 10) 252 | self.assertLess(bBox.size.y, 10) 253 | self.assertLess(bBox.size.z, 10) 254 | 255 | def test_bounding_box_center(self): # Check that the bounding box center is calcualted properly 256 | with SimConnection() as sim: 257 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 258 | bBox = ego.bounding_box 259 | self.assertAlmostEqual(bBox.center.x, (bBox.max.x+bBox.min.x)/2) 260 | self.assertAlmostEqual(bBox.center.y, (bBox.max.y+bBox.min.y)/2) 261 | self.assertAlmostEqual(bBox.center.z, (bBox.max.z+bBox.min.z)/2) 262 | 263 | def test_equality(self): # Check that agent == operation works 264 | with SimConnection() as sim: 265 | ego1 = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 266 | ego2 = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguae2015xe_autowareai, lgsvl.AgentType.EGO, spawnState(sim)) 267 | self.assertTrue(ego1 == ego1) 268 | self.assertFalse(ego1 == ego2) 269 | 270 | @unittest.skip("Cruise Control not implemented yet") 271 | def test_set_fixed_speed(self): 272 | with SimConnection(60) as sim: 273 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 274 | ego.set_fixed_speed(True, 15.0) 275 | self.assertAlmostEqual(ego.state.speed, 0, delta=0.001) 276 | sim.run(5) 277 | self.assertAlmostEqual(ego.state.speed, 15, delta=1) 278 | 279 | def create_EGO(self, sim): # Only create an EGO is none are already spawned 280 | return sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 281 | 282 | # def test_large_throttle(self): 283 | # with SimConnection(60) as sim: 284 | # ego = sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, spawnState(sim)) 285 | # control = lgsvl.VehicleControl() 286 | # control.throttle = 1.0 287 | # ego.apply_control(control, True) 288 | # sim.run(3) 289 | # full_speed = ego.state.speed 290 | 291 | # sim.reset() 292 | # ego = sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, spawnState(sim)) 293 | # control = lgsvl.VehicleControl() 294 | # control.throttle = 0.574 295 | # ego.apply_control(control, True) 296 | # sim.run(3) 297 | # self.assertAlmostEqual(full_speed, ego.state.speed) 298 | 299 | # def test_large_steering(self): 300 | # with SimConnection() as sim: 301 | # ego = sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, spawnState(sim)) 302 | # control = lgsvl.VehicleControl() 303 | # control.throttle = 0.3 304 | # control.steering = -10 305 | # ego.apply_control(control, True) 306 | # sim.run(1) 307 | # self.assertAlmostEqual(ego.state.rotation.y, spawnState(sim).rotation.y, places=4) 308 | 309 | # def test_negative_braking(self): 310 | # with SimConnection() as sim: 311 | # ego = sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, spawnState(sim)) 312 | # control = lgsvl.VehicleControl() 313 | # control.braking = -1 314 | # ego.apply_control(control, True) 315 | # sim.run(1) 316 | # self.assertAlmostEqual(ego.state.speed, 0) 317 | 318 | # def test_negative_throttle(self): 319 | # with SimConnection() as sim: 320 | # ego = sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, spawnState(sim)) 321 | # control = lgsvl.VehicleControl() 322 | # control.throttle = -1 323 | # ego.apply_control(control, True) 324 | # sim.run(1) 325 | # self.assertAlmostEqual(ego.state.speed, 0.00000, places=3) 326 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/tests/test_collisions.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2021 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | import unittest 8 | 9 | import lgsvl 10 | from .common import SimConnection, spawnState 11 | 12 | # TODO add tests for collisions between NPCs, EGO & obstacles 13 | 14 | 15 | class TestCollisions(unittest.TestCase): 16 | def test_ego_collision(self): # Check that a collision between Ego and NPC is reported 17 | with SimConnection() as sim: 18 | mover, bus = self.setup_collision(sim, lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, "SchoolBus", lgsvl.AgentType.NPC) 19 | collisions = [] 20 | 21 | def on_collision(agent1, agent2, contact): 22 | collisions.append([agent1, agent2, contact]) 23 | sim.stop() 24 | 25 | mover.on_collision(on_collision) 26 | bus.on_collision(on_collision) 27 | 28 | sim.run(15.0) 29 | 30 | self.assertGreater(len(collisions), 0) 31 | self.assertInBetween(collisions[0][2], collisions[0][0].state.position, collisions[0][1].state.position, "Ego Collision") 32 | 33 | self.assertTrue(collisions[0][0].name == lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5 or collisions[0][1].name == lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5) 34 | self.assertTrue(True) 35 | 36 | def test_sim_stop(self): # Check that sim.stop works properly 37 | with SimConnection() as sim: 38 | mover, bus = self.setup_collision(sim, lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, "SchoolBus", lgsvl.AgentType.NPC) 39 | collisions = [] 40 | 41 | def on_collision(agent1, agent2, contact): 42 | collisions.append([agent1, agent2, contact]) 43 | sim.stop() 44 | 45 | mover.on_collision(on_collision) 46 | bus.on_collision(on_collision) 47 | 48 | sim.run(15.0) 49 | 50 | self.assertLess(sim.current_time, 15.0) 51 | 52 | def test_ped_collision(self): # Check if a collision between EGO and pedestrian is reported 53 | with SimConnection() as sim: 54 | ego, ped = self.setup_collision(sim, lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, "Howard", lgsvl.AgentType.PEDESTRIAN) 55 | self.assertTrue(isinstance(ego, lgsvl.EgoVehicle)) 56 | self.assertTrue(isinstance(ped, lgsvl.Pedestrian)) 57 | collisions = [] 58 | 59 | def on_collision(agent1, agent2, contact): 60 | collisions.append([agent1, agent2, contact]) 61 | sim.stop() 62 | ped.on_collision(on_collision) 63 | ego.on_collision(on_collision) 64 | 65 | sim.run(15) 66 | self.assertGreater(len(collisions), 0) 67 | self.assertInBetween(collisions[0][2], collisions[0][0].state.position, collisions[0][1].state.position, "Ped Collision") 68 | self.assertTrue(collisions[0][0].name == lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5 or collisions[0][1].name == lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5) 69 | 70 | @unittest.skip("NPCs ignore collisions with Pedestrians, activate this when NPCs use real physics") 71 | def test_ped_npc_collisions(self): # Check that collision between NPC and Pedestrian is reported 72 | with SimConnection() as sim: 73 | state = spawnState(sim) 74 | state.position.y += 10 75 | bus = sim.add_agent("SchoolBus", lgsvl.AgentType.NPC, state) 76 | 77 | state = spawnState(sim) 78 | ped = sim.add_agent("Bob", lgsvl.AgentType.PEDESTRIAN, state) 79 | collisions = [] 80 | def on_collision(agent1, agent2, contact): 81 | collisions.append([agent1, agent2, contact]) 82 | sim.stop() 83 | ped.on_collision(on_collision) 84 | bus.on_collision(on_collision) 85 | sim.run(15) 86 | 87 | self.assertGreater(len(collisions), 0) 88 | self.assertInBetween(collisions[0][2], collisions[0][0].state.position, collisions[0][1].state.position, "Ped/NPC Collision") 89 | self.assertTrue(collisions[0][0].name == "Bob" or collisions[0][1].name == "Bob") 90 | 91 | @unittest.skip("NPCs ignore collisions with other NPCs, activate this when NPCs use real physics") 92 | def test_npc_collision(self): # Check that collision between NPC and NPC is reported 93 | with SimConnection() as sim: 94 | state = spawnState(sim) 95 | state.position.x += 10 96 | sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 97 | jeep, bus = self.setup_collision(sim, "Jeep", lgsvl.AgentType.NPC, "SchoolBus", lgsvl.AgentType.NPC) 98 | collisions = [] 99 | 100 | def on_collision(agent1, agent2, contact): 101 | collisions.append([agent1, agent2, contact]) 102 | sim.stop() 103 | 104 | jeep.on_collision(on_collision) 105 | bus.on_collision(on_collision) 106 | 107 | sim.run(15.0) 108 | 109 | self.assertGreater(len(collisions), 0) 110 | self.assertInBetween(collisions[0][2], collisions[0][0].state.position, collisions[0][1].state.position, "NPC Collision") 111 | self.assertTrue(collisions[0][0].name == "Jeep" or collisions[0][1].name == "Jeep") 112 | self.assertTrue(collisions[0][0].name == "SchoolBus" or collisions[0][1].name == "SchoolBus") 113 | 114 | def test_wall_collision(self): # Check that an EGO collision with a wall is reported properly 115 | with SimConnection() as sim: 116 | state = spawnState(sim) 117 | forward = lgsvl.utils.transform_to_forward(state.transform) 118 | up = lgsvl.utils.transform_to_up(state.transform) 119 | right = lgsvl.utils.transform_to_right(state.transform) 120 | state.transform.position = state.position + 30 * right - 1 * up + 140 * forward 121 | state.velocity = 50 * forward 122 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 123 | collisions = [] 124 | 125 | def on_collision(agent1, agent2, contact): 126 | collisions.append([agent1, agent2, contact]) 127 | sim.stop() 128 | 129 | ego.on_collision(on_collision) 130 | 131 | sim.run(15) 132 | 133 | self.assertGreater(len(collisions), 0) 134 | if collisions[0][0] is None: 135 | self.assertTrue(collisions[0][1].name == lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5) 136 | elif collisions[0][1] is None: 137 | self.assertTrue(collisions[0][0].name == lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5) 138 | else: 139 | self.fail("Collision not with object") 140 | 141 | def setup_collision(self, sim, mover_name, agent_type, still_name, still_type): 142 | # Creates 2 agents, the mover is created with a forward velocity 143 | # still is rotated 90 degree in and in front of the mover 144 | state = spawnState(sim) 145 | forward = lgsvl.utils.transform_to_forward(state.transform) 146 | state.velocity = 50 * forward 147 | mover = sim.add_agent(mover_name, agent_type, state) 148 | 149 | # school bus, 20m ahead, perpendicular to road, stopped 150 | 151 | state = spawnState(sim) 152 | state.transform.position = state.transform.position + 20 * forward 153 | state.transform.rotation.y += 90.0 154 | still = sim.add_agent(still_name, still_type, state) 155 | 156 | return mover, still 157 | 158 | def assertInBetween(self, position, a, b, msg): # Tests that at least one component of the input position vector is between the a and b vectors 159 | xmid = (a.x+b.x)/2 160 | xdiff = abs(a.x-xmid) 161 | xmin = xmid-xdiff 162 | xmax = xmid+xdiff 163 | 164 | ymid = (a.y+b.y)/2 165 | ydiff = abs(a.y-ymid) 166 | ymin = ymid-ydiff 167 | ymax = ymid+ydiff 168 | 169 | zmid = (a.z+b.z)/2 170 | zdiff = abs(a.z-zmid) 171 | zmin = zmid-zdiff 172 | zmax = zmid+zdiff 173 | 174 | validCollision = False 175 | validCollision |= (position.x <= xmax and position.x >= xmin) 176 | validCollision |= (position.y <= ymax and position.y >= ymin) 177 | validCollision |= (position.z <= zmax and position.z >= zmin) 178 | self.assertTrue(validCollision, msg) 179 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/tests/test_manual_features.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2021 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | # DO NOT INCLUDE IN __init__.py 8 | 9 | import unittest 10 | 11 | import lgsvl 12 | from .common import SimConnection, spawnState, TestTimeout 13 | 14 | class TestManual(unittest.TestCase): 15 | @unittest.skip("Windshield wipers no longer supported") 16 | def test_wipers(self): 17 | try: 18 | with SimConnection() as sim: 19 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 20 | control = lgsvl.VehicleControl() 21 | control.windshield_wipers = 1 22 | ego.apply_control(control, True) 23 | sim.run(1) 24 | input("Press Enter if wipers are on low") 25 | 26 | control = lgsvl.VehicleControl() 27 | control.windshield_wipers = 2 28 | ego.apply_control(control, True) 29 | sim.run(1) 30 | input("Press Enter if wipers are on medium") 31 | 32 | control = lgsvl.VehicleControl() 33 | control.windshield_wipers = 3 34 | ego.apply_control(control, True) 35 | sim.run(1) 36 | input("Press Enter if wipers are on high") 37 | except TestTimeout: 38 | self.fail("Wipers were not on") 39 | 40 | def test_headlights(self): 41 | try: 42 | with SimConnection() as sim: 43 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 44 | control = lgsvl.VehicleControl() 45 | control.headlights = 1 46 | ego.apply_control(control, True) 47 | sim.run(1) 48 | input("Press Enter if headlights are on") 49 | 50 | control = lgsvl.VehicleControl() 51 | control.headlights = 2 52 | ego.apply_control(control, True) 53 | sim.run(1) 54 | input("Press Enter if high beams are on") 55 | except TestTimeout: 56 | self.fail("Headlights were not on") 57 | 58 | def test_blinkers(self): 59 | try: 60 | with SimConnection() as sim: 61 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 62 | control = lgsvl.VehicleControl() 63 | control.turn_signal_left = True 64 | ego.apply_control(control, True) 65 | sim.run(3) 66 | input("Press Enter if left turn signal is on") 67 | 68 | control = lgsvl.VehicleControl() 69 | control.turn_signal_right = True 70 | ego.apply_control(control, True) 71 | sim.run(3) 72 | input("Press Enter if right turn signal is on") 73 | except TestTimeout: 74 | self.fail("Turn signals were not on") 75 | 76 | @unittest.skip("Windshield wipers no longer supported") 77 | def test_wiper_large_value(self): 78 | try: 79 | with SimConnection() as sim: 80 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 81 | control = lgsvl.VehicleControl() 82 | control.windshield_wipers = 4 83 | ego.apply_control(control, True) 84 | sim.run(1) 85 | input("Press Enter if wipers are off") 86 | except TestTimeout: 87 | self.fail("Wipers were on") 88 | 89 | @unittest.skip("Windshield wipers no longer supported") 90 | def test_wiper_str(self): 91 | try: 92 | with SimConnection() as sim: 93 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 94 | control = lgsvl.VehicleControl() 95 | control.windshield_wipers = "on" 96 | ego.apply_control(control, True) 97 | sim.run(1) 98 | input("Press Enter if wipers are off") 99 | except TestTimeout: 100 | self.fail("Wipers were on") 101 | 102 | def test_headlights_large_value(self): 103 | try: 104 | with SimConnection() as sim: 105 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 106 | control = lgsvl.VehicleControl() 107 | control.headlights = 123 108 | ego.apply_control(control, True) 109 | sim.run(1) 110 | input("Press Enter if headlights are off") 111 | except TestTimeout: 112 | self.fail("Headlights were on") 113 | 114 | def test_headlights_str(self): 115 | try: 116 | with SimConnection() as sim: 117 | ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 118 | control = lgsvl.VehicleControl() 119 | control.headlights = "123" 120 | ego.apply_control(control, True) 121 | sim.run(1) 122 | input("Press Enter if headlights are off") 123 | except TestTimeout: 124 | self.fail("Headlights were on") 125 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/tests/test_peds.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2021 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | import unittest 8 | import math 9 | 10 | import lgsvl 11 | from .common import SimConnection, cmEqual, mEqual, spawnState 12 | 13 | 14 | class TestPeds(unittest.TestCase): 15 | def test_ped_creation(self): # Check if the different types of Peds can be created 16 | with SimConnection() as sim: 17 | state = spawnState(sim) 18 | forward = lgsvl.utils.transform_to_forward(state.transform) 19 | state.transform.position = state.position - 4 * forward 20 | sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 21 | for name in ["Bob", "EntrepreneurFemale", "Howard", "Johny", \ 22 | "Pamela", "Presley", "Robin", "Stephen", "Zoe"]: 23 | agent = self.create_ped(sim, name, spawnState(sim)) 24 | cmEqual(self, agent.state.position, sim.get_spawn()[0].position, name) 25 | self.assertEqual(agent.name, name) 26 | 27 | def test_ped_random_walk(self): # Check if pedestrians can walk randomly 28 | with SimConnection(40) as sim: 29 | state = spawnState(sim) 30 | forward = lgsvl.utils.transform_to_forward(state.transform) 31 | state.transform.position = state.position - 4 * forward 32 | sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 33 | state = spawnState(sim) 34 | spawnPoint = state.transform.position 35 | 36 | bob = self.create_ped(sim, "Bob", state) 37 | bob.walk_randomly(True) 38 | sim.run(2) 39 | 40 | randPoint = bob.transform.position 41 | self.assertNotAlmostEqual(spawnPoint.x, randPoint.x) 42 | self.assertNotAlmostEqual(spawnPoint.y, randPoint.y) 43 | self.assertNotAlmostEqual(spawnPoint.z, randPoint.z) 44 | 45 | bob.walk_randomly(False) 46 | sim.run(2) 47 | 48 | cmEqual(self, randPoint, bob.state.transform.position, "Ped random walk") 49 | 50 | def test_ped_circle_waypoints(self): # Check if pedestrians can follow waypoints 51 | with SimConnection(60) as sim: 52 | state = spawnState(sim) 53 | forward = lgsvl.utils.transform_to_forward(state.transform) 54 | right = lgsvl.utils.transform_to_right(state.transform) 55 | state.transform.position = state.position - 4 * forward 56 | sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 57 | state = spawnState(sim) 58 | state.transform.position = state.position + 10 * forward 59 | radius = 5 60 | count = 3 61 | waypointCommands = [] 62 | waypoints = [] 63 | layer_mask = 0 | (1 << 0) 64 | for i in range(count): 65 | x = radius * math.cos(i * 2 * math.pi / count) 66 | z = radius * math.sin(i * 2 * math.pi / count) 67 | idle = 1 if i < count//2 else 0 68 | hit = sim.raycast(state.position + z * forward + x * right, lgsvl.Vector(0, -1, 0), layer_mask) 69 | waypointCommands.append(lgsvl.WalkWaypoint(hit.point, idle)) 70 | waypoints.append(hit.point) 71 | 72 | state.transform.position = waypoints[0] 73 | zoe = self.create_ped(sim, "Zoe", state) 74 | 75 | def on_waypoint(agent,index): 76 | msg = "Waypoint " + str(index) 77 | mEqual(self, zoe.state.position, waypoints[index], msg) 78 | if index == len(waypoints)-1: 79 | sim.stop() 80 | 81 | zoe.on_waypoint_reached(on_waypoint) 82 | zoe.follow(waypointCommands, True) 83 | sim.run() 84 | 85 | def test_waypoint_idle_time(self): 86 | with SimConnection(60) as sim: 87 | sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 88 | state = spawnState(sim) 89 | forward = lgsvl.utils.transform_to_forward(state.transform) 90 | right = lgsvl.utils.transform_to_right(state.transform) 91 | state.transform.position = state.position + 10 * forward 92 | zoe = self.create_ped(sim, "Zoe", state) 93 | 94 | def on_waypoint(agent, index): 95 | sim.stop() 96 | 97 | layer_mask = 0 | (1 << 0) 98 | waypoints = [] 99 | hit = sim.raycast(state.position - 2 * right, lgsvl.Vector(0, -1, 0), layer_mask) 100 | waypoints.append(lgsvl.WalkWaypoint(hit.point, 0)) 101 | hit = sim.raycast(state.position - 5 * right, lgsvl.Vector(0, -1, 0), layer_mask) 102 | waypoints.append(lgsvl.WalkWaypoint(hit.point, 0)) 103 | 104 | zoe.on_waypoint_reached(on_waypoint) 105 | zoe.follow(waypoints) 106 | t0 = sim.current_time 107 | sim.run() # reach the first waypoint 108 | sim.run() # reach the second waypoint 109 | t1 = sim.current_time 110 | noIdleTime = t1-t0 111 | 112 | zoe.state = state 113 | waypoints = [] 114 | hit = sim.raycast(state.position - 2 * right, lgsvl.Vector(0, -1, 0), layer_mask) 115 | waypoints.append(lgsvl.WalkWaypoint(hit.point, 2)) 116 | hit = sim.raycast(state.position - 5 * right, lgsvl.Vector(0, -1, 0), layer_mask) 117 | waypoints.append(lgsvl.WalkWaypoint(hit.point, 0)) 118 | zoe.follow(waypoints) 119 | t2 = sim.current_time 120 | sim.run() # reach the first waypoint 121 | sim.run() # reach the second waypoint 122 | t3 = sim.current_time 123 | idleTime = t3-t2 124 | 125 | self.assertAlmostEqual(idleTime-noIdleTime, 2.0, delta=0.5) 126 | 127 | def create_ped(self, sim, name, state): # create the specified Pedestrian 128 | return sim.add_agent(name, lgsvl.AgentType.PEDESTRIAN, state) 129 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/tests/test_sensors.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2021 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | import unittest 8 | import os 9 | import lgsvl 10 | from .common import SimConnection, spawnState, notAlmostEqual 11 | 12 | # TODO add tests for bridge to check if enabled sensor actually sends data 13 | 14 | 15 | class TestSensors(unittest.TestCase): 16 | def test_apollo_5_sensors(self): # Check that Apollo 3.5 sensors are created and are positioned reasonably 17 | with SimConnection(60) as sim: 18 | agent = self.create_EGO(sim, lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5) 19 | expectedSensors = ['Lidar', 'GPS', 'Telephoto Camera', \ 20 | 'Main Camera', 'IMU', 'CAN Bus', 'Radar'] 21 | sensors = agent.get_sensors() 22 | sensorNames = [s.name for s in sensors] 23 | 24 | for sensor in expectedSensors: 25 | with self.subTest(sensor): 26 | self.assertIn(sensor, sensorNames) 27 | 28 | for s in sensors: 29 | msg = "Apollo 3.0 Sensor " + s.name 30 | with self.subTest(msg): 31 | self.valid_sensor(s, msg) 32 | 33 | @unittest.skip("No SF vehicle") 34 | def test_santafe_sensors(self): # Check that Apollo Santa Fe sensors are created and are positioned reasonably 35 | with SimConnection() as sim: 36 | agent = self.create_EGO(sim, lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5) 37 | expectedSensors = ['velodyne', 'GPS', 'Telephoto Camera', 'Main Camera', \ 38 | 'IMU', 'RADAR', 'CANBUS', 'Segmentation Camera', 'Left Camera', 'Right Camera'] 39 | sensors = agent.get_sensors() 40 | sensorNames = [s.name for s in sensors] 41 | 42 | for sensor in expectedSensors: 43 | with self.subTest(sensor): 44 | self.assertIn(sensor, sensorNames) 45 | 46 | for s in sensors: 47 | msg = "Lincoln Apollo Sensor " + s.name 48 | with self.subTest(msg): 49 | self.valid_sensor(s, msg) 50 | 51 | @unittest.skip("No LGSVL Vehicle") 52 | def test_lgsvl_sensors(self): # Check that LGSVL sensors are created and are positioned reasonably 53 | with SimConnection() as sim: 54 | agent = self.create_EGO(sim, lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5) 55 | expectedSensors = ['velodyne', 'GPS', 'Telephoto Camera', 'Main Camera', \ 56 | 'IMU', 'RADAR', 'CANBUS', 'Segmentation Camera', 'Left Camera', 'Right Camera'] 57 | sensors = agent.get_sensors() 58 | sensorNames = [s.name for s in sensors] 59 | 60 | for sensor in expectedSensors: 61 | with self.subTest(sensor): 62 | self.assertIn(sensor, sensorNames) 63 | 64 | for s in sensors: 65 | msg = "lgsvl Sensor " + s.name 66 | with self.subTest(msg): 67 | self.valid_sensor(s, msg) 68 | 69 | @unittest.skip("No EP Vehicle") 70 | def test_ep_sensors(self): # Check that Apollo EP sensors are created and are positioned reasonably 71 | with SimConnection() as sim: 72 | agent = self.create_EGO(sim, "EP_Rigged-apollo") 73 | expectedSensors = ['velodyne', 'GPS', 'Telephoto Camera', 'Main Camera', \ 74 | 'IMU', 'RADAR', 'CANBUS', 'Segmentation Camera', 'Left Camera', 'Right Camera'] 75 | sensors = agent.get_sensors() 76 | sensorNames = [s.name for s in sensors] 77 | 78 | for sensor in expectedSensors: 79 | with self.subTest(sensor): 80 | self.assertIn(sensor, sensorNames) 81 | 82 | for s in sensors: 83 | msg = "EP Sensor " + s.name 84 | with self.subTest(msg): 85 | self.valid_sensor(s, msg) 86 | 87 | def test_apollo_sensors(self): # Check that all the Apollo sensors are there 88 | with SimConnection() as sim: 89 | agent = self.create_EGO(sim, lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5) 90 | expectedSensors = ["Lidar", "GPS", "Telephoto Camera", "Main Camera", "IMU", \ 91 | "CAN Bus", "Radar"] 92 | 93 | sensors = agent.get_sensors() 94 | sensorNames = [s.name for s in sensors] 95 | 96 | for sensor in expectedSensors: 97 | with self.subTest(sensor): 98 | self.assertIn(sensor, sensorNames) 99 | 100 | for s in sensors: 101 | msg = "Apollo Sensor " + s.name 102 | with self.subTest(msg): 103 | self.valid_sensor(s, msg) 104 | 105 | def test_autoware_sensors(self): # Check that all Autoware sensors are there 106 | with SimConnection() as sim: 107 | agent = self.create_EGO(sim, lgsvl.wise.DefaultAssets.ego_jaguae2015xe_autowareai) 108 | expectedSensors = ["Lidar", "GPS", "Main Camera", "IMU"] 109 | 110 | sensors = agent.get_sensors() 111 | sensorNames = [s.name for s in sensors] 112 | 113 | for sensor in expectedSensors: 114 | with self.subTest(sensor): 115 | self.assertIn(sensor, sensorNames) 116 | 117 | for s in sensors: 118 | msg = "Autoware Sensor " + s.name 119 | with self.subTest(msg): 120 | self.valid_sensor(s, msg ) 121 | 122 | def test_save_sensor(self): # Check that sensor results can be saved 123 | with SimConnection(120) as sim: 124 | 125 | path = "main-camera.png" 126 | islocal = os.environ.get("LGSVL__SIMULATOR_HOST", "127.0.0.1") == "127.0.0.1" 127 | 128 | if islocal: 129 | path = os.getcwd() + path 130 | if os.path.isfile(path): 131 | os.remove(path) 132 | 133 | agent = self.create_EGO(sim, lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5) 134 | sensors = agent.get_sensors() 135 | 136 | savedSuccess = False 137 | 138 | for s in sensors: 139 | if s.name == "Main Camera": 140 | savedSuccess = s.save(path) 141 | break 142 | 143 | self.assertTrue(savedSuccess) 144 | 145 | if islocal: 146 | self.assertTrue(os.path.isfile(path)) 147 | self.assertGreater(os.path.getsize(path), 0) 148 | os.remove(path) 149 | 150 | def test_save_lidar(self): # Check that LIDAR sensor results can be saved 151 | with SimConnection(240) as sim: 152 | path = "lidar.pcd" 153 | islocal = os.environ.get("LGSVL__SIMULATOR_HOST", "127.0.0.1") == "127.0.0.1" 154 | 155 | if islocal: 156 | path = os.getcwd() + path 157 | if os.path.isfile(path): 158 | os.remove(path) 159 | 160 | agent = self.create_EGO(sim, lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5) 161 | sensors = agent.get_sensors() 162 | savedSuccess = False 163 | 164 | for s in sensors: 165 | if s.name == "Lidar": 166 | savedSuccess = s.save(path) 167 | break 168 | 169 | self.assertTrue(savedSuccess) 170 | 171 | if islocal: 172 | self.assertTrue(os.path.isfile(path)) 173 | self.assertGreater(os.path.getsize(path), 0) 174 | os.remove(path) 175 | 176 | def test_GPS(self): # Check that the GPS sensor works 177 | with SimConnection() as sim: 178 | state = lgsvl.AgentState() 179 | state.transform = sim.get_spawn()[0] 180 | right = lgsvl.utils.transform_to_right(state.transform) 181 | state.velocity = -50 * right 182 | agent = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 183 | sensors = agent.get_sensors() 184 | initialGPSData = None 185 | gps = None 186 | for s in sensors: 187 | if s.name == "GPS": 188 | gps = s 189 | initialGPSData = gps.data 190 | sim.run(1) 191 | finalGPSData = gps.data 192 | 193 | latChanged = notAlmostEqual(initialGPSData.latitude, finalGPSData.latitude) 194 | lonChanged = notAlmostEqual(initialGPSData.longitude, finalGPSData.longitude) 195 | northingChanged = notAlmostEqual(initialGPSData.northing, finalGPSData.northing) 196 | eastingChanged = notAlmostEqual(initialGPSData.easting, finalGPSData.easting) 197 | self.assertTrue(latChanged or lonChanged) 198 | self.assertTrue(northingChanged or eastingChanged) 199 | self.assertNotAlmostEqual(gps.data.latitude, 0) 200 | self.assertNotAlmostEqual(gps.data.longitude, 0) 201 | self.assertNotAlmostEqual(gps.data.northing, 0) 202 | self.assertNotAlmostEqual(gps.data.easting, 0) 203 | self.assertNotAlmostEqual(gps.data.altitude, 0) 204 | self.assertNotAlmostEqual(gps.data.orientation, 0) 205 | 206 | def test_sensor_disabling(self): # Check if sensors can be enabled 207 | with SimConnection() as sim: 208 | agent = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 209 | for s in agent.get_sensors(): 210 | if s.name == "Lidar": 211 | s.enabled = False 212 | 213 | for s in agent.get_sensors(): 214 | with self.subTest(s.name): 215 | if (s.name == "Lidar"): 216 | self.assertFalse(s.enabled) 217 | else: 218 | self.assertTrue(s.enabled) 219 | 220 | def test_sensor_equality(self): # Check that sensor == operator works 221 | with SimConnection() as sim: 222 | agent = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 223 | prevSensor = None 224 | for s in agent.get_sensors(): 225 | self.assertTrue(s == s) 226 | if prevSensor is not None: 227 | self.assertFalse(s == prevSensor) 228 | prevSensor = s 229 | 230 | def create_EGO(self, sim, name): # Creates the speicified EGO and removes any others 231 | return sim.add_agent(name, lgsvl.AgentType.EGO, spawnState(sim)) 232 | 233 | def valid_sensor(self, sensor, msg): # Checks that the sensor is close to the EGO and not overly rotated 234 | self.assertBetween(sensor.transform.rotation, 0, 360, msg) 235 | self.assertBetween(sensor.transform.position, -10, 10, msg) 236 | 237 | def assertBetween(self, vector, min, max, msg): # Tests that the given vectors components are within the min and max 238 | self.assertGreaterEqual(vector.x, min, msg) 239 | self.assertLessEqual(vector.x, max, msg) 240 | 241 | self.assertGreaterEqual(vector.y, min, msg) 242 | self.assertLessEqual(vector.y, max, msg) 243 | 244 | self.assertGreaterEqual(vector.z, min, msg) 245 | self.assertLessEqual(vector.z, max, msg) 246 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/tests/test_simulator.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2021 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | import unittest 8 | 9 | import lgsvl 10 | 11 | from .common import SimConnection, spawnState 12 | 13 | PROBLEM = "Object reference not set to an instance of an object" 14 | 15 | 16 | class TestSimulator(unittest.TestCase): 17 | 18 | def test_scene(self): # Check if the right scene was loaded 19 | with SimConnection() as sim: 20 | self.assertEqual(sim.current_scene, "BorregasAve") 21 | 22 | def test_unload_scene(self): # Check if a different scene gets loaded 23 | with SimConnection() as sim: 24 | self.assertEqual(sim.current_scene, "BorregasAve") 25 | sim.load("CubeTown") 26 | self.assertEqual(sim.current_scene, "CubeTown") 27 | 28 | def test_spawns(self): # Check if there is at least 1 spawn point for Ego Vehicles 29 | with SimConnection() as sim: 30 | spawns = sim.get_spawn() 31 | self.assertGreater(len(spawns), 0) 32 | 33 | def test_run_time(self): # Check if the simulator runs 2 seconds 34 | with SimConnection() as sim: 35 | sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) 36 | time = 2.0 37 | initial_time = sim.current_time 38 | 39 | sim.run(time) 40 | self.assertAlmostEqual(sim.current_time - initial_time, time, delta=0.1) 41 | 42 | def test_non_realtime(self): # Check if non-realtime simulation runs 43 | with SimConnection() as sim: 44 | initial_time = sim.current_time 45 | sim.run(time_limit=3, time_scale=2) 46 | 47 | self.assertAlmostEqual(sim.current_time-initial_time, 3, delta=0.1) 48 | 49 | def test_reset(self): # Check if sim.reset resets the time and frame 50 | with SimConnection() as sim: 51 | sim.run(1.0) 52 | 53 | sim.reset() 54 | 55 | self.assertAlmostEqual(sim.current_time, 0) 56 | self.assertEqual(sim.current_frame, 0) 57 | 58 | def test_raycast(self): # Check if raycasting works 59 | with SimConnection() as sim: 60 | spawns = sim.get_spawn() 61 | state = lgsvl.AgentState() 62 | state.transform = spawns[0] 63 | forward = lgsvl.utils.transform_to_forward(state.transform) 64 | right = lgsvl.utils.transform_to_right(state.transform) 65 | up = lgsvl.utils.transform_to_up(state.transform) 66 | sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) 67 | 68 | p = spawns[0].position 69 | p.y += 1 70 | layer_mask = 0 71 | for bit in [0, 4, 13, 14, 18]: # do not put 8 here, to not hit EGO vehicle itself 72 | layer_mask |= 1 << bit 73 | 74 | # Right 75 | hit = sim.raycast(p, right, layer_mask) 76 | self.assertTrue(hit) 77 | self.assertAlmostEqual(hit.distance, 15.089282989502, places=3) 78 | 79 | # Left 80 | hit = sim.raycast(p, -right, layer_mask) 81 | self.assertTrue(hit) 82 | self.assertAlmostEqual(hit.distance, 19.7292556762695, places=3) 83 | 84 | # Back 85 | hit = sim.raycast(p, -forward, layer_mask) 86 | self.assertFalse(hit) 87 | 88 | # Front 89 | hit = sim.raycast(p, forward, layer_mask) 90 | self.assertFalse(hit) 91 | 92 | # Up 93 | hit = sim.raycast(p, up, layer_mask) 94 | self.assertFalse(hit) 95 | 96 | # Down 97 | hit = sim.raycast(p, -up, layer_mask) 98 | self.assertTrue(hit) 99 | self.assertAlmostEqual(hit.distance, 1.0837641954422, places=3) 100 | 101 | def test_weather(self): # Check that the weather state can be read properly and changed 102 | with SimConnection() as sim: 103 | rain = sim.weather.rain 104 | fog = sim.weather.fog 105 | wetness = sim.weather.wetness 106 | 107 | self.assertAlmostEqual(rain, 0) 108 | self.assertAlmostEqual(fog, 0) 109 | self.assertAlmostEqual(wetness, 0) 110 | 111 | sim.weather = lgsvl.WeatherState(rain=0.5, fog=0.3, wetness=0.8, cloudiness=0, damage=0) 112 | 113 | rain = sim.weather.rain 114 | fog = sim.weather.fog 115 | wetness = sim.weather.wetness 116 | 117 | self.assertAlmostEqual(rain, 0.5) 118 | self.assertAlmostEqual(fog, 0.3) 119 | self.assertAlmostEqual(wetness, 0.8) 120 | 121 | def test_invalid_weather(self): # Check that the API/Unity properly handles unexpected inputs 122 | with SimConnection() as sim: 123 | rain = sim.weather.rain 124 | fog = sim.weather.fog 125 | wetness = sim.weather.wetness 126 | 127 | self.assertAlmostEqual(rain, 0) 128 | self.assertAlmostEqual(fog, 0) 129 | self.assertAlmostEqual(wetness, 0) 130 | 131 | sim.weather = lgsvl.WeatherState(rain=1.4, fog=-3, wetness="a", cloudiness=0, damage=0) 132 | 133 | rain = sim.weather.rain 134 | fog = sim.weather.fog 135 | wetness = sim.weather.wetness 136 | 137 | self.assertAlmostEqual(rain, 1) 138 | self.assertAlmostEqual(fog, 0) 139 | self.assertAlmostEqual(wetness, 0) 140 | 141 | sim.weather = lgsvl.WeatherState(rain=True, fog=0, wetness=0, cloudiness=0, damage=0) 142 | 143 | rain = sim.weather.rain 144 | self.assertAlmostEqual(rain, 0) 145 | 146 | def test_time_of_day(self): # Check that the time of day is reported properly and can be set 147 | with SimConnection() as sim: 148 | # self.assertAlmostEqual(sim.time_of_day, 9, delta=0.5) 149 | sim.set_time_of_day(18.00) 150 | self.assertAlmostEqual(sim.time_of_day, 18, delta=0.5) 151 | 152 | sim.set_time_of_day(13.5, False) 153 | self.assertAlmostEqual(sim.time_of_day, 13.5, delta=0.5) 154 | sim.run(3) 155 | self.assertGreater(sim.time_of_day, 13.5) 156 | 157 | def test_wrong_time_of_day(self): # Check that the time of day is not broken with inappropriate inputs 158 | with SimConnection() as sim: 159 | sim.set_time_of_day(40) 160 | self.assertAlmostEqual(sim.time_of_day, 0) 161 | with self.assertRaises(TypeError): 162 | sim.set_time_of_day("asdf") 163 | 164 | def test_reset_weather(self): # Check that reset sets the weather variables back to 0 165 | with SimConnection() as sim: 166 | rain = sim.weather.rain 167 | fog = sim.weather.fog 168 | wetness = sim.weather.wetness 169 | self.assertAlmostEqual(rain, 0) 170 | self.assertAlmostEqual(fog, 0) 171 | self.assertAlmostEqual(wetness, 0) 172 | 173 | sim.weather = lgsvl.WeatherState(rain=0.5, fog=0.3, wetness=0.8, cloudiness=0, damage=0) 174 | rain = sim.weather.rain 175 | fog = sim.weather.fog 176 | wetness = sim.weather.wetness 177 | self.assertAlmostEqual(rain, 0.5) 178 | self.assertAlmostEqual(fog, 0.3) 179 | self.assertAlmostEqual(wetness, 0.8) 180 | 181 | sim.reset() 182 | rain = sim.weather.rain 183 | fog = sim.weather.fog 184 | wetness = sim.weather.wetness 185 | self.assertAlmostEqual(rain, 0) 186 | self.assertAlmostEqual(fog, 0) 187 | self.assertAlmostEqual(wetness, 0) 188 | 189 | def test_reset_time(self): # Check that reset sets time back to the default 190 | with SimConnection() as sim: 191 | default_time = sim.time_of_day 192 | sim.set_time_of_day((default_time+5)%24) 193 | sim.reset() 194 | self.assertAlmostEqual(default_time, sim.time_of_day) 195 | 196 | # THIS TEST RUNS LAST 197 | def test_ztypo_map(self): # Check if an exception is raised with a misspelled map name is loaded 198 | #with self.assertRaises(TestTimeout): 199 | with SimConnection() as sim: 200 | with self.assertRaises(Exception) as e: 201 | sim.load("SF") 202 | self.assertFalse(repr(e.exception).startswith(PROBLEM)) 203 | 204 | def test_negative_time(self): # Check that a negative time can be handled properly 205 | with SimConnection() as sim: 206 | initial_time = sim.current_time 207 | sim.run(-5) 208 | post_time = sim.current_time 209 | self.assertAlmostEqual(initial_time, post_time) 210 | 211 | def test_get_gps(self): # Checks that GPS reports the correct values 212 | with SimConnection() as sim: 213 | spawn = sim.get_spawn()[0] 214 | gps = sim.map_to_gps(spawn) 215 | self.assertAlmostEqual(gps.latitude, 37.4173601699318) 216 | self.assertAlmostEqual(gps.longitude, -122.016132757826) 217 | self.assertAlmostEqual(gps.northing, 4141627.34000015) 218 | self.assertAlmostEqual(gps.easting, 587060.970000267) 219 | self.assertAlmostEqual(gps.altitude, -1.03600001335144) 220 | self.assertAlmostEqual(gps.orientation, -104.823394775391) 221 | 222 | def test_from_northing(self): # Check that position vectors are correctly generated given northing and easting 223 | with SimConnection() as sim: 224 | spawn = sim.get_spawn()[0] 225 | location = sim.map_from_gps(northing=4141627.34000015, easting=587060.970000267) 226 | self.assertAlmostEqual(spawn.position.x, location.position.x, places=1) 227 | self.assertAlmostEqual(spawn.position.z, location.position.z, places=1) 228 | 229 | def test_from_latlong(self): # Check that position vectors are correctly generated given latitude and longitude 230 | with SimConnection() as sim: 231 | spawn = sim.get_spawn()[0] 232 | location = sim.map_from_gps(latitude=37.4173601699318, longitude=-122.016132757826) 233 | self.assertAlmostEqual(spawn.position.x, location.position.x, places=1) 234 | self.assertAlmostEqual(spawn.position.z, location.position.z, places=1) 235 | 236 | def test_from_alt_orient(self): # Check that position vectors are correctly generated with altitude and orientation 237 | with SimConnection() as sim: 238 | spawn = sim.get_spawn()[0] 239 | location = sim.map_from_gps(northing=4141627.34000015, easting=587060.970000267, altitude=-1.03600001335144, orientation=-104.823371887207) 240 | self.assertAlmostEqual(spawn.position.y, location.position.y, places=1) 241 | self.assertAlmostEqual(spawn.rotation.y, location.rotation.y, places=1) 242 | 243 | def test_false_latlong(self): # Check that exceptions are thrown when inputting invalid lat long values 244 | with SimConnection() as sim: 245 | with self.assertRaises(ValueError): 246 | sim.map_from_gps(latitude=91, longitude=0) 247 | 248 | with self.assertRaises(ValueError): 249 | sim.map_from_gps(latitude=0, longitude=200) 250 | 251 | def test_false_easting(self): # Check that exceptions are thrown when inputting invalid northing easting values 252 | with SimConnection() as sim: 253 | with self.assertRaises(ValueError): 254 | sim.map_from_gps(easting=1000000000, northing=500000) 255 | 256 | with self.assertRaises(ValueError): 257 | sim.map_from_gps(northing=-50, easting=500000) 258 | 259 | def test_version_info(self): # Check that the sim reports a numerical version number 260 | with SimConnection() as sim: 261 | version = sim.version 262 | self.assertTrue(isinstance(version, str)) 263 | self.assertTrue(isinstance(float(version[:4]), float)) 264 | 265 | def test_lat_northing(self): # Checks that exceptions are thrown if an invalid pair of gps values are given 266 | with SimConnection() as sim: 267 | with self.assertRaises(Exception) as e: 268 | sim.map_from_gps(northing=4812775, latitude=37.7) 269 | self.assertIn("Either latitude and longitude or northing and easting should be specified", repr(e.exception)) 270 | 271 | # def test_both_lat_northing(self): 272 | # with SimConnection() as sim: 273 | # with self.assertRaises(Exception) as e: 274 | # sim.map_from_gps(northing=1, easting=2, latitude=3, longitude=4) 275 | 276 | def test_lat_str(self): # Checks that exceptions are thrown if a string is given for latitude 277 | with SimConnection() as sim: 278 | with self.assertRaises(TypeError): 279 | sim.map_from_gps(latitude="asdf", longitude=2) 280 | 281 | def test_long_str(self): # Checks that exceptions are thrown if a string is given for longitude 282 | with SimConnection() as sim: 283 | with self.assertRaises(TypeError): 284 | sim.map_from_gps(latitude=1, longitude="asdf") 285 | 286 | def test_northing_str(self): # Checks that exceptions are thrown if a string is given for northing 287 | with SimConnection() as sim: 288 | with self.assertRaises(TypeError): 289 | sim.map_from_gps(northing="asdf", easting=2) 290 | 291 | def test_easting_str(self): # Checks that exceptions are thrown if a string is given for easting 292 | with SimConnection() as sim: 293 | with self.assertRaises(TypeError): 294 | sim.map_from_gps(northing=1, easting="asdF") 295 | 296 | def test_altitude_str(self): # Checks that exceptions are thrown if a string is given for altitude 297 | with SimConnection() as sim: 298 | with self.assertRaises(TypeError): 299 | sim.map_from_gps(latitude=1, longitude=2, altitude="asd") 300 | 301 | def test_orientation_str(self): # Checks that exceptions are thrown if a string is given for orientation 302 | with SimConnection() as sim: 303 | with self.assertRaises(TypeError): 304 | sim.map_from_gps(latitude=1, longitude=2, orientation="asdf") 305 | -------------------------------------------------------------------------------- /PythonAPI-Apollo-async/tests/test_utils.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2020 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | import unittest 8 | 9 | import lgsvl 10 | import lgsvl.utils 11 | 12 | 13 | class TestUtils(unittest.TestCase): # Check that transform_to_matrix calculates the right values 14 | def test_transform_to_matrix(self): 15 | transform = lgsvl.Transform(lgsvl.Vector(1, 2, 3), lgsvl.Vector(4, 5, 6)) 16 | expectedMatrix = [[0.9913729386253347, 0.10427383718471564, -0.07941450396586013, 0.0], 17 | [-0.0980843287345578, 0.9920992900156518, 0.07822060602635744, 0.0], 18 | [0.08694343573875718, -0.0697564737441253, 0.9937680178757644, 0.0], 19 | [1, 2, 3, 1.0]] 20 | 21 | matrix = lgsvl.utils.transform_to_matrix(transform) 22 | for i in range(4): 23 | for j in range(4): 24 | self.assertAlmostEqual(matrix[i][j], expectedMatrix[i][j]) 25 | 26 | def test_matrix_multiply(self): # Check that matrix_multiply calculates the right values 27 | inputMatrix = lgsvl.utils.transform_to_matrix(lgsvl.Transform(lgsvl.Vector(1, 2, 3), lgsvl.Vector(4, 5, 6))) 28 | expectedMatrix = [[0.9656881042915112, 0.21236393599051254, -0.1494926216255657, 0.0], 29 | [-0.18774677387638924, 0.9685769782741936, 0.1631250626244768, 0.0], 30 | [0.1794369920860106, -0.12946117505974142, 0.9752139098799174, 0.0], 31 | [2.0560345883724906, 3.8792029959836434, 6.058330761714148, 1.0]] 32 | 33 | matrix = lgsvl.utils.matrix_multiply(inputMatrix, inputMatrix) 34 | for i in range(4): 35 | for j in range(4): 36 | self.assertAlmostEqual(matrix[i][j], expectedMatrix[i][j]) 37 | 38 | def test_matrix_inverse(self): # Check that matrix_inverse calculates the right values 39 | inputMatrix = lgsvl.utils.transform_to_matrix(lgsvl.Transform(lgsvl.Vector(1, 2, 3), lgsvl.Vector(4, 5, 6))) 40 | expectedMatrix = [[0.9913729386253347, -0.0980843287345578, 0.08694343573875718, 0.0], 41 | [0.10427383718471564, 0.9920992900156518, -0.0697564737441253, 0.0], 42 | [-0.07941450396586013, 0.07822060602635744, 0.9937680178757644, 0.0], 43 | [-0.9616771010971856, -2.120776069375818, -2.9287345418778, 1.0]] 44 | 45 | matrix = lgsvl.utils.matrix_inverse(inputMatrix) 46 | for i in range(4): 47 | for j in range(4): 48 | self.assertAlmostEqual(matrix[i][j], expectedMatrix[i][j]) 49 | 50 | def test_vector_multiply(self): # Check that vector_multiply calculates the right values 51 | inputMatrix = lgsvl.utils.transform_to_matrix(lgsvl.Transform(lgsvl.Vector(1, 2, 3), lgsvl.Vector(4, 5, 6))) 52 | inputVector = lgsvl.Vector(10, 20, 30) 53 | expectedVector = lgsvl.Vector(11.560345883724906, 20.792029959836434, 33.58330761714148) 54 | 55 | vector = lgsvl.utils.vector_multiply(inputVector, inputMatrix) 56 | self.assertAlmostEqual(vector.x, expectedVector.x) 57 | self.assertAlmostEqual(vector.y, expectedVector.y) 58 | self.assertAlmostEqual(vector.z, expectedVector.z) 59 | 60 | def test_vector_dot(self): # Check that vector_dot calculates the right values 61 | result = lgsvl.utils.vector_dot(lgsvl.Vector(1, 2, 3), lgsvl.Vector(4, 5, 6)) 62 | self.assertAlmostEqual(result, 32) 63 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # BehAVExplor 2 | This project is the official implementation of paper: 3 | 4 | BehAVExplor: Behavior Diversity Guided Testing for Autonomous Driving Systems 5 | Authors: Mingfei Cheng, Yuan Zhou and Xiaofei Xie 6 | [Accepted to ISSTA 2023](https://2023.issta.org/details/issta-2023-technical-papers/32/BehAVExplor-Behavior-Diversity-Guided-Testing-for-Autonomous-Driving-Systems) 7 | 8 | ## Method Overview 9 | ![framework](figs/MethodOverview.png) 10 | 11 | ## Settings 12 | Step 1: Config Baidu Apollo (no less than 6.0 version) according to [Apollo](https://github.com/ApolloAuto/apollo) 13 | 14 | Step 2: Clone BehAVExplor Project (BehAVExplor.zip) and unzip it under `/apollo` in the apollo container 15 | * Project Structure 16 | ``` 17 | /apollo 18 | ├── BehAVExplor 19 | └── other apollo folders 20 | ``` 21 | 22 | Step 3: Install required Python dependency inside the apollo container 23 | ```angular2html 24 | cd /apollo/BehAVExplor 25 | sh prepare.sh 26 | ``` 27 | 28 | Step 3: Compile BehAVExplor inside the apollo container 29 | ```angular2html 30 | cd /apollo 31 | bazel build //BehAVExplor:main 32 | ``` 33 | 34 | Step 4: Config our custom LGSVL 2021.3 (Download from [link](https://drive.google.com/drive/folders/1t4vSZfipPcCVz2wHbS7OIP-TQ0wBFuU1?usp=sharing)) 35 | * Put simulator.zip at ~/lgsvl outside the apollo container 36 | * Double click ~/lgsvl/simulator/simulator/simulator to open LGSVL Simulator 37 | * NOTE: [LGSVL](https://github.com/lgsvl/simulator) was closed, therefore, we will plan to extend this project to [CARLA](https://github.com/carla-simulator/carla). 38 | 39 | Step 5: Install default map used by BehAVExplor by unzipping `BehavExplor/data/SanFrancisco.zip` at `/apollo/modules/map/data`. Your file structure should look like this 40 | ``` 41 | map 42 | |-data 43 | |-SanFrancisco 44 | |- base_map.bin 45 | |- routing_map.bin 46 | |- ... 47 | ``` 48 | 49 | Step 6: Start Apollo and CyberRT bridge inside the apollo container via 50 | ```angular2html 51 | cd /apollo 52 | bash scripts/bootstrap.sh 53 | bash scripts/bridge 54 | ``` 55 | 56 | Step 7: Start LGSVL. Run in another terminal (current path is `/apollo` inside apollo container) 57 | ```angular2html 58 | ./bazel-bin/BehAVExplor/main --config=/apollo/BehAVExplor/configs/[scenario config yaml file] 59 | ``` 60 | 61 | ## Citation 62 | If you use this code and our results for your research, please cite our paper. 63 | ``` 64 | @inproceedings{BehAVExplor_ISSTA_2023, 65 | title={BehAVExplor: Behavior Diversity Guided Testing for Autonomous Driving Systems}, 66 | author={Cheng, Mingfei and Zhou, Yuan and Xie, Xiaofei}, 67 | booktitle={Proceedings of the 32nd ACM SIGSOFT International Symposium on Software Testing and Analysis (ISSTA)}, 68 | year={2023} 69 | } 70 | ``` 71 | -------------------------------------------------------------------------------- /behavexplor/BUILD: -------------------------------------------------------------------------------- 1 | load("@rules_python//python:defs.bzl", "py_binary", "py_library") 2 | 3 | package(default_visibility = ["//visibility:public"]) 4 | 5 | py_binary( 6 | name = "corpus", 7 | srcs = ["corpus.py"], 8 | deps = [ 9 | "//BehAVExplor/behavexplor:element", 10 | "//BehAVExplor/behavexplor/models:coverage", 11 | ] 12 | ) 13 | 14 | py_binary( 15 | name = "element", 16 | srcs = ["element.py"], 17 | ) 18 | 19 | py_binary( 20 | name = "utils", 21 | srcs = ["utils.py"], 22 | ) -------------------------------------------------------------------------------- /behavexplor/corpus.py: -------------------------------------------------------------------------------- 1 | import os 2 | import copy 3 | import pickle 4 | import numpy as np 5 | 6 | from loguru import logger 7 | 8 | from behavexplor.element import CorpusElement 9 | from behavexplor.models.coverage import CoverageModel 10 | 11 | class InputCorpus(object): 12 | """Class that holds inputs and associated coverage.""" 13 | def __init__(self, save_folder, window_size, cluster_num, threshold_coverage, feature_resample='linear'): 14 | 15 | # just for save 16 | self.window_size = window_size 17 | self.cluster_num = cluster_num 18 | self.threshold_coverage = threshold_coverage 19 | 20 | self.save_folder = save_folder 21 | self.coverage_model = CoverageModel(self.window_size, 22 | self.cluster_num, 23 | self.threshold_coverage) 24 | self.feature_resample = feature_resample 25 | 26 | self.corpus_total = [] # save all elements in the fuzzing 27 | 28 | self.corpus_seeds = [] # save seeds index 29 | self.corpus_fail = [] # save failed index 30 | self.corpus_coverage = [] # keep coverage 31 | 32 | self.parent_element = None 33 | 34 | self.current_selected_index = -1 # parent index 35 | self.current_mutation = None 36 | self.current_index = -1 37 | 38 | def _update_log(self, mutation_str): 39 | if mutation_str is not None: 40 | log_mutation = os.path.join(self.save_folder, 'logs/mutation.log') 41 | with open(log_mutation, 'a') as f: 42 | f.write(mutation_str) 43 | f.write('\n') 44 | 45 | corpus_str = 'total: {:d} seeds: {:d} fail: {:d} coverage: {:d}'.format(len(self.corpus_total), 46 | len(self.corpus_seeds), 47 | len(self.corpus_fail), 48 | len(self.corpus_coverage)) 49 | log_corpus = os.path.join(self.save_folder, 'logs/corpus.log') 50 | with open(log_corpus, 'a') as f: 51 | f.write(corpus_str) 52 | f.write('\n') 53 | 54 | threshold = self.coverage_model.get_threshold_info() 55 | threshold_str = 'coverage: {:.5f}'.format(threshold['coverage']) 56 | log_threshold = os.path.join(self.save_folder, 'logs/model_threshold.log') 57 | with open(log_threshold, 'a') as f: 58 | f.write(threshold_str) 59 | f.write('\n') 60 | 61 | threshold_dyn = self.coverage_model.get_dynamic_threshold_info() 62 | threshold_dyn_str = 'coverage-dyn: {:.5f}'.format(threshold_dyn['coverage']) 63 | log_threshold_dyn = os.path.join(self.save_folder, 'logs/model_threshold_dyn.log') 64 | with open(log_threshold_dyn, 'a') as f: 65 | f.write(threshold_dyn_str) 66 | f.write('\n') 67 | 68 | model_static = self.coverage_model.get_static_info() 69 | model_static_str = 'coverage: {:d}'.format(model_static['coverage']) 70 | log_model_static = os.path.join(self.save_folder, 'logs/model_static.log') 71 | with open(log_model_static, 'a') as f: 72 | f.write(model_static_str) 73 | f.write('\n') 74 | 75 | logger.info(mutation_str) 76 | logger.info(corpus_str) 77 | logger.info(model_static_str) 78 | logger.info(threshold_str) 79 | logger.info(threshold_dyn_str) 80 | 81 | def _extract_element_feature(self, element): 82 | x, x_scale = element.get_vector() 83 | y_behavior, y_trace = self.coverage_model.extract_feature(x, x_scale, resample=self.feature_resample) 84 | element.set_feature_behavior(y_behavior) 85 | element.set_feature_trace(y_trace) 86 | return element 87 | 88 | def initialize(self, scenario_objs, scenario_recorders): 89 | # make sure inits have different routes. 90 | assert len(scenario_objs) == len(scenario_recorders) 91 | self.corpus_total = [] # save all elements in the fuzzing 92 | 93 | self.corpus_seeds = [] # save un-failed index 94 | self.corpus_fail = [] # save failed index 95 | self.corpus_coverage = [] # keep coverage 96 | 97 | self.parent_element = None 98 | self.current_index = 0 99 | 100 | init_size = len(scenario_objs) 101 | X_behavior = [] 102 | 103 | for ele_index in range(init_size): 104 | scenario_obj = scenario_objs[ele_index] 105 | scenario_recorder = scenario_recorders[ele_index] 106 | corpus_element = CorpusElement(self.save_folder, scenario_obj, scenario_recorder) 107 | corpus_element = self._extract_element_feature(corpus_element) 108 | corpus_element.energy_init(0.5, 0.5) 109 | 110 | self.corpus_total.append(corpus_element) 111 | 112 | if corpus_element.is_fail(): 113 | self.corpus_fail.append(self.current_index) 114 | else: 115 | # TODO: add energy 116 | self.corpus_seeds.append(self.current_index) 117 | self.corpus_coverage.append(self.current_index) 118 | # self._update_mutate_recorder(self.current_index, 0, False, False) 119 | # update coverage model 120 | x_behavior, x_trace = corpus_element.get_feature() 121 | X_behavior.append(x_behavior) 122 | self.current_index += 1 123 | 124 | # start if not zero 125 | if len(self.corpus_seeds) > 0: 126 | self.coverage_model.initialize(X_behavior) 127 | else: 128 | logger.warning('[Corpus] initialize with zero element.') 129 | 130 | self._update_log(None) 131 | return len(self.corpus_seeds) 132 | 133 | def sample_scenario_seed(self): 134 | 135 | if len(self.corpus_seeds) <= 0: 136 | logger.warning('[InputCorpus] length of corpus_seeds <= 0') 137 | return None, 'uniform' 138 | 139 | select_probabilities = [] 140 | is_all_zero = True 141 | 142 | for i in range(len(self.corpus_seeds)): 143 | corpus_index = self.corpus_seeds[i] 144 | element = self.corpus_total[corpus_index] 145 | element_energy = element.get_energy() 146 | if element_energy > 0.0: 147 | is_all_zero = False 148 | select_probabilities.append(element_energy) 149 | 150 | if not is_all_zero: 151 | # compute new selection probability 152 | select_probabilities = np.array(select_probabilities) 153 | select_probabilities /= select_probabilities.sum() 154 | # add selection strategy 155 | corpus_index = np.random.choice(self.corpus_seeds, p=select_probabilities) 156 | else: 157 | corpus_index = np.random.choice(self.corpus_seeds) 158 | 159 | self.current_selected_index = corpus_index 160 | self.parent_element = copy.deepcopy(self.corpus_total[self.current_selected_index]) 161 | 162 | sample_scenario_cfg = copy.deepcopy(self.parent_element.get_scenario_cfg()) 163 | sample_seed_energy = self.parent_element.get_energy() 164 | 165 | self.current_mutation = 'uniform' 166 | if sample_seed_energy > 0.5: 167 | self.current_mutation = 'gauss' 168 | logger.info('[InputCorpus] Index: ' + str(self.current_selected_index) + ' Energy: ' + str( 169 | sample_seed_energy) + ' Mutation: ' + str(self.current_mutation)) 170 | 171 | return sample_scenario_cfg, self.current_mutation 172 | 173 | def add_seed(self, scenario_obj, scenario_recorder): 174 | 175 | corpus_element = CorpusElement(self.save_folder, scenario_obj, scenario_recorder) 176 | corpus_element = self._extract_element_feature(corpus_element) 177 | 178 | is_fail = corpus_element.is_fail() 179 | is_novel = False 180 | 181 | corpus_feature_behavior, corpus_feature_trace = corpus_element.get_feature() 182 | feedback_coverage, min_coverage_distance, _ = self.coverage_model.feedback_coverage_behavior(corpus_feature_behavior, is_fail) 183 | 184 | corpus_element.set_feedback(feedback_coverage, min_coverage_distance) 185 | 186 | if is_fail: 187 | self.corpus_fail.append(self.current_index) 188 | else: 189 | if feedback_coverage or corpus_element.get_fitness() < self.parent_element.get_fitness(): 190 | if feedback_coverage: 191 | is_novel = True 192 | self.corpus_coverage.append(self.current_index) 193 | # add to seed 194 | self.corpus_seeds.append(self.current_index) 195 | 196 | delta_coverage = min_coverage_distance 197 | delta_fitness = self.parent_element.get_fitness() - corpus_element.get_fitness() 198 | corpus_element.energy_init(delta_fitness, delta_coverage) 199 | 200 | # update parent energy 201 | if self.current_mutation == 'gauss': 202 | self.corpus_total[self.current_selected_index].energy_step(is_fail, is_novel, delta_fitness, delta_coverage, False) 203 | else: 204 | self.corpus_total[self.current_selected_index].energy_step(is_fail, is_novel, delta_fitness, delta_coverage, True) 205 | 206 | # parent step 207 | self.corpus_total.append(corpus_element) 208 | self.current_index += 1 209 | mutation_str = '(parent) energy: {:.5f} fitness: {:.5f} coverage-dist: {:.5f} energy-update: {:.5f}\n ' \ 210 | '(spring) energy: {:.5f} fitness: {:.5f} coverage-dist: {:.5f} \n'.format( 211 | self.parent_element.get_energy(), self.parent_element.get_fitness(), self.parent_element.get_coverage_distance(), 212 | self.corpus_total[self.current_selected_index].get_energy(), 213 | corpus_element.get_energy(), corpus_element.get_fitness(), corpus_element.get_coverage_distance() 214 | ) # before energy 215 | self._update_log(mutation_str) 216 | 217 | return len(self.corpus_seeds), len(self.corpus_fail) 218 | 219 | def __len__(self): 220 | return len(self.corpus_total) 221 | 222 | def save_models(self, save_folder=None): 223 | if save_folder is None: 224 | save_folder = os.path.join(self.save_folder, 'models') 225 | 226 | save_dict = {} 227 | for k, v in self.__dict__.items(): 228 | if k == 'coverage_model': 229 | save_dict[k] = os.path.join(save_folder, 'coverage.pth') 230 | 231 | f = open(os.path.join(save_folder, 'corpus.pth'), 'wb') 232 | pickle.dump(save_dict, f, 2) 233 | f.close() 234 | # save layer 235 | self.coverage_model.save_model(save_folder) 236 | -------------------------------------------------------------------------------- /behavexplor/element.py: -------------------------------------------------------------------------------- 1 | import copy 2 | 3 | import numpy as np 4 | 5 | class CorpusElement(object): 6 | """Class representing a single element of a corpus.""" 7 | 8 | def __init__(self, save_folder, scenario_cfg, scenario_recorder, init_energy=1.0): 9 | self.parent_id = None 10 | self.feature_behavior = None 11 | self.feature_trace = None 12 | self.scenario_vector = None 13 | self.scenario_time_scale = None 14 | 15 | self.energy = init_energy 16 | self.energy_recorder = { 17 | 'fail': 0, 18 | 'novel': 0, 19 | 'valid': 0, 20 | 'total': 0, 21 | } 22 | 23 | self.save_folder = save_folder 24 | self.scenario_id = scenario_recorder.get_case_id() 25 | self.scenario_cfg = scenario_cfg 26 | self.scenario_recorder = scenario_recorder 27 | 28 | self.fitness = None 29 | self.feedback_coverage = False # Bool - true or false 30 | self.min_coverage_distance = np.inf 31 | 32 | def get_parent_id(self): 33 | return self.parent_id 34 | 35 | def get_scenario_id(self): 36 | return self.scenario_id 37 | 38 | def get_scenario_cfg(self): 39 | return copy.deepcopy(self.scenario_cfg) 40 | 41 | def get_scenario_recorder(self): 42 | return copy.deepcopy(self.scenario_recorder) 43 | 44 | def get_events(self): 45 | case_events = self.scenario_recorder.get_case_event() 46 | return case_events 47 | 48 | def get_vector(self): 49 | if self.scenario_vector is None or self.scenario_time_scale is None: 50 | self.scenario_vector = self.scenario_recorder.get_case_vector() 51 | self.scenario_time_scale = self.scenario_recorder.get_time_scale() 52 | return self.scenario_vector, self.scenario_time_scale 53 | 54 | def get_feature(self): 55 | return self.feature_behavior, self.feature_trace 56 | 57 | def get_energy(self): 58 | if self.energy < 0.0: 59 | self.energy = 0.0 60 | return self.energy 61 | 62 | def get_fitness(self): 63 | case_fitness = self.scenario_recorder.get_case_fitness() 64 | obj_dest = 1.0 - min(case_fitness[0], 10) / 10.0 # may need to be deleted 0 better 65 | obj_collision = min(case_fitness[1], 1.0) # min(all_fitness[1], 1) 0 better 66 | obj_yellow_line = min(case_fitness[2], 1.0) # min(all_fitness[2], 1) 0 better 67 | obj_edge_line = min(case_fitness[3], 1.0) # min(all_fitness[3], 1) 0 better 68 | self.fitness = (obj_dest + obj_collision + obj_yellow_line + obj_edge_line) / 4.0 69 | return self.fitness 70 | 71 | def get_scenario_topology(self): 72 | return self.scenario_cfg.get_scenario_structure() 73 | 74 | def get_coverage_distance(self): 75 | return self.min_coverage_distance 76 | 77 | def set_scenario_cfg(self, scenario_cfg): 78 | self.scenario_cfg = scenario_cfg 79 | 80 | def set_parent_id(self, parent_id): 81 | self.parent_id = parent_id 82 | 83 | def set_feature_behavior(self, x): 84 | self.feature_behavior = x 85 | 86 | def set_feature_trace(self, x): 87 | self.feature_trace = x 88 | 89 | def set_feedback(self, feedback, distance): 90 | self.feedback_coverage = feedback 91 | self.min_coverage_distance = distance 92 | 93 | def energy_step(self, is_fail, is_novel, delta_fitness, delta_coverage, is_uniform): 94 | """ 95 | for new added element: 96 | main part is delta 97 | delta_fitness: -1 - 1, larger better 98 | spring - parent, low value means better 99 | delta_coverage: 0 - 1, larger better 100 | for parent element: 101 | main part is the state of springs, reduce their probability of 102 | """ 103 | benign = True 104 | if is_uniform: 105 | self.energy = self.energy - 0.1 106 | else: 107 | self.energy_recorder['total'] += 1 108 | if is_fail or is_novel: 109 | benign = False 110 | self.energy_recorder['valid'] += 1 111 | if is_fail: 112 | self.energy_recorder['fail'] += 1 113 | if is_novel: 114 | self.energy_recorder['novel'] += 1 115 | 116 | valid_rate = self.energy_recorder['valid'] / float(self.energy_recorder['total']) 117 | 118 | if benign: 119 | ef = -0.1 * (1 - valid_rate) 120 | else: 121 | ef = valid_rate 122 | 123 | self.energy = self.energy + 0.5 * ef + 0.5 * np.tanh(delta_fitness / (1 - delta_coverage + 1e-10)) - 0.05 * 1 124 | 125 | def energy_init(self, delta_fitness, delta_coverage): 126 | """ 127 | delta_fitness: [0, 1] 0 is better 128 | delta_coverage: [0, 1] 1 is better 129 | """ 130 | # delta part max limit to 2 131 | # delta_part = (1.0 - delta_fitness) + delta_coverage 132 | delta_part = delta_fitness / 2.0 + delta_coverage / 2.0 133 | self.energy += delta_part 134 | 135 | def is_fail(self): 136 | fail_lst = [1, 3, 4, 6, 7, 14] 137 | case_events = self.scenario_recorder.get_case_event() 138 | for fail_item in fail_lst: 139 | if fail_item in case_events: 140 | return True 141 | return False 142 | -------------------------------------------------------------------------------- /behavexplor/models/BUILD: -------------------------------------------------------------------------------- 1 | load("@rules_python//python:defs.bzl", "py_binary", "py_library") 2 | 3 | package(default_visibility = ["//visibility:public"]) 4 | 5 | py_binary( 6 | name = "coverage", 7 | srcs = ["coverage.py"], 8 | deps= [ 9 | "//BehAVExplor/behavexplor/models:feature", 10 | ] 11 | ) 12 | 13 | py_binary( 14 | name = "feature", 15 | srcs = ["feature.py"], 16 | ) 17 | -------------------------------------------------------------------------------- /behavexplor/models/coverage.py: -------------------------------------------------------------------------------- 1 | import os 2 | import pickle 3 | import numpy as np 4 | 5 | from sklearn.cluster import KMeans 6 | 7 | from behavexplor.models.feature import FeatureNet 8 | 9 | class ClusterModelBehavior(object): 10 | def __init__(self, cluster_num): 11 | """ 12 | Initial cluster number 13 | """ 14 | self.cluster_model = KMeans(cluster_num) 15 | # @todo: add element data base to record each line 16 | self.cluster_center = [] 17 | self.cluster_data = None 18 | 19 | def search(self, v): 20 | """ 21 | @param: v is the query feature 22 | """ 23 | # v represents the behaviors of a single case 24 | # @format is numpy with shape (n, 64) 25 | # @output is numpy with shape (n, ) 26 | cls_labels = self.cluster_model.predict(v) 27 | # nearest_node = self.AI.get_nns_by_vector(v, 1, include_distances=True) 28 | # label(node id) & distance 29 | return cls_labels 30 | 31 | def update(self, v): 32 | """ 33 | Need to change to load all corpus and re-cluster 34 | """ 35 | # Step1: add new behavior data @format is numpy with shape (n, 64) 36 | if self.cluster_data is None: 37 | self.cluster_data = v 38 | else: 39 | self.cluster_data = np.concatenate([self.cluster_data, v], axis=0) 40 | # Step2: retrain kmeans model. @todo: might add dynamic cluster size 41 | y = self.cluster_model.fit_predict(self.cluster_data) # shape (n, ) 42 | return y 43 | 44 | def get_centers(self): 45 | return self.cluster_model.cluster_centers_ 46 | 47 | def save_model(self, save_path): 48 | f = open(save_path, 'wb') 49 | pickle.dump(self.__dict__, f, 2) 50 | f.close() 51 | 52 | class CoverageModel(object): 53 | 54 | def __init__(self, window_size, cluster_num, threshold_coverage): 55 | self.coverage_centers = [] 56 | self.coverage_centers_index = [] 57 | self.coverage_centers_pointer = 0 58 | 59 | self.window_size = window_size 60 | self.cluster_num = cluster_num 61 | self.threshold_coverage = threshold_coverage 62 | 63 | self.dynamic_threshold = np.inf 64 | 65 | self.feature_layer = FeatureNet(window_size) 66 | self.cluster_layer_behavior = ClusterModelBehavior(self.cluster_num) 67 | 68 | def extract_feature(self, x, x_scale, resample='linear'): 69 | y_behavior, y_trace = self.feature_layer.forward(x, x_scale, resample) 70 | return y_behavior, y_trace 71 | 72 | def initialize(self, X_behavior): 73 | """ 74 | X_behavior: list [item1, item2, ..., itemn] 75 | itemi : array [[x1...], [x2...]] 76 | X_trace: list [item1, item2, ..., itemn] 77 | itemi: list: [(x1, y1), (x2, y2), ..., (xn, yn)] 78 | """ 79 | # behavior model 80 | buffer_feature = None 81 | for i in range(len(X_behavior)): 82 | x = X_behavior[i] # shape (n, 64) 83 | if buffer_feature is None: 84 | buffer_feature = x 85 | else: 86 | buffer_feature = np.concatenate([buffer_feature, x], axis=0) 87 | 88 | self.coverage_centers_index.append([self.coverage_centers_pointer, 89 | self.coverage_centers_pointer + x.shape[0]]) 90 | self.coverage_centers_pointer += x.shape[0] 91 | 92 | # initial train 93 | y = self.cluster_layer_behavior.update(buffer_feature) # n x 64 94 | self.update(y) 95 | 96 | def update(self, y): 97 | self.coverage_centers = [] 98 | for item in self.coverage_centers_index: 99 | start_index = item[0] 100 | end_index = item[1] 101 | y_i = y[start_index:end_index] 102 | self.coverage_centers.append(y_i) 103 | 104 | self._update_threshold() 105 | 106 | def _update_threshold(self): 107 | pattern_num = len(self.coverage_centers) 108 | distance_matrix = np.zeros((pattern_num, pattern_num)) 109 | for i in range(pattern_num): 110 | distance_matrix[i][i] = 1000 111 | for j in range(i + 1, pattern_num): 112 | tmp_distance = self._compute_distance_behavior_states(self.coverage_centers[i], self.coverage_centers[j]) 113 | distance_matrix[i][j] = tmp_distance 114 | distance_matrix[j][i] = tmp_distance 115 | 116 | pattern_min_distance = [] 117 | for i in range(pattern_num): 118 | pattern_i_min = np.min(distance_matrix[i]) 119 | pattern_min_distance.append(pattern_i_min) 120 | pattern_min_distance = np.array(pattern_min_distance) 121 | self.dynamic_threshold = np.mean(pattern_min_distance) 122 | 123 | def feedback_coverage_behavior(self, x, is_fail): 124 | # @format x is the behavior extracted by the feature layer 125 | # @todo: modify this method 126 | y_behavior = self.cluster_layer_behavior.search(x) 127 | find_new_coverage = False 128 | min_feedback = np.inf 129 | for i in range(len(self.coverage_centers)): 130 | cov_feedback = self._compute_distance_behavior_states(y_behavior, self.coverage_centers[i]) 131 | if cov_feedback < min_feedback: 132 | min_feedback = cov_feedback 133 | if min_feedback > self.threshold_coverage: 134 | find_new_coverage = True 135 | if not is_fail: 136 | self.coverage_centers_index.append([self.coverage_centers_pointer, 137 | self.coverage_centers_pointer + x.shape[0]]) 138 | self.coverage_centers_pointer += x.shape[0] 139 | # update behavior model (kmeans) 140 | y = self.cluster_layer_behavior.update(x) 141 | # update existing centers 142 | self.update(y) 143 | 144 | return find_new_coverage, min_feedback, y_behavior 145 | 146 | 147 | @staticmethod 148 | def _compute_distance_behavior_states(y1, y2): 149 | """ 150 | y1 is a list 151 | """ 152 | # y is numpy 153 | y1_length = len(y1) 154 | y2_length = len(y2) 155 | 156 | coverage_score = abs(y1_length - y2_length) 157 | 158 | common_length = min(y1_length, y2_length) 159 | y1_common = y1[:common_length] 160 | y2_common = y2[:common_length] 161 | for i in range(common_length): 162 | y1_e = y1_common[i] 163 | y2_e = y2_common[i] 164 | if y1_e == y2_e: 165 | continue 166 | else: 167 | coverage_score += 1 168 | 169 | coverage_score /= float(max(y1_length, y2_length)) 170 | 171 | return coverage_score 172 | 173 | def get_centers(self): 174 | return self.coverage_centers 175 | 176 | def get_static_info(self): 177 | coverage_center_num = len(self.coverage_centers) 178 | behavioral_center_num = len(self.cluster_layer_behavior.get_centers()) 179 | return { 180 | 'coverage': coverage_center_num, 181 | 'behavior': behavioral_center_num, 182 | } 183 | 184 | def get_threshold_info(self): 185 | return { 186 | 'coverage': min(self.dynamic_threshold, self.threshold_coverage), 187 | } 188 | 189 | def get_dynamic_threshold_info(self): 190 | return { 191 | 'coverage': self.dynamic_threshold, 192 | } 193 | 194 | def save_model(self, save_folder): 195 | save_dict = {} 196 | for k, v in self.__dict__.items(): 197 | if k == 'cluster_layer_behavior': 198 | save_dict[k] = os.path.join(save_folder, 'behavior.pth') 199 | else: 200 | save_dict[k] = v 201 | 202 | f = open(os.path.join(save_folder, 'coverage.pth'), 'wb') 203 | pickle.dump(save_dict, f, 2) 204 | f.close() 205 | # save layer 206 | self.cluster_layer_behavior.save_model(os.path.join(save_folder, 'behavior.pth')) -------------------------------------------------------------------------------- /behavexplor/models/feature.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from tsfresh.feature_extraction import feature_calculators 4 | from scipy import interpolate 5 | 6 | def static_calculator(X): 7 | X_feature = [] 8 | attribution_size = X.shape[1] 9 | for attr_i in range(attribution_size): 10 | attribution_i = X[:, attr_i] 11 | mean = feature_calculators.mean(attribution_i) 12 | minimum = feature_calculators.minimum(attribution_i) 13 | maximum = feature_calculators.maximum(attribution_i) 14 | mean_change = feature_calculators.mean_change(attribution_i) 15 | mean_abs_change = feature_calculators.mean_abs_change(attribution_i) 16 | variance = feature_calculators.variance(attribution_i) 17 | c3 = feature_calculators.c3(attribution_i, 1) 18 | cid_ce = feature_calculators.cid_ce(attribution_i, True) 19 | attribution_i_feature = [mean, variance, minimum, maximum, mean_change, mean_abs_change, c3, cid_ce] 20 | 21 | X_feature += attribution_i_feature 22 | 23 | return X_feature 24 | 25 | class FeatureNet(object): 26 | 27 | def __init__(self, window_size=1): 28 | self.resample_frequency = 0 29 | self.window_size = window_size # unit is second (s) 30 | self.local_feature_extractor = None 31 | 32 | @staticmethod 33 | def input_select(x): 34 | # X: [T, N] T seg 0.1s 35 | # [time, speed, acc, ego_x, ego_y, ego_z, v_x, v_z, acc_x, acc_z, angular_x, angular_y, angular_z, rotation_x, rotation_y, rotation_z] 36 | trace_pos = np.concatenate([x[:, 0:1], x[:, 3:4], x[:, 5:6]], axis=1) # position 37 | # -> 38 | # [time, speed, acc, v_x, v_z, acc_x, acc_z, angular_y, rotation_y] 39 | behavior_vector = np.concatenate([x[:, 0:3], x[:, 6:10], x[:, 11:12], x[:, 14:15]], axis=1) 40 | return behavior_vector, trace_pos 41 | 42 | @staticmethod 43 | def input_resample(x, time_scale, resample='linear', sample_frequency=0.1): 44 | # [time, speed, acc, v_x, v_z, acc_x, acc_z, angular_y, rotation_y] 45 | assert resample in ['none', 'linear', 'quadratic', 'cubic'] 46 | if resample == 'none': 47 | new_x = x[:, 1:] 48 | else: 49 | # X: [T, N] T seg 0.1s 50 | resample_axis = np.arange(time_scale[0], time_scale[1], sample_frequency) 51 | time_axis = x[:, 0] 52 | new_x = [] 53 | for i in range(1, x.shape[1]): 54 | x_i = x[:, i] 55 | f_i = interpolate.interp1d(time_axis, x_i, kind=resample) 56 | new_x_i = f_i(resample_axis) 57 | new_x_i = np.append(new_x_i, x_i[-1]) 58 | new_x.append(new_x_i) 59 | new_x = np.array(new_x) 60 | new_x = new_x.T 61 | # new_x: 62 | # [speed, acc, v_x, v_z, acc_x, acc_z, angular_y, rotation_y] 63 | return new_x 64 | 65 | def forward(self, x, x_scale, resample='linear'): 66 | x_behavior_vector, x_trace_pos = self.input_select(x) 67 | x_behavior_vector = self.input_resample(x_behavior_vector, x_scale, resample) 68 | x_trace_pos = self.input_resample(x_trace_pos, x_scale, resample) 69 | 70 | if self.window_size <= 0: 71 | return x_behavior_vector, x_trace_pos 72 | time_size = x_behavior_vector.shape[0] 73 | if time_size < self.window_size: 74 | last_element = x_behavior_vector[-1:, :] 75 | for _ in range(self.window_size - time_size): 76 | x_behavior_vector = np.concatenate([x_behavior_vector, last_element], axis=0) 77 | 78 | y = [] 79 | for i in range(time_size - self.window_size + 1): 80 | x_segment = x_behavior_vector[i:i+self.window_size] 81 | x_feature = static_calculator(x_segment) 82 | y.append(x_feature) 83 | 84 | return np.array(y), x_trace_pos 85 | 86 | -------------------------------------------------------------------------------- /behavexplor/utils.py: -------------------------------------------------------------------------------- 1 | import os 2 | import shutil 3 | 4 | def clear_and_create(folder_path): 5 | if os.path.exists(folder_path): 6 | shutil.rmtree(folder_path) 7 | os.makedirs(folder_path) 8 | 9 | def create_path(output_path): 10 | clear_and_create(os.path.join(output_path, 'logs')) 11 | clear_and_create(os.path.join(output_path, 'models')) 12 | clear_and_create(os.path.join(output_path, 'simulation/records_lgsvl')) 13 | clear_and_create(os.path.join(output_path, 'simulation/records_apollo')) 14 | clear_and_create(os.path.join(output_path, 'simulation/scenarios')) -------------------------------------------------------------------------------- /common/BUILD: -------------------------------------------------------------------------------- 1 | load("@rules_python//python:defs.bzl", "py_binary", "py_library") 2 | 3 | package(default_visibility = ["//visibility:public"]) 4 | 5 | py_binary( 6 | name = "frame", 7 | srcs = ["frame.py"], 8 | deps= [ 9 | "//BehAVExplor/common:utils", 10 | ] 11 | ) 12 | 13 | py_binary( 14 | name = "runner", 15 | srcs = ["runner.py"], 16 | ) 17 | 18 | py_binary( 19 | name = "scenario", 20 | srcs = ["scenario.py"], 21 | deps= [ 22 | "//BehAVExplor/common:utils", 23 | ] 24 | ) 25 | 26 | py_binary( 27 | name = "simulator", 28 | srcs = ["simulator.py"], 29 | deps= [ 30 | "//BehAVExplor/common:utils", 31 | "//BehAVExplor/common:frame", 32 | ] 33 | ) 34 | 35 | 36 | py_binary( 37 | name = "utils", 38 | srcs = ["utils.py"], 39 | ) 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /common/runner.py: -------------------------------------------------------------------------------- 1 | import os 2 | import pickle 3 | import shutil 4 | 5 | from loguru import logger 6 | 7 | def clear_and_create(folder_path): 8 | if os.path.exists(folder_path): 9 | shutil.rmtree(folder_path) 10 | os.makedirs(folder_path) 11 | 12 | class Runner(object): 13 | 14 | def __init__(self, output_path, simulator): 15 | 16 | self.global_id = 0 17 | 18 | self.SCENARIO_FOLDER = 'scenarios' 19 | self.RECORD_LGSVL_FOLDER = 'records_lgsvl' 20 | self.RECORD_APOLLO_FOLDER = 'records_apollo' 21 | 22 | self.scenario_path = os.path.join(output_path, 'simulation', self.SCENARIO_FOLDER) 23 | self.record_lgsvl_path = os.path.join(output_path, 'simulation', self.RECORD_LGSVL_FOLDER) 24 | self.record_apollo_path = os.path.join(output_path, 'simulation', self.RECORD_APOLLO_FOLDER) 25 | 26 | clear_and_create(self.scenario_path) 27 | clear_and_create(self.record_lgsvl_path) 28 | clear_and_create(self.record_apollo_path) # self.record_path 29 | 30 | self.sim = simulator # save record to records/scenario_name/scenario_id 31 | 32 | self.runner_log = os.path.join(output_path, 'logs/runner.log') 33 | if os.path.exists(self.runner_log): 34 | os.remove(self.runner_log) 35 | 36 | def run(self, scenario_obj, scenario_id=None): 37 | 38 | if scenario_id is None: 39 | scenario_id = 'scenario_' + str(self.global_id) 40 | 41 | scenario_file = os.path.join(self.scenario_path, scenario_id + '.obj') 42 | with open(scenario_file, 'wb') as s_f: 43 | pickle.dump(scenario_obj, s_f) 44 | 45 | record_lgsvl_file = os.path.join(self.record_lgsvl_path, scenario_id + '.obj') 46 | 47 | if os.path.isfile(record_lgsvl_file): 48 | os.remove(record_lgsvl_file) 49 | 50 | while True: 51 | try: 52 | sim_recorder = self.sim.run(scenario_obj, scenario_id, self.record_apollo_path) 53 | except Exception as e: 54 | logger.error('Simulator may has some unexpected error:') 55 | logger.error(' ' + str(e)) 56 | else: 57 | break 58 | 59 | # sim_recorder = self.sim.run(scenario_obj, scenario_id, self.record_apollo_path) 60 | 61 | with open(record_lgsvl_file, 'wb') as f: 62 | pickle.dump(sim_recorder, f) 63 | 64 | sim_events = sim_recorder.obtain_case_event_str() 65 | sim_fitness = sim_recorder.get_case_fitness() 66 | with open(self.runner_log, 'a') as f: 67 | f.write(str(scenario_id) + ' ') 68 | f.write(str(sim_events) + ' ') 69 | f.write(str(list(sim_fitness))) 70 | f.write('\n') 71 | 72 | self.global_id += 1 73 | 74 | logger.info('[Runner] Simulation Result: ') 75 | logger.info('[Runner] Events: ' + str(sim_events)) 76 | logger.info('[Runner] Fitness: ' + str(list(sim_fitness))) 77 | 78 | return sim_recorder, scenario_id 79 | 80 | def close(self): 81 | self.sim.close() -------------------------------------------------------------------------------- /common/scenario.py: -------------------------------------------------------------------------------- 1 | import copy 2 | import random 3 | import numpy as np 4 | 5 | from loguru import logger 6 | 7 | from common import utils 8 | 9 | NPC_AGENT_TYPES = ["Sedan", "SUV", "Jeep", "Hatchback", "SchoolBus"] 10 | 11 | offset_fix_value = 2.5 12 | min_speed_limit = 0.2 13 | 14 | class Scenario(object): 15 | 16 | def __init__(self): 17 | self.ego = None # dict 18 | 19 | # init npc para 20 | self.waypoint_num = None 21 | self.npc_size = None 22 | self.npc_types = [] 23 | self.npc_routes = [] 24 | self.npc_routes_ids = [] 25 | self.npc_waypoints = [] #[[[]]] 26 | 27 | # other oracle 28 | self.lanes = None 29 | self.yellow_lines = [] # [[line_info]] 30 | self.edge_lines = [] # [[line_info]] 31 | self.cross_lines = [] # [[line_info]] 32 | self.routes = [] # ids 33 | self.route_details = [] 34 | 35 | # environment 36 | self.environment = None 37 | 38 | # offset recorder 39 | self.ego_offset_recorder = None 40 | self.current_offset_recorder = None 41 | 42 | def generate_specific_info_wo_npc(self, basic_info, route_info): 43 | 44 | lane_details = route_info['lane_details'] 45 | # obtain legal_lines 46 | ego_start_lane_id = basic_info['ego']['start']['lane_id'] 47 | ego_dest_lane_id = basic_info['ego']['destination']['lane_id'] 48 | 49 | ego_lanes_in = [ego_start_lane_id] 50 | ego_lanes_out = [ego_dest_lane_id] 51 | ego_lanes_mid = [] 52 | 53 | lane_data = lane_details[ego_start_lane_id] 54 | for item in lane_data['right_neighbor_lane']: 55 | if item['id'] not in lane_details.keys(): 56 | continue 57 | ego_lanes_in.append(item['id']) 58 | for item in lane_data['left_neighbor_lane']: 59 | if item['id'] not in lane_details.keys(): 60 | continue 61 | ego_lanes_in.append(item['id']) 62 | 63 | lane_data = lane_details[ego_dest_lane_id] 64 | for item in lane_data['right_neighbor_lane']: 65 | if item['id'] not in lane_details.keys(): 66 | continue 67 | ego_lanes_out.append(item['id']) 68 | for item in lane_data['left_neighbor_lane']: 69 | if item['id'] not in lane_details.keys(): 70 | continue 71 | ego_lanes_out.append(item['id']) 72 | 73 | for lane_id in ego_lanes_in: 74 | # TODO: need check 75 | if lane_id not in lane_details.keys(): 76 | continue 77 | successors = lane_details[lane_id]['successor'] 78 | for j in range(len(successors)): 79 | successor_j = successors[j]['id'] 80 | if successor_j not in lane_details.keys(): 81 | continue 82 | successor_j_data = lane_details[successor_j] 83 | for j_s in successor_j_data['successor']: 84 | if j_s['id'] in ego_lanes_out: 85 | ego_lanes_mid.append(successor_j) 86 | 87 | ego_lanes = ego_lanes_in 88 | ego_lanes += ego_lanes_out 89 | ego_lanes += ego_lanes_mid 90 | ego_lanes = list(set(ego_lanes)) 91 | logger.info('Ego lanes: ' + str(ego_lanes)) 92 | 93 | for lane_id in ego_lanes: 94 | lane_data = lane_details[lane_id] 95 | lane_turn = lane_data['turn'] 96 | # turn: NO_TURN/RIGHT_TURN/LEFT_TURN 97 | # left boundary 98 | left_boundary = lane_data['left_boundary'] 99 | left_boundary_type = left_boundary['type'][0] 100 | left_boundary_points = left_boundary['points'] 101 | if left_boundary_type == 'CURB': 102 | self.edge_lines.append(left_boundary_points) 103 | elif left_boundary_type == 'DOUBLE_YELLOW': 104 | self.yellow_lines.append(left_boundary_points) 105 | elif left_boundary_type == 'DOTTED_WHITE': 106 | if lane_id not in ego_lanes_mid: 107 | self.cross_lines.append(left_boundary_points) 108 | else: 109 | raise RuntimeError('No lane type: ' + str(left_boundary_type)) 110 | 111 | # right boundary 112 | right_boundary = lane_data['right_boundary'] 113 | right_boundary_type = right_boundary['type'][0] 114 | right_boundary_points = right_boundary['points'] 115 | if right_boundary_type == 'CURB': 116 | self.edge_lines.append(right_boundary_points) 117 | elif right_boundary_type == 'DOUBLE_YELLOW': 118 | self.yellow_lines.append(right_boundary_points) 119 | elif right_boundary_type == 'DOTTED_WHITE': 120 | if lane_id not in ego_lanes_mid: 121 | self.cross_lines.append(right_boundary_points) 122 | else: 123 | raise RuntimeError('No lane type: ' + str(right_boundary_type)) 124 | 125 | self.ego = basic_info['ego'] 126 | self.lanes = route_info['lane_details'] 127 | self.route_details = route_info['route_details'] 128 | self.routes = route_info['routes'] # route id 129 | self.waypoint_num = basic_info['npcs']['waypoint'] 130 | self.npc_size = basic_info['npcs']['npc_num'] 131 | self.ego_offset_recorder = {self.ego['start']['lane_id']: [ 132 | [[offset_fix_value, self.lanes[self.ego['start']['lane_id']]['central']['length'] - offset_fix_value]], 133 | [[-200, self.ego['start']['offset'] - 15], [self.ego['start']['offset'] + 15, 9999]] 134 | ]} 135 | self.environment = basic_info['environment'] 136 | logger.info('[Scenario] OK - Generate specific scenario wo NPCs.') 137 | 138 | def mutation_route(self): 139 | offset_recorder = copy.deepcopy(self.ego_offset_recorder) 140 | for i in range(self.npc_size): 141 | if random.random() > 0.5: 142 | self.npc_types[i] = random.choice(NPC_AGENT_TYPES) 143 | self.npc_routes_ids[i] = random.choice(self.routes) 144 | self.npc_routes[i] = self.route_details[self.npc_routes_ids[i]] 145 | 146 | npc_i_route = self.npc_routes[i] 147 | npc_i_offset_speed = [] 148 | for _ in range(len(npc_i_route)): 149 | for _ in range(self.waypoint_num): 150 | npc_i_offset_speed.append([]) 151 | npc_i_offset_speed, offset_recorder = self._uniform_mutation(npc_i_route, offset_recorder) 152 | self.npc_waypoints[i] = npc_i_offset_speed 153 | 154 | def generate_random_abstract_scenario(self): 155 | self.npc_routes = [] 156 | self.npc_types = [] 157 | self.npc_routes_ids = [] 158 | for i in range(self.npc_size): 159 | npc_i_type = random.choice(NPC_AGENT_TYPES) 160 | self.npc_types.append(npc_i_type) 161 | 162 | npc_i_route_id = random.choice(self.routes) 163 | self.npc_routes_ids.append(npc_i_route_id) 164 | 165 | npc_i_route = self.route_details[npc_i_route_id] 166 | self.npc_routes.append(npc_i_route) 167 | # logger.info('[Scenario] OK - Generate random npc configure.') 168 | 169 | def generate_random_concrete_scenario(self): 170 | self.npc_waypoints = [] 171 | offset_recorder = copy.deepcopy(self.ego_offset_recorder) 172 | 173 | for i in range(self.npc_size): 174 | npc_i_route = self.npc_routes[i] 175 | npc_i_offset_speed = [] 176 | for _ in range(len(npc_i_route)): 177 | for _ in range(self.waypoint_num): 178 | npc_i_offset_speed.append([]) 179 | 180 | npc_i_offset_speed, offset_recorder = self._uniform_mutation(npc_i_route, offset_recorder) 181 | self.npc_waypoints.append(npc_i_offset_speed) 182 | 183 | def mutation_uniform(self): 184 | # mutate_npc_types = copy.deepcopy(self.npc_types) 185 | mutate_npc_routes = copy.deepcopy(self.npc_routes) 186 | mutate_npc_routes_ids = copy.deepcopy(self.npc_routes_ids) 187 | mutate_npc_offset_speed = copy.deepcopy(self.npc_waypoints) 188 | offset_recorder = copy.deepcopy(self.ego_offset_recorder) 189 | 190 | # mutate structure & type - drop 191 | # adjust offset & speed 192 | for npc_id in range(self.npc_size): 193 | npc_i_route = mutate_npc_routes[npc_id] 194 | npc_i_offset_speed, offset_recorder = self._uniform_mutation(npc_i_route, offset_recorder) 195 | mutate_npc_offset_speed[npc_id] = npc_i_offset_speed 196 | 197 | self.npc_routes = mutate_npc_routes 198 | self.npc_routes_ids = mutate_npc_routes_ids 199 | self.npc_waypoints = mutate_npc_offset_speed 200 | logger.info('[Scenario] OK - Generate mutated NPC configure with mutation_normal') 201 | 202 | def mutation_gauss(self, offset_mutation, speed_mutation): 203 | # mutate_npc_types = copy.deepcopy(self.npc_types) 204 | mutate_npc_routes = copy.deepcopy(self.npc_routes) 205 | mutate_npc_routes_ids = copy.deepcopy(self.npc_routes_ids) 206 | mutate_npc_offset_speed = copy.deepcopy(self.npc_waypoints) 207 | offset_recorder = copy.deepcopy(self.ego_offset_recorder) 208 | 209 | # mutate structure & type - drop 210 | # adjust offset & speed 211 | for npc_id in range(self.npc_size): 212 | npc_i_route = mutate_npc_routes[npc_id] 213 | npc_i_offset_speed = mutate_npc_offset_speed[npc_id] 214 | npc_i_offset_speed, offset_recorder = self._gauss_mutation(npc_i_route, npc_i_offset_speed, offset_recorder, offset_mutation, speed_mutation) 215 | mutate_npc_offset_speed[npc_id] = npc_i_offset_speed 216 | 217 | self.npc_routes = mutate_npc_routes 218 | self.npc_routes_ids = mutate_npc_routes_ids 219 | self.npc_waypoints = mutate_npc_offset_speed 220 | logger.info('[Scenario] OK - Generate mutated NPC configure with mutation_normal') 221 | 222 | def _gauss_mutation(self, npc_i_route, npc_i_offset_speed, offset_recorder, offset_mutation, speed_mutation): 223 | last_lane_id = None 224 | last_offset = None 225 | last_trace_length = 0.0 226 | # TODO:Fix not the same length of speed & offset 227 | wp_length = len(npc_i_route) * self.waypoint_num 228 | assert wp_length == len(npc_i_offset_speed) 229 | 230 | for wp_i in range(len(npc_i_offset_speed)): 231 | 232 | current_offset = npc_i_offset_speed[wp_i][0] 233 | current_speed = npc_i_offset_speed[wp_i][1] 234 | 235 | wp_i_lane_id_index = wp_i // self.waypoint_num 236 | 237 | wp_i_lane_id = npc_i_route[wp_i_lane_id_index] 238 | wp_i_lane_length = self.lanes[wp_i_lane_id]['central']['length'] 239 | wp_i_lane_speed_limit = self.lanes[wp_i_lane_id]['speed_limit'] 240 | 241 | if random.random() < 0.5: 242 | mutate_speed = current_speed 243 | mutate_offset = current_offset 244 | else: 245 | mutate_speed = random.gauss(current_speed, speed_mutation) 246 | mutate_offset = random.gauss(current_offset, offset_mutation) 247 | 248 | if wp_i == 0: # start 249 | mutate_speed = float(np.clip(mutate_speed, min_speed_limit, wp_i_lane_speed_limit)) 250 | mutate_offset, offset_recorder = utils.check_relocate(offset_recorder, mutate_offset, 251 | wp_i_lane_id, offset_fix_value, 252 | wp_i_lane_length - offset_fix_value) 253 | elif wp_i == len(npc_i_offset_speed) - 1: # destination 254 | mutate_speed = 0.0 255 | if wp_i_lane_id == last_lane_id: 256 | mutate_offset = float(np.clip(mutate_offset, last_offset, wp_i_lane_length + last_trace_length)) 257 | else: 258 | last_trace_length += self.lanes[last_lane_id]['central']['length'] 259 | mutate_offset = float(np.clip(mutate_offset, last_trace_length, wp_i_lane_length + last_trace_length)) 260 | else: 261 | mutate_speed = float(np.clip(mutate_speed, min_speed_limit, wp_i_lane_speed_limit)) 262 | if wp_i_lane_id == last_lane_id: 263 | mutate_offset = float(np.clip(mutate_offset, last_offset, wp_i_lane_length + last_trace_length)) 264 | else: 265 | last_trace_length += self.lanes[last_lane_id]['central']['length'] 266 | mutate_offset = float(np.clip(mutate_offset, last_trace_length, wp_i_lane_length + last_trace_length)) 267 | 268 | npc_i_offset_speed[wp_i][0] = mutate_offset 269 | npc_i_offset_speed[wp_i][1] = mutate_speed 270 | 271 | last_lane_id = wp_i_lane_id 272 | last_offset = mutate_offset 273 | return npc_i_offset_speed, offset_recorder 274 | 275 | def _uniform_mutation(self, npc_i_route, offset_recorder): 276 | 277 | npc_i_offset_speed = [] 278 | for _ in range(len(npc_i_route)): 279 | for _ in range(self.waypoint_num): 280 | npc_i_offset_speed.append([]) 281 | 282 | waypoint_id = 0 283 | last_trace_length = 0.0 284 | last_offset = None 285 | last_lane_id = None 286 | for route_index in range(len(npc_i_route)): 287 | route_lane_id = npc_i_route[route_index] 288 | lane_data = self.lanes[route_lane_id] 289 | lane_length = lane_data['central']['length'] 290 | lane_speed_limit = lane_data['speed_limit'] 291 | 292 | for w_i in range(self.waypoint_num): 293 | if waypoint_id == 0: # start lane & offset & speed 294 | delta_start_offset = random.uniform(offset_fix_value, lane_length - offset_fix_value) 295 | npc_offset, offset_recorder = utils.check_relocate(offset_recorder, delta_start_offset, 296 | route_lane_id, offset_fix_value, 297 | lane_length - offset_fix_value) 298 | npc_speed = random.uniform(min_speed_limit, lane_speed_limit) 299 | elif waypoint_id == len(npc_i_offset_speed) - 1: # end lane & offset & speed 300 | npc_speed = 0.0 301 | if route_lane_id == last_lane_id: 302 | npc_offset = random.uniform(last_offset, lane_length + last_trace_length) 303 | else: 304 | last_trace_length += self.lanes[last_lane_id]['central']['length'] 305 | npc_offset = random.uniform(last_trace_length, lane_length + last_trace_length) 306 | else: 307 | npc_speed = random.uniform(min_speed_limit, lane_speed_limit) 308 | if route_lane_id == last_lane_id: 309 | npc_offset = random.uniform(last_offset, lane_length + last_trace_length) 310 | else: 311 | last_trace_length += self.lanes[last_lane_id]['central']['length'] 312 | npc_offset = random.uniform(last_trace_length, lane_length + last_trace_length) 313 | 314 | last_offset = npc_offset 315 | 316 | npc_i_offset_speed[waypoint_id] = [npc_offset, npc_speed] 317 | waypoint_id += 1 318 | 319 | last_lane_id = route_lane_id 320 | 321 | assert waypoint_id == len(npc_i_offset_speed) 322 | 323 | return npc_i_offset_speed, offset_recorder 324 | 325 | def get_yellow_lines(self): 326 | return self.yellow_lines 327 | 328 | def get_cross_lines(self): 329 | return self.cross_lines 330 | 331 | def get_edge_lines(self): 332 | return self.edge_lines 333 | 334 | def get_ego(self): 335 | return self.ego 336 | 337 | def get_npcs(self): 338 | return copy.deepcopy(self.npc_types), copy.deepcopy(self.npc_routes_ids), copy.deepcopy(self.npc_routes), copy.deepcopy(self.npc_waypoints) 339 | 340 | def get_scenario_structure(self): 341 | return copy.deepcopy(self.npc_routes_ids) 342 | 343 | def get_route_list(self): 344 | return self.routes 345 | 346 | def get_npc_num(self): 347 | return self.npc_size 348 | 349 | def get_lgsvl_input(self): 350 | lgsvl_input = {'ego': self.ego, 351 | 'ego_id': self.ego['agent_type'], 352 | 'npc_types': self.npc_types, 353 | 'npc_waypoints': self.npc_waypoints, 354 | 'npc_lanes_to_follow': self.npc_routes, 355 | 'npc_size': self.npc_size, 356 | 'lanes': self.lanes, 357 | 'yellow_lines': self.yellow_lines, 358 | 'cross_lines': self.cross_lines, 359 | 'edge_lines': self.edge_lines, 360 | 'environment': self.environment 361 | } 362 | 363 | return copy.deepcopy(lgsvl_input) 364 | -------------------------------------------------------------------------------- /common/utils.py: -------------------------------------------------------------------------------- 1 | import os 2 | import time 3 | import math 4 | import numpy as np 5 | 6 | from shapely.geometry import Polygon 7 | from loguru import logger 8 | 9 | def close_modules(dv, modules): 10 | not_all = True 11 | while not_all: 12 | not_all = False 13 | module_status = dv.get_module_status() 14 | for module, status in module_status.items(): 15 | if status and (module in modules): 16 | dv.disable_module(module) 17 | not_all = True 18 | time.sleep(0.5) 19 | 20 | def start_recorder(output_path): 21 | 22 | os.system('nohup cyber_recorder record -a -o ' + output_path + ' &') 23 | os.system('ps -ef|grep cyber_recorder|grep -v grep > check_recorder.txt') 24 | 25 | file_size = int(os.path.getsize('check_recorder.txt')) 26 | while file_size == 0: 27 | os.system('nohup cyber_recorder record -a -o ' + output_path + ' &') 28 | os.system('ps -ef|grep cyber_recorder|grep -v grep > check_recorder.txt') 29 | file_size = int(os.path.getsize('check_recorder.txt')) 30 | 31 | def stop_recorder(): 32 | os.system("kill $(ps -ef|grep cyber_recorder | grep -v grep | awk {'print$2'})") 33 | 34 | def calc_abc_from_line_2d(x0, y0, x1, y1): 35 | a = y0 - y1 36 | b = x1 - x0 37 | c = x0 * y1 - x1 * y0 38 | return a, b, c 39 | 40 | def get_line_cross_point(line1, line2): 41 | a0, b0, c0 = calc_abc_from_line_2d(*line1) 42 | a1, b1, c1 = calc_abc_from_line_2d(*line2) 43 | D = a0 * b1 - a1 * b0 44 | if D == 0: 45 | return None 46 | x = (b0 * c1 - b1 * c0) / D 47 | y = (a1 * c0 - a0 * c1) / D 48 | return x, y 49 | 50 | def right_rotation(coord, theta): 51 | """ 52 | theta : degree 53 | """ 54 | theta = math.radians(theta) 55 | x = coord[1] 56 | y = coord[0] 57 | x1 = x * math.cos(theta) - y * math.sin(theta) 58 | y1 = x * math.sin(theta) + y * math.cos(theta) 59 | return [y1, x1] 60 | 61 | def ego_speed_converter(agent_state): 62 | """ 63 | return np.array ([head, lateral]) 64 | """ 65 | agent_theta = agent_state.transform.rotation.y 66 | velocity_x = np.array([agent_state.velocity.x, 0]) 67 | velocity_z = np.array([0, agent_state.velocity.z]) 68 | 69 | head_vector = [1, 0] 70 | lateral_vector = [0, 1] 71 | 72 | head_vector = np.array(right_rotation(head_vector, agent_theta)) 73 | lateral_vector = np.array(right_rotation(lateral_vector, agent_theta)) 74 | 75 | velocity_head = np.dot(head_vector, velocity_x) + np.dot(head_vector, velocity_z) 76 | velocity_lateral = np.dot(lateral_vector, velocity_x) + np.dot(lateral_vector, velocity_z) 77 | 78 | return np.array([velocity_head, velocity_lateral]) 79 | 80 | def ego_accelerate_converter(agent_state, acceleration): 81 | """ 82 | acceleration: [x, z] 83 | return np.array ([head, lateral]) 84 | """ 85 | agent_theta = agent_state.transform.rotation.y 86 | acceleration_x = np.array([acceleration[0], 0]) 87 | acceleration_z = np.array([0, acceleration[1]]) 88 | 89 | head_vector = [1, 0] 90 | lateral_vector = [0, 1] 91 | 92 | head_vector = np.array(right_rotation(head_vector, agent_theta)) 93 | lateral_vector = np.array(right_rotation(lateral_vector, agent_theta)) 94 | 95 | acceleration_head = np.dot(head_vector, acceleration_x) + np.dot(head_vector, acceleration_z) 96 | acceleration_lateral = np.dot(lateral_vector, acceleration_x) + np.dot(lateral_vector, acceleration_z) 97 | 98 | return np.array([acceleration_head, acceleration_lateral]) 99 | 100 | def get_bbox(agent_state, agent_bbox): 101 | agent_theta = agent_state.transform.rotation.y 102 | #agent_bbox min max (x_min, y_min, z_min) (x_max, y_max, z_max) 103 | 104 | global_x = agent_state.transform.position.x 105 | global_z = agent_state.transform.position.z 106 | x_min = agent_bbox.min.x + 0.1 107 | x_max = agent_bbox.max.x - 0.1 108 | z_min = agent_bbox.min.z + 0.1 109 | z_max = agent_bbox.max.z - 0.1 110 | 111 | line1 = [x_min, z_min, x_max, z_max] 112 | line2 = [x_min, z_max, x_max, z_min] 113 | x_center, z_center = get_line_cross_point(line1, line2) 114 | 115 | coords = [[x_min, z_min], [x_max, z_min], [x_max, z_max], [x_min, z_max]] 116 | new_coords = [] 117 | 118 | for i in range(len(coords)): 119 | coord_i = coords[i] 120 | coord_i[0] = coord_i[0] - x_center 121 | coord_i[1] = coord_i[1] - z_center 122 | new_coord_i = right_rotation(coord_i, agent_theta) 123 | new_coord_i[0] += global_x 124 | new_coord_i[1] += global_z 125 | new_coords.append(new_coord_i) 126 | p1, p2, p3, p4 = new_coords[0], new_coords[1], new_coords[2], new_coords[3] 127 | 128 | agent_poly = Polygon((p1, p2, p3, p4)) 129 | if agent_poly.area <= 0: 130 | logger.warning('agent_poly area <= 0') 131 | return agent_poly 132 | 133 | 134 | def compute_start_position(lane_start, lane_end, lane_length, offset): 135 | """ 136 | lane_start: transform obj 137 | """ 138 | if offset == 0: 139 | x = lane_start.position.x 140 | z = lane_start.position.z 141 | else: 142 | v_x = lane_end.position.x - lane_start.position.x 143 | v_z = lane_end.position.z - lane_start.position.z 144 | ratio = offset / (lane_length + 0.0) 145 | x = lane_start.position.x + ratio * v_x 146 | z = lane_start.position.z + ratio * v_z 147 | 148 | return {'x': x, 'z': z} 149 | 150 | def interval_intersection(A, B): 151 | ans = [] 152 | i = j = 0 153 | 154 | while i < len(A) and j < len(B): 155 | # Let's check if A[i] intersects B[j]. 156 | # lo - the startpoint of the intersection 157 | # hi - the endpoint of the intersection 158 | lo = max(A[i][0], B[j][0]) 159 | hi = min(A[i][1], B[j][1]) 160 | if lo <= hi: 161 | ans.append([lo, hi]) 162 | 163 | # Remove the interval with the smallest endpoint 164 | if A[i][1] < B[j][1]: 165 | i += 1 166 | else: 167 | j += 1 168 | 169 | return ans 170 | 171 | def check_relocate(offset_recorder, offset, lane_id, offset_min, offset_max): 172 | 173 | if not lane_id in offset_recorder.keys(): 174 | lane_interval = [[offset_min, offset_max]] 175 | offset_recorder[lane_id] = [] 176 | offset_recorder[lane_id].append(lane_interval) 177 | offset_recorder[lane_id].append([[-200, offset - 8], [offset + 8, 9999]]) 178 | relocated_offset = float(np.clip(offset, offset_min, offset_max)) 179 | 180 | return relocated_offset, offset_recorder 181 | else: 182 | 183 | lane_offsets = offset_recorder[lane_id] 184 | available_interval = [] 185 | for i in range(1, len(lane_offsets)): 186 | if i == 1: 187 | available_interval = interval_intersection(lane_offsets[i], lane_offsets[i - 1]) 188 | else: 189 | available_interval = interval_intersection(available_interval, lane_offsets[i]) 190 | 191 | if len(available_interval) <= 0: 192 | logger.warning( 193 | '[INPUT CORPUS] Please check the map and location of the NPCs. The scenario config is not suitable.') 194 | 195 | min_dist = 99999 196 | interval_index = -1 197 | for i in range(len(available_interval)): 198 | # compute nearest interval 199 | dist = min([abs(offset - available_interval[i][0]), abs(offset - available_interval[i][1])]) 200 | if dist < min_dist: 201 | min_dist = dist 202 | interval_index = i 203 | relocate_interval = available_interval[interval_index] 204 | relocated_offset = float(np.clip(offset, relocate_interval[0], relocate_interval[1])) 205 | offset_recorder[lane_id].append([[-200, relocated_offset - 8], [relocated_offset + 8, 9999]]) 206 | 207 | return relocated_offset, offset_recorder 208 | 209 | -------------------------------------------------------------------------------- /configs/s1.yaml: -------------------------------------------------------------------------------- 1 | test_method: becov 2 | total_test_time: 43200 # 12h 3 | ego_behavior: 0 # 0, 1, 2 4 | 5 | # path config 6 | output_path: /apollo/BehAVExplor/outputs/becov/s1 7 | scenario_basic_file: /apollo/BehAVExplor/data/junction/junction_basic_info.json 8 | scenario_route_file: /apollo/BehAVExplor/data/junction/junction_routes.json 9 | map_data: /apollo/BehAVExplor/data/map/SanFrancisco.json 10 | 11 | # simulator 12 | max_sim_time: 200 13 | lgsvl_map: SanFrancisco_correct 14 | apollo_map: SanFrancisco 15 | sim_mode: async # [tick or async] 16 | 17 | # model 18 | window_size: 10 19 | cluster_num: 10 20 | threshold_coverage: 0.4 21 | feature_resample: linear 22 | 23 | # fuzzer 24 | init_seed_size: 4 25 | offset_mutation: 12 26 | speed_mutation: 6 27 | child_size: 1 -------------------------------------------------------------------------------- /configs/s2.yaml: -------------------------------------------------------------------------------- 1 | test_method: becov 2 | total_test_time: 43200 # 12h 3 | ego_behavior: 1 # 0, 1, 2 4 | 5 | # path config 6 | output_path: /apollo/BehAVExplor/outputs/becov/s2 7 | scenario_basic_file: /apollo/BehAVExplor/data/junction/junction_basic_info.json 8 | scenario_route_file: /apollo/BehAVExplor/data/junction/junction_routes.json 9 | map_data: /apollo/BehAVExplor/data/map/SanFrancisco.json 10 | 11 | # simulator 12 | max_sim_time: 200 13 | lgsvl_map: SanFrancisco_correct 14 | apollo_map: SanFrancisco 15 | sim_mode: async # [tick or async] 16 | 17 | # model 18 | window_size: 10 19 | cluster_num: 10 20 | threshold_coverage: 0.4 21 | feature_resample: linear 22 | 23 | # fuzzer 24 | init_seed_size: 4 25 | offset_mutation: 12 26 | speed_mutation: 6 27 | child_size: 1 -------------------------------------------------------------------------------- /configs/s3.yaml: -------------------------------------------------------------------------------- 1 | test_method: becov 2 | total_test_time: 43200 # 12h 3 | ego_behavior: 0 # 0, 1 4 | 5 | # path config 6 | output_path: /apollo/BehAVExplor/outputs/becov/s3 7 | scenario_basic_file: /apollo/BehAVExplor/data/straight/straight_basic_info.json 8 | scenario_route_file: /apollo/BehAVExplor/data/straight/straight_routes.json 9 | map_data: /apollo/BehAVExplor/data/map/SanFrancisco.json 10 | 11 | # simulator 12 | max_sim_time: 200 13 | lgsvl_map: SanFrancisco_correct 14 | apollo_map: SanFrancisco 15 | sim_mode: async # [tick or async] 16 | 17 | # model 18 | window_size: 10 19 | cluster_num: 10 20 | threshold_coverage: 0.4 21 | feature_resample: linear 22 | 23 | # fuzzer 24 | init_seed_size: 4 25 | offset_mutation: 12 26 | speed_mutation: 6 27 | child_size: 1 -------------------------------------------------------------------------------- /configs/s4.yaml: -------------------------------------------------------------------------------- 1 | test_method: becov 2 | total_test_time: 43200 # 12h 3 | ego_behavior: 1 # 0, 1 4 | 5 | # path config 6 | output_path: /apollo/BehAVExplor/outputs/becov/s4 7 | scenario_basic_file: /apollo/BehAVExplor/data/straight/straight_basic_info.json 8 | scenario_route_file: /apollo/BehAVExplor/data/straight/straight_routes.json 9 | map_data: /apollo/BehAVExplor/data/map/SanFrancisco.json 10 | 11 | # simulator 12 | max_sim_time: 200 13 | lgsvl_map: SanFrancisco_correct 14 | apollo_map: SanFrancisco 15 | sim_mode: async # [tick or async] 16 | 17 | # model 18 | window_size: 10 19 | cluster_num: 10 20 | threshold_coverage: 0.4 21 | feature_resample: linear 22 | 23 | # fuzzer 24 | init_seed_size: 4 25 | offset_mutation: 12 26 | speed_mutation: 6 27 | child_size: 1 -------------------------------------------------------------------------------- /data/SanFrancisco.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingfeiCheng/BehAVExplor/4124436fe38643196fea675acfcd163d3b369b75/data/SanFrancisco.zip -------------------------------------------------------------------------------- /data/junction/junction_basic_info.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "name": "junction_straight", 4 | "map": "SanFrancisco_correct", 5 | 6 | "ego": { 7 | "agent_type": "2e966a70-4a19-44b5-a5e7-64e00a7bc5de", 8 | "start": { 9 | "lane_id": "lane_477", 10 | "offset": 20 11 | }, 12 | "destination": { 13 | "lane_id": "lane_570", 14 | "offset": 60 15 | } 16 | }, 17 | 18 | "npcs": { 19 | "waypoint": 2, 20 | "npc_num": 5 21 | }, 22 | 23 | "environment": { 24 | "rain": 0, 25 | "fog": 0, 26 | "wetness": 0, 27 | "cloudiness": 0, 28 | "damage": 0, 29 | "time": 0 30 | } 31 | }, 32 | { 33 | "name": "junction_right", 34 | "map": "SanFrancisco_correct", 35 | 36 | "ego": { 37 | "agent_type": "2e966a70-4a19-44b5-a5e7-64e00a7bc5de", 38 | "start": { 39 | "lane_id": "lane_569", 40 | "offset": 10 41 | }, 42 | "destination": { 43 | "lane_id": "lane_557", 44 | "offset": 60 45 | } 46 | }, 47 | 48 | "npcs": { 49 | "waypoint": 2, 50 | "npc_num": 5 51 | }, 52 | 53 | "environment": { 54 | "rain": 0, 55 | "fog": 0, 56 | "wetness": 0, 57 | "cloudiness": 0, 58 | "damage": 0, 59 | "time": 0 60 | } 61 | }, 62 | { 63 | "name": "junction_left", 64 | "map": "SanFrancisco_correct", 65 | 66 | "ego": { 67 | "agent_type": "2e966a70-4a19-44b5-a5e7-64e00a7bc5de", 68 | "start": { 69 | "lane_id": "lane_569", 70 | "offset": 10 71 | }, 72 | "destination": { 73 | "lane_id": "lane_539", 74 | "offset": 60 75 | } 76 | }, 77 | 78 | "npcs": { 79 | "waypoint": 2, 80 | "npc_num": 5 81 | }, 82 | 83 | "environment": { 84 | "rain": 0, 85 | "fog": 0, 86 | "wetness": 0, 87 | "cloudiness": 0, 88 | "damage": 0, 89 | "time": 0 90 | } 91 | } 92 | ] -------------------------------------------------------------------------------- /data/straight/straight_basic_info.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "name": "straight_straight", 4 | "map": "SanFrancisco_correct", 5 | 6 | "ego": { 7 | "agent_type": "2e966a70-4a19-44b5-a5e7-64e00a7bc5de", 8 | "start": { 9 | "lane_id": "lane_626", 10 | "offset": 10 11 | }, 12 | "destination": { 13 | "lane_id": "lane_626", 14 | "offset": 230 15 | } 16 | }, 17 | 18 | "npcs": { 19 | "waypoint": 4, 20 | "npc_num": 6 21 | }, 22 | 23 | "environment": { 24 | "rain": 0, 25 | "fog": 0, 26 | "wetness": 0, 27 | "cloudiness": 0, 28 | "damage": 0, 29 | "time": 0 30 | } 31 | }, 32 | { 33 | "name": "straight_lane_change", 34 | "map": "SanFrancisco_correct", 35 | 36 | "ego": { 37 | "agent_type": "2e966a70-4a19-44b5-a5e7-64e00a7bc5de", 38 | "start": { 39 | "lane_id": "lane_627", 40 | "offset": 10 41 | }, 42 | "destination": { 43 | "lane_id": "lane_625", 44 | "offset": 230 45 | } 46 | }, 47 | 48 | "npcs": { 49 | "waypoint": 4, 50 | "npc_num": 6 51 | }, 52 | 53 | "environment": { 54 | "rain": 0, 55 | "fog": 0, 56 | "wetness": 0, 57 | "cloudiness": 0, 58 | "damage": 0, 59 | "time": 0 60 | } 61 | }, 62 | { 63 | "name": "straight_right_change", 64 | "map": "SanFrancisco_correct", 65 | 66 | "ego": { 67 | "agent_type": "2e966a70-4a19-44b5-a5e7-64e00a7bc5de", 68 | "start": { 69 | "lane_id": "lane_626", 70 | "offset": 10 71 | }, 72 | "destination": { 73 | "lane_id": "lane_625", 74 | "offset": 230 75 | } 76 | }, 77 | 78 | "npcs": { 79 | "waypoint": 4, 80 | "npc_num": 6 81 | }, 82 | 83 | "environment": { 84 | "rain": 0, 85 | "fog": 0, 86 | "wetness": 0, 87 | "cloudiness": 0, 88 | "damage": 0, 89 | "time": 0 90 | } 91 | } 92 | ] -------------------------------------------------------------------------------- /data/straight/straight_routes.json: -------------------------------------------------------------------------------- 1 | {"lane_details": {"lane_625": {"id": "lane_625", "turn": "NO_TURN", "speed_limit": 11.175999641418457, "right_neighbor_lane": [], "left_neighbor_lane": [{"id": "lane_626"}], "predecessor": [], "successor": [{"id": "lane_918"}], "central": {"points": [{"x": 592607.7906494141, "y": 4133802.2943115234}, {"x": 592776.8814697266, "y": 4133979.2387695312}], "length": 244.7469024658203}, "left_boundary": {"type": ["DOTTED_WHITE"], "points": [{"x": 592606.5254516602, "y": 4133803.5033569336}, {"x": 592775.6162757874, "y": 4133980.4478149414}]}, "right_boundary": {"type": ["CURB"], "points": [{"x": 592609.055847168, "y": 4133801.0852661133}, {"x": 592778.1466636658, "y": 4133978.029724121}]}}, "lane_626": {"id": "lane_626", "turn": "NO_TURN", "speed_limit": 11.175999641418457, "right_neighbor_lane": [{"id": "lane_625"}], "left_neighbor_lane": [{"id": "lane_627"}], "predecessor": [], "successor": [{"id": "lane_917"}], "central": {"points": [{"x": 592605.7066040039, "y": 4133805.613586426}, {"x": 592774.085144043, "y": 4133981.812805176}], "length": 243.7160186767578}, "left_boundary": {"type": ["DOTTED_WHITE"], "points": [{"x": 592604.44140625, "y": 4133806.822631836}, {"x": 592772.8199462891, "y": 4133983.021850586}]}, "right_boundary": {"type": ["DOTTED_WHITE"], "points": [{"x": 592606.9718017578, "y": 4133804.4045410156}, {"x": 592775.3503417969, "y": 4133980.6037597656}]}}, "lane_627": {"id": "lane_627", "turn": "NO_TURN", "speed_limit": 11.175999641418457, "right_neighbor_lane": [{"id": "lane_626"}], "left_neighbor_lane": [{"id": "lane_624"}], "predecessor": [{"id": "lane_1284"}], "successor": [{"id": "lane_916"}, {"id": "lane_919"}], "central": {"points": [{"x": 592603.0315551758, "y": 4133808.16998291}, {"x": 592771.321685791, "y": 4133984.2764282227}], "length": 243.58787536621094}, "left_boundary": {"type": ["DOTTED_WHITE"], "points": [{"x": 592601.7663574219, "y": 4133809.3790283203}, {"x": 592770.0564918518, "y": 4133985.485473633}]}, "right_boundary": {"type": ["DOTTED_WHITE"], "points": [{"x": 592604.2967529297, "y": 4133806.9609375}, {"x": 592772.5868797302, "y": 4133983.0673828125}]}}, "lane_145": {"id": "lane_145", "turn": "NO_TURN", "speed_limit": 11.175999641418457, "right_neighbor_lane": [{"id": "lane_146"}], "left_neighbor_lane": [{"id": "lane_144"}], "predecessor": [{"id": "lane_916"}, {"id": "lane_920"}], "successor": [{"id": "lane_908"}], "central": {"points": [{"x": 592775.3795776367, "y": 4133998.1200256348}, {"x": 592650.6118774414, "y": 4134117.3133850098}], "length": 172.55154418945312}, "left_boundary": {"type": ["DOTTED_WHITE"], "points": [{"x": 592774.1707229614, "y": 4133996.8546447754}, {"x": 592649.4030380249, "y": 4134116.0480041504}]}, "right_boundary": {"type": ["DOTTED_WHITE"], "points": [{"x": 592776.588432312, "y": 4133999.385406494}, {"x": 592651.8207168579, "y": 4134118.578765869}]}}, "lane_150": {"id": "lane_150", "turn": "NO_TURN", "speed_limit": 11.175999641418457, "right_neighbor_lane": [{"id": "lane_149"}], "left_neighbor_lane": [{"id": "lane_151"}], "predecessor": [{"id": "lane_917"}], "successor": [{"id": "lane_929"}], "central": {"points": [{"x": 592790.0284118652, "y": 4133998.2844543457}, {"x": 592964.4530792236, "y": 4134180.962585449}], "length": 252.57723999023438}, "left_boundary": {"type": ["DOTTED_WHITE"], "points": [{"x": 592788.7627258301, "y": 4133999.492980957}, {"x": 592963.1873626709, "y": 4134182.171081543}]}, "right_boundary": {"type": ["DOTTED_WHITE"], "points": [{"x": 592791.2940979004, "y": 4133997.0759277344}, {"x": 592965.7187957764, "y": 4134179.7540893555}]}}, "lane_149": {"id": "lane_149", "turn": "NO_TURN", "speed_limit": 11.175999641418457, "right_neighbor_lane": [], "left_neighbor_lane": [{"id": "lane_150"}], "predecessor": [{"id": "lane_918"}, {"id": "lane_923"}], "successor": [{"id": "lane_930"}, {"id": "lane_931"}], "central": {"points": [{"x": 592792.8116760254, "y": 4133995.6243286133}, {"x": 592967.3320007324, "y": 4134178.402770996}], "length": 252.71585083007812}, "left_boundary": {"type": ["DOTTED_WHITE"], "points": [{"x": 592791.5459823608, "y": 4133996.8328552246}, {"x": 592966.0662841797, "y": 4134179.61126709}]}, "right_boundary": {"type": ["CURB"], "points": [{"x": 592794.0773696899, "y": 4133994.415802002}, {"x": 592968.5977172852, "y": 4134177.1942749023}]}}, "lane_918": {"id": "lane_918", "turn": "NO_TURN", "speed_limit": 11.175999641418457, "right_neighbor_lane": [], "left_neighbor_lane": [], "predecessor": [{"id": "lane_625"}], "successor": [{"id": "lane_149"}], "central": {"points": [{"x": 592776.8814697266, "y": 4133979.2387695312}, {"x": 592787.0565490723, "y": 4133989.52444458}, {"x": 592792.8116760254, "y": 4133995.6243286133}], "length": 22.85445213317871}, "left_boundary": {"type": ["DOTTED_WHITE"], "points": [{"x": 592775.6373634338, "y": 4133980.4695129395}, {"x": 592785.8018035889, "y": 4133990.7443237305}, {"x": 592791.5387878418, "y": 4133996.8252868652}]}, "right_boundary": {"type": ["DOTTED_WHITE"], "points": [{"x": 592778.1255760193, "y": 4133978.008026123}, {"x": 592788.3112945557, "y": 4133988.3045654297}, {"x": 592794.084564209, "y": 4133994.4233703613}]}}, "lane_917": {"id": "lane_917", "turn": "NO_TURN", "speed_limit": 11.175999641418457, "right_neighbor_lane": [], "left_neighbor_lane": [], "predecessor": [{"id": "lane_626"}], "successor": [{"id": "lane_150"}], "central": {"points": [{"x": 592774.085144043, "y": 4133981.812805176}, {"x": 592790.0284118652, "y": 4133998.2844543457}], "length": 22.923866271972656}, "left_boundary": {"type": ["DOTTED_WHITE"], "points": [{"x": 592772.8286781311, "y": 4133983.0309143066}, {"x": 592788.7692947388, "y": 4133999.4998168945}]}, "right_boundary": {"type": ["DOTTED_WHITE"], "points": [{"x": 592775.3416099548, "y": 4133980.594696045}, {"x": 592791.2875289917, "y": 4133997.069091797}]}}, "lane_916": {"id": "lane_916", "turn": "LEFT_TURN", "speed_limit": 11.175999641418457, "right_neighbor_lane": [], "left_neighbor_lane": [], "predecessor": [{"id": "lane_627"}], "successor": [{"id": "lane_145"}], "central": {"points": [{"x": 592771.321685791, "y": 4133984.2764282227}, {"x": 592775.779510498, "y": 4133990.773162842}, {"x": 592776.3000488281, "y": 4133994.4120788574}, {"x": 592775.3795776367, "y": 4133998.1200256348}], "length": 15.375518798828125}, "left_boundary": {"type": ["DOTTED_WHITE"], "points": [{"x": 592769.8787117004, "y": 4133985.2665405273}, {"x": 592774.2087554932, "y": 4133991.5446777344}, {"x": 592774.5526351929, "y": 4133994.3169555664}, {"x": 592773.6811294556, "y": 4133997.6983947754}]}, "right_boundary": {"type": ["DOTTED_WHITE"], "points": [{"x": 592772.7646598816, "y": 4133983.286315918}, {"x": 592777.3502655029, "y": 4133990.001647949}, {"x": 592778.0474624634, "y": 4133994.5072021484}, {"x": 592777.0780258179, "y": 4133998.541656494}]}}}, "route_details": {"route_0": ["lane_625", "lane_918", "lane_149"], "route_1": ["lane_626", "lane_917", "lane_150"], "route_2": ["lane_627", "lane_916", "lane_145"]}, "routes": ["route_0", "route_1", "route_2"]} -------------------------------------------------------------------------------- /figs/MethodOverview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingfeiCheng/BehAVExplor/4124436fe38643196fea675acfcd163d3b369b75/figs/MethodOverview.png -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import yaml 4 | import json 5 | import copy 6 | import random 7 | import datetime 8 | import argparse 9 | 10 | from time import strftime, gmtime 11 | 12 | from loguru import logger 13 | from behavexplor.utils import create_path 14 | from behavexplor.corpus import InputCorpus 15 | 16 | from common.runner import Runner 17 | from common.simulator import Simulator 18 | from common.scenario import Scenario 19 | 20 | level = "INFO" 21 | logger.configure(handlers=[{"sink": sys.stderr, "level": level}]) # TODO: fix file output 22 | 23 | class Fuzzer(object): 24 | 25 | def __init__(self, cfgs): 26 | 27 | now = datetime.datetime.now() 28 | date_time = now.strftime("%m-%d-%Y-%H-%M-%S") 29 | self.cfgs = cfgs 30 | self.cfgs['output_path'] = self.cfgs['output_path'] + '-at-' + date_time 31 | self.output_path = self.cfgs['output_path'] 32 | 33 | if not os.path.exists(self.output_path): 34 | os.makedirs(self.output_path) 35 | 36 | create_path(self.output_path) 37 | self.sim = Simulator(self.cfgs['max_sim_time'], self.cfgs['lgsvl_map'], self.cfgs['apollo_map'], sim_mode=self.cfgs['sim_mode']) 38 | 39 | def loop(self, time_limitation): 40 | 41 | scenario_basic_file = self.cfgs['scenario_basic_file'] 42 | scenario_route_file = self.cfgs['scenario_route_file'] 43 | 44 | with open(scenario_basic_file, 'r') as f: 45 | scenario_basic_info = json.load(f) 46 | with open(scenario_route_file, 'r') as f: 47 | scenario_route_info = json.load(f) 48 | 49 | if self.cfgs['ego_behavior'] > len(scenario_basic_info) - 1: 50 | raise RuntimeError('Max ego_behavior is ' + str(len(scenario_basic_info) - 1)) 51 | 52 | scenario_basic_info = scenario_basic_info[self.cfgs['ego_behavior']] 53 | ego_behavior = str(scenario_basic_info['name']) 54 | logger.info('[Fuzzer] Ego Behavior: ' + ego_behavior) 55 | 56 | log_file = os.path.join(self.output_path, 'logs/system.log') 57 | if os.path.exists(log_file): 58 | os.remove(log_file) 59 | logger.add(log_file, level=level) 60 | self.record_cfgs() 61 | 62 | runner = Runner(self.output_path, self.sim) 63 | input_corpus = InputCorpus(self.output_path, 64 | self.cfgs['window_size'], 65 | self.cfgs['cluster_num'], 66 | self.cfgs['threshold_coverage'], 67 | self.cfgs['feature_resample'] 68 | ) 69 | 70 | logger.info('[Fuzzer] Start fuzzer iteration. Total time: ' + str(strftime('%H:%M:%S', gmtime(int(time_limitation))))) 71 | 72 | init_scenario_obj = Scenario() 73 | init_scenario_obj.generate_specific_info_wo_npc(scenario_basic_info, scenario_route_info) 74 | init_scenario_lst = [] 75 | for i in range(self.cfgs['init_seed_size']): 76 | scenario_obj = copy.deepcopy(init_scenario_obj) 77 | scenario_obj.generate_random_abstract_scenario() 78 | scenario_obj.generate_random_concrete_scenario() 79 | init_scenario_lst.append(scenario_obj) 80 | 81 | init_scenario_recorder_lst = [] 82 | for i in range(len(init_scenario_lst)): 83 | scenario_obj = init_scenario_lst[i] 84 | scenario_recorder, scenario_id = runner.run(scenario_obj) 85 | # TODO: add latency detection & rerun 86 | init_scenario_recorder_lst.append(scenario_recorder) 87 | 88 | corpus_seed_size = 0 89 | while corpus_seed_size <= 0: 90 | corpus_seed_size = input_corpus.initialize(init_scenario_lst, 91 | init_scenario_recorder_lst) 92 | if corpus_seed_size <= 0: 93 | logger.warning('[Fuzzer] Init seeds are all failed, generate one new element.') 94 | scenario_obj = copy.deepcopy(random.choice(init_scenario_lst)) 95 | scenario_obj.mutation_uniform() 96 | scenario_recorder, scenario_id = runner.run(scenario_obj) 97 | 98 | init_scenario_lst = [scenario_obj] 99 | init_scenario_recorder_lst = [scenario_recorder] 100 | 101 | start_time = datetime.datetime.now() 102 | iteration = 0 103 | 104 | while True: 105 | logger.info('[Fuzzer] Iterations: ' + str(iteration)) 106 | scenario_obj, mutation_method = input_corpus.sample_scenario_seed() 107 | 108 | for child_i in range(self.cfgs['child_size']): 109 | if mutation_method == 'uniform': 110 | if scenario_obj is not None: 111 | scenario_obj.generate_random_abstract_scenario() 112 | scenario_obj.generate_random_concrete_scenario() 113 | else: 114 | scenario_obj = copy.deepcopy(init_scenario_obj) 115 | scenario_obj.mutation_route() 116 | else: 117 | scenario_obj.mutation_gauss(self.cfgs['offset_mutation'], self.cfgs['speed_mutation']) 118 | 119 | scenario_recorder, scenario_id = runner.run(scenario_obj) 120 | 121 | corpus_seed_size, corpus_fail_size = input_corpus.add_seed(scenario_obj, scenario_recorder) 122 | logger.info('[Fuzzer] corpus_seed_size: ' + str(corpus_seed_size) + ' corpus_fail_size: ' + str(corpus_fail_size)) 123 | 124 | exec_time = int((datetime.datetime.now() - start_time).seconds) 125 | if exec_time >= time_limitation: 126 | logger.info( 127 | '[Fuzzer] Finish testing. Real execution time: ' + str(strftime('%H:%M:%S', gmtime(exec_time)))) 128 | input_corpus.save_models() 129 | self.sim.close() 130 | exit(-1) 131 | 132 | input_corpus.save_models() 133 | iteration += 1 134 | 135 | def record_cfgs(self): 136 | logger.info('[Fuzzer] Record fuzzer configs:') 137 | for k, v in self.cfgs.items(): 138 | logger.info(str(k) + ' : ' + str(v)) 139 | 140 | if __name__ == '__main__': 141 | parser = argparse.ArgumentParser(description='Apollo Coverage Fuzzer Testing.') 142 | parser.add_argument('--config', type=str, help='Test config yaml file.', required=True) 143 | args = parser.parse_args() 144 | 145 | yaml_file = args.config 146 | with open(yaml_file, 'r') as f: 147 | params = yaml.safe_load(f) 148 | 149 | fuzzer = Fuzzer(params) 150 | fuzzer.loop(int(params['total_test_time'])) # seconds -------------------------------------------------------------------------------- /prepare.sh: -------------------------------------------------------------------------------- 1 | pip install -r requirements.txt 2 | 3 | cd /apollo/BehAVExplor/PythonAPI-Apollo-async 4 | python3 -m pip install -r requirements.txt --user . 5 | cd - -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | loguru 2 | sympy 3 | tsfresh 4 | pyyaml 5 | gensim 6 | scikit-learn 7 | pymoo==0.4.2.1 8 | pandas 9 | grpcio-tools==1.34.1 10 | tensorflow==2.5.0 11 | keras==2.4.0 12 | h5py==3.1.0 13 | similaritymeasures --------------------------------------------------------------------------------