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
Unified 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 threading
import functools import functools
from collections import namedtuple
from enum import Enum
from multiprocessing import Process, Queue, Value from multiprocessing import Process, Queue, Value
from abc import ABC, abstractmethod 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_car import SimulatedCar
from openpilot.tools.sim.lib.simulated_sensors import SimulatedSensors 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): def rk_loop(function, hz, exit_event: threading.Event):
rk = Ratekeeper(hz, None) rk = Ratekeeper(hz, None)
@ -80,11 +92,11 @@ Ignition: {self.simulator_state.ignition} Engaged: {self.simulator_state.is_enga
""") """)
@abstractmethod @abstractmethod
def spawn_world(self) -> World: def spawn_world(self, q: Queue) -> World:
pass pass
def _run(self, q: Queue): def _run(self, q: Queue):
self.world = self.spawn_world() self.world = self.spawn_world(q)
self.simulated_car = SimulatedCar() self.simulated_car = SimulatedCar()
self.simulated_sensors = SimulatedSensors(self.dual_camera) 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 # Read manual controls
if not q.empty(): if not q.empty():
message = q.get() message = q.get()
m = message.split('_') if message.type == QueueMessageType.CONTROL_COMMAND:
if m[0] == "steer": m = message.info.split('_')
steer_manual = float(m[1]) if m[0] == "steer":
elif m[0] == "throttle": steer_manual = float(m[1])
throttle_manual = float(m[1]) elif m[0] == "throttle":
elif m[0] == "brake": throttle_manual = float(m[1])
brake_manual = float(m[1]) elif m[0] == "brake":
elif m[0] == "cruise": brake_manual = float(m[1])
if m[1] == "down": elif m[0] == "cruise":
self.simulator_state.cruise_button = CruiseButtons.DECEL_SET if m[1] == "down":
elif m[1] == "up": self.simulator_state.cruise_button = CruiseButtons.DECEL_SET
self.simulator_state.cruise_button = CruiseButtons.RES_ACCEL elif m[1] == "up":
elif m[1] == "cancel": self.simulator_state.cruise_button = CruiseButtons.RES_ACCEL
self.simulator_state.cruise_button = CruiseButtons.CANCEL elif m[1] == "cancel":
elif m[1] == "main": self.simulator_state.cruise_button = CruiseButtons.CANCEL
self.simulator_state.cruise_button = CruiseButtons.MAIN elif m[1] == "main":
elif m[0] == "blinker": self.simulator_state.cruise_button = CruiseButtons.MAIN
if m[1] == "left": elif m[0] == "blinker":
self.simulator_state.left_blinker = True if m[1] == "left":
elif m[1] == "right": self.simulator_state.left_blinker = True
self.simulator_state.right_blinker = True elif m[1] == "right":
elif m[0] == "ignition": self.simulator_state.right_blinker = True
self.simulator_state.ignition = not self.simulator_state.ignition elif m[0] == "ignition":
elif m[0] == "reset": self.simulator_state.ignition = not self.simulator_state.ignition
self.world.reset() elif m[0] == "reset":
elif m[0] == "quit": self.world.reset()
break elif m[0] == "quit":
break
self.simulator_state.user_brake = brake_manual self.simulator_state.user_brake = brake_manual
self.simulator_state.user_gas = throttle_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) super().__init__(dual_camera, high_quality)
def spawn_world(self): def spawn_world(self, queue: Queue):
sensors = { sensors = {
"rgb_road": (RGBCameraRoad, W, H, ) "rgb_road": (RGBCameraRoad, W, H, )
} }
@ -83,4 +83,4 @@ class MetaDriveBridge(SimulatorBridge):
preload_models=False 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 import time
from multiprocessing import Pipe, Array 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, from openpilot.tools.sim.bridge.metadrive.metadrive_process import (metadrive_process, metadrive_simulation_state,
metadrive_vehicle_state) metadrive_vehicle_state)
from openpilot.tools.sim.lib.common import SimulatorState, World from openpilot.tools.sim.lib.common import SimulatorState, World
@ -35,14 +37,14 @@ class MetaDriveWorld(World):
self.vehicle_state_send, self.exit_event)) self.vehicle_state_send, self.exit_event))
self.metadrive_process.start() self.metadrive_process.start()
self.status_q.put({"status": "starting"}) self.status_q.put(QueueMessage(QueueMessageType.START_STATUS, "starting"))
print("----------------------------------------------------------") print("----------------------------------------------------------")
print("---- Spawning Metadrive world, this might take awhile ----") print("---- Spawning Metadrive world, this might take awhile ----")
print("----------------------------------------------------------") print("----------------------------------------------------------")
self.vehicle_state_recv.recv() # wait for a state message to ensure metadrive is launched 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.steer_ratio = 15
self.vc = [0.0,0.0] self.vc = [0.0,0.0]
@ -68,11 +70,7 @@ class MetaDriveWorld(World):
while self.simulation_state_recv.poll(0): while self.simulation_state_recv.poll(0):
md_state: metadrive_simulation_state = self.simulation_state_recv.recv() md_state: metadrive_simulation_state = self.simulation_state_recv.recv()
if md_state.done: if md_state.done:
self.status_q.put({ self.status_q.put(QueueMessage(QueueMessageType.TERMINATION_INFO, md_state.done_info))
"status": "terminating",
"reason": "done",
"done_info": md_state.done_info
})
self.exit_event.set() self.exit_event.set()
def read_sensors(self, state: SimulatorState): def read_sensors(self, state: SimulatorState):
@ -94,9 +92,6 @@ class MetaDriveWorld(World):
self.should_reset = True self.should_reset = True
def close(self, reason: str): def close(self, reason: str):
self.status_q.put({ self.status_q.put(QueueMessage(QueueMessageType.CLOSE_STATUS, reason))
"status": "terminating",
"reason": reason,
})
self.exit_event.set() self.exit_event.set()
self.metadrive_process.join() self.metadrive_process.join()

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

