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 + + 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/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/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/__init__.py b/tools/joystick/maneuver/__init__.py new file mode 100755 index 00000000000000..b5a9e1e1dd391f --- /dev/null +++ b/tools/joystick/maneuver/__init__.py @@ -0,0 +1,127 @@ +#!/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__))) + +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 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() + 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() + 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 + 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 + + 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"]: + 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])) + # TODO calculate accels as slopes of speedss + 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.duration = float(self.object["duration"]) + self.should_interpolate = bool(self.object["should_interpolate"]) + self.__points_to_interpables() + +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) diff --git a/tools/joystick/maneuver/maneuver b/tools/joystick/maneuver/maneuver new file mode 100755 index 00000000000000..2a74a6fc86f646 --- /dev/null +++ b/tools/joystick/maneuver/maneuver @@ -0,0 +1,12 @@ +#!/bin/bash +set -euo pipefail + +num="$1" + +python3 -c "from . import Mem +mem = Mem() +if mem.maneuver_requested(): + print(\"already maneuvering\") + exit() +mem.request(${num}) +" 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] + ] +} +