Brian Hanson
6e7117b177
Cruise warning sign appears above speed limit sign when cruise set speed is too far from the speed limit: - Red (over): cruise >= limit + 10 (if limit >= 50) or + 5 (if < 50) - Green (under): cruise <= limit - 5 - Only when cruise active (not paused/disabled) and limit >= 20 - Nightrider mode: colored text/border on black background Speed limit sign enlarged 5%. 20px gap between signs. Bench mode gains cruiseactive command (0=disabled, 1=active, 2=paused). Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
138 lines
4.9 KiB
Python
138 lines
4.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)
|
|
BenchCruiseActive - 0=disabled, 1=active, 2=paused/standstill (default: 0)
|
|
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
|
|
from openpilot.common.conversions import Conversions as CV
|
|
from openpilot.selfdrive.clearpilot.speed_logic import SpeedState
|
|
|
|
|
|
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("BenchCruiseActive", "0")
|
|
params_mem.put("BenchGear", "P")
|
|
params_mem.put("BenchEngaged", "0")
|
|
|
|
# ClearPilot speed processing
|
|
speed_state = SpeedState()
|
|
|
|
# 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()
|
|
cruise_active_str = (params_mem.get("BenchCruiseActive", encoding="utf-8") or "0").strip()
|
|
engaged = (params_mem.get("BenchEngaged", encoding="utf-8") or "0").strip() == "1"
|
|
|
|
speed_ms = speed_mph * CV.MPH_TO_MS
|
|
speed_limit_ms = speed_limit_mph * CV.MPH_TO_MS
|
|
cruise_ms = cruise_mph * CV.MPH_TO_MS
|
|
gear = gear_map.get(gear_str, car.CarState.GearShifter.park)
|
|
|
|
# Cruise state: 0=disabled, 1=active, 2=paused
|
|
cruise_active = cruise_active_str == "1"
|
|
cruise_standstill = cruise_active_str == "2"
|
|
|
|
# ClearPilot speed processing (~2 Hz at 10 Hz loop)
|
|
if frame % 5 == 0:
|
|
has_speed = speed_mph > 0
|
|
speed_state.update(speed_ms, has_speed, speed_limit_ms, is_metric=False,
|
|
cruise_speed_ms=cruise_ms, cruise_active=cruise_active or cruise_standstill,
|
|
cruise_standstill=cruise_standstill)
|
|
|
|
# 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 * CV.MPH_TO_MS 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()
|