mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-29 18:42:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

maneuversd: support interpolated breakpoints (#33936)

Browse Source 
* breakpoints

* clean up

* fix

* simplify

* np fast

* might as well
This commit is contained in:
Shane Smiskol
2024-11-05 18:19:02 -06:00
committed by GitHub
parent e92ff96de3
commit fd84970833
1 changed files with 18 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/longitudinal_maneuvers/maneuversd.py
+18 -13
View File
@@ -1,4 +1,5 @@
#!/usr/bin/env python3
import numpy as np
from dataclasses import dataclass
from cereal import messaging, car
@@ -10,8 +11,11 @@ from openpilot.common.swaglog import cloudlog
@dataclass
class Action:
accel: float # m/s^2
duration: float # seconds
accel_bp: list[float] # m/s^2
time_bp: list[float] # seconds
def __post_init__(self):
assert len(self.accel_bp) == len(self.time_bp)
@dataclass
@@ -41,11 +45,12 @@ class Maneuver:
return min(max(self.initial_speed - v_ego, -2.), 2.)
action = self.actions[self._action_index]
action_accel = np.interp(self._action_frames * DT_MDL, action.time_bp, action.accel_bp)
self._action_frames += 1
# reached duration of action
if self._action_frames > (action.duration / DT_MDL):
if self._action_frames > (action.time_bp[-1] / DT_MDL):
# next action
if self._action_index < len(self.actions) - 1:
self._action_index += 1
@@ -60,7 +65,7 @@ class Maneuver:
else:
self._finished = True
return action.accel
return float(action_accel)
@property
def finished(self):
@@ -74,47 +79,47 @@ class Maneuver:
MANEUVERS = [
Maneuver(
"come to stop",
[Action(-0.5, 12)],
[Action([-0.5], [12])],
repeat=2,
initial_speed=5.,
),
Maneuver(
"start from stop",
[Action(1.5, 5)],
[Action([1.5], [5])],
repeat=2,
initial_speed=0.,
),
Maneuver(
"creep: alternate between +1m/s^2 and -1m/s^2",
[
Action(1, 3), Action(-1, 3),
Action(1, 3), Action(-1, 3),
Action(1, 3), Action(-1, 3),
Action([1], [3]), Action([-1], [3]),
Action([1], [3]), Action([-1], [3]),
Action([1], [3]), Action([-1], [3]),
],
repeat=2,
initial_speed=0.,
),
Maneuver(
"brake step response: -1m/s^2 from 20mph",
[Action(-1, 3)],
[Action([-1], [3])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
Maneuver(
"brake step response: -4m/s^2 from 20mph",
[Action(-4, 3)],
[Action([-4], [3])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
Maneuver(
"gas step response: +1m/s^2 from 20mph",
[Action(1, 3)],
[Action([1], [3])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
Maneuver(
"gas step response: +4m/s^2 from 20mph",
[Action(4, 3)],
[Action([4], [3])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),