mawei/onepilot
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: 630e1529008ce0c996ba6a111df44877596acfea
This commit is contained in:
Hoang Bui
2024-05-17 22:51:07 -04:00
committed by GitHub
parent e932b0836f
commit 565e06b66a
5 changed files with 76 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
tools/sim/bridge/common.py
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
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
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
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
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