From c1515e88b6c18b9d9861dbca33d1461118337851 Mon Sep 17 00:00:00 2001 From: nick Date: Tue, 13 Dec 2022 01:44:23 -0500 Subject: [PATCH 1/7] long maneuvers https://github.com/commaai/openpilot/issues/26778 --- info.txt | 32 ++++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 info.txt diff --git a/info.txt b/info.txt new file mode 100644 index 00000000000000..0355617580ee0e --- /dev/null +++ b/info.txt @@ -0,0 +1,32 @@ +## programmable longitudinal maneuvers + +probably use with chill mode for reproducible planning + +### Maneuver object +jump instantaneously to velocity at next point, +or interpolate smoothly + +{ + duration: 14, // seconds + should_interpolate: false, + points: [ + // time, velocitymph + {0, 0}, + {5, 10}, + {8, 10}, + {12, 2}, + {14, 0}, + ], +} + +### maneuver execution +once maneuver is selected and commanded +override setspeed from cruisestate with points from maneuver + +### web app +c3 hosted web app to choose maneuver, start/stop, view progress +start with a few maneuvers to choose from + +maybe later add a graph to display progress on phone + + From 2fdcedd9f972fceb2daf48ed33ad1aca054c6991 Mon Sep 17 00:00:00 2001 From: nick Date: Tue, 13 Dec 2022 02:15:33 -0500 Subject: [PATCH 2/7] start maneuvering --- tools/joystick/maneuver/__init__.py | 29 ++++++++++++++++++++++++ tools/joystick/maneuver/maneuvers/0.json | 14 ++++++++++++ 2 files changed, 43 insertions(+) create mode 100755 tools/joystick/maneuver/__init__.py create mode 100644 tools/joystick/maneuver/maneuvers/0.json diff --git a/tools/joystick/maneuver/__init__.py b/tools/joystick/maneuver/__init__.py new file mode 100755 index 00000000000000..9cd9dbbec8c384 --- /dev/null +++ b/tools/joystick/maneuver/__init__.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python3 +import json +import os + +whereami = str(os.path.dirname(os.path.abspath(__file__))) + +maneuvers_directory = whereami + "/maneuvers" + +def available_maneuver_files(): + all_files = os.listdir(maneuvers_directory) + files = [f for f in all_files if f[f.rfind(".")+1:] == "json"] + return files + +class Maneuver: + def get_wolfram_alpha_paste(self): + out = "plot points[" + for point in self.object["points"]: + #plot points[(-5/2, 18),(-2, 14),(-1, 39/10),(0, 1/2),(1, 19/10),(2, 5),(5/2, 17/2)] + out += ("(" + str(point[0]) + "," + str(point[1]) + "),") + out = out[:-1] + "]" + return out + def __init__(self, file_name): + self.file_name = file_name + self.object = json.load(open(maneuvers_directory + "/" + file_name)) + +a = Maneuver("0.json") +print(a.object) +print(a.get_wolfram_alpha_paste()) + diff --git a/tools/joystick/maneuver/maneuvers/0.json b/tools/joystick/maneuver/maneuvers/0.json new file mode 100644 index 00000000000000..6c03dc248c0bde --- /dev/null +++ b/tools/joystick/maneuver/maneuvers/0.json @@ -0,0 +1,14 @@ +{ + "name": "first test maneuver", + "duration": 14, + "should_interpolate": false, + "velocity_unit": "mph", + "points": [ + [0, 0], + [5, 10], + [8, 10], + [12, 2], + [14, 0] + ] +} + From 6b6bd2f3a10f38096d24ca00ba3ed6a0cdf85a1b Mon Sep 17 00:00:00 2001 From: nick Date: Tue, 13 Dec 2022 03:18:44 -0500 Subject: [PATCH 3/7] start maneuver controller and communication for start/stop/request --- tools/joystick/maneuver/__init__.py | 100 ++++++++++++++++++++++++++-- 1 file changed, 95 insertions(+), 5 deletions(-) diff --git a/tools/joystick/maneuver/__init__.py b/tools/joystick/maneuver/__init__.py index 9cd9dbbec8c384..18ec22530f9862 100755 --- a/tools/joystick/maneuver/__init__.py +++ b/tools/joystick/maneuver/__init__.py @@ -1,6 +1,15 @@ #!/usr/bin/env python3 +import datetime import json +import atexit import os +from common.numpy_fast import interp +from common.conversions import Conversions as CV + + +# TODO: use maneuvercontroller in longitudinal planner + +# TODO: make script that uses mem to start a maneuver or something whereami = str(os.path.dirname(os.path.abspath(__file__))) @@ -11,19 +20,100 @@ def available_maneuver_files(): files = [f for f in all_files if f[f.rfind(".")+1:] == "json"] return files +class ManeuverController: + maneuvering = False + maneuver = None + t = 0. + start_time = None + def update(self, vcruise): + should_start_maneuver = not self.maneuvering and self.mem.maneuver_requested() + should_stop_maneuver = self.maneuvering and not self.mem.maneuver_requested() + if not self.maneuvering and not should_start_maneuver: + return vcruise + + if should_start_maneuver: + self.t = 0. + self.start_time = datetime.datetime.now() + file_name = str(self.mem.maneuver_num()) + ".json" + self.maneuver = Maneuver(file_name) + self.maneuvering = True + elif should_stop_maneuver: + self.maneuvering = False + self.maneuver = None + self.t = 0. + self.start_time = None + # TODO + # do/finish this and see what else i missed and put it into the car + # start over ssh for now + + dt = datetime.datetime.now() - self.start_time + self.t = float(dt.seconds) + float(dt.microseconds) / 1.0e6 + return self.maneuver.get_velocity_kph(self.t) + def set_finished(self): + self.maneuvering = False + self.maneuver = None + def request_maneuver_start(self, maneuver): + self.maneuver = maneuver + self.maneuvering = True + def __init__(self): + self.mem = Mem(autounlink=True) + class Maneuver: def get_wolfram_alpha_paste(self): out = "plot points[" for point in self.object["points"]: - #plot points[(-5/2, 18),(-2, 14),(-1, 39/10),(0, 1/2),(1, 19/10),(2, 5),(5/2, 17/2)] out += ("(" + str(point[0]) + "," + str(point[1]) + "),") out = out[:-1] + "]" return out + def get_velocity_kph(self, t): + if self.should_interpolate: + return interp(t, self.times, self.speeds) + else: + return "ffff" + def __points_to_interpables(self): + times = [] + speeds_kph = [] + conversions = { + "mph": CV.MPH_TO_KPH, + "kph": 1., + "mps": CV.MS_TO_KPH, + "m/s": CV.MS_TO_KPH, + "si": CV.MS_TO_KPH, + } + unit = self.object["velocity_unit"] + conversion = conversions[unit] + for point in self.object["points"]: + times.append(float(point[0])) + speeds_kph.append(float(point[1]) * conversion) + self.times = times + self.speeds = speeds_kph def __init__(self, file_name): self.file_name = file_name self.object = json.load(open(maneuvers_directory + "/" + file_name)) + self.should_interpolate = bool(self.object["should_interpolate"]) + self.__points_to_interpables() -a = Maneuver("0.json") -print(a.object) -print(a.get_wolfram_alpha_paste()) - +class Mem: + __mem = None + name = "maneuvermem" + size = 2 + def maneuver_requested(self): + return bool(buf[0]) + def maneuver_num(self): + return buf[1] + def maneuver_request(self, maneuver_num): + self.buf[1] = maneuver_num + self.buf[0] = 1 + def maneuver_finish(self): + self.__mem.buf[0:size] = bytearray([0 for _ in range(size)]) + def __create_or_connect(self): + self.__mem = shared_memory.SharedMemory(name=self.name, create=True, size=self.size) + def __cleanup(self): + self.__mem.close() + if self.__shouldunlink: + self.__mem.unlink() + def __init__(self, autounlink=False): + self.__shouldunlink = autounlink + self.__create_or_connect() + self.__mem.buf[0:size] = bytearray([0 for _ in range(size)]) + atexit.register(self.__cleanup) From 272767c9709fe93fe7f01f911277ffebb8bf3fb1 Mon Sep 17 00:00:00 2001 From: nick Date: Tue, 13 Dec 2022 03:18:52 -0500 Subject: [PATCH 4/7] untested From 4e20e327526b30a82f12859a579887fd96b386b7 Mon Sep 17 00:00:00 2001 From: nick Date: Tue, 13 Dec 2022 03:37:04 -0500 Subject: [PATCH 5/7] long planner and script to start --- selfdrive/controls/lib/longitudinal_planner.py | 4 ++++ tools/joystick/maneuver/__init__.py | 9 ++++++++- tools/joystick/maneuver/maneuver.sh | 11 +++++++++++ 3 files changed, 23 insertions(+), 1 deletion(-) create mode 100644 tools/joystick/maneuver/maneuver.sh diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index a0f63183237497..c8bab736d6e8a5 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -3,6 +3,7 @@ import numpy as np from common.numpy_fast import clip, interp +from tools.joystick.maneuver import ManeuverController import cereal.messaging as messaging from common.conversions import Conversions as CV from common.filter_simple import FirstOrderFilter @@ -46,6 +47,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0): + self.mc = ManeuverController() self.CP = CP self.mpc = LongitudinalMpc() self.fcw = False @@ -81,6 +83,8 @@ def update(self, sm): v_ego = sm['carState'].vEgo v_cruise_kph = sm['controlsState'].vCruise v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX) + v_override = self.mc.update(vcruise_kph) + v_cruise_kph = v_override v_cruise = v_cruise_kph * CV.KPH_TO_MS long_control_off = sm['controlsState'].longControlState == LongCtrlState.off diff --git a/tools/joystick/maneuver/__init__.py b/tools/joystick/maneuver/__init__.py index 18ec22530f9862..5f674b28b45491 100755 --- a/tools/joystick/maneuver/__init__.py +++ b/tools/joystick/maneuver/__init__.py @@ -27,10 +27,14 @@ class ManeuverController: start_time = None def update(self, vcruise): should_start_maneuver = not self.maneuvering and self.mem.maneuver_requested() - should_stop_maneuver = self.maneuvering and not self.mem.maneuver_requested() if not self.maneuvering and not should_start_maneuver: return vcruise + # TODO this is wrong. this function decides when to stop + #should_stop_maneuver = self.maneuvering and not self.mem.maneuver_requested() + + should_stop_maneuver = self.maneuvering && self.t > self.maneuver.duration + if should_start_maneuver: self.t = 0. self.start_time = datetime.datetime.now() @@ -42,6 +46,8 @@ def update(self, vcruise): self.maneuver = None self.t = 0. self.start_time = None + self.mem.maneuver_finish() + return vcruise # TODO # do/finish this and see what else i missed and put it into the car # start over ssh for now @@ -90,6 +96,7 @@ def __points_to_interpables(self): def __init__(self, file_name): self.file_name = file_name self.object = json.load(open(maneuvers_directory + "/" + file_name)) + self.duration = float(self.object["duration"]) self.should_interpolate = bool(self.object["should_interpolate"]) self.__points_to_interpables() diff --git a/tools/joystick/maneuver/maneuver.sh b/tools/joystick/maneuver/maneuver.sh new file mode 100644 index 00000000000000..30942ce5a150d6 --- /dev/null +++ b/tools/joystick/maneuver/maneuver.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +num="$1" + +python3 -c "from . import Mem +mem = Mem() +if mem.maneuver_requested(): + print(\"already maneuvering\") + exit() +mem.request(${num}) +" From 2c05dd651925751aff3db24d61c7ca39b8f6bc90 Mon Sep 17 00:00:00 2001 From: nick Date: Tue, 13 Dec 2022 03:40:47 -0500 Subject: [PATCH 6/7] make --- tools/joystick/maneuver/Makefile | 5 +++++ tools/joystick/maneuver/{maneuver.sh => maneuver} | 1 + 2 files changed, 6 insertions(+) create mode 100644 tools/joystick/maneuver/Makefile rename tools/joystick/maneuver/{maneuver.sh => maneuver} (89%) mode change 100644 => 100755 diff --git a/tools/joystick/maneuver/Makefile b/tools/joystick/maneuver/Makefile new file mode 100644 index 00000000000000..bf2c457bfd21c4 --- /dev/null +++ b/tools/joystick/maneuver/Makefile @@ -0,0 +1,5 @@ + + + +run: + ./maneuver 0 diff --git a/tools/joystick/maneuver/maneuver.sh b/tools/joystick/maneuver/maneuver old mode 100644 new mode 100755 similarity index 89% rename from tools/joystick/maneuver/maneuver.sh rename to tools/joystick/maneuver/maneuver index 30942ce5a150d6..2a74a6fc86f646 --- a/tools/joystick/maneuver/maneuver.sh +++ b/tools/joystick/maneuver/maneuver @@ -1,4 +1,5 @@ #!/bin/bash +set -euo pipefail num="$1" From ba537ea705e4195ae872a3900c4740505b4addf6 Mon Sep 17 00:00:00 2001 From: ntegan1 <--global> Date: Fri, 16 Dec 2022 12:55:44 -0500 Subject: [PATCH 7/7] next steps --- selfdrive/controls/controlsd.py | 7 +++++++ tools/joystick/maneuver/__init__.py | 1 + 2 files changed, 8 insertions(+) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 36f0b559c2eaa8..04e6e2065e3fa6 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -596,6 +596,13 @@ def state_control(self, CS): # accel PID loop pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS) t_since_plan = (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) * DT_CTRL + + ## TODO + # when starting a maneuver + self.LoC.reset(v_pid=CS.vEgo) #vpid = vego or first point in plan + + # fill planfrom maneuver + actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) # Steering PID loop and lateral MPC diff --git a/tools/joystick/maneuver/__init__.py b/tools/joystick/maneuver/__init__.py index 5f674b28b45491..b5a9e1e1dd391f 100755 --- a/tools/joystick/maneuver/__init__.py +++ b/tools/joystick/maneuver/__init__.py @@ -90,6 +90,7 @@ def __points_to_interpables(self): conversion = conversions[unit] for point in self.object["points"]: times.append(float(point[0])) + # TODO calculate accels as slopes of speedss speeds_kph.append(float(point[1]) * conversion) self.times = times self.speeds = speeds_kph