From da95fb53abebd37aad8a4bad814906664070f412 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 28 Apr 2024 04:48:48 -0400 Subject: [PATCH 01/46] finish failure on crossing any line --- .../sim/bridge/metadrive/metadrive_bridge.py | 15 ++- .../sim/bridge/metadrive/metadrive_process.py | 2 +- tools/sim/run_bridge.py | 8 +- tools/sim/scenarios/metadrive/stay_in_lane.py | 92 ------------------- tools/sim/tests/test_metadrive_bridge.py | 7 +- tools/sim/tests/test_sim_bridge.py | 10 ++ 6 files changed, 33 insertions(+), 101 deletions(-) delete mode 100755 tools/sim/scenarios/metadrive/stay_in_lane.py diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index 5c91238c55d8f1..02c4ba371558b1 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -1,3 +1,4 @@ +from collections import namedtuple from multiprocessing import Queue from metadrive.component.sensors.base_camera import _cuda_enable @@ -44,12 +45,17 @@ def create_map(track_size=60): ] ) +ci_failure_config = namedtuple("ci_failure_config", + ["out_of_route_done", "on_continuous_line_done", "on_broken_line_done"], + defaults=[False, False, False]) class MetaDriveBridge(SimulatorBridge): TICKS_PER_FRAME = 5 - def __init__(self, dual_camera, high_quality): + def __init__(self, dual_camera, high_quality, track_size, ci): self.should_render = False + self.ci_config = ci_failure_config() if not ci else ci_failure_config(True, True, True) + self.track_size = track_size super().__init__(dual_camera, high_quality) @@ -71,13 +77,14 @@ def spawn_world(self): image_on_cuda=_cuda_enable, image_observation=True, interface_panel=[], - out_of_route_done=False, - on_continuous_line_done=False, + out_of_route_done=self.ci_config.out_of_route_done, + on_continuous_line_done=self.ci_config.on_continuous_line_done, + on_broken_line_done=self.ci_config.on_broken_line_done, crash_vehicle_done=False, crash_object_done=False, arrive_dest_done=False, traffic_density=0.0, # traffic is incredibly expensive - map_config=create_map(), + map_config=create_map(self.track_size), decision_repeat=1, physics_world_step_size=self.TICKS_PER_FRAME/100, preload_models=False diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py index 79eefcd5451387..c50f26642e799b 100644 --- a/tools/sim/bridge/metadrive/metadrive_process.py +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -78,7 +78,7 @@ def get_cam_as_rgb(cam): cam.get_cam().reparentTo(env.vehicle.origin) cam.get_cam().setPos(C3_POSITION) cam.get_cam().setHpr(C3_HPR) - img = cam.perceive(clip=False) + img = cam.perceive(to_float=False) if type(img) != np.ndarray: img = img.get() # convert cupy array to numpy return img diff --git a/tools/sim/run_bridge.py b/tools/sim/run_bridge.py index 0992ef4bfe7318..a105d7f623b30b 100755 --- a/tools/sim/run_bridge.py +++ b/tools/sim/run_bridge.py @@ -6,10 +6,10 @@ from openpilot.tools.sim.bridge.metadrive.metadrive_bridge import MetaDriveBridge -def create_bridge(dual_camera, high_quality): +def create_bridge(dual_camera, high_quality, track_size, ci): queue: Any = Queue() - simulator_bridge = MetaDriveBridge(dual_camera, high_quality) + simulator_bridge = MetaDriveBridge(dual_camera, high_quality, track_size, ci) simulator_process = simulator_bridge.run(queue) return queue, simulator_process, simulator_bridge @@ -23,13 +23,15 @@ def parse_args(add_args=None): parser.add_argument('--joystick', action='store_true') parser.add_argument('--high_quality', action='store_true') parser.add_argument('--dual_camera', action='store_true') + parser.add_argument('--track_size', type=int, default=60) + parser.add_argument('--ci', action="store_true") return parser.parse_args(add_args) if __name__ == "__main__": args = parse_args() - queue, simulator_process, simulator_bridge = create_bridge(args.dual_camera, args.high_quality) + queue, simulator_process, simulator_bridge = create_bridge(args.dual_camera, args.high_quality, args.track_size, args.ci) if args.joystick: # start input poll for joystick diff --git a/tools/sim/scenarios/metadrive/stay_in_lane.py b/tools/sim/scenarios/metadrive/stay_in_lane.py deleted file mode 100755 index 683ce551629d7c..00000000000000 --- a/tools/sim/scenarios/metadrive/stay_in_lane.py +++ /dev/null @@ -1,92 +0,0 @@ -#!/usr/bin/env python - -from multiprocessing import Queue - -from metadrive.component.sensors.base_camera import _cuda_enable -from metadrive.component.map.pg_map import MapGenerateMethod - -from openpilot.tools.sim.bridge.common import SimulatorBridge -from openpilot.tools.sim.bridge.metadrive.metadrive_common import RGBCameraRoad, RGBCameraWide -from openpilot.tools.sim.bridge.metadrive.metadrive_world import MetaDriveWorld -from openpilot.tools.sim.lib.camerad import W, H - - -def create_map(): - return dict( - type=MapGenerateMethod.PG_MAP_FILE, - lane_num=2, - lane_width=3.5, - config=[ - { - "id": "S", - "pre_block_socket_index": 0, - "length": 60, - }, - { - "id": "C", - "pre_block_socket_index": 0, - "length": 60, - "radius": 600, - "angle": 45, - "dir": 0, - }, - ] - ) - - -class MetaDriveBridge(SimulatorBridge): - TICKS_PER_FRAME = 5 - - def __init__(self, world_status_q, dual_camera, high_quality): - self.world_status_q = world_status_q - self.should_render = False - - super().__init__(dual_camera, high_quality) - - def spawn_world(self): - sensors = { - "rgb_road": (RGBCameraRoad, W, H, ) - } - - if self.dual_camera: - sensors["rgb_wide"] = (RGBCameraWide, W, H) - - config = dict( - use_render=self.should_render, - vehicle_config=dict( - enable_reverse=False, - image_source="rgb_road", - ), - sensors=sensors, - image_on_cuda=_cuda_enable, - image_observation=True, - interface_panel=[], - out_of_route_done=True, - on_continuous_line_done=True, - crash_vehicle_done=True, - crash_object_done=True, - arrive_dest_done=True, - traffic_density=0.0, - map_config=create_map(), - map_region_size=2048, - decision_repeat=1, - physics_world_step_size=self.TICKS_PER_FRAME/100, - preload_models=False - ) - - return MetaDriveWorld(world_status_q, config, self.dual_camera) - - -if __name__ == "__main__": - command_queue: Queue = Queue() - world_status_q: Queue = Queue() - simulator_bridge = MetaDriveBridge(world_status_q, True, False) - simulator_process = simulator_bridge.run(command_queue) - - while True: - world_status = world_status_q.get() - print(f"World Status: {str(world_status)}") - if world_status["status"] == "terminating": - break - - simulator_process.join() diff --git a/tools/sim/tests/test_metadrive_bridge.py b/tools/sim/tests/test_metadrive_bridge.py index 6cb8e1465ef015..ea9bc5209846d5 100755 --- a/tools/sim/tests/test_metadrive_bridge.py +++ b/tools/sim/tests/test_metadrive_bridge.py @@ -1,15 +1,20 @@ #!/usr/bin/env python3 import pytest import unittest +import sys from openpilot.tools.sim.bridge.metadrive.metadrive_bridge import MetaDriveBridge from openpilot.tools.sim.tests.test_sim_bridge import TestSimBridgeBase @pytest.mark.slow class TestMetaDriveBridge(TestSimBridgeBase): + TRACK_SIZE = 60 + def create_bridge(self): - return MetaDriveBridge(False, False) + return MetaDriveBridge(False, False, self.TRACK_SIZE, True) if __name__ == "__main__": + if len(sys.argv) > 1: + TestMetaDriveBridge.TRACK_SIZE = int(sys.argv.pop()) unittest.main() diff --git a/tools/sim/tests/test_sim_bridge.py b/tools/sim/tests/test_sim_bridge.py index 504914c5626a61..fca07f8a8ffe6e 100644 --- a/tools/sim/tests/test_sim_bridge.py +++ b/tools/sim/tests/test_sim_bridge.py @@ -72,6 +72,16 @@ def test_engage(self): self.assertEqual(min_counts_control_active, control_active, f"Simulator did not engage a minimal of {min_counts_control_active} steps was {control_active}") + failure_states = [] + while bridge.started.value and sm['controlsState'].active: + state = q.get() + if isinstance(state, dict) and state["status"] == "terminating": + done_info = state["done_info"] + failure_states = [done_state for done_state in done_info if done_state != "arrive_dest" and done_info[done_state]] + if len(failure_states) > 0: + break + self.assertEqual(len(failure_states), 0, f"Simulator fails to finish a loop. Failure states: ${failure_states}") + def tearDown(self): print("Test shutting down. CommIssues are acceptable") for p in reversed(self.processes): From 8dc4894e6b4e00fe910ccb4d4a654490e8c3c2a4 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 28 Apr 2024 04:54:26 -0400 Subject: [PATCH 02/46] update --- tools/sim/bridge/metadrive/metadrive_bridge.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index 02c4ba371558b1..57758ff75ea160 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -45,7 +45,7 @@ def create_map(track_size=60): ] ) -ci_failure_config = namedtuple("ci_failure_config", +failure_config = namedtuple("failure_config", ["out_of_route_done", "on_continuous_line_done", "on_broken_line_done"], defaults=[False, False, False]) @@ -54,7 +54,7 @@ class MetaDriveBridge(SimulatorBridge): def __init__(self, dual_camera, high_quality, track_size, ci): self.should_render = False - self.ci_config = ci_failure_config() if not ci else ci_failure_config(True, True, True) + self.failure_config = failure_config(True, True, True) if ci else failure_config() self.track_size = track_size super().__init__(dual_camera, high_quality) @@ -77,9 +77,9 @@ def spawn_world(self): image_on_cuda=_cuda_enable, image_observation=True, interface_panel=[], - out_of_route_done=self.ci_config.out_of_route_done, - on_continuous_line_done=self.ci_config.on_continuous_line_done, - on_broken_line_done=self.ci_config.on_broken_line_done, + out_of_route_done=self.failure_config.out_of_route_done, + on_continuous_line_done=self.failure_config.on_continuous_line_done, + on_broken_line_done=self.failure_config.on_broken_line_done, crash_vehicle_done=False, crash_object_done=False, arrive_dest_done=False, From 88de3740e0388074126c4780481e9abb1f2b5705 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 28 Apr 2024 17:59:19 -0400 Subject: [PATCH 03/46] standardize queue messages --- tools/sim/bridge/common.py | 10 ++++-- .../sim/bridge/metadrive/metadrive_bridge.py | 4 +-- tools/sim/bridge/metadrive/metadrive_world.py | 18 ++++------ tools/sim/lib/keyboard_ctrl.py | 33 +++++++++++-------- tools/sim/lib/manual_ctrl.py | 17 ++++++---- tools/sim/tests/test_sim_bridge.py | 3 +- 6 files changed, 46 insertions(+), 39 deletions(-) diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index 3bd2f35772e018..2dc062b13fe909 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -2,6 +2,7 @@ import threading import functools +from collections import namedtuple from multiprocessing import Process, Queue, Value from abc import ABC, abstractmethod @@ -14,6 +15,7 @@ from openpilot.tools.sim.lib.simulated_car import SimulatedCar from openpilot.tools.sim.lib.simulated_sensors import SimulatedSensors +QueueMessage = namedtuple("QueueMessage", ["type", "info"], defaults=[None]) def rk_loop(function, hz, exit_event: threading.Event): rk = Ratekeeper(hz, None) @@ -79,11 +81,11 @@ def print_status(self): """) @abstractmethod - def spawn_world(self) -> World: + def spawn_world(self, q: Queue) -> World: pass def _run(self, q: Queue): - self.world = self.spawn_world() + self.world = self.spawn_world(q) self.simulated_car = SimulatedCar() self.simulated_sensors = SimulatedSensors(self.dual_camera) @@ -113,7 +115,9 @@ def _run(self, q: Queue): # Read manual controls if not q.empty(): message = q.get() - m = message.split('_') + if message.type != "control_command": + continue + m = message.info.split('_') if m[0] == "steer": steer_manual = float(m[1]) elif m[0] == "throttle": diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index 5c91238c55d8f1..cbac215092c148 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -53,7 +53,7 @@ def __init__(self, dual_camera, high_quality): super().__init__(dual_camera, high_quality) - def spawn_world(self): + def spawn_world(self, queue: Queue): sensors = { "rgb_road": (RGBCameraRoad, W, H, ) } @@ -83,4 +83,4 @@ def spawn_world(self): preload_models=False ) - return MetaDriveWorld(Queue(), config, self.dual_camera) + return MetaDriveWorld(queue, config, self.dual_camera) diff --git a/tools/sim/bridge/metadrive/metadrive_world.py b/tools/sim/bridge/metadrive/metadrive_world.py index 357020506950ad..c6b680621b4eef 100644 --- a/tools/sim/bridge/metadrive/metadrive_world.py +++ b/tools/sim/bridge/metadrive/metadrive_world.py @@ -5,6 +5,8 @@ import time from multiprocessing import Pipe, Array + +from openpilot.tools.sim.bridge.common import QueueMessage from openpilot.tools.sim.bridge.metadrive.metadrive_process import (metadrive_process, metadrive_simulation_state, metadrive_vehicle_state) from openpilot.tools.sim.lib.common import SimulatorState, World @@ -35,14 +37,14 @@ def __init__(self, status_q, config, dual_camera = False): self.vehicle_state_send, self.exit_event)) self.metadrive_process.start() - self.status_q.put({"status": "starting"}) + self.status_q.put(QueueMessage("starting")) print("----------------------------------------------------------") print("---- Spawning Metadrive world, this might take awhile ----") print("----------------------------------------------------------") self.vehicle_state_recv.recv() # wait for a state message to ensure metadrive is launched - self.status_q.put({"status": "started"}) + self.status_q.put(QueueMessage("started")) self.steer_ratio = 15 self.vc = [0.0,0.0] @@ -68,12 +70,7 @@ def read_state(self): while self.simulation_state_recv.poll(0): md_state: metadrive_simulation_state = self.simulation_state_recv.recv() if md_state.done: - self.status_q.put({ - "status": "terminating", - "reason": "done", - "done_info": md_state.done_info - }) - self.exit_event.set() + self.close(md_state.done_info) def read_sensors(self, state: SimulatorState): while self.vehicle_state_recv.poll(0): @@ -94,9 +91,6 @@ def reset(self): self.should_reset = True def close(self, reason: str): - self.status_q.put({ - "status": "terminating", - "reason": reason, - }) + self.status_q.put(QueueMessage("terminating", reason)) self.exit_event.set() self.metadrive_process.join() diff --git a/tools/sim/lib/keyboard_ctrl.py b/tools/sim/lib/keyboard_ctrl.py index ea255d9ce42d33..4671e0484cd0f4 100644 --- a/tools/sim/lib/keyboard_ctrl.py +++ b/tools/sim/lib/keyboard_ctrl.py @@ -2,10 +2,13 @@ import termios import time +from multiprocessing import Queue from termios import (BRKINT, CS8, CSIZE, ECHO, ICANON, ICRNL, IEXTEN, INPCK, ISTRIP, IXON, PARENB, VMIN, VTIME) from typing import NoReturn +from openpilot.tools.sim.bridge.common import QueueMessage + # Indexes for termios list. IFLAG = 0 OFLAG = 1 @@ -52,38 +55,40 @@ def getch() -> str: def print_keyboard_help(): print(f"Keyboard Commands:\n{KEYBOARD_HELP}") -def keyboard_poll_thread(q: 'Queue[str]'): +def keyboard_poll_thread(q: Queue[QueueMessage]): print_keyboard_help() while True: c = getch() if c == '1': - q.put("cruise_up") + message = QueueMessage("control_command", "cruise_up") elif c == '2': - q.put("cruise_down") + message = QueueMessage("control_command", "cruise_down") elif c == '3': - q.put("cruise_cancel") + message = QueueMessage("control_command", "cruise_cancel") elif c == 'w': - q.put("throttle_%f" % 1.0) + message = QueueMessage("control_command", f"throttle_{1.0}") elif c == 'a': - q.put("steer_%f" % -0.15) + message = QueueMessage("control_command", f"steer_{-0.15}") elif c == 's': - q.put("brake_%f" % 1.0) + message = QueueMessage("control_command", f"brake_{1.0}") elif c == 'd': - q.put("steer_%f" % 0.15) + message = QueueMessage("control_command", f"steer_{0.15}") elif c == 'z': - q.put("blinker_left") + message = QueueMessage("control_command", "blinker_left") elif c == 'x': - q.put("blinker_right") + message = QueueMessage("control_command", "blinker_right") elif c == 'i': - q.put("ignition") + message = QueueMessage("control_command", "ignition") elif c == 'r': - q.put("reset") + message = QueueMessage("control_command", "reset") elif c == 'q': - q.put("quit") + message = QueueMessage("control_command", "quit") + q.put(message) break else: print_keyboard_help() + q.put(message) def test(q: 'Queue[str]') -> NoReturn: while True: @@ -92,7 +97,7 @@ def test(q: 'Queue[str]') -> NoReturn: if __name__ == '__main__': from multiprocessing import Process, Queue - q: Queue[str] = Queue() + q: Queue[QueueMessage] = Queue() p = Process(target=test, args=(q,)) p.daemon = True p.start() diff --git a/tools/sim/lib/manual_ctrl.py b/tools/sim/lib/manual_ctrl.py index 8a72296538246b..28553a20e43c04 100755 --- a/tools/sim/lib/manual_ctrl.py +++ b/tools/sim/lib/manual_ctrl.py @@ -6,6 +6,8 @@ from fcntl import ioctl from typing import NoReturn +from openpilot.tools.sim.bridge.commons import QueueMessage + # Iterate over the joystick devices. print('Available devices:') for fn in os.listdir('/dev/input'): @@ -153,33 +155,34 @@ def wheel_poll_thread(q: 'Queue[str]') -> NoReturn: fvalue = value / 32767.0 axis_states[axis] = fvalue normalized = (1 - fvalue) * 50 - q.put(f"throttle_{normalized:f}") + message = QueueMessage("control_command", f"throttle_{normalized:f}") elif axis == "rz": # brake fvalue = value / 32767.0 axis_states[axis] = fvalue normalized = (1 - fvalue) * 50 - q.put(f"brake_{normalized:f}") + message = QueueMessage("control_command", f"brake_{normalized:f}") elif axis == "x": # steer angle fvalue = value / 32767.0 axis_states[axis] = fvalue normalized = fvalue - q.put(f"steer_{normalized:f}") + message = QueueMessage("control_command", f"steer_{normalized:f}") elif mtype & 0x01: # buttons if value == 1: # press down if number in [0, 19]: # X - q.put("cruise_down") + message = QueueMessage("control_command", "cruise_down") elif number in [3, 18]: # triangle - q.put("cruise_up") + message = QueueMessage("control_command", "cruise_up") elif number in [1, 6]: # square - q.put("cruise_cancel") + message = QueueMessage("control_command", "cruise_cancel") elif number in [10, 21]: # R3 - q.put("reverse_switch") + message = QueueMessage("control_command", "reverse_switch") + q.put(message) if __name__ == '__main__': from multiprocessing import Process, Queue diff --git a/tools/sim/tests/test_sim_bridge.py b/tools/sim/tests/test_sim_bridge.py index 504914c5626a61..abb00b3ee9f7be 100644 --- a/tools/sim/tests/test_sim_bridge.py +++ b/tools/sim/tests/test_sim_bridge.py @@ -7,6 +7,7 @@ from cereal import messaging from openpilot.common.basedir import BASEDIR +from openpilot.tools.sim.bridge.common import QueueMessage SIM_DIR = os.path.join(BASEDIR, "tools/sim") @@ -62,7 +63,7 @@ def test_engage(self): while time.monotonic() < start_time + max_time_per_step: sm.update() - q.put("cruise_down") # Try engaging + q.put(QueueMessage("control_command", "cruise_down")) # Try engaging if sm.all_alive() and sm['controlsState'].active: control_active += 1 From f10158ca7de7b44a3423762449ee340ee7aad898 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 28 Apr 2024 18:17:04 -0400 Subject: [PATCH 04/46] update control_command_gen --- tools/sim/bridge/common.py | 3 +++ .../sim/bridge/metadrive/metadrive_process.py | 3 ++- tools/sim/lib/keyboard_ctrl.py | 27 ++++++++++--------- tools/sim/lib/manual_ctrl.py | 16 +++++------ tools/sim/tests/test_sim_bridge.py | 4 +-- 5 files changed, 29 insertions(+), 24 deletions(-) diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index 2dc062b13fe909..eb4dbb3fd5e22e 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -17,6 +17,9 @@ QueueMessage = namedtuple("QueueMessage", ["type", "info"], defaults=[None]) +def control_cmd_gen(cmd: str): + return QueueMessage("control_command", cmd) + def rk_loop(function, hz, exit_event: threading.Event): rk = Ratekeeper(hz, None) while not exit_event.is_set(): diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py index 79eefcd5451387..80e58ddaf695a1 100644 --- a/tools/sim/bridge/metadrive/metadrive_process.py +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -78,7 +78,8 @@ def get_cam_as_rgb(cam): cam.get_cam().reparentTo(env.vehicle.origin) cam.get_cam().setPos(C3_POSITION) cam.get_cam().setHpr(C3_HPR) - img = cam.perceive(clip=False) + #img = cam.perceive(clip=False) + img = cam.perceive(to_float=False) if type(img) != np.ndarray: img = img.get() # convert cupy array to numpy return img diff --git a/tools/sim/lib/keyboard_ctrl.py b/tools/sim/lib/keyboard_ctrl.py index 4671e0484cd0f4..d822db2c3c2762 100644 --- a/tools/sim/lib/keyboard_ctrl.py +++ b/tools/sim/lib/keyboard_ctrl.py @@ -7,7 +7,7 @@ ISTRIP, IXON, PARENB, VMIN, VTIME) from typing import NoReturn -from openpilot.tools.sim.bridge.common import QueueMessage +from openpilot.tools.sim.bridge.common import QueueMessage, control_cmd_gen # Indexes for termios list. IFLAG = 0 @@ -61,29 +61,30 @@ def keyboard_poll_thread(q: Queue[QueueMessage]): while True: c = getch() if c == '1': - message = QueueMessage("control_command", "cruise_up") + message = control_cmd_gen("cruise_up") elif c == '2': - message = QueueMessage("control_command", "cruise_down") + message = control_cmd_gen("cruise_down") elif c == '3': - message = QueueMessage("control_command", "cruise_cancel") + message = control_cmd_gen("cruise_cancel") elif c == 'w': - message = QueueMessage("control_command", f"throttle_{1.0}") + message = control_cmd_gen(f"throttle_{1.0}") elif c == 'a': - message = QueueMessage("control_command", f"steer_{-0.15}") + message = control_cmd_gen(f"steer_{-0.15}") elif c == 's': - message = QueueMessage("control_command", f"brake_{1.0}") + message = control_cmd_gen(f"brake_{1.0}") + print(message) elif c == 'd': - message = QueueMessage("control_command", f"steer_{0.15}") + message = control_cmd_gen(f"steer_{0.15}") elif c == 'z': - message = QueueMessage("control_command", "blinker_left") + message = control_cmd_gen("blinker_left") elif c == 'x': - message = QueueMessage("control_command", "blinker_right") + message = control_cmd_gen("blinker_right") elif c == 'i': - message = QueueMessage("control_command", "ignition") + message = control_cmd_gen("ignition") elif c == 'r': - message = QueueMessage("control_command", "reset") + message = control_cmd_gen("reset") elif c == 'q': - message = QueueMessage("control_command", "quit") + message = control_cmd_gen("quit") q.put(message) break else: diff --git a/tools/sim/lib/manual_ctrl.py b/tools/sim/lib/manual_ctrl.py index 28553a20e43c04..dbd65a1ed4a090 100755 --- a/tools/sim/lib/manual_ctrl.py +++ b/tools/sim/lib/manual_ctrl.py @@ -6,7 +6,7 @@ from fcntl import ioctl from typing import NoReturn -from openpilot.tools.sim.bridge.commons import QueueMessage +from openpilot.tools.sim.bridge.common import control_cmd_gen # Iterate over the joystick devices. print('Available devices:') @@ -155,33 +155,33 @@ def wheel_poll_thread(q: 'Queue[str]') -> NoReturn: fvalue = value / 32767.0 axis_states[axis] = fvalue normalized = (1 - fvalue) * 50 - message = QueueMessage("control_command", f"throttle_{normalized:f}") + message = control_cmd_gen(f"throttle_{normalized:f}") elif axis == "rz": # brake fvalue = value / 32767.0 axis_states[axis] = fvalue normalized = (1 - fvalue) * 50 - message = QueueMessage("control_command", f"brake_{normalized:f}") + message = control_cmd_gen(f"brake_{normalized:f}") elif axis == "x": # steer angle fvalue = value / 32767.0 axis_states[axis] = fvalue normalized = fvalue - message = QueueMessage("control_command", f"steer_{normalized:f}") + message = control_cmd_gen(f"steer_{normalized:f}") elif mtype & 0x01: # buttons if value == 1: # press down if number in [0, 19]: # X - message = QueueMessage("control_command", "cruise_down") + message = control_cmd_gen("cruise_down") elif number in [3, 18]: # triangle - message = QueueMessage("control_command", "cruise_up") + message = control_cmd_gen("cruise_up") elif number in [1, 6]: # square - message = QueueMessage("control_command", "cruise_cancel") + message = control_cmd_gen("cruise_cancel") elif number in [10, 21]: # R3 - message = QueueMessage("control_command", "reverse_switch") + message = control_cmd_gen("reverse_switch") q.put(message) if __name__ == '__main__': diff --git a/tools/sim/tests/test_sim_bridge.py b/tools/sim/tests/test_sim_bridge.py index abb00b3ee9f7be..d13f5dae94e453 100644 --- a/tools/sim/tests/test_sim_bridge.py +++ b/tools/sim/tests/test_sim_bridge.py @@ -7,7 +7,7 @@ from cereal import messaging from openpilot.common.basedir import BASEDIR -from openpilot.tools.sim.bridge.common import QueueMessage +from openpilot.tools.sim.bridge.common import control_cmd_gen SIM_DIR = os.path.join(BASEDIR, "tools/sim") @@ -63,7 +63,7 @@ def test_engage(self): while time.monotonic() < start_time + max_time_per_step: sm.update() - q.put(QueueMessage("control_command", "cruise_down")) # Try engaging + q.put(control_cmd_gen("cruise_down")) # Try engaging if sm.all_alive() and sm['controlsState'].active: control_active += 1 From 6bfefc4a78afd049ef34f1b8205c2103a234e79f Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 28 Apr 2024 18:25:54 -0400 Subject: [PATCH 05/46] fix --- tools/sim/bridge/metadrive/metadrive_process.py | 3 +-- tools/sim/lib/keyboard_ctrl.py | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py index 80e58ddaf695a1..79eefcd5451387 100644 --- a/tools/sim/bridge/metadrive/metadrive_process.py +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -78,8 +78,7 @@ def get_cam_as_rgb(cam): cam.get_cam().reparentTo(env.vehicle.origin) cam.get_cam().setPos(C3_POSITION) cam.get_cam().setHpr(C3_HPR) - #img = cam.perceive(clip=False) - img = cam.perceive(to_float=False) + img = cam.perceive(clip=False) if type(img) != np.ndarray: img = img.get() # convert cupy array to numpy return img diff --git a/tools/sim/lib/keyboard_ctrl.py b/tools/sim/lib/keyboard_ctrl.py index d822db2c3c2762..a82de7c3118942 100644 --- a/tools/sim/lib/keyboard_ctrl.py +++ b/tools/sim/lib/keyboard_ctrl.py @@ -55,7 +55,7 @@ def getch() -> str: def print_keyboard_help(): print(f"Keyboard Commands:\n{KEYBOARD_HELP}") -def keyboard_poll_thread(q: Queue[QueueMessage]): +def keyboard_poll_thread(q: 'Queue[QueueMessage]'): print_keyboard_help() while True: @@ -72,7 +72,6 @@ def keyboard_poll_thread(q: Queue[QueueMessage]): message = control_cmd_gen(f"steer_{-0.15}") elif c == 's': message = control_cmd_gen(f"brake_{1.0}") - print(message) elif c == 'd': message = control_cmd_gen(f"steer_{0.15}") elif c == 'z': From 96cae212363a115499c21e441bd476ffdd484879 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 28 Apr 2024 18:27:37 -0400 Subject: [PATCH 06/46] fix logic --- tools/sim/lib/keyboard_ctrl.py | 1 + 1 file changed, 1 insertion(+) diff --git a/tools/sim/lib/keyboard_ctrl.py b/tools/sim/lib/keyboard_ctrl.py index a82de7c3118942..f39d6012135f10 100644 --- a/tools/sim/lib/keyboard_ctrl.py +++ b/tools/sim/lib/keyboard_ctrl.py @@ -88,6 +88,7 @@ def keyboard_poll_thread(q: 'Queue[QueueMessage]'): break else: print_keyboard_help() + continue q.put(message) def test(q: 'Queue[str]') -> NoReturn: From abf54b69e49f0d25a12a2f605b9c92691df9650e Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 28 Apr 2024 19:59:03 -0400 Subject: [PATCH 07/46] update closing type --- tools/sim/bridge/common.py | 6 +++--- tools/sim/bridge/metadrive/metadrive_world.py | 10 +++++----- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index eb4dbb3fd5e22e..4b94956fc07fb3 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -62,14 +62,14 @@ def bridge_keep_alive(self, q: Queue, retries: int): try: self._run(q) finally: - self.close("bridge terminated") + self.close("close_status", "bridge terminated") - def close(self, reason): + def close(self, closing_type, reason): self.started.value = False self._exit_event.set() if self.world is not None: - self.world.close(reason) + self.world.close(closing_type, reason) def run(self, queue, retries=-1): bridge_p = Process(name="bridge", target=self.bridge_keep_alive, args=(queue, retries)) diff --git a/tools/sim/bridge/metadrive/metadrive_world.py b/tools/sim/bridge/metadrive/metadrive_world.py index c6b680621b4eef..33788b8150cd43 100644 --- a/tools/sim/bridge/metadrive/metadrive_world.py +++ b/tools/sim/bridge/metadrive/metadrive_world.py @@ -37,14 +37,14 @@ def __init__(self, status_q, config, dual_camera = False): self.vehicle_state_send, self.exit_event)) self.metadrive_process.start() - self.status_q.put(QueueMessage("starting")) + self.status_q.put(QueueMessage("start_status", "starting")) print("----------------------------------------------------------") print("---- Spawning Metadrive world, this might take awhile ----") print("----------------------------------------------------------") self.vehicle_state_recv.recv() # wait for a state message to ensure metadrive is launched - self.status_q.put(QueueMessage("started")) + self.status_q.put(QueueMessage("start_status", "started")) self.steer_ratio = 15 self.vc = [0.0,0.0] @@ -70,7 +70,7 @@ def read_state(self): while self.simulation_state_recv.poll(0): md_state: metadrive_simulation_state = self.simulation_state_recv.recv() if md_state.done: - self.close(md_state.done_info) + self.close("termination_status", md_state.done_info) def read_sensors(self, state: SimulatorState): while self.vehicle_state_recv.poll(0): @@ -90,7 +90,7 @@ def tick(self): def reset(self): self.should_reset = True - def close(self, reason: str): - self.status_q.put(QueueMessage("terminating", reason)) + def close(self, closing_type, reason): + self.status_q.put(QueueMessage(closing_type, reason)) self.exit_event.set() self.metadrive_process.join() From fe19f3bd5cc162f46ebb73eb20a5364be14fead5 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 28 Apr 2024 20:30:36 -0400 Subject: [PATCH 08/46] update test --- tools/sim/bridge/metadrive/metadrive_bridge.py | 2 +- tools/sim/run_bridge.py | 8 +++----- tools/sim/tests/test_sim_bridge.py | 13 ++++++++----- 3 files changed, 12 insertions(+), 11 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index 8c7266051a4dec..7b696bb84076cc 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -52,7 +52,7 @@ def create_map(track_size=60): class MetaDriveBridge(SimulatorBridge): TICKS_PER_FRAME = 5 - def __init__(self, dual_camera, high_quality, track_size, ci): + def __init__(self, dual_camera, high_quality, track_size=60, ci=False): self.should_render = False self.failure_config = failure_config(True, True, True) if ci else failure_config() self.track_size = track_size diff --git a/tools/sim/run_bridge.py b/tools/sim/run_bridge.py index a105d7f623b30b..0992ef4bfe7318 100755 --- a/tools/sim/run_bridge.py +++ b/tools/sim/run_bridge.py @@ -6,10 +6,10 @@ from openpilot.tools.sim.bridge.metadrive.metadrive_bridge import MetaDriveBridge -def create_bridge(dual_camera, high_quality, track_size, ci): +def create_bridge(dual_camera, high_quality): queue: Any = Queue() - simulator_bridge = MetaDriveBridge(dual_camera, high_quality, track_size, ci) + simulator_bridge = MetaDriveBridge(dual_camera, high_quality) simulator_process = simulator_bridge.run(queue) return queue, simulator_process, simulator_bridge @@ -23,15 +23,13 @@ def parse_args(add_args=None): parser.add_argument('--joystick', action='store_true') parser.add_argument('--high_quality', action='store_true') parser.add_argument('--dual_camera', action='store_true') - parser.add_argument('--track_size', type=int, default=60) - parser.add_argument('--ci', action="store_true") return parser.parse_args(add_args) if __name__ == "__main__": args = parse_args() - queue, simulator_process, simulator_bridge = create_bridge(args.dual_camera, args.high_quality, args.track_size, args.ci) + queue, simulator_process, simulator_bridge = create_bridge(args.dual_camera, args.high_quality) if args.joystick: # start input poll for joystick diff --git a/tools/sim/tests/test_sim_bridge.py b/tools/sim/tests/test_sim_bridge.py index 428068aa7ee927..09b08131777fd9 100644 --- a/tools/sim/tests/test_sim_bridge.py +++ b/tools/sim/tests/test_sim_bridge.py @@ -74,13 +74,16 @@ def test_engage(self): self.assertEqual(min_counts_control_active, control_active, f"Simulator did not engage a minimal of {min_counts_control_active} steps was {control_active}") failure_states = [] - while bridge.started.value and sm['controlsState'].active: + while bridge.started.value: + continue + + state = None + while not q.empty(): state = q.get() - if isinstance(state, dict) and state["status"] == "terminating": - done_info = state["done_info"] + if state.type == "termination_status": + done_info = state.info failure_states = [done_state for done_state in done_info if done_state != "arrive_dest" and done_info[done_state]] - if len(failure_states) > 0: - break + break self.assertEqual(len(failure_states), 0, f"Simulator fails to finish a loop. Failure states: ${failure_states}") def tearDown(self): From ff0e76921fff6000e835aa9fbfa2d02379717894 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 28 Apr 2024 20:39:11 -0400 Subject: [PATCH 09/46] update logic --- tools/sim/bridge/common.py | 57 +++++++++++++++++++------------------- 1 file changed, 28 insertions(+), 29 deletions(-) diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index 4b94956fc07fb3..6cc759f9775bf7 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -118,35 +118,34 @@ def _run(self, q: Queue): # Read manual controls if not q.empty(): message = q.get() - if message.type != "control_command": - continue - m = message.info.split('_') - if m[0] == "steer": - steer_manual = float(m[1]) - elif m[0] == "throttle": - throttle_manual = float(m[1]) - elif m[0] == "brake": - brake_manual = float(m[1]) - elif m[0] == "cruise": - if m[1] == "down": - self.simulator_state.cruise_button = CruiseButtons.DECEL_SET - elif m[1] == "up": - self.simulator_state.cruise_button = CruiseButtons.RES_ACCEL - elif m[1] == "cancel": - self.simulator_state.cruise_button = CruiseButtons.CANCEL - elif m[1] == "main": - self.simulator_state.cruise_button = CruiseButtons.MAIN - elif m[0] == "blinker": - if m[1] == "left": - self.simulator_state.left_blinker = True - elif m[1] == "right": - self.simulator_state.right_blinker = True - elif m[0] == "ignition": - self.simulator_state.ignition = not self.simulator_state.ignition - elif m[0] == "reset": - self.world.reset() - elif m[0] == "quit": - break + if message.type == "control_command": + m = message.info.split('_') + if m[0] == "steer": + steer_manual = float(m[1]) + elif m[0] == "throttle": + throttle_manual = float(m[1]) + elif m[0] == "brake": + brake_manual = float(m[1]) + elif m[0] == "cruise": + if m[1] == "down": + self.simulator_state.cruise_button = CruiseButtons.DECEL_SET + elif m[1] == "up": + self.simulator_state.cruise_button = CruiseButtons.RES_ACCEL + elif m[1] == "cancel": + self.simulator_state.cruise_button = CruiseButtons.CANCEL + elif m[1] == "main": + self.simulator_state.cruise_button = CruiseButtons.MAIN + elif m[0] == "blinker": + if m[1] == "left": + self.simulator_state.left_blinker = True + elif m[1] == "right": + self.simulator_state.right_blinker = True + elif m[0] == "ignition": + self.simulator_state.ignition = not self.simulator_state.ignition + elif m[0] == "reset": + self.world.reset() + elif m[0] == "quit": + break self.simulator_state.user_brake = brake_manual self.simulator_state.user_gas = throttle_manual From 1cd0edba34ac8a0d3a3cbccf5bf537836fb9ca00 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Mon, 29 Apr 2024 00:02:35 -0400 Subject: [PATCH 10/46] update test --- tools/sim/bridge/common.py | 17 ++++++++++++----- tools/sim/bridge/metadrive/metadrive_world.py | 13 +++++++------ tools/sim/tests/test_sim_bridge.py | 6 +++--- 3 files changed, 22 insertions(+), 14 deletions(-) diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index 6cc759f9775bf7..a127bde8c0d6ee 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -3,6 +3,7 @@ import functools from collections import namedtuple +from enum import Enum from multiprocessing import Process, Queue, Value from abc import ABC, abstractmethod @@ -17,8 +18,14 @@ QueueMessage = namedtuple("QueueMessage", ["type", "info"], defaults=[None]) +class QueueMessageType(Enum): + START_STATUS = 0 + CONTROL_COMMAND = 1 + TERMINATION_STATUS = 2 + CLOSE_STATUS = 3 + def control_cmd_gen(cmd: str): - return QueueMessage("control_command", cmd) + return QueueMessage(QueueMessageType.CONTROL_COMMAND, cmd) def rk_loop(function, hz, exit_event: threading.Event): rk = Ratekeeper(hz, None) @@ -62,14 +69,14 @@ def bridge_keep_alive(self, q: Queue, retries: int): try: self._run(q) finally: - self.close("close_status", "bridge terminated") + self.close("bridge terminated") - def close(self, closing_type, reason): + def close(self, reason): self.started.value = False self._exit_event.set() if self.world is not None: - self.world.close(closing_type, reason) + self.world.close(reason) def run(self, queue, retries=-1): bridge_p = Process(name="bridge", target=self.bridge_keep_alive, args=(queue, retries)) @@ -118,7 +125,7 @@ def _run(self, q: Queue): # Read manual controls if not q.empty(): message = q.get() - if message.type == "control_command": + if message.type == QueueMessageType.CONTROL_COMMAND: m = message.info.split('_') if m[0] == "steer": steer_manual = float(m[1]) diff --git a/tools/sim/bridge/metadrive/metadrive_world.py b/tools/sim/bridge/metadrive/metadrive_world.py index 33788b8150cd43..70f9fdefaa5eb4 100644 --- a/tools/sim/bridge/metadrive/metadrive_world.py +++ b/tools/sim/bridge/metadrive/metadrive_world.py @@ -6,7 +6,7 @@ from multiprocessing import Pipe, Array -from openpilot.tools.sim.bridge.common import QueueMessage +from openpilot.tools.sim.bridge.common import QueueMessage, QueueMessageType from openpilot.tools.sim.bridge.metadrive.metadrive_process import (metadrive_process, metadrive_simulation_state, metadrive_vehicle_state) from openpilot.tools.sim.lib.common import SimulatorState, World @@ -37,14 +37,14 @@ def __init__(self, status_q, config, dual_camera = False): self.vehicle_state_send, self.exit_event)) self.metadrive_process.start() - self.status_q.put(QueueMessage("start_status", "starting")) + self.status_q.put(QueueMessage(QueueMessageType.START_STATUS, "starting")) print("----------------------------------------------------------") print("---- Spawning Metadrive world, this might take awhile ----") print("----------------------------------------------------------") self.vehicle_state_recv.recv() # wait for a state message to ensure metadrive is launched - self.status_q.put(QueueMessage("start_status", "started")) + self.status_q.put(QueueMessage(QueueMessageType.START_STATUS, "started")) self.steer_ratio = 15 self.vc = [0.0,0.0] @@ -70,7 +70,8 @@ def read_state(self): while self.simulation_state_recv.poll(0): md_state: metadrive_simulation_state = self.simulation_state_recv.recv() if md_state.done: - self.close("termination_status", md_state.done_info) + self.status_q.put(QueueMessage(QueueMessageType.TERMINATION_STATUS, md_state.done_info)) + self.exit_event.set() def read_sensors(self, state: SimulatorState): while self.vehicle_state_recv.poll(0): @@ -90,7 +91,7 @@ def tick(self): def reset(self): self.should_reset = True - def close(self, closing_type, reason): - self.status_q.put(QueueMessage(closing_type, reason)) + def close(self, reason): + self.status_q.put(QueueMessage(QueueMessageType.CLOSE_STATUS, reason)) self.exit_event.set() self.metadrive_process.join() diff --git a/tools/sim/tests/test_sim_bridge.py b/tools/sim/tests/test_sim_bridge.py index 09b08131777fd9..eb3ac677d825db 100644 --- a/tools/sim/tests/test_sim_bridge.py +++ b/tools/sim/tests/test_sim_bridge.py @@ -7,7 +7,7 @@ from cereal import messaging from openpilot.common.basedir import BASEDIR -from openpilot.tools.sim.bridge.common import control_cmd_gen +from openpilot.tools.sim.bridge.common import control_cmd_gen, QueueMessageType SIM_DIR = os.path.join(BASEDIR, "tools/sim") @@ -80,11 +80,11 @@ def test_engage(self): state = None while not q.empty(): state = q.get() - if state.type == "termination_status": + if state.type == QueueMessageType.TERMINATION_STATUS: done_info = state.info failure_states = [done_state for done_state in done_info if done_state != "arrive_dest" and done_info[done_state]] break - self.assertEqual(len(failure_states), 0, f"Simulator fails to finish a loop. Failure states: ${failure_states}") + self.assertEqual(len(failure_states), 0, f"Simulator fails to finish a loop. Failure states: {failure_states}") def tearDown(self): print("Test shutting down. CommIssues are acceptable") From 270e7a5100b20d0790d572914fa56067d16f0054 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Mon, 6 May 2024 23:11:14 -0400 Subject: [PATCH 11/46] add out of lane to local --- .../sim/bridge/metadrive/metadrive_bridge.py | 29 +++++++++++-------- .../sim/bridge/metadrive/metadrive_process.py | 11 ++++--- tools/sim/bridge/metadrive/metadrive_world.py | 4 +-- 3 files changed, 26 insertions(+), 18 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index 7b696bb84076cc..8da1679a54a6ef 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -27,11 +27,11 @@ def curve_block(length, angle=45, direction=0): "dir": direction } -def create_map(track_size=60): - return dict( +def create_map(track_size=60, ci=False): + mtd_map = dict( type=MapGenerateMethod.PG_MAP_FILE, lane_num=2, - lane_width=3.5, + lane_width=4, config=[ None, straight_block(track_size), @@ -45,8 +45,13 @@ def create_map(track_size=60): ] ) -failure_config = namedtuple("failure_config", - ["out_of_route_done", "on_continuous_line_done", "on_broken_line_done"], + if ci: + mtd_map["config"] = mtd_map["config"][:-3] + + return mtd_map + +done_config = namedtuple("done_config", + ["out_of_route_done", "on_continuous_line_done", "arrive_dest"], defaults=[False, False, False]) class MetaDriveBridge(SimulatorBridge): @@ -54,8 +59,9 @@ class MetaDriveBridge(SimulatorBridge): def __init__(self, dual_camera, high_quality, track_size=60, ci=False): self.should_render = False - self.failure_config = failure_config(True, True, True) if ci else failure_config() + self.done_config = done_config(True, True, True) if ci else done_config() self.track_size = track_size + self.ci = ci super().__init__(dual_camera, high_quality) @@ -77,17 +83,16 @@ def spawn_world(self, queue: Queue): image_on_cuda=_cuda_enable, image_observation=True, interface_panel=[], - out_of_route_done=self.failure_config.out_of_route_done, - on_continuous_line_done=self.failure_config.on_continuous_line_done, - on_broken_line_done=self.failure_config.on_broken_line_done, + out_of_route_done=self.done_config.out_of_route_done, + on_continuous_line_done=self.done_config.on_continuous_line_done, crash_vehicle_done=False, crash_object_done=False, - arrive_dest_done=False, + arrive_dest_done=self.done_config.arrive_dest, traffic_density=0.0, # traffic is incredibly expensive - map_config=create_map(self.track_size), + map_config=create_map(self.track_size, self.ci), decision_repeat=1, physics_world_step_size=self.TICKS_PER_FRAME/100, preload_models=False ) - return MetaDriveWorld(queue, config, self.dual_camera) + return MetaDriveWorld(queue, config, self.dual_camera, self.ci) diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py index c50f26642e799b..de72eaf931f727 100644 --- a/tools/sim/bridge/metadrive/metadrive_process.py +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -49,7 +49,7 @@ def arrive_destination_patch(self, *args, **kwargs): def metadrive_process(dual_camera: bool, config: dict, camera_array, wide_camera_array, image_lock, controls_recv: Connection, simulation_state_send: Connection, vehicle_state_send: Connection, - exit_event): + exit_event, ci): arrive_dest_done = config.pop("arrive_dest_done", True) apply_metadrive_patches(arrive_dest_done) @@ -78,7 +78,7 @@ def get_cam_as_rgb(cam): cam.get_cam().reparentTo(env.vehicle.origin) cam.get_cam().setPos(C3_POSITION) cam.get_cam().setHpr(C3_HPR) - img = cam.perceive(to_float=False) + img = cam.perceive(clip=False) if type(img) != np.ndarray: img = img.get() # convert cupy array to numpy return img @@ -113,8 +113,11 @@ def get_cam_as_rgb(cam): if rk.frame % 5 == 0: obs, _, terminated, _, info = env.step(vc) - if terminated: - done_result = env.done_function("default_agent") + if terminated or (env.vehicle.on_broken_line and ci): + if terminated: + done_result = env.done_function("default_agent") + elif env.vehicle.on_broken_line: + done_result = (True, {"out_of_road" : True}) simulation_state = metadrive_simulation_state( running=False, done=done_result[0], diff --git a/tools/sim/bridge/metadrive/metadrive_world.py b/tools/sim/bridge/metadrive/metadrive_world.py index 70f9fdefaa5eb4..34333ad7e9f790 100644 --- a/tools/sim/bridge/metadrive/metadrive_world.py +++ b/tools/sim/bridge/metadrive/metadrive_world.py @@ -14,7 +14,7 @@ class MetaDriveWorld(World): - def __init__(self, status_q, config, dual_camera = False): + def __init__(self, status_q, config, dual_camera=False, ci=False): super().__init__(dual_camera) self.status_q = status_q self.camera_array = Array(ctypes.c_uint8, W*H*3) @@ -34,7 +34,7 @@ def __init__(self, status_q, config, dual_camera = False): functools.partial(metadrive_process, dual_camera, config, self.camera_array, self.wide_camera_array, self.image_lock, self.controls_recv, self.simulation_state_send, - self.vehicle_state_send, self.exit_event)) + self.vehicle_state_send, self.exit_event, ci)) self.metadrive_process.start() self.status_q.put(QueueMessage(QueueMessageType.START_STATUS, "starting")) From a808cba587ef22f05163fc522e36681b095c5d85 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Wed, 15 May 2024 14:05:57 -0400 Subject: [PATCH 12/46] ci arrive_dest --- tools/sim/bridge/metadrive/metadrive_bridge.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index 8da1679a54a6ef..03c7c976392a4e 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -44,9 +44,8 @@ def create_map(track_size=60, ci=False): curve_block(track_size*2, 90), ] ) - if ci: - mtd_map["config"] = mtd_map["config"][:-3] + mtd_map["config"] = mtd_map["config"][1:] return mtd_map From 46fda0bc79a94e81cf49c37f3517a05683e64ca9 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sat, 18 May 2024 12:33:03 -0400 Subject: [PATCH 13/46] pytest integration --- opendbc | 2 +- panda | 2 +- .../sim/bridge/metadrive/metadrive_bridge.py | 2 + tools/sim/tests/conftest.py | 8 ++++ tools/sim/tests/test_metadrive_bridge.py | 18 ++++---- tools/sim/tests/test_sim_bridge.py | 44 ++++++++++++------- 6 files changed, 50 insertions(+), 26 deletions(-) create mode 100644 tools/sim/tests/conftest.py diff --git a/opendbc b/opendbc index 91a9bb4824381c..d53c8f558cc486 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 91a9bb4824381c39b8a15b443d5b265ec550b62b +Subproject commit d53c8f558cc4861ce6563498a3663ee9b57daf4b diff --git a/panda b/panda index 2cf3b84c77f6ba..cade0d5e75cdd6 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 2cf3b84c77f6ba541619d53edc291b5b94033821 +Subproject commit cade0d5e75cdd6a659e1ded21a45d8f3c6c62c18 diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index 03c7c976392a4e..43b0f5cf5b5151 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -44,6 +44,8 @@ def create_map(track_size=60, ci=False): curve_block(track_size*2, 90), ] ) + + # None block is to make sure we have a complete loop, but having this would make metadrive detect wrong destination for run in CI to complete if ci: mtd_map["config"] = mtd_map["config"][1:] diff --git a/tools/sim/tests/conftest.py b/tools/sim/tests/conftest.py new file mode 100644 index 00000000000000..5d050d896614de --- /dev/null +++ b/tools/sim/tests/conftest.py @@ -0,0 +1,8 @@ +import pytest + +def pytest_addoption(parser): + parser.addoption("--track_size", action="store", default=60, type=int, help="Set the TRACK_SIZE for tests") + +@pytest.fixture +def track_size(request): + return request.config.getoption("--track_size") \ No newline at end of file diff --git a/tools/sim/tests/test_metadrive_bridge.py b/tools/sim/tests/test_metadrive_bridge.py index ea9bc5209846d5..e0df7a7e58fe91 100755 --- a/tools/sim/tests/test_metadrive_bridge.py +++ b/tools/sim/tests/test_metadrive_bridge.py @@ -1,7 +1,9 @@ #!/usr/bin/env python3 import pytest -import unittest -import sys +import warnings + +# Since metadrive depends on pkg_resources, and pkg_resources is deprecated as an API +warnings.filterwarnings("ignore", category=DeprecationWarning) from openpilot.tools.sim.bridge.metadrive.metadrive_bridge import MetaDriveBridge from openpilot.tools.sim.tests.test_sim_bridge import TestSimBridgeBase @@ -10,11 +12,9 @@ class TestMetaDriveBridge(TestSimBridgeBase): TRACK_SIZE = 60 - def create_bridge(self): - return MetaDriveBridge(False, False, self.TRACK_SIZE, True) - + @pytest.fixture(autouse=True) + def setup_class(self, track_size): + self.track_size = track_size -if __name__ == "__main__": - if len(sys.argv) > 1: - TestMetaDriveBridge.TRACK_SIZE = int(sys.argv.pop()) - unittest.main() + def create_bridge(self): + return MetaDriveBridge(False, False, self.track_size, True) \ No newline at end of file diff --git a/tools/sim/tests/test_sim_bridge.py b/tools/sim/tests/test_sim_bridge.py index 9b3cd0d71675c7..07c777a280e7dd 100644 --- a/tools/sim/tests/test_sim_bridge.py +++ b/tools/sim/tests/test_sim_bridge.py @@ -1,7 +1,7 @@ import os import subprocess import time -import unittest +import pytest from multiprocessing import Queue @@ -11,13 +11,13 @@ SIM_DIR = os.path.join(BASEDIR, "tools/sim") -class TestSimBridgeBase(unittest.TestCase): +class TestSimBridgeBase: @classmethod - def setUpClass(cls): + def setup_class(cls): if cls is TestSimBridgeBase: - raise unittest.SkipTest("Don't run this base class, run test_metadrive_bridge.py instead") + raise pytest.skip("Don't run this base class, run test_metadrive_bridge.py instead") - def setUp(self): + def setup_method(self): self.processes = [] def test_engage(self): @@ -37,7 +37,7 @@ def test_engage(self): start_waiting = time.monotonic() while not bridge.started.value and time.monotonic() < start_waiting + max_time_per_step: time.sleep(0.1) - self.assertEqual(p_bridge.exitcode, None, f"Bridge process should be running, but exited with code {p_bridge.exitcode}") + assert p_bridge.exitcode is None, f"Bridge process should be running, but exited with code {p_bridge.exitcode}" start_time = time.monotonic() no_car_events_issues_once = False @@ -53,8 +53,8 @@ def test_engage(self): no_car_events_issues_once = True break - self.assertTrue(no_car_events_issues_once, - f"Failed because no messages received, or CarEvents '{car_event_issues}' or processes not running '{not_running}'") + assert no_car_events_issues_once, \ + f"Failed because no messages received, or CarEvents '{car_event_issues}' or processes not running '{not_running}'" start_time = time.monotonic() min_counts_control_active = 100 @@ -69,7 +69,25 @@ def test_engage(self): if control_active == min_counts_control_active: break - self.assertEqual(min_counts_control_active, control_active, f"Simulator did not engage a minimal of {min_counts_control_active} steps was {control_active}") + assert min_counts_control_active == control_active, f"Simulator did not engage a minimal of {min_counts_control_active} steps was {control_active}" + + @pytest.mark.skipif(bool(int(os.getenv("CI", 0))), reason="slow on GHA") + def test_driving(self): + p_manager = subprocess.Popen("./launch_openpilot.sh", cwd=SIM_DIR) + self.processes.append(p_manager) + + q = Queue() + bridge = self.create_bridge() + p_bridge = bridge.run(q, retries=10) + self.processes.append(p_bridge) + + max_time_per_step = 60 + + # Wait for bridge to startup + start_waiting = time.monotonic() + while not bridge.started.value and time.monotonic() < start_waiting + max_time_per_step: + time.sleep(0.1) + assert p_bridge.exitcode is None, f"Bridge process should be running, but exited with code {p_bridge.exitcode}" failure_states = [] while bridge.started.value: @@ -82,9 +100,9 @@ def test_engage(self): done_info = state.info failure_states = [done_state for done_state in done_info if done_state != "arrive_dest" and done_info[done_state]] break - self.assertEqual(len(failure_states), 0, f"Simulator fails to finish a loop. Failure states: {failure_states}") + assert len(failure_states) == 0, f"Simulator fails to finish a loop. Failure states: {failure_states}" - def tearDown(self): + def teardown_method(self): print("Test shutting down. CommIssues are acceptable") for p in reversed(self.processes): p.terminate() @@ -94,7 +112,3 @@ def tearDown(self): p.wait(15) else: p.join(15) - - -if __name__ == "__main__": - unittest.main() From 9f905fae334799327f233cd41ecd86dadf5d184f Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sat, 18 May 2024 13:06:07 -0400 Subject: [PATCH 14/46] update ci_config --- .../sim/bridge/metadrive/metadrive_bridge.py | 61 +++++++++---------- tools/sim/tests/test_sim_bridge.py | 2 +- 2 files changed, 30 insertions(+), 33 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index 43b0f5cf5b5151..bbbe692aaf139d 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -27,45 +27,42 @@ def curve_block(length, angle=45, direction=0): "dir": direction } -def create_map(track_size=60, ci=False): - mtd_map = dict( - type=MapGenerateMethod.PG_MAP_FILE, - lane_num=2, - lane_width=4, - config=[ - None, - straight_block(track_size), - curve_block(track_size*2, 90), - straight_block(track_size), - curve_block(track_size*2, 90), - straight_block(track_size), - curve_block(track_size*2, 90), - straight_block(track_size), - curve_block(track_size*2, 90), - ] - ) - - # None block is to make sure we have a complete loop, but having this would make metadrive detect wrong destination for run in CI to complete - if ci: - mtd_map["config"] = mtd_map["config"][1:] - - return mtd_map - -done_config = namedtuple("done_config", - ["out_of_route_done", "on_continuous_line_done", "arrive_dest"], - defaults=[False, False, False]) +ci_config = namedtuple("ci_config", ["out_of_route_done", "on_continuous_line_done", "arrive_dest"], defaults=[False, False, False]) + class MetaDriveBridge(SimulatorBridge): TICKS_PER_FRAME = 5 def __init__(self, dual_camera, high_quality, track_size=60, ci=False): self.should_render = False - self.done_config = done_config(True, True, True) if ci else done_config() + self.ci_config = ci_config(True, True, True) if ci else ci_config() self.track_size = track_size self.ci = ci super().__init__(dual_camera, high_quality) + def create_map(self, track_size=60): + mtd_map = dict( + type=MapGenerateMethod.PG_MAP_FILE, + lane_num=2, + lane_width=4, + config=[ + None, + straight_block(track_size), + curve_block(track_size*2, 90), + straight_block(track_size), + curve_block(track_size*2, 90), + straight_block(track_size), + curve_block(track_size*2, 90), + straight_block(track_size), + curve_block(track_size*2, 90), + ] + ) + # None block is to make sure we have a complete loop, but having this would make metadrive detect wrong destination for run in CI to complete + mtd_map["config"] = mtd_map["config"][1:] if self.ci else mtd_map["config"] + + return mtd_map + def spawn_world(self, queue: Queue): sensors = { "rgb_road": (RGBCameraRoad, W, H, ) @@ -84,13 +81,13 @@ def spawn_world(self, queue: Queue): image_on_cuda=_cuda_enable, image_observation=True, interface_panel=[], - out_of_route_done=self.done_config.out_of_route_done, - on_continuous_line_done=self.done_config.on_continuous_line_done, + out_of_route_done=self.ci_config.out_of_route_done, + on_continuous_line_done=self.ci_config.on_continuous_line_done, crash_vehicle_done=False, crash_object_done=False, - arrive_dest_done=self.done_config.arrive_dest, + arrive_dest_done=self.ci_config.arrive_dest, traffic_density=0.0, # traffic is incredibly expensive - map_config=create_map(self.track_size, self.ci), + map_config=self.create_map(self.track_size), decision_repeat=1, physics_world_step_size=self.TICKS_PER_FRAME/100, preload_models=False diff --git a/tools/sim/tests/test_sim_bridge.py b/tools/sim/tests/test_sim_bridge.py index 07c777a280e7dd..a975365cb66688 100644 --- a/tools/sim/tests/test_sim_bridge.py +++ b/tools/sim/tests/test_sim_bridge.py @@ -96,7 +96,7 @@ def test_driving(self): state = None while not q.empty(): state = q.get() - if state.type == QueueMessageType.TERMINATION_STATUS: + if state.type == QueueMessageType.TERMINATION_INFO: done_info = state.info failure_states = [done_state for done_state in done_info if done_state != "arrive_dest" and done_info[done_state]] break From ff672678cbabfdb5a31d880f9286f54b62d76b44 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sat, 18 May 2024 13:08:51 -0400 Subject: [PATCH 15/46] fix ruff --- tools/sim/tests/conftest.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/sim/tests/conftest.py b/tools/sim/tests/conftest.py index 5d050d896614de..14b500d9edf917 100644 --- a/tools/sim/tests/conftest.py +++ b/tools/sim/tests/conftest.py @@ -5,4 +5,4 @@ def pytest_addoption(parser): @pytest.fixture def track_size(request): - return request.config.getoption("--track_size") \ No newline at end of file + return request.config.getoption("--track_size") From caa5e8eb0615cbc3e1d66eeedd636c715796c09b Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sat, 18 May 2024 16:22:43 -0400 Subject: [PATCH 16/46] move test termination to time --- .../sim/bridge/metadrive/metadrive_bridge.py | 57 ++++++++++--------- .../sim/bridge/metadrive/metadrive_process.py | 11 +++- tools/sim/bridge/metadrive/metadrive_world.py | 5 +- tools/sim/tests/conftest.py | 6 +- tools/sim/tests/test_metadrive_bridge.py | 6 +- tools/sim/tests/test_sim_bridge.py | 4 +- 6 files changed, 50 insertions(+), 39 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index bbbe692aaf139d..c6999be3b8d5bd 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -1,3 +1,5 @@ +import math + from collections import namedtuple from multiprocessing import Queue @@ -27,41 +29,40 @@ def curve_block(length, angle=45, direction=0): "dir": direction } -ci_config = namedtuple("ci_config", ["out_of_route_done", "on_continuous_line_done", "arrive_dest"], defaults=[False, False, False]) +def create_map(track_size=60): + mtd_map = dict( + type=MapGenerateMethod.PG_MAP_FILE, + lane_num=2, + lane_width=4, + config=[ + None, + straight_block(track_size), + curve_block(track_size*2, 90), + straight_block(track_size), + curve_block(track_size*2, 90), + straight_block(track_size), + curve_block(track_size*2, 90), + straight_block(track_size), + curve_block(track_size*2, 90), + ] + ) + + return mtd_map + +ci_config = namedtuple("ci_config", ["out_of_route_done", "on_continuous_line_done"], defaults=[False, False]) class MetaDriveBridge(SimulatorBridge): TICKS_PER_FRAME = 5 - def __init__(self, dual_camera, high_quality, track_size=60, ci=False): + def __init__(self, dual_camera, high_quality, time_done=math.inf, ci=False): self.should_render = False - self.ci_config = ci_config(True, True, True) if ci else ci_config() - self.track_size = track_size + self.ci_config = ci_config(True, True) if ci else ci_config() self.ci = ci + self.time_done = time_done if self.ci else math.inf super().__init__(dual_camera, high_quality) - def create_map(self, track_size=60): - mtd_map = dict( - type=MapGenerateMethod.PG_MAP_FILE, - lane_num=2, - lane_width=4, - config=[ - None, - straight_block(track_size), - curve_block(track_size*2, 90), - straight_block(track_size), - curve_block(track_size*2, 90), - straight_block(track_size), - curve_block(track_size*2, 90), - straight_block(track_size), - curve_block(track_size*2, 90), - ] - ) - # None block is to make sure we have a complete loop, but having this would make metadrive detect wrong destination for run in CI to complete - mtd_map["config"] = mtd_map["config"][1:] if self.ci else mtd_map["config"] - - return mtd_map def spawn_world(self, queue: Queue): sensors = { @@ -85,12 +86,12 @@ def spawn_world(self, queue: Queue): on_continuous_line_done=self.ci_config.on_continuous_line_done, crash_vehicle_done=False, crash_object_done=False, - arrive_dest_done=self.ci_config.arrive_dest, + arrive_dest_done=False, traffic_density=0.0, # traffic is incredibly expensive - map_config=self.create_map(self.track_size), + map_config=create_map(), decision_repeat=1, physics_world_step_size=self.TICKS_PER_FRAME/100, preload_models=False ) - return MetaDriveWorld(queue, config, self.dual_camera, self.ci) + return MetaDriveWorld(queue, config, self.time_done, self.dual_camera, self.ci) diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py index de72eaf931f727..083d17e64a3b39 100644 --- a/tools/sim/bridge/metadrive/metadrive_process.py +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -1,4 +1,5 @@ import math +import time import numpy as np from collections import namedtuple @@ -49,7 +50,7 @@ def arrive_destination_patch(self, *args, **kwargs): def metadrive_process(dual_camera: bool, config: dict, camera_array, wide_camera_array, image_lock, controls_recv: Connection, simulation_state_send: Connection, vehicle_state_send: Connection, - exit_event, ci): + exit_event, start_time, ci, time_done): arrive_dest_done = config.pop("arrive_dest_done", True) apply_metadrive_patches(arrive_dest_done) @@ -113,11 +114,17 @@ def get_cam_as_rgb(cam): if rk.frame % 5 == 0: obs, _, terminated, _, info = env.step(vc) - if terminated or (env.vehicle.on_broken_line and ci): + current_time = time.monotonic() + time_out = True if current_time - start_time >= time_done else False + + if terminated or ((env.vehicle.on_broken_line or time_out) and ci): if terminated: done_result = env.done_function("default_agent") elif env.vehicle.on_broken_line: done_result = (True, {"out_of_road" : True}) + elif time_out: + done_result = (True, {"time_done" : True}) + simulation_state = metadrive_simulation_state( running=False, done=done_result[0], diff --git a/tools/sim/bridge/metadrive/metadrive_world.py b/tools/sim/bridge/metadrive/metadrive_world.py index e5b94ad6f8b971..8db6b21e38c5b7 100644 --- a/tools/sim/bridge/metadrive/metadrive_world.py +++ b/tools/sim/bridge/metadrive/metadrive_world.py @@ -14,7 +14,7 @@ class MetaDriveWorld(World): - def __init__(self, status_q, config, dual_camera=False, ci=False): + def __init__(self, status_q, config, time_done, dual_camera=False, ci=False): super().__init__(dual_camera) self.status_q = status_q self.camera_array = Array(ctypes.c_uint8, W*H*3) @@ -30,11 +30,12 @@ def __init__(self, status_q, config, dual_camera=False, ci=False): self.exit_event = multiprocessing.Event() + start_time = time.monotonic() self.metadrive_process = multiprocessing.Process(name="metadrive process", target= functools.partial(metadrive_process, dual_camera, config, self.camera_array, self.wide_camera_array, self.image_lock, self.controls_recv, self.simulation_state_send, - self.vehicle_state_send, self.exit_event, ci)) + self.vehicle_state_send, self.exit_event, start_time, ci, time_done)) self.metadrive_process.start() self.status_q.put(QueueMessage(QueueMessageType.START_STATUS, "starting")) diff --git a/tools/sim/tests/conftest.py b/tools/sim/tests/conftest.py index 14b500d9edf917..5b9102d3be97c5 100644 --- a/tools/sim/tests/conftest.py +++ b/tools/sim/tests/conftest.py @@ -1,8 +1,8 @@ import pytest def pytest_addoption(parser): - parser.addoption("--track_size", action="store", default=60, type=int, help="Set the TRACK_SIZE for tests") + parser.addoption("--time_done", action="store", default=60, type=int, help="Seconds to run metadrive drive") @pytest.fixture -def track_size(request): - return request.config.getoption("--track_size") +def time_done(request): + return request.config.getoption("--time_done") diff --git a/tools/sim/tests/test_metadrive_bridge.py b/tools/sim/tests/test_metadrive_bridge.py index ed0172dc901d26..06f76b71594432 100755 --- a/tools/sim/tests/test_metadrive_bridge.py +++ b/tools/sim/tests/test_metadrive_bridge.py @@ -11,8 +11,8 @@ @pytest.mark.slow class TestMetaDriveBridge(TestSimBridgeBase): @pytest.fixture(autouse=True) - def setup_class(self, track_size): - self.track_size = track_size + def setup_class(self, time_done): + self.time_done = time_done def create_bridge(self): - return MetaDriveBridge(False, False, self.track_size, True) + return MetaDriveBridge(False, False, self.time_done, True) diff --git a/tools/sim/tests/test_sim_bridge.py b/tools/sim/tests/test_sim_bridge.py index a975365cb66688..a326d7250598a7 100644 --- a/tools/sim/tests/test_sim_bridge.py +++ b/tools/sim/tests/test_sim_bridge.py @@ -71,6 +71,7 @@ def test_engage(self): assert min_counts_control_active == control_active, f"Simulator did not engage a minimal of {min_counts_control_active} steps was {control_active}" + # TODO: merge this into the above test to be run in GHA (another bounty) @pytest.mark.skipif(bool(int(os.getenv("CI", 0))), reason="slow on GHA") def test_driving(self): p_manager = subprocess.Popen("./launch_openpilot.sh", cwd=SIM_DIR) @@ -89,6 +90,7 @@ def test_driving(self): time.sleep(0.1) assert p_bridge.exitcode is None, f"Bridge process should be running, but exited with code {p_bridge.exitcode}" + success_done_states = ("arrive_dest", "time_done") failure_states = [] while bridge.started.value: continue @@ -98,7 +100,7 @@ def test_driving(self): state = q.get() if state.type == QueueMessageType.TERMINATION_INFO: done_info = state.info - failure_states = [done_state for done_state in done_info if done_state != "arrive_dest" and done_info[done_state]] + failure_states = [done_state for done_state in done_info if done_state not in success_done_states and done_info[done_state]] break assert len(failure_states) == 0, f"Simulator fails to finish a loop. Failure states: {failure_states}" From b0479e41877d411b98a7de1172c590b04b3eddc4 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sat, 18 May 2024 16:25:09 -0400 Subject: [PATCH 17/46] better --- tools/sim/bridge/metadrive/metadrive_bridge.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index c6999be3b8d5bd..26cf0a0781e561 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -30,7 +30,7 @@ def curve_block(length, angle=45, direction=0): } def create_map(track_size=60): - mtd_map = dict( + return dict( type=MapGenerateMethod.PG_MAP_FILE, lane_num=2, lane_width=4, @@ -47,8 +47,6 @@ def create_map(track_size=60): ] ) - return mtd_map - ci_config = namedtuple("ci_config", ["out_of_route_done", "on_continuous_line_done"], defaults=[False, False]) From dd0d851f1a4fcfb2542f5c907e6bdd9d3f6eebe2 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sat, 18 May 2024 16:48:22 -0400 Subject: [PATCH 18/46] better order --- tools/sim/bridge/metadrive/metadrive_process.py | 2 +- tools/sim/bridge/metadrive/metadrive_world.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py index 083d17e64a3b39..2027cf6aa9dee9 100644 --- a/tools/sim/bridge/metadrive/metadrive_process.py +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -50,7 +50,7 @@ def arrive_destination_patch(self, *args, **kwargs): def metadrive_process(dual_camera: bool, config: dict, camera_array, wide_camera_array, image_lock, controls_recv: Connection, simulation_state_send: Connection, vehicle_state_send: Connection, - exit_event, start_time, ci, time_done): + exit_event, start_time, time_done, ci): arrive_dest_done = config.pop("arrive_dest_done", True) apply_metadrive_patches(arrive_dest_done) diff --git a/tools/sim/bridge/metadrive/metadrive_world.py b/tools/sim/bridge/metadrive/metadrive_world.py index 8db6b21e38c5b7..aed58dfebcd73a 100644 --- a/tools/sim/bridge/metadrive/metadrive_world.py +++ b/tools/sim/bridge/metadrive/metadrive_world.py @@ -35,7 +35,7 @@ def __init__(self, status_q, config, time_done, dual_camera=False, ci=False): functools.partial(metadrive_process, dual_camera, config, self.camera_array, self.wide_camera_array, self.image_lock, self.controls_recv, self.simulation_state_send, - self.vehicle_state_send, self.exit_event, start_time, ci, time_done)) + self.vehicle_state_send, self.exit_event, start_time, time_done, ci)) self.metadrive_process.start() self.status_q.put(QueueMessage(QueueMessageType.START_STATUS, "starting")) From 4898dcd0df6641b4f35f9670542f5630c628a6fc Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sat, 18 May 2024 20:43:23 -0400 Subject: [PATCH 19/46] curve_len --- tools/sim/bridge/metadrive/metadrive_bridge.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index 26cf0a0781e561..c10525d619073d 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -30,20 +30,21 @@ def curve_block(length, angle=45, direction=0): } def create_map(track_size=60): + curve_len = track_size * 2 return dict( type=MapGenerateMethod.PG_MAP_FILE, lane_num=2, - lane_width=4, + lane_width=4.5, config=[ None, straight_block(track_size), - curve_block(track_size*2, 90), + curve_block(curve_len, 90), straight_block(track_size), - curve_block(track_size*2, 90), + curve_block(curve_len, 90), straight_block(track_size), - curve_block(track_size*2, 90), + curve_block(curve_len, 90), straight_block(track_size), - curve_block(track_size*2, 90), + curve_block(curve_len, 90), ] ) From f0ef637b0949193b7668d1c9cca2be1d7b186ae6 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sat, 18 May 2024 20:57:56 -0400 Subject: [PATCH 20/46] add buffer --- tools/sim/bridge/metadrive/metadrive_bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index c10525d619073d..d4a94f98bd5f6c 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -30,7 +30,7 @@ def curve_block(length, angle=45, direction=0): } def create_map(track_size=60): - curve_len = track_size * 2 + curve_len = track_size * 2 + 10 return dict( type=MapGenerateMethod.PG_MAP_FILE, lane_num=2, From 7ff5d22edc56ed4aab2a047590ba68f540f116e7 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sat, 18 May 2024 21:00:20 -0400 Subject: [PATCH 21/46] cleanup --- tools/sim/bridge/metadrive/metadrive_process.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py index 2027cf6aa9dee9..5ca3a519df5c5e 100644 --- a/tools/sim/bridge/metadrive/metadrive_process.py +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -113,9 +113,7 @@ def get_cam_as_rgb(cam): if rk.frame % 5 == 0: obs, _, terminated, _, info = env.step(vc) - - current_time = time.monotonic() - time_out = True if current_time - start_time >= time_done else False + time_out = True if time.monotonic() - start_time >= time_done else False if terminated or ((env.vehicle.on_broken_line or time_out) and ci): if terminated: From affc13e1b1c9c08a7418c84cdae96b06e2dd99b6 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sat, 18 May 2024 21:03:32 -0400 Subject: [PATCH 22/46] cleanup --- tools/sim/bridge/metadrive/metadrive_bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index d4a94f98bd5f6c..cd76ab4ad0e988 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -56,9 +56,9 @@ class MetaDriveBridge(SimulatorBridge): def __init__(self, dual_camera, high_quality, time_done=math.inf, ci=False): self.should_render = False - self.ci_config = ci_config(True, True) if ci else ci_config() self.ci = ci self.time_done = time_done if self.ci else math.inf + self.ci_config = ci_config(True, True) if self.ci else ci_config() super().__init__(dual_camera, high_quality) From 77bab202f24ac147aee94514a389b48876c4b5bf Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sat, 18 May 2024 21:24:05 -0400 Subject: [PATCH 23/46] cleanup --- tools/sim/bridge/metadrive/metadrive_bridge.py | 18 +++++++++--------- .../sim/bridge/metadrive/metadrive_process.py | 4 ++-- tools/sim/bridge/metadrive/metadrive_world.py | 4 ++-- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index cd76ab4ad0e988..3f6e2f0b9eb754 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -30,7 +30,7 @@ def curve_block(length, angle=45, direction=0): } def create_map(track_size=60): - curve_len = track_size * 2 + 10 + curve_len = track_size * 2 + 10 # emperically reliable for driving test return dict( type=MapGenerateMethod.PG_MAP_FILE, lane_num=2, @@ -48,17 +48,17 @@ def create_map(track_size=60): ] ) -ci_config = namedtuple("ci_config", ["out_of_route_done", "on_continuous_line_done"], defaults=[False, False]) +done_config = namedtuple("done_config", ["out_of_route_done", "on_continuous_line_done"], defaults=[False, False]) class MetaDriveBridge(SimulatorBridge): TICKS_PER_FRAME = 5 - def __init__(self, dual_camera, high_quality, time_done=math.inf, ci=False): + def __init__(self, dual_camera, high_quality, time_done=math.inf, test_run=False): self.should_render = False - self.ci = ci - self.time_done = time_done if self.ci else math.inf - self.ci_config = ci_config(True, True) if self.ci else ci_config() + self.test_run = test_run + self.time_done = time_done if self.test_run else math.inf + self.done_config = done_config(True, True) if self.test_run else done_config() super().__init__(dual_camera, high_quality) @@ -81,8 +81,8 @@ def spawn_world(self, queue: Queue): image_on_cuda=_cuda_enable, image_observation=True, interface_panel=[], - out_of_route_done=self.ci_config.out_of_route_done, - on_continuous_line_done=self.ci_config.on_continuous_line_done, + out_of_route_done=self.done_config.out_of_route_done, + on_continuous_line_done=self.done_config.on_continuous_line_done, crash_vehicle_done=False, crash_object_done=False, arrive_dest_done=False, @@ -93,4 +93,4 @@ def spawn_world(self, queue: Queue): preload_models=False ) - return MetaDriveWorld(queue, config, self.time_done, self.dual_camera, self.ci) + return MetaDriveWorld(queue, config, self.time_done, self.test_run, self.dual_camera) diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py index 5ca3a519df5c5e..93c79a8dbc1794 100644 --- a/tools/sim/bridge/metadrive/metadrive_process.py +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -50,7 +50,7 @@ def arrive_destination_patch(self, *args, **kwargs): def metadrive_process(dual_camera: bool, config: dict, camera_array, wide_camera_array, image_lock, controls_recv: Connection, simulation_state_send: Connection, vehicle_state_send: Connection, - exit_event, start_time, time_done, ci): + exit_event, start_time, time_done, test_run): arrive_dest_done = config.pop("arrive_dest_done", True) apply_metadrive_patches(arrive_dest_done) @@ -115,7 +115,7 @@ def get_cam_as_rgb(cam): obs, _, terminated, _, info = env.step(vc) time_out = True if time.monotonic() - start_time >= time_done else False - if terminated or ((env.vehicle.on_broken_line or time_out) and ci): + if terminated or ((env.vehicle.on_broken_line or time_out) and test_run): if terminated: done_result = env.done_function("default_agent") elif env.vehicle.on_broken_line: diff --git a/tools/sim/bridge/metadrive/metadrive_world.py b/tools/sim/bridge/metadrive/metadrive_world.py index aed58dfebcd73a..15277da28ad92e 100644 --- a/tools/sim/bridge/metadrive/metadrive_world.py +++ b/tools/sim/bridge/metadrive/metadrive_world.py @@ -14,7 +14,7 @@ class MetaDriveWorld(World): - def __init__(self, status_q, config, time_done, dual_camera=False, ci=False): + def __init__(self, status_q, config, time_done, test_run, dual_camera=False): super().__init__(dual_camera) self.status_q = status_q self.camera_array = Array(ctypes.c_uint8, W*H*3) @@ -35,7 +35,7 @@ def __init__(self, status_q, config, time_done, dual_camera=False, ci=False): functools.partial(metadrive_process, dual_camera, config, self.camera_array, self.wide_camera_array, self.image_lock, self.controls_recv, self.simulation_state_send, - self.vehicle_state_send, self.exit_event, start_time, time_done, ci)) + self.vehicle_state_send, self.exit_event, start_time, time_done, test_run)) self.metadrive_process.start() self.status_q.put(QueueMessage(QueueMessageType.START_STATUS, "starting")) From 93ee681e8be1e46151a103927a6ca6eec7a73c2d Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 19 May 2024 00:00:37 -0400 Subject: [PATCH 24/46] cleanup --- tools/sim/bridge/metadrive/metadrive_bridge.py | 2 +- tools/sim/tests/test_sim_bridge.py | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index 3f6e2f0b9eb754..aee0e21a0f312a 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -30,7 +30,7 @@ def curve_block(length, angle=45, direction=0): } def create_map(track_size=60): - curve_len = track_size * 2 + 10 # emperically reliable for driving test + curve_len = track_size * 2 + 10 # empirically reliable for driving test return dict( type=MapGenerateMethod.PG_MAP_FILE, lane_num=2, diff --git a/tools/sim/tests/test_sim_bridge.py b/tools/sim/tests/test_sim_bridge.py index a326d7250598a7..8a3492affc3385 100644 --- a/tools/sim/tests/test_sim_bridge.py +++ b/tools/sim/tests/test_sim_bridge.py @@ -90,7 +90,6 @@ def test_driving(self): time.sleep(0.1) assert p_bridge.exitcode is None, f"Bridge process should be running, but exited with code {p_bridge.exitcode}" - success_done_states = ("arrive_dest", "time_done") failure_states = [] while bridge.started.value: continue @@ -100,7 +99,7 @@ def test_driving(self): state = q.get() if state.type == QueueMessageType.TERMINATION_INFO: done_info = state.info - failure_states = [done_state for done_state in done_info if done_state not in success_done_states and done_info[done_state]] + failure_states = [done_state for done_state in done_info if done_state != "time_done" and done_info[done_state]] break assert len(failure_states) == 0, f"Simulator fails to finish a loop. Failure states: {failure_states}" From 306d2d7aa2edc0910c0a1f04f44dce7c4a5d1367 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 19 May 2024 00:11:22 -0400 Subject: [PATCH 25/46] out_of_lane --- tools/sim/bridge/metadrive/metadrive_process.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py index 93c79a8dbc1794..611d5feb6fbd0a 100644 --- a/tools/sim/bridge/metadrive/metadrive_process.py +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -119,7 +119,7 @@ def get_cam_as_rgb(cam): if terminated: done_result = env.done_function("default_agent") elif env.vehicle.on_broken_line: - done_result = (True, {"out_of_road" : True}) + done_result = (True, {"out_of_lane" : True}) elif time_out: done_result = (True, {"time_done" : True}) From bb5f8936dc798419641a0448447b0e798c6f4714 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 19 May 2024 00:19:01 -0400 Subject: [PATCH 26/46] cleanup --- tools/sim/bridge/metadrive/metadrive_bridge.py | 1 - 1 file changed, 1 deletion(-) diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index aee0e21a0f312a..8a52441a634e28 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -62,7 +62,6 @@ def __init__(self, dual_camera, high_quality, time_done=math.inf, test_run=False super().__init__(dual_camera, high_quality) - def spawn_world(self, queue: Queue): sensors = { "rgb_road": (RGBCameraRoad, W, H, ) From 7076a12a752244d0c41123f0611be466800bd19b Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 19 May 2024 00:28:19 -0400 Subject: [PATCH 27/46] merge tests --- tools/sim/tests/test_sim_bridge.py | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/tools/sim/tests/test_sim_bridge.py b/tools/sim/tests/test_sim_bridge.py index 8a3492affc3385..c287b1578c762a 100644 --- a/tools/sim/tests/test_sim_bridge.py +++ b/tools/sim/tests/test_sim_bridge.py @@ -71,25 +71,6 @@ def test_engage(self): assert min_counts_control_active == control_active, f"Simulator did not engage a minimal of {min_counts_control_active} steps was {control_active}" - # TODO: merge this into the above test to be run in GHA (another bounty) - @pytest.mark.skipif(bool(int(os.getenv("CI", 0))), reason="slow on GHA") - def test_driving(self): - p_manager = subprocess.Popen("./launch_openpilot.sh", cwd=SIM_DIR) - self.processes.append(p_manager) - - q = Queue() - bridge = self.create_bridge() - p_bridge = bridge.run(q, retries=10) - self.processes.append(p_bridge) - - max_time_per_step = 60 - - # Wait for bridge to startup - start_waiting = time.monotonic() - while not bridge.started.value and time.monotonic() < start_waiting + max_time_per_step: - time.sleep(0.1) - assert p_bridge.exitcode is None, f"Bridge process should be running, but exited with code {p_bridge.exitcode}" - failure_states = [] while bridge.started.value: continue From f9ded06e41ecd96228ee1874ffd4e4599b4290d8 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 19 May 2024 00:33:43 -0400 Subject: [PATCH 28/46] run 90s --- .github/workflows/tools_tests.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/tools_tests.yaml b/.github/workflows/tools_tests.yaml index f33f8b77571d82..04a18946a889b6 100644 --- a/.github/workflows/tools_tests.yaml +++ b/.github/workflows/tools_tests.yaml @@ -76,7 +76,7 @@ jobs: ${{ env.RUN }} "export MAPBOX_TOKEN='pk.eyJ1Ijoiam5ld2IiLCJhIjoiY2xxNW8zZXprMGw1ZzJwbzZneHd2NHljbSJ9.gV7VPRfbXFetD-1OVF0XZg' && \ source selfdrive/test/setup_xvfb.sh && \ source selfdrive/test/setup_vsound.sh && \ - CI=1 pytest tools/sim/tests/test_metadrive_bridge.py -W ignore::pyopencl.CompilerWarning" + CI=1 pytest tools/sim/tests/test_metadrive_bridge.py --time_done=90 -W ignore::pyopencl.CompilerWarning" devcontainer: name: devcontainer From 858d617b33d854dc32ae1958e6afe1e95e2bad02 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 19 May 2024 16:20:51 -0400 Subject: [PATCH 29/46] change test name --- tools/sim/tests/test_sim_bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/sim/tests/test_sim_bridge.py b/tools/sim/tests/test_sim_bridge.py index c287b1578c762a..35b81adecd37b1 100644 --- a/tools/sim/tests/test_sim_bridge.py +++ b/tools/sim/tests/test_sim_bridge.py @@ -20,7 +20,7 @@ def setup_class(cls): def setup_method(self): self.processes = [] - def test_engage(self): + def test_driving(self): # Startup manager and bridge.py. Check processes are running, then engage and verify. p_manager = subprocess.Popen("./launch_openpilot.sh", cwd=SIM_DIR) self.processes.append(p_manager) From dfc7bd324b717d18bd7be820c35750155e2e2a12 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 19 May 2024 18:47:28 -0400 Subject: [PATCH 30/46] local out of lane detect --- tools/sim/bridge/metadrive/metadrive_bridge.py | 9 +++------ tools/sim/bridge/metadrive/metadrive_process.py | 5 +++-- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index 8a52441a634e28..379b235176937c 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -30,7 +30,7 @@ def curve_block(length, angle=45, direction=0): } def create_map(track_size=60): - curve_len = track_size * 2 + 10 # empirically reliable for driving test + curve_len = track_size * 2 return dict( type=MapGenerateMethod.PG_MAP_FILE, lane_num=2, @@ -48,8 +48,6 @@ def create_map(track_size=60): ] ) -done_config = namedtuple("done_config", ["out_of_route_done", "on_continuous_line_done"], defaults=[False, False]) - class MetaDriveBridge(SimulatorBridge): TICKS_PER_FRAME = 5 @@ -58,7 +56,6 @@ def __init__(self, dual_camera, high_quality, time_done=math.inf, test_run=False self.should_render = False self.test_run = test_run self.time_done = time_done if self.test_run else math.inf - self.done_config = done_config(True, True) if self.test_run else done_config() super().__init__(dual_camera, high_quality) @@ -80,8 +77,8 @@ def spawn_world(self, queue: Queue): image_on_cuda=_cuda_enable, image_observation=True, interface_panel=[], - out_of_route_done=self.done_config.out_of_route_done, - on_continuous_line_done=self.done_config.on_continuous_line_done, + out_of_route_done=False, + on_continuous_line_done=False, crash_vehicle_done=False, crash_object_done=False, arrive_dest_done=False, diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py index 611d5feb6fbd0a..1e9acfa5eaca5e 100644 --- a/tools/sim/bridge/metadrive/metadrive_process.py +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -112,10 +112,11 @@ def get_cam_as_rgb(cam): reset() if rk.frame % 5 == 0: - obs, _, terminated, _, info = env.step(vc) + _, _, terminated, _, _ = env.step(vc) time_out = True if time.monotonic() - start_time >= time_done else False + out_of_lane = env.vehicle.on_broken_line or env.vehicle.on_yellow_continuous_line or env.vehicle.on_white_continuous_line or env.vehicle.crash_sidewalk - if terminated or ((env.vehicle.on_broken_line or time_out) and test_run): + if terminated or ((out_of_lane or time_out) and test_run): if terminated: done_result = env.done_function("default_agent") elif env.vehicle.on_broken_line: From c4ae5106a4f38c6d3cd75b12a5c48e34abea2f0f Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 19 May 2024 18:51:31 -0400 Subject: [PATCH 31/46] out_of_lane --- tools/sim/bridge/metadrive/metadrive_process.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py index 1e9acfa5eaca5e..f53b048c5242d4 100644 --- a/tools/sim/bridge/metadrive/metadrive_process.py +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -119,7 +119,7 @@ def get_cam_as_rgb(cam): if terminated or ((out_of_lane or time_out) and test_run): if terminated: done_result = env.done_function("default_agent") - elif env.vehicle.on_broken_line: + elif out_of_lane: done_result = (True, {"out_of_lane" : True}) elif time_out: done_result = (True, {"time_done" : True}) From febab140e19bced0e9097459ef5c963be65af438 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 19 May 2024 19:46:20 -0400 Subject: [PATCH 32/46] static anal --- tools/sim/bridge/metadrive/metadrive_bridge.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index 379b235176937c..994a28d26fa937 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -1,6 +1,4 @@ import math - -from collections import namedtuple from multiprocessing import Queue from metadrive.component.sensors.base_camera import _cuda_enable From 05b9b492a0f894ed51cdfabe54dbcc24aa95d02d Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Sun, 19 May 2024 21:25:41 -0400 Subject: [PATCH 33/46] cleanup --- tools/sim/tests/test_sim_bridge.py | 1 - 1 file changed, 1 deletion(-) diff --git a/tools/sim/tests/test_sim_bridge.py b/tools/sim/tests/test_sim_bridge.py index 35b81adecd37b1..d807f011e1cbd3 100644 --- a/tools/sim/tests/test_sim_bridge.py +++ b/tools/sim/tests/test_sim_bridge.py @@ -75,7 +75,6 @@ def test_driving(self): while bridge.started.value: continue - state = None while not q.empty(): state = q.get() if state.type == QueueMessageType.TERMINATION_INFO: From 4579458189a967c7ff5e0f97e573b5025dbd8f1d Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Mon, 20 May 2024 09:40:17 -0400 Subject: [PATCH 34/46] test_duration --- .github/workflows/tools_tests.yaml | 2 +- tools/sim/bridge/metadrive/metadrive_bridge.py | 6 +++--- tools/sim/bridge/metadrive/metadrive_process.py | 10 +++++----- tools/sim/bridge/metadrive/metadrive_world.py | 4 ++-- tools/sim/tests/conftest.py | 6 +++--- tools/sim/tests/test_metadrive_bridge.py | 6 +++--- tools/sim/tests/test_sim_bridge.py | 2 +- 7 files changed, 18 insertions(+), 18 deletions(-) diff --git a/.github/workflows/tools_tests.yaml b/.github/workflows/tools_tests.yaml index 04a18946a889b6..aff0db4076cebf 100644 --- a/.github/workflows/tools_tests.yaml +++ b/.github/workflows/tools_tests.yaml @@ -76,7 +76,7 @@ jobs: ${{ env.RUN }} "export MAPBOX_TOKEN='pk.eyJ1Ijoiam5ld2IiLCJhIjoiY2xxNW8zZXprMGw1ZzJwbzZneHd2NHljbSJ9.gV7VPRfbXFetD-1OVF0XZg' && \ source selfdrive/test/setup_xvfb.sh && \ source selfdrive/test/setup_vsound.sh && \ - CI=1 pytest tools/sim/tests/test_metadrive_bridge.py --time_done=90 -W ignore::pyopencl.CompilerWarning" + CI=1 pytest tools/sim/tests/test_metadrive_bridge.py --test_duration=90 -W ignore::pyopencl.CompilerWarning" devcontainer: name: devcontainer diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index 994a28d26fa937..aa5eaf45c457c9 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -50,10 +50,10 @@ def create_map(track_size=60): class MetaDriveBridge(SimulatorBridge): TICKS_PER_FRAME = 5 - def __init__(self, dual_camera, high_quality, time_done=math.inf, test_run=False): + def __init__(self, dual_camera, high_quality, test_duration=math.inf, test_run=False): self.should_render = False self.test_run = test_run - self.time_done = time_done if self.test_run else math.inf + self.test_duration = test_duration if self.test_run else math.inf super().__init__(dual_camera, high_quality) @@ -87,4 +87,4 @@ def spawn_world(self, queue: Queue): preload_models=False ) - return MetaDriveWorld(queue, config, self.time_done, self.test_run, self.dual_camera) + return MetaDriveWorld(queue, config, self.test_duration, self.test_run, self.dual_camera) diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py index f53b048c5242d4..d96c155a35ff8e 100644 --- a/tools/sim/bridge/metadrive/metadrive_process.py +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -50,7 +50,7 @@ def arrive_destination_patch(self, *args, **kwargs): def metadrive_process(dual_camera: bool, config: dict, camera_array, wide_camera_array, image_lock, controls_recv: Connection, simulation_state_send: Connection, vehicle_state_send: Connection, - exit_event, start_time, time_done, test_run): + exit_event, start_time, test_duration, test_run): arrive_dest_done = config.pop("arrive_dest_done", True) apply_metadrive_patches(arrive_dest_done) @@ -113,16 +113,16 @@ def get_cam_as_rgb(cam): if rk.frame % 5 == 0: _, _, terminated, _, _ = env.step(vc) - time_out = True if time.monotonic() - start_time >= time_done else False + timeout = True if time.monotonic() - start_time >= test_duration else False out_of_lane = env.vehicle.on_broken_line or env.vehicle.on_yellow_continuous_line or env.vehicle.on_white_continuous_line or env.vehicle.crash_sidewalk - if terminated or ((out_of_lane or time_out) and test_run): + if terminated or ((out_of_lane or timeout) and test_run): if terminated: done_result = env.done_function("default_agent") elif out_of_lane: done_result = (True, {"out_of_lane" : True}) - elif time_out: - done_result = (True, {"time_done" : True}) + elif timeout: + done_result = (True, {"timeout" : True}) simulation_state = metadrive_simulation_state( running=False, diff --git a/tools/sim/bridge/metadrive/metadrive_world.py b/tools/sim/bridge/metadrive/metadrive_world.py index 15277da28ad92e..3341de00300041 100644 --- a/tools/sim/bridge/metadrive/metadrive_world.py +++ b/tools/sim/bridge/metadrive/metadrive_world.py @@ -14,7 +14,7 @@ class MetaDriveWorld(World): - def __init__(self, status_q, config, time_done, test_run, dual_camera=False): + def __init__(self, status_q, config, test_duration, test_run, dual_camera=False): super().__init__(dual_camera) self.status_q = status_q self.camera_array = Array(ctypes.c_uint8, W*H*3) @@ -35,7 +35,7 @@ def __init__(self, status_q, config, time_done, test_run, dual_camera=False): functools.partial(metadrive_process, dual_camera, config, self.camera_array, self.wide_camera_array, self.image_lock, self.controls_recv, self.simulation_state_send, - self.vehicle_state_send, self.exit_event, start_time, time_done, test_run)) + self.vehicle_state_send, self.exit_event, start_time, test_duration, test_run)) self.metadrive_process.start() self.status_q.put(QueueMessage(QueueMessageType.START_STATUS, "starting")) diff --git a/tools/sim/tests/conftest.py b/tools/sim/tests/conftest.py index 5b9102d3be97c5..ddf66352767502 100644 --- a/tools/sim/tests/conftest.py +++ b/tools/sim/tests/conftest.py @@ -1,8 +1,8 @@ import pytest def pytest_addoption(parser): - parser.addoption("--time_done", action="store", default=60, type=int, help="Seconds to run metadrive drive") + parser.addoption("--test_duration", action="store", default=60, type=int, help="Seconds to run metadrive drive") @pytest.fixture -def time_done(request): - return request.config.getoption("--time_done") +def test_duration(request): + return request.config.getoption("--test_duration") diff --git a/tools/sim/tests/test_metadrive_bridge.py b/tools/sim/tests/test_metadrive_bridge.py index 06f76b71594432..f3959b3878c616 100755 --- a/tools/sim/tests/test_metadrive_bridge.py +++ b/tools/sim/tests/test_metadrive_bridge.py @@ -11,8 +11,8 @@ @pytest.mark.slow class TestMetaDriveBridge(TestSimBridgeBase): @pytest.fixture(autouse=True) - def setup_class(self, time_done): - self.time_done = time_done + def setup_class(self, test_duration): + self.test_duration = test_duration def create_bridge(self): - return MetaDriveBridge(False, False, self.time_done, True) + return MetaDriveBridge(False, False, self.test_duration, True) diff --git a/tools/sim/tests/test_sim_bridge.py b/tools/sim/tests/test_sim_bridge.py index d807f011e1cbd3..aaa90f153fd7e2 100644 --- a/tools/sim/tests/test_sim_bridge.py +++ b/tools/sim/tests/test_sim_bridge.py @@ -79,7 +79,7 @@ def test_driving(self): state = q.get() if state.type == QueueMessageType.TERMINATION_INFO: done_info = state.info - failure_states = [done_state for done_state in done_info if done_state != "time_done" and done_info[done_state]] + failure_states = [done_state for done_state in done_info if done_state != "timeout" and done_info[done_state]] break assert len(failure_states) == 0, f"Simulator fails to finish a loop. Failure states: {failure_states}" From 6717bac241b66c8eb2ccd1780fef99f7b89b9115 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Mon, 20 May 2024 13:21:40 -0400 Subject: [PATCH 35/46] change setup_class -> setup_create_bridge --- tools/sim/tests/test_metadrive_bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/sim/tests/test_metadrive_bridge.py b/tools/sim/tests/test_metadrive_bridge.py index f3959b3878c616..bdfce44fa33119 100755 --- a/tools/sim/tests/test_metadrive_bridge.py +++ b/tools/sim/tests/test_metadrive_bridge.py @@ -11,7 +11,7 @@ @pytest.mark.slow class TestMetaDriveBridge(TestSimBridgeBase): @pytest.fixture(autouse=True) - def setup_class(self, test_duration): + def setup_create_bridge(self, test_duration): self.test_duration = test_duration def create_bridge(self): From 3b644dd4fb47396f1cd8fc30d13ae4b5e8853def Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Mon, 20 May 2024 18:02:29 -0400 Subject: [PATCH 36/46] no print state during test --- tools/sim/bridge/common.py | 4 +++- tools/sim/bridge/metadrive/metadrive_bridge.py | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index 275a9a7640e5fa..5cb520f6345f7c 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -60,6 +60,8 @@ def __init__(self, dual_camera, high_quality): self.past_startup_engaged = False self.startup_button_prev = True + self.test_run = False + def _on_shutdown(self, signal, frame): self.shutdown() @@ -194,7 +196,7 @@ def _run(self, q: Queue): self.world.read_cameras() if self.rk.frame % 25 == 0: - self.print_status() + not self.test_run and self.print_status() # don't print during test, so no print/IO Block between OP and metadrive proc self.started.value = True diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index aa5eaf45c457c9..74dc1210b9518b 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -51,12 +51,12 @@ class MetaDriveBridge(SimulatorBridge): TICKS_PER_FRAME = 5 def __init__(self, dual_camera, high_quality, test_duration=math.inf, test_run=False): + super().__init__(dual_camera, high_quality) + self.should_render = False self.test_run = test_run self.test_duration = test_duration if self.test_run else math.inf - super().__init__(dual_camera, high_quality) - def spawn_world(self, queue: Queue): sensors = { "rgb_road": (RGBCameraRoad, W, H, ) From d70e9f81ade432bab78e5404fe84b70337587291 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Tue, 21 May 2024 10:18:57 -0400 Subject: [PATCH 37/46] new out_of_lane detect --- tools/sim/bridge/metadrive/metadrive_process.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py index d96c155a35ff8e..1507684b04e026 100644 --- a/tools/sim/bridge/metadrive/metadrive_process.py +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -61,9 +61,15 @@ def metadrive_process(dual_camera: bool, config: dict, camera_array, wide_camera env = MetaDriveEnv(config) + def get_current_lane_info(vehicle): + _, lane_info, on_lane = vehicle.navigation._get_current_lane(vehicle) + lane_idx = lane_info[2] if lane_info is not None else None + return lane_idx, on_lane + def reset(): env.reset() env.vehicle.config["max_speed_km_h"] = 1000 + lane_idx_prev, _ = get_current_lane_info(env.vehicle) simulation_state = metadrive_simulation_state( running=True, @@ -72,7 +78,9 @@ def reset(): ) simulation_state_send.send(simulation_state) - reset() + return lane_idx_prev + + lane_idx_prev = reset() def get_cam_as_rgb(cam): cam = env.engine.sensors[cam] @@ -114,7 +122,8 @@ def get_cam_as_rgb(cam): if rk.frame % 5 == 0: _, _, terminated, _, _ = env.step(vc) timeout = True if time.monotonic() - start_time >= test_duration else False - out_of_lane = env.vehicle.on_broken_line or env.vehicle.on_yellow_continuous_line or env.vehicle.on_white_continuous_line or env.vehicle.crash_sidewalk + lane_idx_curr, on_lane = get_current_lane_info(env.vehicle) + out_of_lane = lane_idx_curr != lane_idx_prev or not on_lane if terminated or ((out_of_lane or timeout) and test_run): if terminated: From 80fe90cac602f2a3c8757de3ecb7199372925903 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Tue, 21 May 2024 10:20:08 -0400 Subject: [PATCH 38/46] cleanup print in common.py --- tools/sim/bridge/common.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index 5cb520f6345f7c..64407143f7765e 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -195,8 +195,9 @@ def _run(self, q: Queue): self.world.tick() self.world.read_cameras() - if self.rk.frame % 25 == 0: - not self.test_run and self.print_status() # don't print during test, so no print/IO Block between OP and metadrive proc + # don't print during test, so no print/IO Block between OP and metadrive processes + if not self.test_run and self.rk.frame % 25 == 0: + self.print_status() self.started.value = True From 7b19b9b461bf714564658e22113ba0556a02fe10 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Tue, 21 May 2024 10:49:29 -0400 Subject: [PATCH 39/46] fix --- tools/sim/bridge/metadrive/metadrive_process.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py index 1507684b04e026..90305f2aef83bc 100644 --- a/tools/sim/bridge/metadrive/metadrive_process.py +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -117,7 +117,7 @@ def get_cam_as_rgb(cam): vc = [steer_metadrive, gas] if should_reset: - reset() + lane_idx_prev = reset() if rk.frame % 5 == 0: _, _, terminated, _, _ = env.step(vc) From 9f052e64632c83af60e5c5fc2a2afdba50874164 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Tue, 21 May 2024 11:02:00 -0400 Subject: [PATCH 40/46] fix --- tools/sim/bridge/metadrive/metadrive_process.py | 1 + 1 file changed, 1 insertion(+) diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py index 90305f2aef83bc..ec10b96b3eec05 100644 --- a/tools/sim/bridge/metadrive/metadrive_process.py +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -124,6 +124,7 @@ def get_cam_as_rgb(cam): timeout = True if time.monotonic() - start_time >= test_duration else False lane_idx_curr, on_lane = get_current_lane_info(env.vehicle) out_of_lane = lane_idx_curr != lane_idx_prev or not on_lane + lane_idx_prev = lane_idx_curr if terminated or ((out_of_lane or timeout) and test_run): if terminated: From 5ba473e8c84e6111433520fd4c9214a2c6027542 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Wed, 22 May 2024 00:14:36 -0400 Subject: [PATCH 41/46] check distance vs time --- tools/sim/bridge/metadrive/metadrive_world.py | 40 +++++++++++++++++-- 1 file changed, 36 insertions(+), 4 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_world.py b/tools/sim/bridge/metadrive/metadrive_world.py index 3341de00300041..b11cccb7999271 100644 --- a/tools/sim/bridge/metadrive/metadrive_world.py +++ b/tools/sim/bridge/metadrive/metadrive_world.py @@ -30,12 +30,18 @@ def __init__(self, status_q, config, test_duration, test_run, dual_camera=False) self.exit_event = multiprocessing.Event() - start_time = time.monotonic() + self.test_run = test_run + + self.start_time = time.monotonic() + self.first_engage = None + self.last_check_timestamp = 0 + self.distance_moved = 0 + self.metadrive_process = multiprocessing.Process(name="metadrive process", target= functools.partial(metadrive_process, dual_camera, config, self.camera_array, self.wide_camera_array, self.image_lock, self.controls_recv, self.simulation_state_send, - self.vehicle_state_send, self.exit_event, start_time, test_duration, test_run)) + self.vehicle_state_send, self.exit_event, self.start_time, test_duration, self.test_run)) self.metadrive_process.start() self.status_q.put(QueueMessage(QueueMessageType.START_STATUS, "starting")) @@ -44,7 +50,7 @@ def __init__(self, status_q, config, test_duration, test_run, dual_camera=False) print("---- Spawning Metadrive world, this might take awhile ----") print("----------------------------------------------------------") - self.vehicle_state_recv.recv() # wait for a state message to ensure metadrive is launched + self.vehicle_last_pos = self.vehicle_state_recv.recv().position # wait for a state message to ensure metadrive is launched self.status_q.put(QueueMessage(QueueMessageType.START_STATUS, "started")) self.steer_ratio = 15 @@ -77,12 +83,38 @@ def read_state(self): def read_sensors(self, state: SimulatorState): while self.vehicle_state_recv.poll(0): md_vehicle: metadrive_vehicle_state = self.vehicle_state_recv.recv() + curr_pos = md_vehicle.position + state.velocity = md_vehicle.velocity state.bearing = md_vehicle.bearing state.steering_angle = md_vehicle.steering_angle - state.gps.from_xy(md_vehicle.position) + state.gps.from_xy(curr_pos) state.valid = True + is_engaged = state.is_engaged + if is_engaged and self.first_engage is None: + self.first_engage = time.monotonic() + not_moving_after_engaged = is_engaged and time.monotonic() - self.first_engage >= 5 and self.test_run # check moving 5 seconds after engaged + + x_dist = abs(curr_pos[0] - self.vehicle_last_pos[0]) + y_dist = abs(curr_pos[1] - self.vehicle_last_pos[1]) + dist_threshold = 1 + if x_dist >= dist_threshold or y_dist >= dist_threshold: # position not the same during staying still, > threshold is considered moving + self.distance_moved += x_dist + y_dist + + time_check_threshold = 5 + current_time = time.monotonic() + since_last_check = current_time - self.last_check_timestamp + if since_last_check >= time_check_threshold and not_moving_after_engaged and self.distance_moved == 0: + self.status_q.put(QueueMessage(QueueMessageType.TERMINATION_INFO, {"vehicle_not_moving" : True})) + self.exit_event.set() + + # update every seconds + if since_last_check >= time_check_threshold: + self.last_check_timestamp = current_time + self.distance_moved = 0 + self.vehicle_last_pos = curr_pos + def read_cameras(self): pass From dfc14ec8e52393cfbe3b180da0cea066e9160d69 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Wed, 22 May 2024 00:17:17 -0400 Subject: [PATCH 42/46] cleanup --- tools/sim/bridge/metadrive/metadrive_world.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_world.py b/tools/sim/bridge/metadrive/metadrive_world.py index b11cccb7999271..ec987e0a8478f5 100644 --- a/tools/sim/bridge/metadrive/metadrive_world.py +++ b/tools/sim/bridge/metadrive/metadrive_world.py @@ -94,7 +94,7 @@ def read_sensors(self, state: SimulatorState): is_engaged = state.is_engaged if is_engaged and self.first_engage is None: self.first_engage = time.monotonic() - not_moving_after_engaged = is_engaged and time.monotonic() - self.first_engage >= 5 and self.test_run # check moving 5 seconds after engaged + not_moving_after_engaged = is_engaged and time.monotonic() - self.first_engage >= 5 and self.test_run # check moving 5 seconds after engaged, doesn't move right away x_dist = abs(curr_pos[0] - self.vehicle_last_pos[0]) y_dist = abs(curr_pos[1] - self.vehicle_last_pos[1]) @@ -109,7 +109,6 @@ def read_sensors(self, state: SimulatorState): self.status_q.put(QueueMessage(QueueMessageType.TERMINATION_INFO, {"vehicle_not_moving" : True})) self.exit_event.set() - # update every seconds if since_last_check >= time_check_threshold: self.last_check_timestamp = current_time self.distance_moved = 0 From 6479e3333a427a799aabc5c4302be59b97d48077 Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Wed, 22 May 2024 00:48:30 -0400 Subject: [PATCH 43/46] cleanup increase check time --- tools/sim/bridge/metadrive/metadrive_world.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_world.py b/tools/sim/bridge/metadrive/metadrive_world.py index ec987e0a8478f5..c264c70f4f5a68 100644 --- a/tools/sim/bridge/metadrive/metadrive_world.py +++ b/tools/sim/bridge/metadrive/metadrive_world.py @@ -94,7 +94,8 @@ def read_sensors(self, state: SimulatorState): is_engaged = state.is_engaged if is_engaged and self.first_engage is None: self.first_engage = time.monotonic() - not_moving_after_engaged = is_engaged and time.monotonic() - self.first_engage >= 5 and self.test_run # check moving 5 seconds after engaged, doesn't move right away + # check moving 5 seconds after engaged, doesn't move right away + after_engaged_check = is_engaged and time.monotonic() - self.first_engage >= 5 and self.test_run x_dist = abs(curr_pos[0] - self.vehicle_last_pos[0]) y_dist = abs(curr_pos[1] - self.vehicle_last_pos[1]) @@ -102,10 +103,10 @@ def read_sensors(self, state: SimulatorState): if x_dist >= dist_threshold or y_dist >= dist_threshold: # position not the same during staying still, > threshold is considered moving self.distance_moved += x_dist + y_dist - time_check_threshold = 5 + time_check_threshold = 30 current_time = time.monotonic() since_last_check = current_time - self.last_check_timestamp - if since_last_check >= time_check_threshold and not_moving_after_engaged and self.distance_moved == 0: + if since_last_check >= time_check_threshold and after_engaged_check and self.distance_moved == 0: self.status_q.put(QueueMessage(QueueMessageType.TERMINATION_INFO, {"vehicle_not_moving" : True})) self.exit_event.set() From 71ba53d1d31cf34c0037081f86417c87ac468dbc Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Wed, 22 May 2024 00:57:39 -0400 Subject: [PATCH 44/46] minimum bridge test time --- tools/sim/tests/test_metadrive_bridge.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/tools/sim/tests/test_metadrive_bridge.py b/tools/sim/tests/test_metadrive_bridge.py index 962c0500fa6b9e..d413b2293b7778 100755 --- a/tools/sim/tests/test_metadrive_bridge.py +++ b/tools/sim/tests/test_metadrive_bridge.py @@ -13,6 +13,9 @@ class TestMetaDriveBridge(TestSimBridgeBase): @pytest.fixture(autouse=True) def setup_create_bridge(self, test_duration): + # run bridge test for at least 60s, since not-moving check runs after 30s + if test_duration < 60: + test_duration = 60 self.test_duration = test_duration def create_bridge(self): From 8c0d071630bd1b208cf1fc52006138c463ad951c Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Wed, 22 May 2024 01:01:02 -0400 Subject: [PATCH 45/46] wording --- tools/sim/tests/test_metadrive_bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/sim/tests/test_metadrive_bridge.py b/tools/sim/tests/test_metadrive_bridge.py index d413b2293b7778..f06110184b00ef 100755 --- a/tools/sim/tests/test_metadrive_bridge.py +++ b/tools/sim/tests/test_metadrive_bridge.py @@ -13,7 +13,7 @@ class TestMetaDriveBridge(TestSimBridgeBase): @pytest.fixture(autouse=True) def setup_create_bridge(self, test_duration): - # run bridge test for at least 60s, since not-moving check runs after 30s + # run bridge test for at least 60s, since not-moving check runs every 30s if test_duration < 60: test_duration = 60 self.test_duration = test_duration From 2b4363143ea7e7c09fb7568c74628a105d7afa3b Mon Sep 17 00:00:00 2001 From: bongbui321 Date: Wed, 22 May 2024 11:56:39 -0400 Subject: [PATCH 46/46] cleanup --- tools/sim/bridge/metadrive/metadrive_world.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tools/sim/bridge/metadrive/metadrive_world.py b/tools/sim/bridge/metadrive/metadrive_world.py index c264c70f4f5a68..5d1a2f307459e1 100644 --- a/tools/sim/bridge/metadrive/metadrive_world.py +++ b/tools/sim/bridge/metadrive/metadrive_world.py @@ -106,11 +106,11 @@ def read_sensors(self, state: SimulatorState): time_check_threshold = 30 current_time = time.monotonic() since_last_check = current_time - self.last_check_timestamp - if since_last_check >= time_check_threshold and after_engaged_check and self.distance_moved == 0: - self.status_q.put(QueueMessage(QueueMessageType.TERMINATION_INFO, {"vehicle_not_moving" : True})) - self.exit_event.set() - if since_last_check >= time_check_threshold: + if after_engaged_check and self.distance_moved == 0: + self.status_q.put(QueueMessage(QueueMessageType.TERMINATION_INFO, {"vehicle_not_moving" : True})) + self.exit_event.set() + self.last_check_timestamp = current_time self.distance_moved = 0 self.vehicle_last_pos = curr_pos