mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 22:23:56 +08:00
Code Issues Packages Projects Releases Wiki Activity

Improved steering accuracy measurement script (#23583)

Browse Source 
* move steering accuracy measurement script

* git rebase is utterly worthless

* fix header width
old-commit-hash: c007c7e681
This commit is contained in:
Jason Young
2022-05-12 09:09:37 -04:00
committed by GitHub
parent 2dfba97175
commit a5b2304b06
2 changed files with 131 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

81
selfdrive/debug/internal/measure_steering_accuracy.py
View File

@@ -1,81 +0,0 @@
#!/usr/bin/env python3
# type: ignore
import os
import argparse
import signal
from collections import defaultdict
import cereal.messaging as messaging
def sigint_handler(signal, frame):
print("handler!")
exit(0)
signal.signal(signal.SIGINT, sigint_handler)
if __name__ == "__main__":
parser = argparse.ArgumentParser(description='Sniff a communication socket')
parser.add_argument('--addr', default='127.0.0.1')
args = parser.parse_args()
if args.addr != "127.0.0.1":
os.environ["ZMQ"] = "1"
messaging.context = messaging.Context()
carControl = messaging.sub_sock('carControl', addr=args.addr, conflate=True)
sm = messaging.SubMaster(['carState', 'carControl', 'controlsState'], addr=args.addr)
msg_cnt = 0
stats = defaultdict(lambda: {'err': 0, "cnt": 0, "=": 0, "+": 0, "-": 0})
cnt = 0
total_error = 0
while messaging.recv_one(carControl):
sm.update()
msg_cnt += 1
actual_speed = sm['carState'].vEgo
enabled = sm['controlsState'].enabled
steer_override = sm['controlsState'].steerOverride
# must be above 10 m/s, engaged and not overriding steering
if actual_speed > 10.0 and enabled and not steer_override:
cnt += 1
# wait 5 seconds after engage/override
if cnt >= 500:
# calculate error before rounding
actual_angle = sm['controlsState'].angleSteers
desired_angle = sm['carControl'].actuators.steeringAngleDeg
angle_error = abs(desired_angle - actual_angle)
# round numbers
actual_angle = round(actual_angle, 1)
desired_angle = round(desired_angle, 1)
angle_error = round(angle_error, 2)
angle_abs = int(abs(round(desired_angle, 0)))
# collect stats
stats[angle_abs]["err"] += angle_error
stats[angle_abs]["cnt"] += 1
if actual_angle == desired_angle:
stats[angle_abs]["="] += 1
else:
if desired_angle == 0.:
overshoot = True
else:
overshoot = desired_angle < actual_angle if desired_angle > 0. else desired_angle > actual_angle
stats[angle_abs]["+" if overshoot else "-"] += 1
else:
cnt = 0
if msg_cnt % 100 == 0:
print(chr(27) + "[2J")
if cnt != 0:
print("COLLECTING ...")
else:
print("DISABLED (speed too low, not engaged, or steer override)")
for k in sorted(stats.keys()):
v = stats[k]
print(f'angle: {k:#2} | error: {round(v["err"] / v["cnt"], 2):2.2f} | =:{int(v["="] / v["cnt"] * 100):#3}% | +:{int(v["+"] / v["cnt"] * 100):#4}% | -:{int(v["-"] / v["cnt"] * 100):#3}% | count: {v["cnt"]:#4}')

131
tools/tuning/measure_steering_accuracy.py Executable file
View File

@@ -0,0 +1,131 @@
#!/usr/bin/env python3
# type: ignore
import os
import time
import argparse
import signal
from collections import defaultdict
import cereal.messaging as messaging
def sigint_handler(signal, frame):
print("handler!")
exit(0)
signal.signal(signal.SIGINT, sigint_handler)
if __name__ == "__main__":
parser = argparse.ArgumentParser(description='Sniff a communication socket')
parser.add_argument('control_type', help="[pid|indi|lqr|angle]")
parser.add_argument('--addr', default='127.0.0.1', help="IP address for optional ZMQ listener, default to msgq")
parser.add_argument('--group', default='all', help="speed group to display, [crawl|slow|medium|fast|veryfast|germany|all], default to all")
args = parser.parse_args()
if args.addr != "127.0.0.1":
os.environ["ZMQ"] = "1"
messaging.context = messaging.Context()
all_groups = {"germany": (45, "45 - up m/s // 162 - up km/h // 101 - up mph"),
"veryfast": (35, "35 - 45 m/s // 126 - 162 km/h // 78 - 101 mph"),
"fast": (25, "25 - 35 m/s // 90 - 126 km/h // 56 - 78 mph"),
"medium": (15, "15 - 25 m/s // 54 - 90 km/h // 34 - 56 mph"),
"slow": (5, " 5 - 15 m/s // 18 - 54 km/h // 11 - 34 mph"),
"crawl": (0, " 0 - 5 m/s // 0 - 18 km/h // 0 - 11 mph")}
if args.group == "all":
display_groups = all_groups.keys()
elif args.group in all_groups.keys():
display_groups = [args.group]
else:
raise ValueError("invalid speed group, see help")
speed_group_stats = {}
for group in all_groups:
speed_group_stats[group] = defaultdict(lambda: {'err': 0, "cnt": 0, "=": 0, "+": 0, "-": 0, "steer": 0, "limited": 0, "saturated": 0, "dpp": 0})
carControl = messaging.sub_sock('carControl', addr=args.addr, conflate=True)
sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'lateralPlan'], addr=args.addr)
time.sleep(1) # Make sure all submaster data is available before going further
msg_cnt = 0
cnt = 0
total_error = 0
while messaging.recv_one(carControl):
sm.update()
msg_cnt += 1
if args.control_type == "pid":
control_state = sm['controlsState'].lateralControlState.pidState
elif args.control_type == "indi":
control_state = sm['controlsState'].lateralControlState.indiState
elif args.control_type == "lqr":
control_state = sm['controlsState'].lateralControlState.lqrState
elif args.control_type == "angle":
control_state = sm['controlsState'].lateralControlState.angleState
else:
raise ValueError("invalid lateral control type, see help")
v_ego = sm['carState'].vEgo
active = sm['controlsState'].active
steer = sm['carControl'].actuatorsOutput.steer
standstill = sm['carState'].standstill
steer_limited = sm['carState'].steeringRateLimited
overriding = sm['carState'].steeringPressed
changing_lanes = sm['lateralPlan'].laneChangeState != 0
d_path_points = sm['lateralPlan'].dPathPoints
# must be engaged, not at standstill, not overriding steering, and not changing lanes
if active and not standstill and not overriding and not changing_lanes:
cnt += 1
# wait 5 seconds after engage / standstill / override / lane change
if cnt >= 500:
actual_angle = control_state.steeringAngleDeg
desired_angle = control_state.steeringAngleDesiredDeg
# calculate error before rounding, then round for stats grouping
angle_error = abs(desired_angle - actual_angle)
actual_angle = round(actual_angle, 1)
desired_angle = round(desired_angle, 1)
angle_error = round(angle_error, 2)
angle_abs = int(abs(round(desired_angle, 0)))
for group, group_props in all_groups.items():
if v_ego > group_props[0]:
# collect stats
speed_group_stats[group][angle_abs]["cnt"] += 1
speed_group_stats[group][angle_abs]["err"] += angle_error
speed_group_stats[group][angle_abs]["steer"] += abs(steer)
if len(d_path_points):
speed_group_stats[group][angle_abs]["dpp"] += abs(d_path_points[0])
if steer_limited:
speed_group_stats[group][angle_abs]["limited"] += 1
if control_state.saturated:
speed_group_stats[group][angle_abs]["saturated"] += 1
if actual_angle == desired_angle:
speed_group_stats[group][angle_abs]["="] += 1
else:
if desired_angle == 0.:
overshoot = True
else:
overshoot = desired_angle < actual_angle if desired_angle > 0. else desired_angle > actual_angle
speed_group_stats[group][angle_abs]["+" if overshoot else "-"] += 1
break
else:
cnt = 0
if msg_cnt % 100 == 0:
print(chr(27) + "[2J")
if cnt != 0:
print("COLLECTING ...\n")
else:
print("DISABLED (not active, standstill, steering override, or lane change)\n")
for group in display_groups:
if len(speed_group_stats[group]) > 0:
print(f"speed group: {group:10s} {all_groups[group][1]:>96s}")
print(f" {'-'*118}")
for k in sorted(speed_group_stats[group].keys()):
v = speed_group_stats[group][k]
print(f' {k:#2}° | actuator:{int(v["steer"] / v["cnt"] * 100):#3}% | error: {round(v["err"] / v["cnt"], 2):2.2f}° | -:{int(v["-"] / v["cnt"] * 100):#3}% | =:{int(v["="] / v["cnt"] * 100):#3}% | +:{int(v["+"] / v["cnt"] * 100):#3}% | lim:{v["limited"]:#5} | sat:{v["saturated"]:#5} | path dev: {round(v["dpp"] / v["cnt"], 2):2.2f}m | total: {v["cnt"]:#5}')
print("")