sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Simulator: standardize queue messages (#32313)

Browse Source 
* standardize queue messages

* update control_command_gen

* fix

* fix logic

* update closing type

* qmessagetype -> enum

* update type hint

* old close() makes more sense

* cleanup

* fix

* revert that

* revert that

* better name

* actually this is better
old-commit-hash: 630e152900
pull/32199/head
Hoang Bui 11 months ago committed by GitHub
parent e932b0836f
commit 565e06b66a
5 changed files with 76 additions and 63 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 71
      tools/sim/bridge/common.py
  2. 4
      tools/sim/bridge/metadrive/metadrive_bridge.py
  3. 17
      tools/sim/bridge/metadrive/metadrive_world.py
  4. 31
      tools/sim/lib/keyboard_ctrl.py
  5. 16
      tools/sim/lib/manual_ctrl.py

71
tools/sim/bridge/common.py
Unescape View File

@ -2,6 +2,8 @@ import signal
import threading
import functools
from collections import namedtuple
from enum import Enum
from multiprocessing import Process, Queue, Value
from abc import ABC, abstractmethod
@ -14,6 +16,16 @@ from openpilot.tools.sim.lib.common import SimulatorState, World
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])
class QueueMessageType(Enum):
START_STATUS = 0
CONTROL_COMMAND = 1
TERMINATION_INFO = 2
CLOSE_STATUS = 3
def control_cmd_gen(cmd: str):
return QueueMessage(QueueMessageType.CONTROL_COMMAND, cmd)
def rk_loop(function, hz, exit_event: threading.Event):
rk = Ratekeeper(hz, None)
@ -80,11 +92,11 @@ Ignition: {self.simulator_state.ignition} Engaged: {self.simulator_state.is_enga
""")
@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)
@ -114,33 +126,34 @@ Ignition: {self.simulator_state.ignition} Engaged: {self.simulator_state.is_enga
# Read manual controls
if not q.empty():
message = q.get()
m = message.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 == QueueMessageType.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

4
tools/sim/bridge/metadrive/metadrive_bridge.py
Unescape View File

@ -53,7 +53,7 @@ class MetaDriveBridge(SimulatorBridge):
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 @@ class MetaDriveBridge(SimulatorBridge):
preload_models=False
)
return MetaDriveWorld(Queue(), config, self.dual_camera)
return MetaDriveWorld(queue, config, self.dual_camera)

17
tools/sim/bridge/metadrive/metadrive_world.py
Unescape View File

@ -5,6 +5,8 @@ import numpy as np
import time
from multiprocessing import Pipe, Array
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
@ -35,14 +37,14 @@ class MetaDriveWorld(World):
self.vehicle_state_send, self.exit_event))
self.metadrive_process.start()
self.status_q.put({"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({"status": "started"})
self.status_q.put(QueueMessage(QueueMessageType.START_STATUS, "started"))
self.steer_ratio = 15
self.vc = [0.0,0.0]
@ -68,11 +70,7 @@ class MetaDriveWorld(World):
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.status_q.put(QueueMessage(QueueMessageType.TERMINATION_INFO, md_state.done_info))
self.exit_event.set()
def read_sensors(self, state: SimulatorState):
@ -94,9 +92,6 @@ class MetaDriveWorld(World):
self.should_reset = True
def close(self, reason: str):
self.status_q.put({
"status": "terminating",
"reason": reason,
})
self.status_q.put(QueueMessage(QueueMessageType.CLOSE_STATUS, reason))
self.exit_event.set()
self.metadrive_process.join()

31
tools/sim/lib/keyboard_ctrl.py
Unescape View File

@ -2,10 +2,13 @@ import sys
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, control_cmd_gen
# Indexes for termios list.
IFLAG = 0
OFLAG = 1
@ -52,35 +55,35 @@ 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")
q.put(control_cmd_gen("cruise_up"))
elif c == '2':
q.put("cruise_down")
q.put(control_cmd_gen("cruise_down"))
elif c == '3':
q.put("cruise_cancel")
q.put(control_cmd_gen("cruise_cancel"))
elif c == 'w':
q.put("throttle_%f" % 1.0)
q.put(control_cmd_gen(f"throttle_{1.0}"))
elif c == 'a':
q.put("steer_%f" % -0.15)
q.put(control_cmd_gen(f"steer_{-0.15}"))
elif c == 's':
q.put("brake_%f" % 1.0)
q.put(control_cmd_gen(f"brake_{1.0}"))
elif c == 'd':
q.put("steer_%f" % 0.15)
q.put(control_cmd_gen(f"steer_{0.15}"))
elif c == 'z':
q.put("blinker_left")
q.put(control_cmd_gen("blinker_left"))
elif c == 'x':
q.put("blinker_right")
q.put(control_cmd_gen("blinker_right"))
elif c == 'i':
q.put("ignition")
q.put(control_cmd_gen("ignition"))
elif c == 'r':
q.put("reset")
q.put(control_cmd_gen("reset"))
elif c == 'q':
q.put("quit")
q.put(control_cmd_gen("quit"))
break
else:
print_keyboard_help()
@ -92,7 +95,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()

16
tools/sim/lib/manual_ctrl.py
Unescape View File

@ -6,6 +6,8 @@ import struct
from fcntl import ioctl
from typing import NoReturn
from openpilot.tools.sim.bridge.common import control_cmd_gen
# Iterate over the joystick devices.
print('Available devices:')
for fn in os.listdir('/dev/input'):
@ -153,33 +155,33 @@ 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}")
q.put(control_cmd_gen(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}")
q.put(control_cmd_gen(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}")
q.put(control_cmd_gen(f"steer_{normalized:f}"))
elif mtype & 0x01: # buttons
if value == 1: # press down
if number in [0, 19]: # X
q.put("cruise_down")
q.put(control_cmd_gen("cruise_down"))
elif number in [3, 18]: # triangle
q.put("cruise_up")
q.put(control_cmd_gen("cruise_up"))
elif number in [1, 6]: # square
q.put("cruise_cancel")
q.put(control_cmd_gen("cruise_cancel"))
elif number in [10, 21]: # R3
q.put("reverse_switch")
q.put(control_cmd_gen("reverse_switch"))
if __name__ == '__main__':
from multiprocessing import Process, Queue

Write Preview
Loading…
Cancel
Save