@ -2,10 +2,13 @@ import sys
import termios import termios
import time import time
from multiprocessing import Queue
from termios import (BRKINT, CS8, CSIZE, ECHO, ICANON, ICRNL, IEXTEN, INPCK, from termios import (BRKINT, CS8, CSIZE, ECHO, ICANON, ICRNL, IEXTEN, INPCK,
ISTRIP, IXON, PARENB, VMIN, VTIME) ISTRIP, IXON, PARENB, VMIN, VTIME)
from typing import NoReturn from typing import NoReturn
from openpilot.tools.sim.bridge.common import QueueMessage, control_cmd_gen
# Indexes for termios list. # Indexes for termios list.
IFLAG = 0 IFLAG = 0
OFLAG = 1 OFLAG = 1
@ -52,35 +55,35 @@ def getch() -> str:
def print_keyboard_help(): def print_keyboard_help():
print(f"Keyboard Commands:\n{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() print_keyboard_help()
while True: while True:
c = getch() c = getch()
if c == '1': if c == '1':
q.put("cruise_up") q.put(control_cmd_gen("cruise_up"))
elif c == '2': elif c == '2':
q.put("cruise_down") q.put(control_cmd_gen("cruise_down"))
elif c == '3': elif c == '3':
q.put("cruise_cancel") q.put(control_cmd_gen("cruise_cancel"))
elif c == 'w': elif c == 'w':
q.put("throttle_%f" % 1.0) q.put(control_cmd_gen(f"throttle_{1.0}"))
elif c == 'a': elif c == 'a':
q.put("steer_%f" % -0.15) q.put(control_cmd_gen(f"steer_{-0.15}"))
elif c == 's': elif c == 's':
q.put("brake_%f" % 1.0) q.put(control_cmd_gen(f"brake_{1.0}"))
elif c == 'd': elif c == 'd':
q.put("steer_%f" % 0.15) q.put(control_cmd_gen(f"steer_{0.15}"))
elif c == 'z': elif c == 'z':
q.put("blinker_left") q.put(control_cmd_gen("blinker_left"))
elif c == 'x': elif c == 'x':
q.put("blinker_right") q.put(control_cmd_gen("blinker_right"))
elif c == 'i': elif c == 'i':
q.put("ignition") q.put(control_cmd_gen("ignition"))
elif c == 'r': elif c == 'r':
q.put("reset") q.put(control_cmd_gen("reset"))
elif c == 'q': elif c == 'q':
q.put("quit") q.put(control_cmd_gen("quit"))
break break
else: else:
print_keyboard_help() print_keyboard_help()
@ -92,7 +95,7 @@ def test(q: 'Queue[str]') -> NoReturn:
if __name__ == '__main__': if __name__ == '__main__':
from multiprocessing import Process, Queue from multiprocessing import Process, Queue
q: Queue[str] = Queue() q: 'Queue[QueueMessage]' = Queue()
p = Process(target=test, args=(q,)) p = Process(target=test, args=(q,))
p.daemon = True p.daemon = True
p.start() p.start()

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

@ -6,6 +6,8 @@ import struct
from fcntl import ioctl from fcntl import ioctl
from typing import NoReturn from typing import NoReturn
from openpilot.tools.sim.bridge.common import control_cmd_gen
# Iterate over the joystick devices. # Iterate over the joystick devices.
print('Available devices:') print('Available devices:')
for fn in os.listdir('/dev/input'): for fn in os.listdir('/dev/input'):
@ -153,33 +155,33 @@ def wheel_poll_thread(q: 'Queue[str]') -> NoReturn:
fvalue = value / 32767.0 fvalue = value / 32767.0
axis_states[axis] = fvalue axis_states[axis] = fvalue
normalized = (1 - fvalue) * 50 normalized = (1 - fvalue) * 50
q.put(f"throttle_{normalized:f}") q.put(control_cmd_gen(f"throttle_{normalized:f}"))
elif axis == "rz": # brake elif axis == "rz": # brake
fvalue = value / 32767.0 fvalue = value / 32767.0
axis_states[axis] = fvalue axis_states[axis] = fvalue
normalized = (1 - fvalue) * 50 normalized = (1 - fvalue) * 50
q.put(f"brake_{normalized:f}") q.put(control_cmd_gen(f"brake_{normalized:f}"))
elif axis == "x": # steer angle elif axis == "x": # steer angle
fvalue = value / 32767.0 fvalue = value / 32767.0
axis_states[axis] = fvalue axis_states[axis] = fvalue
normalized = fvalue normalized = fvalue
q.put(f"steer_{normalized:f}") q.put(control_cmd_gen(f"steer_{normalized:f}"))
elif mtype & 0x01: # buttons elif mtype & 0x01: # buttons
if value == 1: # press down if value == 1: # press down
if number in [0, 19]: # X if number in [0, 19]: # X
q.put("cruise_down") q.put(control_cmd_gen("cruise_down"))
elif number in [3, 18]: # triangle elif number in [3, 18]: # triangle
q.put("cruise_up") q.put(control_cmd_gen("cruise_up"))
elif number in [1, 6]: # square elif number in [1, 6]: # square
q.put("cruise_cancel") q.put(control_cmd_gen("cruise_cancel"))
elif number in [10, 21]: # R3 elif number in [10, 21]: # R3
q.put("reverse_switch") q.put(control_cmd_gen("reverse_switch"))
if __name__ == '__main__': if __name__ == '__main__':
from multiprocessing import Process, Queue from multiprocessing import Process, Queue

Write Preview
Loading…
Cancel
Save