brianhansonxyz
b70543a003
- Unblocked thermald and camerad — thermald manages CPU cores and reads fake pandaStates for ignition, bench no longer publishes deviceState - camerad starts with live camera feed on bench - Force nvg->update() every frame for HUD repaint without camera dependency - Blue blinking telemetry circle uses getBool pattern - camerad debug logging for startup diagnostics Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
119 lines
3.9 KiB
Python
119 lines
3.9 KiB
Python
#!/usr/bin/env python3
|
|
"""
|
|
ClearPilot bench onroad simulator.
|
|
|
|
Publishes fake messages to make the UI go onroad and display
|
|
configurable vehicle state. Control values via params in /dev/shm/params:
|
|
|
|
BenchSpeed - vehicle speed in mph (default: 0)
|
|
BenchSpeedLimit - speed limit in mph (default: 0, 0=hidden)
|
|
BenchCruiseSpeed - cruise set speed in mph (default: 0, 0=not set)
|
|
BenchGear - P, D, R, N (default: P)
|
|
BenchEngaged - 0 or 1, cruise engaged (default: 0)
|
|
|
|
Usage:
|
|
su - comma -c "PYTHONPATH=/data/openpilot python3 -m selfdrive.clearpilot.bench_onroad"
|
|
|
|
To stop: Ctrl+C or kill the process. UI returns to offroad.
|
|
"""
|
|
import time
|
|
|
|
import cereal.messaging as messaging
|
|
from cereal import log, car
|
|
from openpilot.common.params import Params
|
|
|
|
MS_PER_MPH = 0.44704
|
|
|
|
|
|
def main():
|
|
params = Params()
|
|
params_mem = Params("/dev/shm/params")
|
|
|
|
# Set defaults
|
|
params_mem.put("BenchSpeed", "0")
|
|
params_mem.put("BenchSpeedLimit", "0")
|
|
params_mem.put("BenchCruiseSpeed", "0")
|
|
params_mem.put("BenchGear", "P")
|
|
params_mem.put("BenchEngaged", "0")
|
|
|
|
# thermald handles deviceState (reads our fake pandaStates for ignition)
|
|
pm = messaging.PubMaster([
|
|
"pandaStates", "carState", "controlsState",
|
|
"driverMonitoringState", "liveCalibration",
|
|
])
|
|
|
|
print("Bench onroad simulator started")
|
|
print("Set values in /dev/shm/params/d/Bench*")
|
|
print(" BenchSpeed=0 BenchSpeedLimit=0 BenchCruiseSpeed=0 BenchGear=P BenchEngaged=0")
|
|
|
|
gear_map = {
|
|
"P": car.CarState.GearShifter.park,
|
|
"D": car.CarState.GearShifter.drive,
|
|
"R": car.CarState.GearShifter.reverse,
|
|
"N": car.CarState.GearShifter.neutral,
|
|
}
|
|
|
|
frame = 0
|
|
try:
|
|
while True:
|
|
# Read bench params
|
|
speed_mph = float((params_mem.get("BenchSpeed", encoding="utf-8") or "0").strip())
|
|
speed_limit_mph = float((params_mem.get("BenchSpeedLimit", encoding="utf-8") or "0").strip())
|
|
cruise_mph = float((params_mem.get("BenchCruiseSpeed", encoding="utf-8") or "0").strip())
|
|
gear_str = (params_mem.get("BenchGear", encoding="utf-8") or "P").strip().upper()
|
|
engaged = (params_mem.get("BenchEngaged", encoding="utf-8") or "0").strip() == "1"
|
|
|
|
speed_ms = speed_mph * MS_PER_MPH
|
|
gear = gear_map.get(gear_str, car.CarState.GearShifter.park)
|
|
|
|
# pandaStates — 10 Hz (thermald reads ignition from this)
|
|
if frame % 1 == 0:
|
|
dat = messaging.new_message("pandaStates", 1)
|
|
dat.pandaStates[0].ignitionLine = True
|
|
dat.pandaStates[0].pandaType = log.PandaState.PandaType.tres
|
|
pm.send("pandaStates", dat)
|
|
|
|
# carState — 10 Hz
|
|
dat = messaging.new_message("carState")
|
|
cs = dat.carState
|
|
cs.vEgo = speed_ms
|
|
cs.vEgoCluster = speed_ms
|
|
cs.gearShifter = gear
|
|
cs.standstill = speed_ms < 0.1
|
|
cs.cruiseState.available = True
|
|
cs.cruiseState.enabled = engaged
|
|
cs.cruiseState.speed = cruise_mph * MS_PER_MPH if cruise_mph > 0 else 0
|
|
pm.send("carState", dat)
|
|
|
|
# controlsState — 10 Hz
|
|
dat = messaging.new_message("controlsState")
|
|
ctl = dat.controlsState
|
|
ctl.vCruise = cruise_mph * 1.60934 if cruise_mph > 0 else 255 # km/h or 255=not set
|
|
ctl.vCruiseCluster = ctl.vCruise
|
|
ctl.enabled = engaged
|
|
ctl.active = engaged
|
|
pm.send("controlsState", dat)
|
|
|
|
# driverMonitoringState — low freq
|
|
if frame % 10 == 0:
|
|
dat = messaging.new_message("driverMonitoringState")
|
|
dat.driverMonitoringState.isActiveMode = True
|
|
pm.send("driverMonitoringState", dat)
|
|
|
|
# liveCalibration — low freq
|
|
if frame % 50 == 0:
|
|
dat = messaging.new_message("liveCalibration")
|
|
dat.liveCalibration.calStatus = log.LiveCalibrationData.Status.calibrated
|
|
dat.liveCalibration.rpyCalib = [0.0, 0.0, 0.0]
|
|
pm.send("liveCalibration", dat)
|
|
|
|
frame += 1
|
|
time.sleep(0.1) # 10 Hz base loop
|
|
|
|
except KeyboardInterrupt:
|
|
print("\nBench simulator stopped")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|