diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index ae5a372f2bae264..7ee9ac38d183ccd 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -21,7 +21,8 @@ "--volume=/tmp/comma_download_cache:/tmp/comma_download_cache", "--volume=/tmp/devcontainer_scons_cache:/tmp/scons_cache", "--shm-size=1G", - "--add-host=host.docker.internal:host-gateway" // required to use host.docker.internal on linux + "--add-host=host.docker.internal:host-gateway", // required to use host.docker.internal on linux + "--publish=0.0.0.0:8070-8079:8070-8079" // body ZMQ services ], "features": { "ghcr.io/devcontainers/features/common-utils:2": { diff --git a/.github/workflows/prebuilt.yaml b/.github/workflows/prebuilt.yaml index 8b16ea90b91b58d..8c31bcaac044485 100644 --- a/.github/workflows/prebuilt.yaml +++ b/.github/workflows/prebuilt.yaml @@ -27,6 +27,7 @@ jobs: - uses: actions/checkout@v3 with: submodules: true + - run: git lfs pull - name: Build and Push docker image run: | $DOCKER_LOGIN diff --git a/Dockerfile.openpilot_base b/Dockerfile.openpilot_base index be151678b32bffe..8b434536df6a06c 100644 --- a/Dockerfile.openpilot_base +++ b/Dockerfile.openpilot_base @@ -38,8 +38,7 @@ COPY --chown=$USER tools/install_python_dependencies.sh /tmp/tools/ RUN cd /tmp && \ tools/install_python_dependencies.sh && \ rm -rf /tmp/* && \ - rm -rf /home/$USER/.cache && \ - pip uninstall -y poetry + rm -rf /home/$USER/.cache USER root RUN sudo git config --global --add safe.directory /tmp/openpilot \ No newline at end of file diff --git a/docs/CARS.md b/docs/CARS.md index ad6c42714336619..8bcc418296964e0 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -168,7 +168,7 @@ A supported vehicle is one that just works when you install a comma device. All |Lincoln|Aviator 2020-21|Co-Pilot360 Plus|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |MAN|eTGE 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 J533 connector
- 1 USB-C coupler
- 1 angled mount (8 degrees)
- 1 comma 3X
- 1 harness box
- 1 long OBD-C cable
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |MAN|TGE 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 J533 connector
- 1 USB-C coupler
- 1 angled mount (8 degrees)
- 1 comma 3X
- 1 harness box
- 1 long OBD-C cable
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| -|Mazda|CX-5 2022-23|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Mazda connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| +|Mazda|CX-5 2022-24|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Mazda connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Mazda connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 Nissan B connector
- 1 RJ45 cable (7 ft)
- 1 USB-C coupler
- 1 comma 3X
- 1 harness box
- 1 long OBD-C cable
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Nissan|Leaf 2018-23|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 Nissan A connector
- 1 RJ45 cable (7 ft)
- 1 USB-C coupler
- 1 comma 3X
- 1 harness box
- 1 long OBD-C cable
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| @@ -210,7 +210,7 @@ A supported vehicle is one that just works when you install a comma device. All |Toyota|Camry 2018-20|All|Stock|0 mph[8](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Toyota|Camry 2021-23|All|openpilot|0 mph[8](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| -|Toyota|Camry Hybrid 2021-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| +|Toyota|Camry Hybrid 2021-24|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Toyota|Corolla 2017-19|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Toyota|Corolla Cross (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index 0230be2297ea934..b638a73e6332b4d 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -46,7 +46,7 @@ class MazdaCarInfo(CarInfo): CAR.MAZDA3: MazdaCarInfo("Mazda 3 2017-18"), CAR.MAZDA6: MazdaCarInfo("Mazda 6 2017-20"), CAR.CX9_2021: MazdaCarInfo("Mazda CX-9 2021-23", video_link="https://youtu.be/dA3duO4a0O4"), - CAR.CX5_2022: MazdaCarInfo("Mazda CX-5 2022-23"), + CAR.CX5_2022: MazdaCarInfo("Mazda CX-5 2022-24"), } @@ -97,6 +97,7 @@ class Buttons: b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x760, None): [ + b'KGWD-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'KSD5-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x706, None): [ @@ -110,6 +111,7 @@ class Buttons: b'SH51-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXDL-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXFG-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PXFG-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, CAR.CX5: { diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 4b69cf07aa0672c..4e78365244bd977 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -131,7 +131,7 @@ class ToyotaCarInfo(CarInfo): CAR.CAMRY: ToyotaCarInfo("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]), CAR.CAMRYH: ToyotaCarInfo("Toyota Camry Hybrid 2018-20", video_link="https://www.youtube.com/watch?v=Q2DYY0AWKgk"), CAR.CAMRY_TSS2: ToyotaCarInfo("Toyota Camry 2021-23", footnotes=[Footnote.CAMRY]), - CAR.CAMRYH_TSS2: ToyotaCarInfo("Toyota Camry Hybrid 2021-23"), + CAR.CAMRYH_TSS2: ToyotaCarInfo("Toyota Camry Hybrid 2021-24"), CAR.CHR: ToyotaCarInfo("Toyota C-HR 2017-20"), CAR.CHR_TSS2: ToyotaCarInfo("Toyota C-HR 2021"), CAR.CHRH: ToyotaCarInfo("Toyota C-HR Hybrid 2017-20"), diff --git a/selfdrive/debug/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py index 6b5ec36935f07de..75409e3f872225b 100755 --- a/selfdrive/debug/vw_mqb_config.py +++ b/selfdrive/debug/vw_mqb_config.py @@ -144,6 +144,7 @@ class ACCESS_TYPE_LEVEL_1(IntEnum): uds_client.write_data_by_identifier(VOLKSWAGEN_DATA_IDENTIFIER_TYPE.CODING, new_coding) # type: ignore except (NegativeResponseError, MessageTimeoutError): print("Writing new configuration failed!") + print("Make sure the comma processes are stopped: tmux kill-session -t comma") quit() try: diff --git a/tools/cabana/signalview.cc b/tools/cabana/signalview.cc index b2798e9f3bcfb17..4a07e25beb9612f 100644 --- a/tools/cabana/signalview.cc +++ b/tools/cabana/signalview.cc @@ -395,7 +395,7 @@ QWidget *SignalItemDelegate::createEditor(QWidget *parent, const QStyleOptionVie else e->setValidator(double_validator); if (item->type == SignalModel::Item::Name) { - QCompleter *completer = new QCompleter(dbc()->signalNames()); + QCompleter *completer = new QCompleter(dbc()->signalNames(), e); completer->setCaseSensitivity(Qt::CaseInsensitive); completer->setFilterMode(Qt::MatchContains); e->setCompleter(completer); diff --git a/tools/sim/README.md b/tools/sim/README.md index 69a89aefab40137..b10c957c3e2dc1c 100644 --- a/tools/sim/README.md +++ b/tools/sim/README.md @@ -1,58 +1,76 @@ openpilot in simulator ===================== -openpilot implements a [bridge](bridge.py) that allows it to run in the [CARLA simulator](https://carla.org/). +openpilot implements a [bridge](run_bridge.py) that allows it to run in the [MetaDrive simulator](https://github.com/metadriverse/metadrive) or [CARLA simulator](https://carla.org/). -## System Requirements +## Launching openpilot +First, start openpilot. +``` bash +# Run locally +./tools/sim/launch_openpilot.sh +``` -openpilot doesn't have any extreme hardware requirements, however CARLA requires an NVIDIA graphics card and is very resource-intensive and may not run smoothly on your system. -For this case, we have the simulator in low quality by default. +## Bridge usage +``` +$ ./run_bridge.py -h +usage: run_bridge.py [-h] [--joystick] [--high_quality] [--dual_camera] [--simulator SIMULATOR] [--town TOWN] [--spawn_point NUM_SELECTED_SPAWN_POINT] [--host HOST] [--port PORT] -You can also check out the [CARLA python documentation](https://carla.readthedocs.io/en/latest/python_api/) to find more parameters to tune that might increase performance on your system. +Bridge between the simulator and openpilot. -## Running the simulator -Start Carla simulator, openpilot and bridge processes located in tools/sim: -``` bash -# Terminal 1 -./start_carla.sh +options: + -h, --help show this help message and exit + --joystick + --high_quality + --dual_camera + --simulator SIMULATOR + --town TOWN + --spawn_point NUM_SELECTED_SPAWN_POINT + --host HOST + --port PORT +``` -# Terminal 2 - Run openpilot and bridge in one Docker: -./start_openpilot_docker.sh +#### Bridge Controls: +- To engage openpilot press 2, then press 1 to increase the speed and 2 to decrease. +- To disengage, press "S" (simulates a user brake) -# Running the latest local code execute - # Terminal 2: - ./launch_openpilot.sh - # Terminal 3 - ./bridge.py -``` +#### All inputs: -### Bridge usage -_Same commands hold for start_openpilot_docker_ ``` -$ ./bridge.py -h -Usage: bridge.py [options] -Bridge between CARLA and openpilot. +| key | functionality | +|------|-----------------------| +| 1 | Cruise Resume / Accel | +| 2 | Cruise Set / Decel | +| 3 | Cruise Cancel | +| r | Reset Simulation | +| i | Toggle Ignition | +| q | Exit all | +| wasd | Control manually | +``` -Options: - -h, --help show this help message and exit - --joystick Use joystick input to control the car - --high_quality Set simulator to higher quality (requires good GPU) - --town TOWN Select map to drive in - --spawn_point NUM Number of the spawn point to start in - --host HOST Host address of Carla client (127.0.0.1 as default) - --port PORT Port of Carla client (2000 as default) +## MetaDrive + +### Launching Metadrive +Start bridge processes located in tools/sim: +``` bash +./run_bridge.py --simulator metadrive ``` -To engage openpilot press 1 a few times while focused on bridge.py to increase the cruise speed. -All inputs: +## Carla -| key | functionality | -|:----:|:-----------------:| -| 1 | Cruise up 1 mph | -| 2 | Cruise down 1 mph | -| 3 | Cruise cancel | -| q | Exit all | -| wasd | Control manually | +openpilot doesn't have any extreme hardware requirements, however CARLA requires an NVIDIA graphics card and is very resource-intensive and may not run smoothly on your system. +For this case, we have the simulator in low quality by default. + +You can also check out the [CARLA python documentation](https://carla.readthedocs.io/en/latest/python_api/) to find more parameters to tune that might increase performance on your system. + +### Launching Carla +Start Carla simulator and bridge processes located in tools/sim: +``` bash +# Terminal 1 +./start_carla.sh + +# Terminal 2 +./run_bridge.py --simulator carla +``` ## Further Reading diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index b48ced1955ebb6f..a0e6538b2ce758c 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -68,7 +68,7 @@ def close(self): self.world.close() def run(self, queue, retries=-1): - bridge_p = Process(target=self.bridge_keep_alive, args=(queue, retries), daemon=True) + bridge_p = Process(name="bridge", target=self.bridge_keep_alive, args=(queue, retries)) bridge_p.start() return bridge_p @@ -96,6 +96,10 @@ def _run(self, q: Queue): 100, self._exit_event)) self.simulated_car_thread.start() + self.simulated_camera_thread = threading.Thread(target=rk_loop, args=(functools.partial(self.simulated_sensors.send_camera_images, self.world), + 20, self._exit_event)) + self.simulated_camera_thread.start() + # Simulation tends to be slow in the initial steps. This prevents lagging later for _ in range(20): self.world.tick() @@ -105,6 +109,8 @@ def _run(self, q: Queue): throttle_op = steer_op = brake_op = 0.0 self.simulator_state.cruise_button = 0 + self.simulator_state.left_blinker = False + self.simulator_state.right_blinker = False throttle_manual = steer_manual = brake_manual = 0. @@ -127,6 +133,11 @@ def _run(self, q: Queue): 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": @@ -136,6 +147,7 @@ def _run(self, q: Queue): self.simulator_state.user_brake = brake_manual self.simulator_state.user_gas = throttle_manual + self.simulator_state.user_torque = steer_manual * 10000 steer_manual = steer_manual * -40 @@ -143,7 +155,8 @@ def _run(self, q: Queue): self.simulated_sensors.update(self.simulator_state, self.world) self.simulated_car.sm.update(0) - self.simulator_state.is_engaged = self.simulated_car.sm['controlsState'].active + controlsState = self.simulated_car.sm['controlsState'] + self.simulator_state.is_engaged = controlsState.active if self.simulator_state.is_engaged: throttle_op = clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 0.0, 1.0) @@ -151,7 +164,7 @@ def _run(self, q: Queue): steer_op = self.simulated_car.sm['carControl'].actuators.steeringAngleDeg self.past_startup_engaged = True - elif not self.past_startup_engaged: + elif not self.past_startup_engaged and controlsState.engageable: self.simulator_state.cruise_button = CruiseButtons.DECEL_SET # force engagement on startup throttle_out = throttle_op if self.simulator_state.is_engaged else throttle_manual diff --git a/tools/sim/bridge/metadrive/metadrive_bridge.py b/tools/sim/bridge/metadrive/metadrive_bridge.py index 35e8180a9261e56..555adc9531b35d7 100644 --- a/tools/sim/bridge/metadrive/metadrive_bridge.py +++ b/tools/sim/bridge/metadrive/metadrive_bridge.py @@ -1,39 +1,50 @@ +import numpy as np + from metadrive.component.sensors.rgb_camera import RGBCamera from metadrive.component.sensors.base_camera import _cuda_enable from metadrive.component.map.pg_map import MapGenerateMethod -from metadrive.engine.core.engine_core import EngineCore -from metadrive.engine.core.image_buffer import ImageBuffer -from metadrive.envs.metadrive_env import MetaDriveEnv -from metadrive.obs.image_obs import ImageObservation -from panda3d.core import Vec3 +from panda3d.core import Vec3, Texture, GraphicsOutput from openpilot.tools.sim.bridge.common import SimulatorBridge from openpilot.tools.sim.bridge.metadrive.metadrive_world import MetaDriveWorld from openpilot.tools.sim.lib.camerad import W, H -def apply_metadrive_patches(): - # By default, metadrive won't try to use cuda images unless it's used as a sensor for vehicles, so patch that in - def add_image_sensor_patched(self, name: str, cls, args): - if self.global_config["image_on_cuda"]:# and name == self.global_config["vehicle_config"]["image_source"]: - sensor = cls(*args, self, cuda=True) - else: - sensor = cls(*args, self, cuda=False) - assert isinstance(sensor, ImageBuffer), "This API is for adding image sensor" - self.sensors[name] = sensor +C3_POSITION = Vec3(0, 0, 1) + - EngineCore.add_image_sensor = add_image_sensor_patched +class CopyRamRGBCamera(RGBCamera): + """Camera which copies its content into RAM during the render process, for faster image grabbing.""" + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.cpu_texture = Texture() + self.buffer.addRenderTexture(self.cpu_texture, GraphicsOutput.RTMCopyRam) - # we aren't going to use the built-in observation stack, so disable it to save time - def observe_patched(self, vehicle): - return self.state + def get_rgb_array_cpu(self): + origin_img = self.cpu_texture + img = np.frombuffer(origin_img.getRamImage().getData(), dtype=np.uint8) + img = img.reshape((origin_img.getYSize(), origin_img.getXSize(), -1)) + img = img[:,:,:3] # RGBA to RGB + # img = np.swapaxes(img, 1, 0) + img = img[::-1] # Flip on vertical axis + return img - ImageObservation.observe = observe_patched - def arrive_destination_patch(self, vehicle): - return False +class RGBCameraWide(CopyRamRGBCamera): + def __init__(self, *args, **kwargs): + super(RGBCameraWide, self).__init__(*args, **kwargs) + cam = self.get_cam() + cam.setPos(C3_POSITION) + lens = self.get_lens() + lens.setFov(160) - MetaDriveEnv._is_arrive_destination = arrive_destination_patch +class RGBCameraRoad(CopyRamRGBCamera): + def __init__(self, *args, **kwargs): + super(RGBCameraRoad, self).__init__(*args, **kwargs) + cam = self.get_cam() + cam.setPos(C3_POSITION) + lens = self.get_lens() + lens.setFov(40) def straight_block(length): @@ -55,7 +66,7 @@ def curve_block(length, angle=45, direction=0): class MetaDriveBridge(SimulatorBridge): - TICKS_PER_FRAME = 2 + TICKS_PER_FRAME = 5 def __init__(self, args): self.should_render = False @@ -63,30 +74,6 @@ def __init__(self, args): super(MetaDriveBridge, self).__init__(args) def spawn_world(self): - print("----------------------------------------------------------") - print("---- Spawning Metadrive world, this might take awhile ----") - print("----------------------------------------------------------") - - apply_metadrive_patches() - - C3_POSITION = Vec3(0, 0, 1) - - class RGBCameraWide(RGBCamera): - def __init__(self, *args, **kwargs): - super(RGBCameraWide, self).__init__(*args, **kwargs) - cam = self.get_cam() - cam.setPos(C3_POSITION) - lens = self.get_lens() - lens.setFov(160) - - class RGBCameraRoad(RGBCamera): - def __init__(self, *args, **kwargs): - super(RGBCameraRoad, self).__init__(*args, **kwargs) - cam = self.get_cam() - cam.setPos(C3_POSITION) - lens = self.get_lens() - lens.setFov(40) - sensors = { "rgb_road": (RGBCameraRoad, W, H, ) } @@ -94,39 +81,38 @@ def __init__(self, *args, **kwargs): if self.dual_camera: sensors["rgb_wide"] = (RGBCameraWide, W, H) - env = MetaDriveEnv( - dict( - use_render=self.should_render, - vehicle_config=dict( - enable_reverse=False, - image_source="rgb_road", - spawn_longitude=15 - ), - sensors=sensors, - image_on_cuda=_cuda_enable, - image_observation=True, - interface_panel=[], - out_of_route_done=False, - on_continuous_line_done=False, - crash_vehicle_done=False, - crash_object_done=False, - map_config=dict( - type=MapGenerateMethod.PG_MAP_FILE, - config=[ - None, - straight_block(120), - curve_block(120, 90), - straight_block(120), - curve_block(120, 90), - straight_block(120), - curve_block(120, 90), - straight_block(120), - curve_block(120, 90), - ] - ) - ) - ) - - env.reset() - - return MetaDriveWorld(env) \ No newline at end of file + config = dict( + use_render=self.should_render, + vehicle_config=dict( + enable_reverse=False, + image_source="rgb_road", + spawn_longitude=15 + ), + sensors=sensors, + image_on_cuda=_cuda_enable, + image_observation=True, + interface_panel=[], + out_of_route_done=False, + on_continuous_line_done=False, + crash_vehicle_done=False, + crash_object_done=False, + traffic_density=0.0, # traffic is incredibly expensive + map_config=dict( + type=MapGenerateMethod.PG_MAP_FILE, + config=[ + None, + straight_block(120), + curve_block(240, 90), + straight_block(120), + curve_block(240, 90), + straight_block(120), + curve_block(240, 90), + straight_block(120), + curve_block(240, 90), + ] + ), + decision_repeat=1, + physics_world_step_size=self.TICKS_PER_FRAME/100 + ) + + return MetaDriveWorld(config) diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py new file mode 100644 index 000000000000000..00dd0371fd97234 --- /dev/null +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -0,0 +1,94 @@ +import math +import numpy as np + +from collections import namedtuple +from multiprocessing.connection import Connection + +from metadrive.engine.core.engine_core import EngineCore +from metadrive.engine.core.image_buffer import ImageBuffer +from metadrive.envs.metadrive_env import MetaDriveEnv +from metadrive.obs.image_obs import ImageObservation + +from openpilot.common.realtime import Ratekeeper +from openpilot.tools.sim.lib.common import vec3 +from openpilot.tools.sim.lib.camerad import W, H + + +metadrive_state = namedtuple("metadrive_state", ["velocity", "position", "bearing", "steering_angle"]) + +def apply_metadrive_patches(): + # By default, metadrive won't try to use cuda images unless it's used as a sensor for vehicles, so patch that in + def add_image_sensor_patched(self, name: str, cls, args): + if self.global_config["image_on_cuda"]:# and name == self.global_config["vehicle_config"]["image_source"]: + sensor = cls(*args, self, cuda=True) + else: + sensor = cls(*args, self, cuda=False) + assert isinstance(sensor, ImageBuffer), "This API is for adding image sensor" + self.sensors[name] = sensor + + EngineCore.add_image_sensor = add_image_sensor_patched + + # we aren't going to use the built-in observation stack, so disable it to save time + def observe_patched(self, vehicle): + return self.state + + ImageObservation.observe = observe_patched + + def arrive_destination_patch(self, vehicle): + return False + + MetaDriveEnv._is_arrive_destination = arrive_destination_patch + +def metadrive_process(dual_camera: bool, config: dict, camera_array, controls_recv: Connection, state_send: Connection, exit_event): + apply_metadrive_patches() + + road_image = np.frombuffer(camera_array.get_obj(), dtype=np.uint8).reshape((H, W, 3)) + + env = MetaDriveEnv(config) + env.reset() + + def get_cam_as_rgb(cam): + cam = env.engine.sensors[cam] + img = cam.perceive(env.vehicle, clip=False) + if type(img) != np.ndarray: + img = img.get() # convert cupy array to numpy + return img + + rk = Ratekeeper(100, None) + + steer_ratio = 15 + vc = [0,0] + + while not exit_event.is_set(): + state = metadrive_state( + velocity=vec3(x=float(env.vehicle.velocity[0]), y=float(env.vehicle.velocity[1]), z=0), + position=env.vehicle.position, + bearing=float(math.degrees(env.vehicle.heading_theta)), + steering_angle=env.vehicle.steering * env.vehicle.MAX_STEERING + ) + + state_send.send(state) + + if controls_recv.poll(0): + while controls_recv.poll(0): + steer_angle, gas, reset = controls_recv.recv() + + steer_metadrive = steer_angle * 1 / (env.vehicle.MAX_STEERING * steer_ratio) + steer_metadrive = np.clip(steer_metadrive, -1, 1) + + vc = [steer_metadrive, gas] + + if reset: + env.reset() + + if rk.frame % 5 == 0: + obs, _, terminated, _, info = env.step(vc) + + if terminated: + env.reset() + + #if dual_camera: + # wide_road_image = get_cam_as_rgb("rgb_wide") + road_image[...] = get_cam_as_rgb("rgb_road") + + rk.keep_time() \ No newline at end of file diff --git a/tools/sim/bridge/metadrive/metadrive_world.py b/tools/sim/bridge/metadrive/metadrive_world.py index 45d794ae49c0f8c..447857db2bac34e 100644 --- a/tools/sim/bridge/metadrive/metadrive_world.py +++ b/tools/sim/bridge/metadrive/metadrive_world.py @@ -1,35 +1,45 @@ -import math +import ctypes +import functools +import multiprocessing import numpy as np import time -from openpilot.tools.sim.lib.common import SimulatorState, World, vec3 +from multiprocessing import Pipe, Array +from openpilot.tools.sim.bridge.metadrive.metadrive_process import metadrive_process, metadrive_state +from openpilot.tools.sim.lib.common import SimulatorState, World +from openpilot.tools.sim.lib.camerad import W, H class MetaDriveWorld(World): - def __init__(self, env, dual_camera = False): + def __init__(self, config, dual_camera = False): super().__init__(dual_camera) - self.env = env - self.dual_camera = dual_camera + self.camera_array = Array(ctypes.c_uint8, W*H*3) + self.road_image = np.frombuffer(self.camera_array.get_obj(), dtype=np.uint8).reshape((H, W, 3)) - self.steer_ratio = 15 + self.controls_send, self.controls_recv = Pipe() + self.state_send, self.state_recv = Pipe() - self.vc = [0.0,0.0] + self.exit_event = multiprocessing.Event() - self.reset_time = 0 + self.metadrive_process = multiprocessing.Process(name="metadrive process", target= + functools.partial(metadrive_process, dual_camera, config, + self.camera_array, self.controls_recv, self.state_send, self.exit_event)) + self.metadrive_process.start() - def get_cam_as_rgb(self, cam): - cam = self.env.engine.sensors[cam] - img = cam.perceive(self.env.vehicle, clip=False) - if type(img) != np.ndarray: - img = img.get() # convert cupy array to numpy - return img + print("----------------------------------------------------------") + print("---- Spawning Metadrive world, this might take awhile ----") + print("----------------------------------------------------------") - def apply_controls(self, steer_angle, throttle_out, brake_out): - steer_metadrive = steer_angle * 1 / (self.env.vehicle.MAX_STEERING * self.steer_ratio) - steer_metadrive = np.clip(steer_metadrive, -1, 1) + self.state_recv.recv() # wait for a state message to ensure metadrive is launched + self.steer_ratio = 15 + self.vc = [0.0,0.0] + self.reset_time = 0 + self.should_reset = False + + def apply_controls(self, steer_angle, throttle_out, brake_out): if (time.monotonic() - self.reset_time) > 5: - self.vc[0] = steer_metadrive + self.vc[0] = steer_angle if throttle_out: self.vc[1] = throttle_out/10 @@ -39,27 +49,26 @@ def apply_controls(self, steer_angle, throttle_out, brake_out): self.vc[0] = 0 self.vc[1] = 0 + self.controls_send.send([*self.vc, self.should_reset]) + def read_sensors(self, state: SimulatorState): - state.velocity = vec3(x=float(self.env.vehicle.velocity[0]), y=float(self.env.vehicle.velocity[1]), z=0) - state.gps.from_xy(self.env.vehicle.position) - state.bearing = float(math.degrees(self.env.vehicle.heading_theta)) - state.steering_angle = self.env.vehicle.steering * self.env.vehicle.MAX_STEERING - state.valid = True + while self.state_recv.poll(0): + md_state: metadrive_state = self.state_recv.recv() + state.velocity = md_state.velocity + state.bearing = md_state.bearing + state.steering_angle = md_state.steering_angle + state.gps.from_xy(md_state.position) + state.valid = True def read_cameras(self): - if self.dual_camera: - self.wide_road_image = self.get_cam_as_rgb("rgb_wide") - self.road_image = self.get_cam_as_rgb("rgb_road") + pass def tick(self): - obs, _, terminated, _, info = self.env.step(self.vc) - - if terminated: - self.reset() + pass def reset(self): - self.env.reset() - self.reset_time = time.monotonic() + self.should_reset = True def close(self): - pass \ No newline at end of file + self.exit_event.set() + self.metadrive_process.join() diff --git a/tools/sim/lib/common.py b/tools/sim/lib/common.py index 19012b1c2ddba24..b7aa66f5c51cd0e 100644 --- a/tools/sim/lib/common.py +++ b/tools/sim/lib/common.py @@ -49,9 +49,13 @@ def __init__(self): self.user_gas: float = 0 self.user_brake: float = 0 + self.user_torque: float = 0 self.cruise_button = 0 + self.left_blinker = False + self.right_blinker = False + @property def speed(self): return math.sqrt(self.velocity.x ** 2 + self.velocity.y ** 2 + self.velocity.z ** 2) diff --git a/tools/sim/lib/keyboard_ctrl.py b/tools/sim/lib/keyboard_ctrl.py index 2f27e069691cd55..c37f6ea4792b660 100644 --- a/tools/sim/lib/keyboard_ctrl.py +++ b/tools/sim/lib/keyboard_ctrl.py @@ -19,15 +19,15 @@ KEYBOARD_HELP = """ - | key | functionality | - ----------------------------- - | 1 | Cruise Resume | - | 2 | Cruise Set | - | 3 | Cruise Cancel | - | r | Reset Simulation | - | i | Toggle Ignition | - | q | Exit all | - | wasd | Control manually | + | key | functionality | + |------|-----------------------| + | 1 | Cruise Resume / Accel | + | 2 | Cruise Set / Decel | + | 3 | Cruise Cancel | + | r | Reset Simulation | + | i | Toggle Ignition | + | q | Exit all | + | wasd | Control manually | """ @@ -67,6 +67,10 @@ def keyboard_poll_thread(q: 'Queue[str]'): q.put("brake_%f" % 1.0) elif c == 'd': q.put("steer_%f" % -0.15) + elif c == 'z': + q.put("blinker_left") + elif c == 'x': + q.put("blinker_right") elif c == 'i': q.put("ignition") elif c == 'r': diff --git a/tools/sim/lib/simulated_car.py b/tools/sim/lib/simulated_car.py index 41b58063b95bd47..ab60fad8d38e33b 100644 --- a/tools/sim/lib/simulated_car.py +++ b/tools/sim/lib/simulated_car.py @@ -59,7 +59,7 @@ def send_can_messages(self, simulator_state: SimulatorState): msg.append(self.packer.make_can_msg("GEARBOX", 0, {"GEAR": 4, "GEAR_SHIFTER": 8})) msg.append(self.packer.make_can_msg("GAS_PEDAL_2", 0, {})) msg.append(self.packer.make_can_msg("SEATBELT_STATUS", 0, {"SEATBELT_DRIVER_LATCHED": 1})) - msg.append(self.packer.make_can_msg("STEER_STATUS", 0, {})) + msg.append(self.packer.make_can_msg("STEER_STATUS", 0, {"STEER_TORQUE_SENSOR": simulator_state.user_torque})) msg.append(self.packer.make_can_msg("STEERING_SENSORS", 0, {"STEER_ANGLE": simulator_state.steering_angle})) msg.append(self.packer.make_can_msg("VSA_STATUS", 0, {})) msg.append(self.packer.make_can_msg("STANDSTILL", 0, {"WHEELS_MOVING": 1 if simulator_state.speed >= 1.0 else 0})) @@ -68,7 +68,12 @@ def send_can_messages(self, simulator_state: SimulatorState): msg.append(self.packer.make_can_msg("DOORS_STATUS", 0, {})) msg.append(self.packer.make_can_msg("CRUISE_PARAMS", 0, {})) msg.append(self.packer.make_can_msg("CRUISE", 0, {})) - msg.append(self.packer.make_can_msg("SCM_FEEDBACK", 0, {"MAIN_ON": 1})) + msg.append(self.packer.make_can_msg("SCM_FEEDBACK", 0, + { + "MAIN_ON": 1, + "LEFT_BLINKER": simulator_state.left_blinker, + "RIGHT_BLINKER": simulator_state.right_blinker + })) msg.append(self.packer.make_can_msg("POWERTRAIN_DATA", 0, { "ACC_STATUS": int(simulator_state.is_engaged), diff --git a/tools/sim/lib/simulated_sensors.py b/tools/sim/lib/simulated_sensors.py index dd55c02f02d3ef5..4520ed35ae1f4bd 100644 --- a/tools/sim/lib/simulated_sensors.py +++ b/tools/sim/lib/simulated_sensors.py @@ -122,6 +122,4 @@ def update(self, simulator_state: 'SimulatorState', world: 'World'): if (now - self.last_perp_update) > 0.25: self.send_peripheral_state() - self.last_perp_update = now - - self.send_camera_images(world) \ No newline at end of file + self.last_perp_update = now \ No newline at end of file diff --git a/tools/sim/run_bridge.py b/tools/sim/run_bridge.py index 2f0b99b20d7c473..9914503cd0995c3 100755 --- a/tools/sim/run_bridge.py +++ b/tools/sim/run_bridge.py @@ -11,7 +11,7 @@ def parse_args(add_args=None): - parser = argparse.ArgumentParser(description='Bridge between CARLA and openpilot.') + parser = argparse.ArgumentParser(description='Bridge between the simulator and openpilot.') parser.add_argument('--joystick', action='store_true') parser.add_argument('--high_quality', action='store_true') parser.add_argument('--dual_camera', action='store_true') diff --git a/tools/sim/tmux_script.sh b/tools/sim/tmux_script.sh index 333fce304bfe623..2a1e74958a88add 100755 --- a/tools/sim/tmux_script.sh +++ b/tools/sim/tmux_script.sh @@ -2,5 +2,5 @@ tmux new -d -s carla-sim tmux send-keys "./launch_openpilot.sh" ENTER tmux neww -tmux send-keys "./bridge.py $*" ENTER +tmux send-keys "./run_bridge.py $*" ENTER tmux a -t carla-sim