diff --git a/Carla/VirEnv/mainVirCarla.py b/Carla/VirEnv/mainVirCarla.py index a91f3848..f4b4a625 100644 --- a/Carla/VirEnv/mainVirCarla.py +++ b/Carla/VirEnv/mainVirCarla.py @@ -167,6 +167,10 @@ def main(argv=None): os.environ['FIXS_CONFIG_YAML'] = os.path.abspath(args.configPath) cs = config.Carla_setup egoCfg = config.Ego_setup # the ego, once (FIXS#305) + # XilSetup.SpeedTo*: xil -- the ego's speedDesired carries the bench's speed to TrafficLayer + xilSpeedWanted = 'xil' in (config.Xil_setup['SpeedToTraffic'], config.Xil_setup['SpeedToApplication']) + if xilSpeedWanted: + from CommonLib.fixs import _driver as _fixsDriver verbose = cs['EnableVerboseLog'] feed, carlaStep, poseRefresh = resolveCadence(cs) @@ -284,6 +288,7 @@ def main(argv=None): "No physics ego is spawned.") lastAdvisory = cs['EgoTargetSpeed'] cellSpeed = None # what the cell last commanded (cellOnTraffic only) + xilSpeedSeen = False try: # Checked here rather than inside the bring-up, so a missing route fails @@ -489,6 +494,13 @@ def main(argv=None): # target is verifiable. d.speed = es.speed d.speedDesired = lastAdvisory + if xilSpeedWanted: + # the bench's speed; the measured one until the bench first answers + vDyno = getattr(_fixsDriver._instance, 'vDyno', None) + d.speedDesired = es.speed if vDyno is None else vDyno + if vDyno is not None and not xilSpeedSeen: + xilSpeedSeen = True + print("XilSetup.SpeedTo*: the bench's speed reaches TrafficLayer from t=%.2f" % simTime) d.positionX, d.positionY, d.positionZ = es.x, es.y, es.z d.heading = es.heading d.grade = es.grade diff --git a/Carla/templates/driver_template.py b/Carla/templates/driver_template.py index 06616ed8..4bc65866 100644 --- a/Carla/templates/driver_template.py +++ b/Carla/templates/driver_template.py @@ -43,27 +43,27 @@ # --------------------------------------------------------------------------- # -# YOUR OWN DRIVING. None of the driver's logic runs -- no ceilings, no pedal -# law, no agent. FIXS still builds the class and calls it every step, so you -# write a function, never __init__ or a method named control. +# YOUR OWN DRIVING. None of the driver's logic runs unless you call it -- no +# ceilings, no pedal law, no agent. FIXS still builds the class and calls it +# every step, so you write a function, never __init__ or a method named control. # # Uncomment it, then switch the Driver line at the end of the file to # the one that names it. # -# A DYNAMOMETER STILL WORKS: uncomment the block above too and call it inside -# my_control, wherever you want it -- `target = dyno.exchange(target, 0.0, dt)`. -# What you cannot do is pass BOTH to fixs.driver(): replacing the driving -# leaves no point in the tick for an exchange to be called from, so that is -# refused rather than silently ignored. +# THE DRIVER IS STILL THERE: drv = fixs.driver.instance() inside my_control +# gives this run's driver -- drv.default(ego, dt) is its whole step, and +# drv.signalCeiling(ego), drv.leaderCeiling(ego), vRef, aRef = drv.reference(control), +# drv.exchange(vRef, aRef) and the rest are its pieces. Pass xil= as well and +# drv.exchange reaches your dyno. # # ego.speed / positionX / positionY / heading / acceleration are LIVE. -# speedDesired, signalLightColor, precedingVehicleDistance were HELD since -# the last feed, ego.feedAge seconds ago. +# speedDesired, accelerationDesired, signalLightColor, precedingVehicleDistance +# were HELD since the last feed, ego.feedAge seconds ago: the upstream +# controller's raw command, on every step. # # speedDesired IS DUAL-USE: the advisory arrives in it, and a speed command -# goes back out through it. Read it only when ego.feedAge is 0 -- between -# feeds you are reading back your own last command. (Measured before that was -# understood: 1299 of 2554 ticks matched the previous tick's own target.) +# goes back out through it. FIXS puts the advisory back before each step, so +# reading it never returns your own last command. # # steerAngleDesired IS AN ANGLE, in radians, where a CARLA agent's # control.steer is normalised [-1, 1]. Commanding through fixs.carla does @@ -83,5 +83,5 @@ #: carries the '#'. The name on the left is yours: fixs.driver() tells FIXS #: what it built, so nothing here has to be spelled a particular way. Driver = fixs.driver() # the driver as it is -# Driver = fixs.driver(exchange) # ... with a dynamometer -# Driver = fixs.driver(usercontrol=my_control) # your own driving +# Driver = fixs.driver(xil=exchange) # ... with a dynamometer +# Driver = fixs.driver(control=my_control) # your own driving diff --git a/CommonLib/ConfigHelper.cpp b/CommonLib/ConfigHelper.cpp index 33aed9e4..bf82ab35 100644 --- a/CommonLib/ConfigHelper.cpp +++ b/CommonLib/ConfigHelper.cpp @@ -1,5 +1,6 @@ #include "ConfigHelper.h" #include "PlatformCompat.h" // #65: windows.h/shlwapi live behind this now +#include using namespace std; @@ -302,6 +303,19 @@ int ConfigHelper::getConfig(string configName) { parserSubscription(node, "DetectorSubscription", XilSetup.DetectorSubscription); + XilSetup.SpeedToTraffic = parserString(node, "SpeedToTraffic", "virenv"); + XilSetup.SpeedToApplication = parserString(node, "SpeedToApplication", "traffic"); + for (std::string* v : { &XilSetup.SpeedToTraffic, &XilSetup.SpeedToApplication }) + std::transform(v->begin(), v->end(), v->begin(), [](unsigned char c) { return (char)std::tolower(c); }); + if (XilSetup.SpeedToTraffic != "virenv" && XilSetup.SpeedToTraffic != "xil") { + printf("ERROR: XilSetup.SpeedToTraffic must be one of virenv|xil, got '%s'\n", XilSetup.SpeedToTraffic.c_str()); + exit(-1); + } + if (XilSetup.SpeedToApplication != "traffic" && XilSetup.SpeedToApplication != "xil") { + printf("ERROR: XilSetup.SpeedToApplication must be one of traffic|xil, got '%s'\n", XilSetup.SpeedToApplication.c_str()); + exit(-1); + } + // =========================================================================== // Process subscription @@ -959,6 +973,17 @@ int ConfigHelper::getConfig(string configName) { } } + // The bench's speed arrives in the speedDesired of a virtual environment's ego record. + if (XilSetup.SpeedToTraffic == "xil" || XilSetup.SpeedToApplication == "xil") { + if (!XilSetup.EnableXil || EgoSetup.Dynamics != "virenv" || SimulationSetup.EnableExternalDynamics + || SimulationSetup.SelectedTrafficSimulator != "SUMO" || !CarlaSetup.EnablePythonBackend) { + printf("ERROR: XilSetup.SpeedToTraffic/SpeedToApplication: xil needs XilSetup.EnableXil: true,\n" + " EgoSetup.Dynamics: virenv, SimulationSetup.EnableExternalDynamics: false,\n" + " SelectedTrafficSimulator: SUMO and CarlaSetup.EnablePythonBackend: true.\n"); + exit(-1); + } + } + return 0; } @@ -976,6 +1001,8 @@ void ConfigHelper::resetConfig() { XilSetup.VehicleSubscription.clear(); XilSetup.SignalSubscription.clear(); XilSetup.DetectorSubscription.clear(); + XilSetup.SpeedToTraffic.clear(); + XilSetup.SpeedToApplication.clear(); } diff --git a/CommonLib/ConfigHelper.h b/CommonLib/ConfigHelper.h index c0a8d132..dfa82f39 100644 --- a/CommonLib/ConfigHelper.h +++ b/CommonLib/ConfigHelper.h @@ -103,6 +103,12 @@ struct XilSetup_t { Subscription_t DetectorSubscription; + // The ego speed SUMO is given: virenv (the virtual environment's) | xil (the bench's) + std::string SpeedToTraffic; + + // The ego speed applications receive: traffic (the traffic simulator's) | xil (the bench's) + std::string SpeedToApplication; + }; diff --git a/CommonLib/ConfigHelper.py b/CommonLib/ConfigHelper.py index 7d28f6ae..0091df95 100644 --- a/CommonLib/ConfigHelper.py +++ b/CommonLib/ConfigHelper.py @@ -96,6 +96,12 @@ def getConfig(self, configName): raise SystemExit( "ERROR: XilSetup.Transport must be one of inprocess|udp|tcp, got '%s'" % self.Xil_setup["Transport"]) + # The ego speed SUMO is given (virenv | xil) and applications receive (traffic | xil). + for key, allowed in (("SpeedToTraffic", ("virenv", "xil")), ("SpeedToApplication", ("traffic", "xil"))): + value = self.parserString(xil_node, key, allowed[0]).strip().lower() + if value not in allowed: + raise SystemExit("ERROR: XilSetup.%s must be one of %s, got '%s'" % (key, "|".join(allowed), value)) + self.Xil_setup[key] = value # Carla Setup carla_node = config.get("CarlaSetup", {}) @@ -203,6 +209,16 @@ def _egoKey(name, fallback): raise SystemExit( "ERROR: EgoSetup.Dynamics must be one of traffic|virenv|xil, got '%s'" % dyn) self.Ego_setup["Dynamics"] = dyn + # The bench's speed arrives in the speedDesired of a virtual environment's ego record. + if "xil" in (self.Xil_setup["SpeedToTraffic"], self.Xil_setup["SpeedToApplication"]) and ( + not self.Xil_setup["EnableXil"] or dyn != "virenv" + or self.simulation_setup["EnableExternalDynamics"] + or self.simulation_setup["SelectedTrafficSimulator"] != "SUMO" + or not self.Carla_setup.get("EnablePythonBackend", True)): + raise SystemExit( + "ERROR: XilSetup.SpeedToTraffic/SpeedToApplication: xil needs XilSetup.EnableXil: true,\n" + " EgoSetup.Dynamics: virenv, SimulationSetup.EnableExternalDynamics: false,\n" + " SelectedTrafficSimulator: SUMO and CarlaSetup.EnablePythonBackend: true.") # A dynamometer in the controller's loop is not a plant that owns the # ego: whoever integrates the ego still owns position, heading and # everything lateral, and the bench supplies one longitudinal number the diff --git a/CommonLib/VirEnv/EgoControllerHost.py b/CommonLib/VirEnv/EgoControllerHost.py index be7db8e1..285f9fcb 100644 --- a/CommonLib/VirEnv/EgoControllerHost.py +++ b/CommonLib/VirEnv/EgoControllerHost.py @@ -67,6 +67,7 @@ def control(ego, dt): LIVE, this call positionX/Y/Z, heading, speed, acceleration HELD since the feed speedDesired (the eco advisory -- genuinely 10 Hz) signalLightColor (the traffic simulator owns the phase) + feedSpeed (the feed's speed, which the eco advisory was planned from) ego.feedAge seconds since the last feed: how old the held ones are A controller that ignores ``feedAge`` is fine. One that differentiates a held @@ -402,6 +403,9 @@ def loadController(spec, appRoot=None): #: follow it. Beside _feedAge for the same reason, and cleared with it. _heldInputs = {} +#: The ego speed the last feed carried, before the live state overwrites it. +_feedSpeed = [None] + #: Fields the traffic simulator OWNS and a controller also WRITES. #: #: The record is one object used in both directions, so a controller's command @@ -479,6 +483,7 @@ def runController(backend, controller, ego, dt, onFeed, maxSteerRad): resetFeedAge() for name in _DUAL_USE: _heldInputs[name] = getattr(ego, name, None) + _feedSpeed[0] = getattr(ego, 'speed', None) else: _feedAge[0] += dt # Put the feed's value back before asking the controller for a new @@ -501,6 +506,7 @@ def runController(backend, controller, ego, dt, onFeed, maxSteerRad): object.__setattr__(ego, 'heading', es.heading) object.__setattr__(ego, 'speed', es.speed) object.__setattr__(ego, 'feedAge', _feedAge[0]) + object.__setattr__(ego, 'feedSpeed', _feedSpeed[0]) # Clear what the LAST step wrote before asking for this one. The record # survives every sub-step of a feed, so without this _written only ever diff --git a/CommonLib/fixs/__init__.py b/CommonLib/fixs/__init__.py index a6274759..ee8806fa 100644 --- a/CommonLib/fixs/__init__.py +++ b/CommonLib/fixs/__init__.py @@ -80,6 +80,7 @@ import math import os import socket +import struct import sys import time as _time import typing @@ -96,21 +97,23 @@ 'emit', 'transport', 'commandKind', 'Vehicle', 'MAX_STEER_RAD', 'Shutdown', 'FixsError', 'NotConnected', 'ProtocolError', - 'driver', + 'driver', 'accelerationFromSpeed', 'speedFromAcceleration', ] -def driver(exchange=None, **options): +def driver(exchange=None, *, xil=None, control=None, usercontrol=None, **options): """A ready-made controller for the ego -- see :mod:`CommonLib.fixs._driver`. - Controller = fixs.driver() # no cell - Controller = fixs.driver(exchange) # yours: (vref, dt) -> mps + Controller = fixs.driver() # no cell + Controller = fixs.driver(xil=exchange) # yours: (vref, dt) or (vref, aref, dt) -> mps + Controller = fixs.driver(control=my_control) # your own step: my_control(ego, dt) Imported lazily: the driver pulls in fixs.carla and CARLA's agents, which - a SUMO-only or CarMaker run has no reason to load. + a SUMO-only or CarMaker run has no reason to load. `exchange` and + `usercontrol` are the old names, kept for one release (see _driver._oldNames). """ from CommonLib.fixs._driver import driver as _driver - return _driver(exchange, **options) + return _driver(exchange, xil=xil, control=control, usercontrol=usercontrol, **options) def _driverSet(**options): @@ -119,7 +122,41 @@ def _driverSet(**options): setDefaults(**options) +def _driverInstance(): + """fixs.driver.instance(): the driver FIXS built for this run, from inside control(ego, dt).""" + from CommonLib.fixs._driver import instance + return instance() + + driver.set = _driverSet +driver.instance = _driverInstance + + +_F32 = struct.Struct(' m/s: speed + accelerationDesired * step, at the wire's float32, never below 0.""" + return max(0.0, _f32(speed + accelerationDesired * step)) + + +def accelerationFromSpeed(speed, speedDesired, step): + """(m/s, m/s, s) -> m/s^2: the float32 accelerationDesired that + speedFromAcceleration turns back into speedDesired exactly, or the nearest + one where the float32 wire has none (a speed between 0 and ~0.25 m/s).""" + target = _f32(speedDesired) + a = _f32((target - speed) / step) + ulp = 2.0 ** (math.frexp(a)[1] - 24) if a else 2.0 ** -149 + for k in (0, 1, -1, 2, -2, 3, -3): + b = _f32(a + k * ulp) + if speedFromAcceleration(speed, b, step) == target: + return b + return a #: Full-lock front road-wheel angle [rad]. `steerAngleDesired` is an ANGLE on @@ -233,10 +270,8 @@ class Vehicle(VehData): is available here with no change to this module. """ - #: Longitudinal control. TrafficLayer consumes one of these, never both: - #: ConfigHelper.cpp:256 requires exactly one in VehicleMessageField, and - #: TrafficHelper.cpp:801 reports that SUMO does not implement the - #: acceleration form. + #: Longitudinal control. SUMO acts on speedDesired; accelerationDesired may + #: ride with it for a controller downstream (fixs.driver referenceMode). LONGITUDINAL_FIELDS = frozenset({'speedDesired', 'accelerationDesired'}) #: The pedals. CarlaBackend::applyEgoActuation reads throttle, brake AND @@ -1060,12 +1095,7 @@ def _validateCommand(record): ) elif kind == 'speedsteer': - if written >= Vehicle.LONGITUDINAL_FIELDS: - raise ProtocolError( - f"'{who}': set speedDesired or accelerationDesired, not both -- " - f"TrafficLayer accepts exactly one longitudinal command " - f"(ConfigHelper.cpp:256)." - ) + pass # one or both longitudinal fields, with or without steer elif written & Vehicle.STEER_FIELD: # Steer alone became representable when it left PEDAL_FIELDS. It is not @@ -1088,7 +1118,7 @@ def __getattr__(name): # side, which a client that is not driving an ego has no reason to load, and # sumo reads scenario files a controller may never touch. Bound into the # module afterwards, so the cost is once and later lookups are ordinary. - if name in ('sumo', 'carla', 'config'): + if name in ('sumo', 'carla', 'config', 'xil'): import importlib module = importlib.import_module(f'{__name__}.{name}') globals()[name] = module diff --git a/CommonLib/fixs/_driver.py b/CommonLib/fixs/_driver.py index ec6d67a5..b9cf4994 100644 --- a/CommonLib/fixs/_driver.py +++ b/CommonLib/fixs/_driver.py @@ -14,29 +14,34 @@ def exchange(vref, dt): return my_dyno_rig.run_point(vref) # a speed in, the speed reached out - Controller = fixs.driver(exchange) + Controller = fixs.driver(xil=exchange) Your rig keeps its own API, its own protocol and its own rate; ``exchange`` is the glue, and it is the whole of what this module asks for. Pass nothing and nothing is in the loop. This module never goes looking for a dyno -- deciding one is there belongs to the caller, so passing a function IS -the declaration. For the simulated dyno, wrap the one FIXS already has:: +the declaration. For the simulated dyno, pass the one FIXS already has; its +exchange takes (vref, aref, dt), and the driver supplies aref:: dyno = fixs.xil.dynosim() - Controller = fixs.driver(lambda vref, dt: dyno.exchange(vref, 0.0, dt)) + Controller = fixs.driver(xil=dyno.exchange) which is a function like any other and holds no privilege over yours. -WHERE IT GOES IN THE LOOP -- after every decision, before any command:: +YOUR OWN STEP, using as much of this driver as you want:: - want = target(ego) the advisory, capped by the signal and the leader - want = exchange(want) what a real vehicle did with that - drive(want) pedals, or a speed for the plant to close on + def my_control(ego, dt): + drv = fixs.driver.instance() # the driver FIXS built for this run + target = min(drv.advisoryOf(ego) or drv.fallbackSpeed, + drv.signalCeiling(ego), drv.leaderCeiling(ego)) + ... -Next step the driver reads ``ego.speed`` back, so the dyno is in the loop and -not beside it. ``target`` and ``drive`` are public for anyone who wants that -ordering in their own hands. + Controller = fixs.driver(xil=exchange, control=my_control) + +``default(ego, dt)`` is the stock step, and the methods it calls are public: +controlPassive, shiftIntoFirstGear, advisoryOf, signalCeiling, leaderCeiling, +stepAgent, reference, exchange, speedToPedal, logStep. HOW IT DECIDES A SPEED. Every input that can demand less is turned into a SPEED -- the fastest the ego may go and still stop comfortably for the thing that @@ -63,9 +68,12 @@ def exchange(vref, dt): from __future__ import annotations import argparse +import inspect import math +import warnings -from . import FixsError # noqa: E402 +from . import FixsError, speedFromAcceleration # noqa: E402 +from .. import fixs as _fixs # noqa: E402 from . import carla as carla # noqa: E402 # CARLA's vendored agents: importing the package puts them on sys.path. from agents.navigation.behavior_agent import BehaviorAgent # noqa: E402 @@ -112,6 +120,14 @@ def exchange(vref, dt): #: Fix the lag (lead term) before selecting this. PEDAL_LOOP = 'speed' +#: What the upstream controller sends, and so what the driver reads: 'speed' +#: (speedDesired), 'accel' (accelerationDesired) or 'both'. The one not sent is +#: rebuilt from the feed's speed; 'both' takes each as sent, and they may differ. +REFERENCE_MODE = 'speed' + +#: The eco controller's step [s]: speedDesired = feedSpeed + accelerationDesired * FEED_STEP. +FEED_STEP = 0.1 + class Tuning(object): """The driver's gains, as ONE value. @@ -279,7 +295,7 @@ def __repr__(self): #: The options fixs.driver(**options) and fixs.driver.set(**options) take. _OPTION_NAMES = frozenset({'shape', 'loop', 'tuning', 'limits', 'idealSpeedTracking', - 'startInGear'}) + 'startInGear', 'referenceMode', 'feedStep'}) #: Set by fixs.driver.set(); every driver built afterwards starts from these. _DEFAULTS = {} @@ -309,6 +325,8 @@ def _options(config, overrides=None): default=over.get('shape', COMMAND_SHAPE)) p.add_argument('--pedal-loop', choices=('speed', 'accel'), default=over.get('loop', PEDAL_LOOP)) + p.add_argument('--reference-mode', choices=('speed', 'accel', 'both'), + default=over.get('referenceMode', REFERENCE_MODE)) #: One option, not six: a gain ladder is then one token per rung, and the #: whole tuning state is one string that lands in the log verbatim. p.add_argument('--tune', default=None, metavar='kv=0.3,ki=0.6') @@ -329,6 +347,7 @@ def _options(config, overrides=None): opt.idealSpeedTracking = bool( over.get('idealSpeedTracking', IDEAL_SPEED_TRACKING)) opt.startInGear = bool(over.get('startInGear', False)) + opt.feedStep = float(over.get('feedStep', FEED_STEP)) return opt @@ -337,10 +356,15 @@ class Controller: #: Both set by :func:`driver`. None means "ask the scenario". _EXCHANGE = None + #: True for an exchange(vref, aref, dt), False for exchange(vref, dt). + _EXCHANGE_TAKES_AREF = False _OPTIONS = None - _USERCONTROL = None + #: The user's control(ego, dt), run in place of default(). None runs default(). + _CONTROL = None def __init__(self, config, egoId): + global _instance + _instance = self opt = _options(config, getattr(self, '_OPTIONS', None)) self.shape = opt.command_shape self.loop = opt.pedal_loop @@ -354,6 +378,10 @@ def __init__(self, config, egoId): #: spawned car in neutral below ~15% throttle, so a creeping pedal never moves it. self.startInGear = opt.startInGear self._inGear = False + self.referenceMode = opt.reference_mode + self.feedStep = opt.feedStep + #: This feed's speed and acceleration commands. None until a feed is read. + self.feedVref, self.feedAref = None, 0.0 self.dt = float(config.get('CarlaTimeStep') or 0.1) self.fallbackSpeed = float(config.get('EgoTargetSpeed') or 8.33) self.useAdvisory = USE_ADVISORY @@ -368,13 +396,12 @@ def __init__(self, config, egoId): self.passive = currentDynamics() == 'traffic' self.log = _openLog( config.get('EgoControllerLog', '_datalog/agent_embedded.csv'), - 'fixs.driver shape=%s loop=%s %s %s exchange=%s' - % (self.shape, self.loop, self.tuning, self.limits, + 'fixs.driver shape=%s loop=%s reference=%s %s %s exchange=%s' + % (self.shape, self.loop, self.referenceMode, self.tuning, self.limits, getattr(self._EXCHANGE, '__name__', 'none'))) self.steps, self.elapsed = 0, 0.0 - #: Held between feeds -- see _advisoryOf. + #: Held between feeds -- see advisoryOf. self.advisory = None - self.vSignal = self.vLeader = _NO_LIMIT #: Who answers "you asked for this speed -- what did you reach?". #: Whatever was handed to fixs.driver(), and nothing else: this module #: never goes looking for a cell. Deciding one is in the loop is the @@ -409,7 +436,6 @@ def __init__(self, config, egoId): #: acceleration the inner stage closes on. self._dbg = None self.speedInteg = 0.0 - self.vRef = None self.pedal = 0.0 self._vPrev = 0.0 self._aMeas = 0.0 @@ -417,8 +443,11 @@ def __init__(self, config, egoId): print('[driver] exchange in the loop (%s)' % getattr(self._EXCHANGE, '__name__', 'exchange'), flush=True) - def exchange(self, vRef): - """(mps) -> mps -- what the dyno did with the speed we asked for. + def exchange(self, vRef, aRef=None): + """(mps, mps^2) -> mps -- what the dyno did with the speed and acceleration we asked for. + + A 3-argument exchange gets aRef; a 2-argument one gets vRef alone. + Without aRef, the one that goes with vRef is used (see reference()). An exchange that cannot answer this tick returns None, and the REFERENCE goes through untouched: it is the only value that cannot @@ -426,14 +455,44 @@ def exchange(self, vRef): Those ticks are counted, because a run that ends with many of them did not test what it claims to have tested. """ - reached = self._exchange(vRef, self.dt) + if self._EXCHANGE_TAKES_AREF: + reached = self._exchange(vRef, self._arefFor(vRef) if aRef is None else aRef, self.dt) + else: + reached = self._exchange(vRef, self.dt) if reached is None: self.misses += 1 return vRef self.vDyno = float(reached) return self.vDyno - def _build(self): + def _readFeed(self, ego): + """The feed's vref, per referenceMode. Keeps vref and aref as feedVref, feedAref; the one not read is rebuilt.""" + v = getattr(ego, 'feedSpeed', None) + v = float(ego.speed if v is None else v) + declared = _fixs._declaredFields + if (self.referenceMode != 'speed' and declared is not None + and 'accelerationDesired' not in declared): + raise FixsError("referenceMode=%r reads accelerationDesired, which is not in " + "VehicleMessageField" % self.referenceMode) + if self.referenceMode == 'accel': + aref = float(ego.accelerationDesired) + vref = speedFromAcceleration(v, aref, self.feedStep) + else: + vref = float(getattr(ego, 'speedDesired', 0.0) or 0.0) + if self.referenceMode == 'both': + aref = float(ego.accelerationDesired) + else: + aref = (vref - v) / self.feedStep + self.feedVref, self.feedAref = vref, aref + return vref + + def _arefFor(self, vRef): + """The acceleration that goes with vRef: the feed's, plus whatever this driver changed the speed by.""" + if self.feedVref is None: + return 0.0 + return self.feedAref + (vRef - self.feedVref) / self.feedStep + + def build(self): """Built on the first controlled tick, not in __init__: the traffic simulator inserts the ego, so `carla.ego` does not exist until the bridge adopts it.""" @@ -483,118 +542,76 @@ def _build(self): self.agent.ignore_traffic_lights(True) def control(self, ego, dt): + """FIXS calls this every step: the control passed to fixs.driver(), else default().""" if dt > 0: self.dt = dt self.elapsed += dt + self.steps += 1 + if self._CONTROL is not None: + self._CONTROL(ego, dt) + else: + self.default(ego, dt) - # YOUR driving, if you handed some to fixs.driver(). It is called with - # the same (ego, dt) FIXS calls us with, and writes its command the - # same way -- ego.set(...). Nothing of ours runs: no ceilings, no - # pedal law, no agent. You wanted your logic, so you get your logic. - if self._USERCONTROL is not None: - self._USERCONTROL(ego, dt) - self.steps += 1 - return - + def default(self, ego, dt): + """The stock step: a target speed, CARLA's agent planning toward it, the dyno, the command.""" if self.passive: - self._controlPassive(ego) + self.controlPassive(ego) return - - if self.startInGear and not self._inGear: - # No pedal command this step, so the host does not overwrite the gear. - carla.ego.apply_control(carla.VehicleControl(manual_gear_shift=True, gear=1)) - self._inGear = True - self.steps += 1 + if self.shiftIntoFirstGear(): return - if self.agent is None: - self._build() - - advisory = self._advisoryOf(ego) + # 1. Target speed: the advisory, capped by the signal and the leader. + advisory = self.advisoryOf(ego) wanted = advisory if advisory is not None else self.fallbackSpeed + vSignal = self.signalCeiling(ego) + vLeader = self.leaderCeiling(ego) + target = max(0.0, min(wanted, vSignal, vLeader)) - # EVERY CONSTRAINT IS A SPEED, so combining them is a min() and there is - # no arbitration left to get wrong. Each is an envelope: the fastest this - # controller may go and still stop comfortably for the thing it names. - # The agent's own following model and obstacle sweep still run beneath - # this ceiling -- they see what the wire cannot. - self.vSignal = self._signalCeiling(ego) - self.vLeader = self._leaderCeiling(ego) - target = max(0.0, min(wanted, self.vSignal, self.vLeader)) - self.agent._behavior.max_speed = target * 3.6 - - # The agent's OWN obstacle sweep reaches a constant too: - # collision_and_car_avoid_manager takes max(min_proximity_threshold, - # speed_limit/3), which is about 13 m here, centre to centre. Measured on - # this corridor, a hazard entered that sweep at 5.6 m while the ego held - # 10.1 m/s and contact followed 0.05 s later; a full-brake stop from - # 10.5 m/s takes 9.8-11.0 m. Give the sweep the same stopping distance - # the envelopes use, so the agent's own following model is engaged while - # there is still room for it to act. It still decides. - self.agent._behavior.min_proximity_threshold = max( - 10.0, ego.speed * ego.speed / (2.0 * self.limits.comfortDecel) - + self.limits.leaderMargin) - - # NOT a ceiling from the bench. Capping the agent's target with the - # bench's MEASURED speed latches: the bench starts at zero, so the - # ceiling is zero, so the reference handed to the bench is zero, and it - # never moves. Measured -- the ego sat at the spawn point for the whole - # 800 s. The agent plans against the signal and the leader; the bench's - # speed is what the pedal law chases, not a limit on intent. - - carla.refresh(ego) - control = self.agent.run_step() - if _isEmergencyStop(control, self.agent._max_brake): - # BehaviorAgent.run_step RETURNS from emergency_stop before the local - # planner runs, so the plan does not advance while the ego brakes -- - # and the ego is still moving. Measured on this corridor: a stop - # beginning at 7.4 m/s carried the ego over the head of its own - # waypoint queue (0.21 m at the closest) and 8.5 m past it. The - # planner purges by distance, so a waypoint 8.5 m BEHIND is never - # purged; when the brake lifted, the lateral controller went to full - # lock to go back for it and drove the ego in a circle 9.8 m off the - # corridor, into another vehicle. - # - # Running the planner and discarding its command restores exactly - # what the early return skipped -- the purge, on CARLA's own rule -- - # and invents no geometry of its own. - self.agent.get_local_planner().run_step() + # 2. CARLA's agent plans toward it, watching for vehicles out to its stopping distance. + minLookahead = max(10.0, ego.speed * ego.speed / (2.0 * self.limits.comfortDecel) + + self.limits.leaderMargin) + control = self.stepAgent(ego, speed=target, minLookaheadDist=minLookahead) + vRef, aRef = self.reference(control) - # THE COMMAND. A FIXS controller commands by writing to the ego record; - # control()'s return value is ignored. steerAngleDesired is an ANGLE in - # radians where the agent's steer is normalised [-1, 1], and the plant - # divides by the same constant. - vRef = self._reference(control) - #: What the loop was actually told to chase. lp._target_speed is - #: NOT this: it goes stale at the route limit whenever the agent - #: panic-brakes, so a figure drawn from it shows the ego being - #: asked for 53 km/h at the exact moment it was asked for zero. - self.vRef = vRef - - # THE BENCH, when there is one. The agent has decided a speed; the bench - # says what a vehicle with mass and a torque delay actually reaches, and - # THAT is what CARLA is told to hold. Next tick the agent reads back - # ego.speed, so the bench is in the loop rather than beside it. - if self.benchInLoop: - vRef = self.vRef = self.exchange(vRef) + # 3. The dyno: the speed a real vehicle reaches, which CARLA is told to hold. + vHold = self.exchange(vRef, aRef) if self.benchInLoop else vRef - # THE COMMAND, in CARLA's own two shapes. fixs.carla relays them onto - # the ego record and converts the normalised steer to the wire's radians - # in the one place that conversion belongs. + # 4. Command, and log. if self.shape == 'speed': carla.apply_ackermann_control(carla.VehicleAckermannControl( - speed=max(0.0, vRef), steer=control.steer)) + speed=max(0.0, vHold), steer=control.steer)) else: - # The SHAPE decides who closes the speed loop; the bench only - # decides what vRef is. Gating this on the bench made 'pedals' - # silently fall through to the agent's own pedals whenever - # EnableXil was off, so the law under test never ran. - thr, brk = self._speedToPedal(vRef, ego.speed) + thr, brk = self.speedToPedal(vHold, ego.speed) carla.apply_control(carla.VehicleControl( throttle=thr, brake=brk, steer=control.steer)) - self._logStep(ego, control, target, advisory) + self.logStep(ego, control, advisory=advisory, target=target, + vSignal=vSignal, vLeader=vLeader, vRef=vRef, aRef=aRef) + + def shiftIntoFirstGear(self): + """With startInGear, put the ego in first gear on its first step and return True; that step sends nothing else.""" + if not self.startInGear or self._inGear: + return False + # A pedal command in the same step would overwrite the gear. + carla.ego.apply_control(carla.VehicleControl(manual_gear_shift=True, gear=1)) + self._inGear = True + return True + + def stepAgent(self, ego, speed, minLookaheadDist): + """CARLA's agent plans one step toward `speed` [m/s], watching for vehicles at least + `minLookaheadDist` [m] ahead. Built on the first call. Returns its control.""" + if self.agent is None: + self.build() + self.agent._behavior.max_speed = speed * 3.6 + # The agent looks out to this or speed_limit/3, whichever is farther, centre to centre. + self.agent._behavior.min_proximity_threshold = minLookaheadDist + carla.refresh(ego) + control = self.agent.run_step() + if self.isEmergencyStop(control): + # run_step skips the planner on an emergency stop; advance it, or the ego later steers back for passed waypoints. + self.agent.get_local_planner().run_step() + return control - def _controlPassive(self, ego): + def controlPassive(self, ego): """TELEPORT: the cell is the whole driver. No CARLA actor, so no agent, no steering, no obstacle sweep -- the @@ -607,13 +624,13 @@ def _controlPassive(self, ego): THE CEILINGS DO NOT APPLY HERE, and that is the whole of the difference from the virenv path. - There, the advisory is a TARGET a CARLA agent tracks, and _signalCeiling - and _leaderCeiling are that agent's envelope -- it plans in a world the + There, the advisory is a TARGET a CARLA agent tracks, and signalCeiling + and leaderCeiling are that agent's envelope -- it plans in a world the wire cannot see, so it is given room to be wrong in. Here the advisory is nobody's target: the eco controller writes speedDesired and the traffic simulator integrates it. That IS the rung below. Applying the envelopes on top double-counts a stop the eco controller has already - planned -- _signalCeiling alone commands a 2 m/s^2 deceleration to the + planned -- signalCeiling alone commands a 2 m/s^2 deceleration to the bar from as far as 200 m out -- and the ego slows long before a bar it was never going to overrun. @@ -628,7 +645,7 @@ def _controlPassive(self, ego): Anything else and the cell is not the only thing being measured. """ # The eco controller's command, as received, and nothing else done to - # it. Not _advisoryOf: that drops anything <= 0.01 as 'no advisory', and + # it. Not advisoryOf: that drops anything <= 0.01 as 'no advisory', and # a command of ZERO is exactly what the eco controller writes AT a stop # bar -- read as absent, it was replaced by fallbackSpeed and the ego # was told to accelerate away from the bar it had just been asked to @@ -638,16 +655,10 @@ def _controlPassive(self, ego): # Safe to read the field directly: it is dual-use, but runController # restores the feed's value before every call, so what is here is the # eco controller's, never this driver's own last command (FIXS#305). - advisory = float(getattr(ego, 'speedDesired', 0.0) or 0.0) + advisory = self._readFeed(ego) self.advisory = advisory - #: Neither computed nor applied. Logged as such, so a passive trace - #: cannot be read as though an envelope had bound. - self.vSignal = self.vLeader = _NO_LIMIT - target = advisory - - vRef = self.vRef = target - if self.benchInLoop: - vRef = self.vRef = self.exchange(vRef) + vRef, aRef = advisory, self.feedAref + vHold = self.exchange(vRef, aRef) if self.benchInLoop else vRef # THE COMMAND. speedDesired only: steer belongs to whoever owns the # lateral, and here that is the traffic simulator. @@ -658,10 +669,12 @@ def _controlPassive(self, ego): # simulator is asked to hold. A scenario therefore carries the same # Controller line as its virenv sibling, and the pair differs by # Dynamics alone. - ego.set(speedDesired=max(0.0, vRef)) - self._logStep(ego, None, target, advisory) + ego.set(speedDesired=max(0.0, vHold)) + # No ceilings on this rung: logged blank, so the trace cannot read as though one bound. + self.logStep(ego, None, advisory=advisory, target=advisory, + vSignal=_NO_LIMIT, vLeader=_NO_LIMIT, vRef=vRef, aRef=aRef) - def _advisoryOf(self, ego): + def advisoryOf(self, ego): """The eco controller's advisory, read only on the tick it is new. `speedDesired` is one field used in both directions: the eco controller @@ -673,14 +686,14 @@ def _advisoryOf(self, ego): sub-tick. `feedAge` is 0 exactly on the tick a feed lands, so that is when the field still holds the advisory; between feeds the last one stands. """ + if float(getattr(ego, 'feedAge', 0.0) or 0.0) <= 1e-9: + v = self._readFeed(ego) + self.advisory = v if v > 0.01 else None if not self.useAdvisory: return None - if float(getattr(ego, 'feedAge', 0.0) or 0.0) <= 1e-9: - v = ego.speedDesired - self.advisory = float(v) if (v or 0) > 0.01 else None return self.advisory - def _signalCeiling(self, ego): + def signalCeiling(self, ego): """The speed the next signal allows. The traffic simulator answers for the movement THIS ego will make -- its @@ -705,7 +718,7 @@ def _signalCeiling(self, ego): return _NO_LIMIT return _stopBy(dist - lim.stopMargin, lim.comfortDecel) - def _leaderCeiling(self, ego): + def leaderCeiling(self, ego): """The speed the vehicle ahead allows. This does NOT replace the agent's own obstacle sweep. The wire's leader @@ -733,7 +746,7 @@ def _leaderCeiling(self, ego): + 2.0 * self.limits.comfortDecel * max(0.0, gap - self.limits.leaderMargin)) - def _speedToPedal(self, vTarget, vEgo): + def speedToPedal(self, vTarget, vEgo): """(float, float) -> (throttle, brake) that puts CARLA on vTarget. PEDAL_LOOP picks which error the loop closes on. Either way one @@ -779,19 +792,31 @@ def _speedToPedal(self, vTarget, vEgo): self._dbg = (error, demand, self._aMeas, self.pedal) return (max(0.0, self.pedal), max(0.0, -self.pedal)) - def _reference(self, control): - """The speed the agent is asking for, in m/s. + def reference(self, control): + """(vRef, aRef): the speed the agent is asking for [m/s], and the acceleration that goes with it [m/s^2]. - The agent's own longitudinal target, except when it panic-brakes: an + vRef is the agent's own longitudinal target, except when it panic-brakes: an emergency stop leaves that target untouched, so reading it alone would command the ego to keep driving through the thing the agent stopped for. A full-brake command IS the target speed being zero. + + aRef is the eco controller's acceleration (feedAref), plus the change the + caps and the agent made to its speed (vRef - feedVref) over one feed step. """ - if _isEmergencyStop(control, self.agent._max_brake): - return 0.0 - return max(0.0, self.agent.get_local_planner()._target_speed / 3.6) + if self.isEmergencyStop(control): + vRef = 0.0 + else: + vRef = max(0.0, self.agent.get_local_planner()._target_speed / 3.6) + return vRef, self._arefFor(vRef) + + def isEmergencyStop(self, control): + """Is the agent's command its emergency_stop rather than a planned one?""" + return _isEmergencyStop(control, self.agent._max_brake) def shutdown(self): + global _instance + if _instance is self: + _instance = None if self.log is not None: self.log.close() self.log = None @@ -803,13 +828,11 @@ def shutdown(self): % (self.misses, self.steps), flush=True) print('[driver] %d control steps' % self.steps, flush=True) - def _logStep(self, ego, cmd, target, advisory): - """This application's instrumentation. It records what FIXS supplied - beside what the agent concluded -- vehicles seen, the wire's limit, the - speed the agent settled on -- because two defects that stopped the ego - dead were invisible without those three and obvious with them - (ORNL-Real-Sim/FIXS#355).""" - self.steps += 1 + def logStep(self, ego, cmd, *, advisory, target, vSignal, vLeader, vRef, aRef=None): + """One row of the driver log: what FIXS supplied, what this step decided, and what the agent concluded. + + vRef and aRef are what the step sent the dyno; vDyno is what came back. + """ if self.log is None: return # Agent-side columns are blank on a passive run: there is no planner, @@ -833,59 +856,95 @@ def _logStep(self, ego, cmd, target, advisory): ego.signalLightDistance, nSeen, float(getattr(ego, 'speedLimit', 0.0) or 0.0), agentTarget, - _col(self.vSignal), _col(self.vLeader), + _col(vSignal), _col(vLeader), '' if self.vDyno is None else '%.4f' % self.vDyno) - + (',' if self.vRef is None else ',%.4f' % self.vRef) + + (',' if vRef is None else ',%.4f' % vRef) + + (',' if aRef is None else ',%.4f' % aRef) + (',,,,\n' if self._dbg is None else ',%.4f,%.4f,%.4f,%.4f\n' % self._dbg)) -def driver(exchange=None, usercontrol=None, **options): - """(callable) -> class -- the controller the scenario should name. +def _takesAref(exchange): + """True for exchange(vref, aref, dt), False for exchange(vref, dt); anything else is refused.""" + params = inspect.signature(exchange).parameters.values() + n = sum(1 for p in params if p.kind in (p.POSITIONAL_ONLY, p.POSITIONAL_OR_KEYWORD) + and p.default is p.empty) + if n not in (2, 3): + raise TypeError('fixs.driver(xil=): takes (vref, dt) or (vref, aref, dt), ' + 'got %d required arguments' % n) + return n == 3 - ``exchange(vref, dt) -> mps`` is yours: a speed goes in, the speed your - dyno reached comes back. Return None on a tick it could not answer and the - reference passes through untouched; those are counted and reported. - Omit it and nothing is in the loop. For the simulated cell, wrap - ``fixs.xil.dynosim().exchange(vref, aref, dt)``; for yours, pass yours:: +def _oldNames(exchange, usercontrol, xil, control): + """fixs.driver(exchange) and usercontrol=, translated to xil= and control= with a warning. - Controller = fixs.driver(exchange) + Kept for one release. To remove: delete this function, the `exchange` and + `usercontrol` parameters of driver() (here and in fixs/__init__.py), and the + line in driver() that calls it. + """ + if exchange is not None: + if xil is not None: + raise TypeError('fixs.driver(): pass the dyno once, as xil=') + warnings.warn('fixs.driver(exchange) is now fixs.driver(xil=exchange)', + FutureWarning, stacklevel=4) + xil = exchange + if usercontrol is not None: + if control is not None: + raise TypeError('fixs.driver(): pass your step once, as control=') + warnings.warn('fixs.driver(usercontrol=fn) is now fixs.driver(control=fn)', + FutureWarning, stacklevel=4) + control = usercontrol + return xil, control + + +def driver(exchange=None, *, xil=None, control=None, usercontrol=None, **options): + """-> class -- the controller the scenario should name. + + ``xil(vref, dt) -> mps`` is yours: a speed goes in, the speed your dyno + reached comes back. ``xil(vref, aref, dt)`` also gets the acceleration that + goes with vref. Return None on a tick it could not answer and the reference + passes through untouched; those are counted and reported. Omit it and + nothing is in the loop. For the simulated cell, pass + ``fixs.xil.dynosim().exchange``. + + ``control(ego, dt)`` is your own step, called in place of :meth:`Controller.default`. + It reaches this driver through :func:`instance` when it wants any of it. Returns a CLASS, and ALSO records it here, so the name you give it is yours:: - Driver = fixs.driver(exchange) # any name - fixs.driver(exchange) # or none at all - - The loader asks :func:`built` before it scans the module for a name. The - scenario still says which FILE -- FIXS has nothing to import otherwise -- - but nothing inside it has to be spelled a particular way. + Driver = fixs.driver(xil=exchange) # any name + fixs.driver(xil=exchange) # or none at all ``options`` override the module defaults and anything given to ``fixs.driver.set()`` before this call; the scenario's own ``--command-shape`` still wins over all of them, being nearer the run. """ - for name, fn in (('exchange', exchange), ('usercontrol', usercontrol)): + xil, control = _oldNames(exchange, usercontrol, xil, control) + for name, fn in (('xil', xil), ('control', control)): if fn is not None and not callable(fn): - raise TypeError('fixs.driver(%s): %r is not callable' % (name, fn)) - if usercontrol is not None and exchange is not None: - # Your control() decides when to ask a cell and what to do with the - # answer. Taking an exchange too would mean we called it somewhere - # inside logic we are not running. - raise TypeError( - 'fixs.driver(): usercontrol replaces the driving, so an exchange ' - 'here would never be called -- ask your cell inside your own ' - 'control(ego, dt).') + raise TypeError('fixs.driver(%s=): %r is not callable' % (name, fn)) cls = type('Controller', (Controller,), - dict(_EXCHANGE=staticmethod(exchange) if exchange else None, - _USERCONTROL=staticmethod(usercontrol) if usercontrol - else None, + dict(_EXCHANGE=staticmethod(xil) if xil else None, + _EXCHANGE_TAKES_AREF=_takesAref(xil) if xil else False, + _CONTROL=staticmethod(control) if control else None, _OPTIONS=dict(_DEFAULTS, **options))) _BUILT.append(cls) return cls +#: The driver of the run in progress; see :func:`instance`. +_instance = None + + +def instance(): + """The driver FIXS built for this run, for a control(ego, dt) passed to fixs.driver().""" + if _instance is None: + raise FixsError('fixs.driver.instance(): no driver yet -- call it inside your ' + 'control(ego, dt), not when the file is imported') + return _instance + + #: Every class :func:`driver` has built. The loader clears this before it #: imports a controller file and reads it after, so "how many" is asked of one #: FILE rather than of the process -- a test, or a second controller, may build @@ -938,6 +997,6 @@ def _openLog(path, provenance=''): f.write('# %s' % provenance + chr(10)) f.write('t,feedAge,x,y,speed,advisory,target,throttle,brake,steer,' 'wpLeft,leaderGap,signalColor,signalDist,nSeen,' - 'wireLimit,agentTarget,vSignal,vLeader,vDyno,vRef,' + 'wireLimit,agentTarget,vSignal,vLeader,vDyno,vRef,aRef,' 'pidError,pidDemand,pidAMeas,pidPedal\n') return f diff --git a/CommonLib/xil/driver.py b/CommonLib/xil/driver.py index 7a4df63d..ae2cc656 100644 --- a/CommonLib/xil/driver.py +++ b/CommonLib/xil/driver.py @@ -15,9 +15,12 @@ a coupling. """ +from collections import deque + class RobotDriver(object): - """PI on speed error, producing one pedal split into throttle and brake. + """PI on speed error (``mode='speed'``) or on acceleration error + (``mode='accel'``), producing one pedal split into throttle and brake. One axis, not two: positive is throttle, negative is brake, and they are never both non-zero. That is what a driver does and what a VehicleControl @@ -27,7 +30,13 @@ class RobotDriver(object): def __init__(self, kp=0.45, ki=0.25, max_throttle=1.0, max_brake=1.0, standstill_ref_mps=0.05, standstill_brake=0.3, max_accel_mps2=None, max_decel_mps2=None, - stop_speed_mps=0.0, stop_accel_mps2=0.0): + stop_speed_mps=0.0, stop_accel_mps2=0.0, delay_s=0.0, + actuator=None, mode='speed', kp_accel=0.05, ki_accel=0.5, + accel_filter_s=0.1): + if mode not in ('speed', 'accel'): + raise ValueError("mode must be 'speed' or 'accel', got %r" % (mode,)) + #: 'speed' chases v_ref; 'accel' chases a_ref and ignores v_ref. + self.mode = mode self.kp = kp self.ki = ki self.max_throttle = max_throttle @@ -58,8 +67,24 @@ def __init__(self, kp=0.45, ki=0.25, max_throttle=1.0, max_brake=1.0, #: less than stop_accel_mps2 (openpilot: 0.3 and 0.1). 0.0 is off. self.stop_speed_mps = stop_speed_mps self.stop_accel_mps2 = stop_accel_mps2 + #: Transport delay between the robot's command and the pedal, in s; 0.0 is + #: off. Held per step, so it resolves to whole steps of the caller's dt. + self.delay_s = delay_s + #: Your pedal dynamics, after the delay: actuator(throttle, brake, dt) + #: -> (throttle, brake). Its reset(), if it has one, runs with ours. + self.actuator = actuator + #: Accel mode: PI gains on a_ref - a_meas, no feedforward. + self.kp_accel = kp_accel + self.ki_accel = ki_accel + #: Time constant of the first-order filter on dv/dt that gives a_meas. + self.accel_filter_s = accel_filter_s + self.a_meas = 0.0 + self._v_prev = None self.integral = 0.0 self.pedal = 0.0 + self._clock = 0.0 + self._sent = deque() + self._applied = (0.0, 0.0) #: How far the ramp may sit from the measured speed. Anti-windup for the #: setpoint itself: without it, a ramp advancing past a vehicle that cannot @@ -70,14 +95,42 @@ def reset(self): self.integral = 0.0 self.pedal = 0.0 self._ramp = None + self._clock = 0.0 + self._sent.clear() + self._applied = (0.0, 0.0) + self.a_meas = 0.0 + self._v_prev = None + if hasattr(self.actuator, 'reset'): + self.actuator.reset() def step(self, v_ref, v_measured, dt, a_ref=None): - """Returns (throttle, brake), each in [0, 1], never both positive. + """Returns (throttle, brake), each in [0, 1], never both positive, + unless an ``actuator`` says otherwise. - ``a_ref`` is the acceleration command, read only by the hold below. + ``a_ref`` is the acceleration command: the reference in accel mode, + otherwise read only by the hold. With ``delay_s`` set, this is the command of ``delay_s`` ago, and + (0, 0) until there is one; ``pedal`` stays the command just made. """ if dt <= 0.0: raise ValueError('dt must be positive') + command = self._command(v_ref, v_measured, dt, a_ref) + if self.delay_s > 0.0: + self._sent.append((self._clock, command)) + due = self._clock - self.delay_s + 1e-9 + self._clock += dt + while self._sent and self._sent[0][0] <= due: + self._applied = self._sent.popleft()[1] + command = self._applied + if self.actuator is not None: + command = self.actuator(command[0], command[1], dt) + return command + + def _command(self, v_ref, v_measured, dt, a_ref): + if self.mode == 'accel' and a_ref is None: + raise ValueError("mode 'accel' needs a_ref") + if self._v_prev is not None: + self.a_meas += ((v_measured - self._v_prev) / dt - self.a_meas) * dt / (self.accel_filter_s + dt) + self._v_prev = v_measured if a_ref is not None and v_measured < self.stop_speed_mps \ and a_ref < self.stop_accel_mps2: @@ -85,6 +138,9 @@ def step(self, v_ref, v_measured, dt, a_ref=None): self.pedal = -self.standstill_brake return 0.0, min(self.max_brake, self.standstill_brake) + if self.mode == 'accel': + return self._accel(a_ref, v_measured, dt) + # THE ENVELOPE. A RAMPED setpoint, advanced from its own last value -- # not the incoming reference clamped to the measured speed. # @@ -119,11 +175,23 @@ def step(self, v_ref, v_measured, dt, a_ref=None): self.pedal = -self.standstill_brake return 0.0, min(self.max_brake, self.standstill_brake) - err = v_ref - v_measured - raw = self.kp * err + self.ki * self.integral + return self._pi(v_ref - v_measured, self.kp, self.ki, dt) + + def _accel(self, a_ref, v_measured, dt): + if self.max_accel_mps2 is not None: + a_ref = min(a_ref, self.max_accel_mps2) + if self.max_decel_mps2 is not None: + a_ref = max(a_ref, -self.max_decel_mps2) + if a_ref <= 0.0 and v_measured <= self.standstill_ref_mps: + self.integral = 0.0 + self.pedal = -self.standstill_brake + return 0.0, min(self.max_brake, self.standstill_brake) + return self._pi(a_ref - self.a_meas, self.kp_accel, self.ki_accel, dt) + + def _pi(self, err, kp, ki, dt): + raw = kp * err + ki * self.integral if -self.max_brake < raw < self.max_throttle: self.integral += err * dt # anti-windup: freeze at the rail self.pedal = max(-self.max_brake, - min(self.max_throttle, - self.kp * err + self.ki * self.integral)) + min(self.max_throttle, kp * err + ki * self.integral)) return (self.pedal, 0.0) if self.pedal >= 0.0 else (0.0, -self.pedal) diff --git a/TrafficLayer/TrafficLayer/mainTrafficLayer.cpp b/TrafficLayer/TrafficLayer/mainTrafficLayer.cpp index 60aabbb8..f3977417 100644 --- a/TrafficLayer/TrafficLayer/mainTrafficLayer.cpp +++ b/TrafficLayer/TrafficLayer/mainTrafficLayer.cpp @@ -1032,6 +1032,12 @@ int main(int argc, char* argv[]) { // initConnection() accepted them before the loop (no warm-up, no CarMaker). bool clientsAccepted = !ENABLE_VEH_SIMULATOR && !WARMUP_CONFIGURED; + // XilSetup.SpeedTo*: xil -- per virtual-environment ego, the bench's speed the bridge last returned + struct XilSpeed_t { float speed; float prevSpeed; bool hasPrev; }; + unordered_map xilSpeed; + const bool XIL_SPEED = Config_c.XilSetup.SpeedToTraffic == "xil" || Config_c.XilSetup.SpeedToApplication == "xil"; + double xilStep = 0; // the traffic simulator's step, read once it is connected + //while (simTime <= tSimuEnd && ii < nT) { while (!g_shutdown.shutdownRequested) { @@ -1279,6 +1285,18 @@ int main(int argc, char* argv[]) { // (lowest-port) client, so single-client flows stay byte-identical. unordered_map pubSrc = MsgServer_c.VehDataRecv_um; for (auto& bIt : MsgClient_c.VehDataRecv_um) pubSrc[bIt.first] = bIt.second; + // SpeedToApplication: xil -- applications get the bench's speed; the bridge keeps SUMO's + if (Config_c.XilSetup.SpeedToApplication == "xil" && curPort != Config_c.CarlaSetup.CarlaClientPort) { + if (xilStep <= 0) xilStep = SUMO_TRACI_NAMESPACE::Simulation::getDeltaT(); + for (auto& xIt : xilSpeed) { + auto pIt = pubSrc.find(xIt.first); + if (pIt == pubSrc.end()) continue; + pIt->second.speed = xIt.second.speed; + // SUMO's acceleration: (v_k - v_k-1) / step + if (xIt.second.hasPrev) + pIt->second.acceleration = (float)(((double)xIt.second.speed - (double)xIt.second.prevSpeed) / xilStep); + } + } for (auto it : pubSrc) { if (ENABLE_VEH_SIMULATOR) { // if id is ego, and is the first socket (XIL), send it @@ -1402,6 +1420,20 @@ int main(int argc, char* argv[]) { // ---- #174 sequential: RECV this client's return, merge into B ---- // (last-writer-wins per id; feeds the NEXT client's overlaid publish). + // The bridge's ego records carry the bench's speed in speedDesired. Earlier + // clients' records for those egos are set aside so only the bridge's are read. + const bool fromBridge = XIL_SPEED && curPort == Config_c.CarlaSetup.CarlaClientPort; + unordered_map setAside; + if (fromBridge) { + for (auto rIt = MsgClient_c.VehDataRecv_um.begin(); rIt != MsgClient_c.VehDataRecv_um.end();) { + if (Traffic_c.externalEgoOwnerOf(rIt->first) == TrafficHelper::ExternalEgoOwner::VirEnv) { + setAside.insert(*rIt); + rIt = MsgClient_c.VehDataRecv_um.erase(rIt); + } + else ++rIt; + } + } + int simStateRecvC; float simTimeRecvC; if (Sock_c.recvData(actualClientSock[iC], &simStateRecvC, &simTimeRecvC, MsgClient_c) < 0) { if (FIXS::Platform::socketErrorCode() != FIXS::Platform::kSocketErrInterrupted && @@ -1425,6 +1457,20 @@ int main(int argc, char* argv[]) { for (auto& r : idToRemove) MsgClient_c.VehDataRecv_um.erase(r); } + if (fromBridge) { + for (auto& rIt : MsgClient_c.VehDataRecv_um) { + if (Traffic_c.externalEgoOwnerOf(rIt.first) != TrafficHelper::ExternalEgoOwner::VirEnv) continue; + VehFullData_t& r = rIt.second; + auto xIt = xilSpeed.find(rIt.first); + if (xIt == xilSpeed.end()) xilSpeed[rIt.first] = { r.speedDesired, 0.0f, false }; + else xIt->second = { r.speedDesired, xIt->second.speed, true }; + // SpeedToTraffic: xil -- SUMO's setSpeed reads speed + if (Config_c.XilSetup.SpeedToTraffic == "xil") r.speed = r.speedDesired; + } + // an ego the bridge did not return this tick keeps the earlier client's record + for (auto& kv : setAside) MsgClient_c.VehDataRecv_um.insert(kv); + } + } } catch (const std::exception& e) { diff --git a/tests/Python/unit/test_controller_fixs_import.py b/tests/Python/unit/test_controller_fixs_import.py index 4355448e..dd21510f 100644 --- a/tests/Python/unit/test_controller_fixs_import.py +++ b/tests/Python/unit/test_controller_fixs_import.py @@ -28,6 +28,13 @@ def test_the_relay_resolves_to_the_same_one(): assert sys.modules['fixs.carla'] is CommonLib.fixs.carla +def test_xil_needs_no_import_of_its_own(): + host._letControllerImportFixs() + import fixs + import CommonLib.fixs.xil + assert fixs.xil is CommonLib.fixs.xil + + def test_an_import_executed_by_a_controller_sees_the_live_records(): """What the controller actually writes: `import fixs` then read the feed. The object it gets must be the one the engine mutates, not a twin.""" diff --git a/tests/Python/unit/test_fixs_api.py b/tests/Python/unit/test_fixs_api.py index aa6d017f..020247c0 100644 --- a/tests/Python/unit/test_fixs_api.py +++ b/tests/Python/unit/test_fixs_api.py @@ -393,14 +393,13 @@ def test_complete_actuation_is_accepted(): fixs.close() -def test_both_longitudinal_commands_are_rejected(): - """ConfigHelper.cpp:256 accepts exactly one.""" - _install([(1, 0.1, [_veh('ego')])], fields=FULL_FIELDS) +def test_both_longitudinal_commands_are_sent(): + """speedDesired for SUMO, accelerationDesired beside it for a downstream controller.""" + sock = _install([(1, 0.1, [_veh('ego')])], fields=FULL_FIELDS) fixs.recv() fixs.vehicle.get('ego').set(speedDesired=9.0, accelerationDesired=1.0) - with pytest.raises(fixs.ProtocolError) as excinfo: - fixs.send() - assert 'not both' in str(excinfo.value) + fixs.send() + assert _recordCount(sock.sent[0], FULL_FIELDS) == 1 fixs.close() diff --git a/tests/Python/unit/test_fixs_driver.py b/tests/Python/unit/test_fixs_driver.py index 61b74e57..c5c9bb30 100644 --- a/tests/Python/unit/test_fixs_driver.py +++ b/tests/Python/unit/test_fixs_driver.py @@ -74,7 +74,7 @@ def test_it_returns_a_class_because_that_is_what_the_loader_expects(): def test_a_non_callable_exchange_is_refused_at_the_call_not_at_the_tick(): with pytest.raises(TypeError): - driver(42) + driver(xil=42) # -- the three cases the seam has to answer --------------------------------- @@ -96,7 +96,7 @@ def test_the_scenario_alone_brings_nothing(scenario): @pytest.mark.parametrize('scenario', [False], indirect=True) def test_your_function_needs_no_scenario_flag(scenario): """Passing one IS the declaration.""" - d = _build(driver(lambda v, dt: v)) + d = _build(driver(xil=lambda v, dt: v)) assert d.benchInLoop is True @@ -110,7 +110,7 @@ def test_the_simulated_dyno_is_passed_in_like_any_other(scenario): def exchange(vref, dt): return bench.exchange(vref, 0.0, dt) - d = _build(driver(exchange)) + d = _build(driver(xil=exchange)) assert d.benchInLoop is True assert d.exchange(10.0) > 0.0 # it answered assert bench.sim.vehicle.mass_kg == 900.0 # stated in code, not the yaml @@ -133,7 +133,7 @@ def test_the_driver_does_not_import_xil(): def test_a_tick_the_dyno_cannot_answer_passes_the_reference_through(scenario): """None back must not become 0 m/s: that would invent a stop.""" answers = [7.5, None, 7.6] - d = _build(driver(lambda v, dt: answers.pop(0))) + d = _build(driver(xil=lambda v, dt: answers.pop(0))) assert d.exchange(9.0) == 7.5 # the cell answered assert d.exchange(9.0) == 9.0 # it did not -- the reference stands assert d.misses == 1 @@ -187,7 +187,7 @@ def test_the_scenario_beats_the_factory_for_gains_too(): def test_the_run_records_what_drove_it(scenario, tmp_path): """A batch once reported gains it had not used. The log says.""" path = tmp_path / 'agent.csv' - cls = driver(lambda v, dt: v, shape='pedals', tuning=Tuning(kv=0.33)) + cls = driver(xil=lambda v, dt: v, shape='pedals', tuning=Tuning(kv=0.33)) obj = cls.__new__(cls) Controller.__init__(obj, {'EgoControllerLog': str(path)}, 'ego') obj.shutdown() @@ -212,23 +212,23 @@ def test_a_stopped_car_asked_for_nothing_holds_the_brake(): """The full-stop rule -- and the line that broke rung 4 in review, because it reads a gain before the law had bound it.""" d = _pedalLaw() - assert d._speedToPedal(0.0, 0.0) == (0.0, 1.0) + assert d.speedToPedal(0.0, 0.0) == (0.0, 1.0) def test_zero_speed_error_still_holds_a_pedal(): """The whole reason the integral exists: road load has to be paid.""" d = _pedalLaw() for _ in range(40): - thr, brk = d._speedToPedal(8.0, 8.0 - 0.2) # a standing shortfall + thr, brk = d.speedToPedal(8.0, 8.0 - 0.2) # a standing shortfall assert thr > 0.0 - thr, brk = d._speedToPedal(8.0, 8.0) # error now zero + thr, brk = d.speedToPedal(8.0, 8.0) # error now zero assert thr > 0.0, 'the pedal collapsed when the error did' def test_the_gains_reach_the_law(): """A tuning that cannot move the pedal is a tuning nobody is applying.""" slow, fast = _pedalLaw(kv=0.01), _pedalLaw(kv=0.9) - assert fast._speedToPedal(9.0, 4.0)[0] > slow._speedToPedal(9.0, 4.0)[0] + assert fast.speedToPedal(9.0, 4.0)[0] > slow.speedToPedal(9.0, 4.0)[0] # -- the limits, and that they reach the ceilings --------------------------- @@ -338,7 +338,7 @@ def test_the_template_names_keys_and_files_that_exist(): def test_the_template_shows_the_other_two_forms(): src = io.open(_template(), encoding='utf-8').read() - assert 'exchange' in src and 'usercontrol' in src + assert 'xil=' in src and 'control=' in src # -- your own driving, through the same factory ------------------------------ @@ -353,7 +353,7 @@ def set(self, **kw): @pytest.mark.parametrize('scenario', [False], indirect=True) -def test_usercontrol_replaces_the_driving_entirely(scenario): +def test_control_replaces_the_driving_entirely(scenario): """Your logic, called with the same (ego, dt), writing the same way -- and none of the driver's own runs.""" seen = [] @@ -362,31 +362,86 @@ def mine(ego, dt): seen.append(dt) ego.set(speedDesired=9.0) - d = _build(driver(usercontrol=mine)) + d = _build(driver(control=mine)) ego = _FakeEgo() d.control(ego, 0.05) assert seen == [0.05] assert ego.cmd == {'speedDesired': 9.0} assert d.agent is None, 'the CARLA agent was built for logic that is not ours' + assert (d.dt, d.steps) == (0.05, 1) @pytest.mark.parametrize('scenario', [False], indirect=True) -def test_usercontrol_needs_no_name_and_no_method(scenario): +def test_control_needs_no_name_and_no_method(scenario): """The point of it: you write a function, not a class with control().""" - from CommonLib.VirEnv.EgoControllerHost import loadController - cls = driver(usercontrol=lambda ego, dt: None) + cls = driver(control=lambda ego, dt: None) assert callable(getattr(cls, 'control', None)) -def test_usercontrol_and_exchange_together_is_refused(): - """Your control decides when to ask a cell; ours would never call it.""" - with pytest.raises(TypeError): - driver(exchange=lambda v, dt: v, usercontrol=lambda ego, dt: None) +@pytest.mark.parametrize('scenario', [False], indirect=True) +def test_control_reaches_the_driver_and_its_dyno(scenario): + """Your step gets this run's driver from instance(), and the xil passed beside it.""" + got = [] + + def mine(ego, dt): + drv = fixs.driver.instance() + got.append((drv, drv.exchange(7.0))) + d = _build(driver(xil=lambda v, dt: v - 0.5, control=mine)) + d.control(_FakeEgo(), 0.05) + assert got == [(d, 6.5)] -def test_a_non_callable_usercontrol_is_refused_at_the_call(): + +def test_instance_outside_a_run_is_refused(): + from CommonLib.fixs import _driver as mod + mod._instance = None + with pytest.raises(fixs.FixsError, match='inside your'): + fixs.driver.instance() + + +@pytest.mark.parametrize('scenario', [False], indirect=True) +def test_shutdown_ends_the_instance(scenario): + d = _build(driver()) + assert fixs.driver.instance() is d + d.shutdown() + with pytest.raises(fixs.FixsError): + fixs.driver.instance() + + +def test_a_non_callable_control_is_refused_at_the_call(): with pytest.raises(TypeError): - driver(usercontrol=42) + driver(control=42) + + +# -- the old names, kept for one release (_driver._oldNames) ------------------ + +def _ex(v, dt): + return v + + +def _mine(ego, dt): + pass + + +@pytest.mark.parametrize('call', [lambda: fixs.driver(_ex), lambda: fixs.driver(exchange=_ex)]) +def test_the_old_exchange_still_means_xil_and_says_so(call): + with pytest.warns(FutureWarning, match='xil=exchange') as caught: + cls = call() + assert cls._EXCHANGE is _ex + assert caught[0].filename == __file__, 'the warning should name the caller' + + +def test_the_old_usercontrol_still_means_control_and_says_so(): + with pytest.warns(FutureWarning, match='control=fn'): + cls = fixs.driver(usercontrol=_mine) + assert cls._CONTROL is _mine + + +@pytest.mark.parametrize('kw', [dict(exchange=_ex, xil=_ex), + dict(usercontrol=_mine, control=_mine)]) +def test_an_old_and_a_new_name_together_is_refused(kw): + with pytest.raises(TypeError, match='once'): + fixs.driver(**kw) def test_the_template_makes_exactly_one_driver(): @@ -460,7 +515,7 @@ def passive(scenario_off): def test_the_cell_drives_a_traffic_owned_ego(passive): """No agent is built and no simulator is touched: the cell's answer is written straight onto the record for the traffic simulator to integrate.""" - d = _build(driver(lambda v, dt: v * 0.5)) + d = _build(driver(xil=lambda v, dt: v * 0.5)) assert d.passive is True ego = _passiveEgo(speedDesired=9.0) @@ -475,7 +530,7 @@ def test_the_cell_drives_a_traffic_owned_ego(passive): def test_passive_leaves_the_lateral_alone(passive): """Steering belongs to whoever owns the lateral, and on this rung that is the traffic simulator. Writing one would be a command nothing asked for.""" - d = _build(driver(lambda v, dt: v)) + d = _build(driver(xil=lambda v, dt: v)) ego = _passiveEgo() d.control(ego, 0.1) assert 'steerAngleDesired' not in ego._written @@ -486,7 +541,7 @@ def test_a_null_cell_reproduces_the_rung_below(passive): the advisory exactly as the eco controller wrote it -- because that, with no cell at all, IS the rung below. If this drifts, a dyno comparison is not measuring the dyno.""" - d = _build(driver(lambda v, dt: v)) + d = _build(driver(xil=lambda v, dt: v)) for advisory in (0.25, 4.0, 9.75, 13.4): ego = _passiveEgo(speedDesired=advisory) d.control(ego, 0.1) @@ -506,7 +561,7 @@ def test_an_almost_massless_cell_tracks_the_advisory(passive): """A bench with almost no dynamics must be almost invisible. This is the cheap check that a dyno run can be trusted: make the cell nearly ideal and the two rungs should sit on top of each other.""" - d = _build(driver(lambda v, dt: v * 0.999)) + d = _build(driver(xil=lambda v, dt: v * 0.999)) ego = _passiveEgo(speedDesired=12.0) d.control(ego, 0.1) assert ego.speedDesired == pytest.approx(12.0, rel=2e-3) @@ -521,7 +576,7 @@ def test_the_stop_bar_is_not_planned_twice(passive): 200 m out, and the ego crawled toward a bar it was never going to overrun. Safety is not lost by leaving it out -- SumoSetup.SpeedMode keeps the traffic simulator's own red-light and safe-speed checks on.""" - d = _build(driver(lambda v, dt: v)) + d = _build(driver(xil=lambda v, dt: v)) ego = _passiveEgo(speedDesired=11.0, signalLightColor=1, # _RED signalLightDistance=150.0, speed=11.0) d.control(ego, 0.1) @@ -531,7 +586,7 @@ def test_the_stop_bar_is_not_planned_twice(passive): def test_a_leader_is_not_planned_twice(passive): """Same, for the vehicle ahead: the traffic simulator's car-following owns the gap on this rung, and it still runs under SpeedMode.""" - d = _build(driver(lambda v, dt: v)) + d = _build(driver(xil=lambda v, dt: v)) ego = _passiveEgo(speedDesired=11.0, hasPrecedingVehicle=1, precedingVehicleDistance=6.0, precedingVehicleSpeed=0.0) d.control(ego, 0.1) @@ -543,7 +598,7 @@ def test_the_command_shape_is_inert_when_nothing_integrates_pedals(passive): plant here, so both shapes arrive at the same place and a scenario carries the same Controller line as its virenv sibling.""" for shape in ('speed', 'pedals'): - d = _build(driver(lambda v, dt: v * 0.5, shape=shape)) + d = _build(driver(xil=lambda v, dt: v * 0.5, shape=shape)) ego = _passiveEgo(speedDesired=8.0) d.control(ego, 0.1) assert ego.speedDesired == pytest.approx(4.0), shape @@ -559,7 +614,7 @@ def test_a_zero_advisory_is_a_command_not_an_absence(passive): just been asked to stop at. The cell must see the zero, and the zero must reach the traffic simulator.""" seen = [] - d = _build(driver(lambda v, dt: (seen.append(v), v)[1])) + d = _build(driver(xil=lambda v, dt: (seen.append(v), v)[1])) ego = _passiveEgo(speedDesired=0.0, speed=0.0) d.control(ego, 0.1) assert seen == [pytest.approx(0.0)], 'the cell must be asked for the stop' @@ -582,12 +637,180 @@ def test_start_in_gear_is_opt_in_and_set_before_building(scenario, monkeypatch): """CARLA's autobox leaves a spawned car in neutral below ~15% throttle.""" import CommonLib.fixs._driver as drv monkeypatch.setattr(drv, '_DEFAULTS', {}) - assert _build(driver(lambda v, dt: v)).startInGear is False + assert _build(driver(xil=lambda v, dt: v)).startInGear is False fixs.driver.set(startInGear=True) - assert _build(driver(lambda v, dt: v)).startInGear is True - assert _build(driver(lambda v, dt: v, startInGear=False)).startInGear is False + assert _build(driver(xil=lambda v, dt: v)).startInGear is True + assert _build(driver(xil=lambda v, dt: v, startInGear=False)).startInGear is False + + +class _FakeCarla: + """The fixs.carla calls the agent steps make, recorded.""" + def __init__(self): + self.calls = [] + self.ego = self + + def apply_control(self, control): + self.calls.append(('apply_control', control)) + + def VehicleControl(self, **kw): # noqa: N802 + return kw + + def refresh(self, ego): + self.calls.append(('refresh', ego)) + + +@pytest.mark.parametrize('scenario', [False], indirect=True) +def test_first_gear_takes_the_first_step_only(scenario, monkeypatch): + import CommonLib.fixs._driver as drv + d = _build(driver(startInGear=True)) + off = _build(driver()) + fake = _FakeCarla() + monkeypatch.setattr(drv, 'carla', fake) + assert d.shiftIntoFirstGear() is True + assert d.shiftIntoFirstGear() is False + assert off.shiftIntoFirstGear() is False + assert fake.calls == [('apply_control', {'manual_gear_shift': True, 'gear': 1})] + + +@pytest.mark.parametrize('scenario', [False], indirect=True) +def test_step_agent_aims_the_agent_then_plans(scenario, monkeypatch): + """The speed goes to the agent in km/h; an emergency stop also advances the planner.""" + import types + import CommonLib.fixs._driver as drv + planner = types.SimpleNamespace(steps=0) + planner.run_step = lambda: setattr(planner, 'steps', planner.steps + 1) + stop = types.SimpleNamespace(throttle=0.0, brake=1.0, steer=0.0) + agent = types.SimpleNamespace(_behavior=types.SimpleNamespace(), _max_brake=1.0, + run_step=lambda: stop, get_local_planner=lambda: planner) + d = _build(driver()) + d.agent = agent + fake = _FakeCarla() + monkeypatch.setattr(drv, 'carla', fake) + assert d.stepAgent('ego', speed=10.0, minLookaheadDist=12.5) is stop + assert agent._behavior.max_speed == pytest.approx(36.0) + assert agent._behavior.min_proximity_threshold == 12.5 + assert fake.calls == [('refresh', 'ego')] + assert planner.steps == 1 def test_driver_set_refuses_an_unknown_option(): with pytest.raises(TypeError): fixs.driver.set(startInGera=True) + + +# -- referenceMode: what the driver reads from the eco controller ------------ + +def test_the_acceleration_rebuilds_the_exact_float32_speed(): + """Exact at a stop and above 0.25 m/s; in between, within float32 of the acceleration.""" + import random + rng = random.Random(0) + for _ in range(50000): + v = fixs._f32(rng.uniform(0.0, 30.0)) + target = fixs._f32(max(0.0, v + rng.uniform(-3.0, 2.0) * 0.1)) + a = fixs.accelerationFromSpeed(v, target, 0.1) + assert a == fixs._f32(a) + got = fixs.speedFromAcceleration(v, a, 0.1) + if target == 0.0 or target > 0.25: + assert got == target, (v, target) + else: + assert abs(got - target) < 2e-8, (v, target) + + +def test_the_reference_mode_comes_from_the_scenario(): + assert _options({}).reference_mode == 'speed' + assert _options({'EgoControllerArgs': ['--reference-mode', 'accel']}).reference_mode == 'accel' + assert _options({}, {'referenceMode': 'both'}).reference_mode == 'both' + + +@pytest.mark.parametrize('mode', ['speed', 'accel', 'both']) +def test_every_reference_mode_drives_the_same_speed(passive, mode): + v, target = fixs._f32(12.3), fixs._f32(12.41) + d = _build(driver(xil=lambda vref, dt: vref, referenceMode=mode)) + ego = _passiveEgo(speed=v, feedSpeed=v, speedDesired=target, + accelerationDesired=fixs.accelerationFromSpeed(v, target, 0.1)) + d.control(ego, 0.1) + assert ego.speedDesired == target + + +def test_both_takes_the_pair_as_sent_even_when_they_disagree(passive): + got = [] + + def exchange(vref, aref, dt): + got.append((vref, aref)) + return vref + + d = _build(driver(xil=exchange, referenceMode='both')) + ego = _passiveEgo(speed=10.0, feedSpeed=10.0, speedDesired=8.5, accelerationDesired=-1.5) + d.control(ego, 0.1) + assert got[0] == (pytest.approx(8.5), pytest.approx(-1.5)) + assert ego.speedDesired == pytest.approx(8.5) + + +def test_accel_mode_refuses_a_wire_without_the_field(passive): + fixs._declaredFields = frozenset({'id', 'speed', 'speedDesired'}) + d = _build(driver(xil=lambda vref, dt: vref, referenceMode='accel')) + with pytest.raises(fixs.FixsError, match='accelerationDesired'): + d.control(_passiveEgo(feedSpeed=4.0), 0.1) + + +def test_a_three_argument_exchange_gets_the_acceleration(passive): + got = [] + + def exchange(vref, aref, dt): + got.append((vref, aref)) + return vref + + d = _build(driver(xil=exchange, referenceMode='accel')) + d.control(_passiveEgo(speed=10.0, feedSpeed=10.0, speedDesired=0.0, + accelerationDesired=1.5), 0.1) + assert got[0][0] == pytest.approx(10.15, abs=1e-5) + assert got[0][1] == pytest.approx(1.5) + + +def test_the_acceleration_follows_a_speed_the_driver_changed(): + d = _build(driver(xil=lambda vref, aref, dt: vref)) + d.feedVref, d.feedAref, d.feedStep = 10.0, 1.0, 0.1 + assert d._arefFor(10.0) == pytest.approx(1.0) + assert d._arefFor(9.8) == pytest.approx(-1.0) + + +def _agentAt(targetMps): + from types import SimpleNamespace + planner = SimpleNamespace(_target_speed=targetMps * 3.6) + return SimpleNamespace(get_local_planner=lambda: planner, _max_brake=0.5) + + +def test_reference_returns_the_agents_speed_and_the_acceleration_that_goes_with_it(): + from types import SimpleNamespace + d = _build(driver(xil=lambda vref, aref, dt: vref)) + d.feedVref, d.feedAref, d.feedStep = 10.0, 1.0, 0.1 + d.agent = _agentAt(9.8) + vRef, aRef = d.reference(SimpleNamespace(throttle=0.3, brake=0.0)) + assert vRef == pytest.approx(9.8) + assert aRef == pytest.approx(-1.0) + vRef, aRef = d.reference(SimpleNamespace(throttle=0.0, brake=0.5)) # emergency stop + assert vRef == 0.0 + assert aRef == pytest.approx(1.0 - 10.0 / 0.1) + + +def test_exchange_hands_the_dyno_the_acceleration_it_is_given(): + got = [] + + def exchange(vref, aref, dt): + got.append((vref, aref)) + return vref + + d = _build(driver(xil=exchange)) + d.feedVref, d.feedAref, d.feedStep = 10.0, 1.0, 0.1 + d.exchange(9.8, 0.25) + d.exchange(9.8) # without one: the acceleration that goes with 9.8 + assert got == [(pytest.approx(9.8), pytest.approx(0.25)), (pytest.approx(9.8), pytest.approx(-1.0))] + + +def test_the_exchange_says_what_it_takes_by_its_arguments(): + from CommonLib.fixs import xil + assert driver(xil=lambda vref, dt: vref)._EXCHANGE_TAKES_AREF is False + assert driver(xil=lambda vref, aref, dt: vref)._EXCHANGE_TAKES_AREF is True + assert driver(xil=xil.dynosim().exchange)._EXCHANGE_TAKES_AREF is True + with pytest.raises(TypeError): + driver(xil=lambda vref: vref) diff --git a/tests/Python/unit/test_xil_dyno.py b/tests/Python/unit/test_xil_dyno.py index 6bdd568c..0b924f00 100644 --- a/tests/Python/unit/test_xil_dyno.py +++ b/tests/Python/unit/test_xil_dyno.py @@ -294,6 +294,98 @@ def test_separate_throttle_and_brake_ceilings(): assert thr == pytest.approx(0.25) +def test_a_delay_applies_the_command_of_that_long_ago(): + plain, late = RobotDriver(), RobotDriver(delay_s=0.1) + sent = [plain.step(20.0, 0.0, 0.05) for _ in range(6)] + got = [late.step(20.0, 0.0, 0.05) for _ in range(6)] + assert got[:2] == [(0.0, 0.0)] * 2 + assert got[2:] == sent[:4] + assert late.pedal == plain.pedal + + +def test_reset_empties_the_delay_line(): + driver = RobotDriver(delay_s=0.1) + for _ in range(5): + driver.step(20.0, 0.0, 0.05) + driver.reset() + assert driver.step(20.0, 0.0, 0.05) == (0.0, 0.0) + + +def test_the_actuator_gets_the_delayed_command_and_has_the_last_word(): + seen = [] + + def half(thr, brk, dt): + seen.append((thr, brk, dt)) + return 0.5 * thr, brk + + plain = RobotDriver() + late = RobotDriver(delay_s=0.05, actuator=half) + sent = [plain.step(20.0, 0.0, 0.05) for _ in range(3)] + got = [late.step(20.0, 0.0, 0.05) for _ in range(3)] + assert seen == [(0.0, 0.0, 0.05)] + [c + (0.05,) for c in sent[:2]] + assert got == [(0.0, 0.0)] + [(0.5 * t, b) for t, b in sent[:2]] + + +def test_reset_resets_the_actuator_if_it_can(): + class Lag(object): + resets = 0 + + def __call__(self, thr, brk, dt): + return thr, brk + + def reset(self): + self.resets += 1 + + lag = Lag() + DynoSim(driver=RobotDriver(actuator=lag)).reset() + assert lag.resets == 1 + + +def test_a_delayed_driver_still_settles_on_the_reference(): + sim = DynoSim(driver=RobotDriver(delay_s=0.2)) + for _ in range(int(60.0 / 0.05)): + state = sim.step(15.0, 0.05) + assert state.speed == pytest.approx(15.0, abs=0.05) + + +def test_an_unknown_mode_is_rejected(): + with pytest.raises(ValueError): + RobotDriver(mode='torque') + + +def test_accel_mode_needs_an_acceleration_command(): + with pytest.raises(ValueError): + RobotDriver(mode='accel').step(5.0, 0.0, 0.05) + + +def test_accel_mode_tracks_a_constant_acceleration(): + sim = DynoSim(driver=RobotDriver(mode='accel')) + speeds = [sim.step(0.0, 0.05, a_ref=1.0).speed for _ in range(160)] + assert speeds[-1] == pytest.approx(8.0, abs=0.3) + assert (speeds[-1] - speeds[-81]) / 4.0 == pytest.approx(1.0, abs=0.02) + + +def test_accel_mode_ignores_the_speed_reference(): + a, b = RobotDriver(mode='accel'), RobotDriver(mode='accel') + for k in range(20): + v = 0.1 * k + assert a.step(0.0, v, 0.05, a_ref=0.5) == b.step(30.0, v, 0.05, a_ref=0.5) + + +def test_accel_mode_caps_the_command_at_the_envelope(): + sim = DynoSim(driver=RobotDriver(mode='accel', max_accel_mps2=1.8)) + speeds = [sim.step(0.0, 0.05, a_ref=5.0).speed for _ in range(160)] + assert (speeds[-1] - speeds[-81]) / 4.0 == pytest.approx(1.8, abs=0.05) + + +def test_accel_mode_holds_the_brake_at_rest_when_not_asked_to_move(): + driver = RobotDriver(mode='accel') + for _ in range(20): + thr, brk = driver.step(0.0, 0.0, 0.05, a_ref=0.0) + assert thr == 0.0 and brk == pytest.approx(0.3) + assert driver.integral == 0.0 + + def test_the_bench_can_fail_to_reach_the_reference(): """The gap between asked and achieved is the point of having a plant.""" sim = DynoSim() diff --git a/tests/VirEnv/test_ego_controller.py b/tests/VirEnv/test_ego_controller.py index b9779a6a..0bdf124d 100644 --- a/tests/VirEnv/test_ego_controller.py +++ b/tests/VirEnv/test_ego_controller.py @@ -115,11 +115,11 @@ def test_steer_alone_rejected(): fixs._validateCommand(ego) -def test_both_longitudinal_rejected(): +def test_both_longitudinal_accepted(): ego = makeEgo() write(ego, speedDesired=8.3, accelerationDesired=1.0) - with pytest.raises(fixs.ProtocolError, match='not both'): - fixs._validateCommand(ego) + assert fixs.commandKind(ego) == 'speedsteer' + fixs._validateCommand(ego) def test_nothing_written_is_not_an_error(): @@ -308,6 +308,19 @@ def test_feedage_is_zero_on_a_feed_and_grows_between(tmp_path): assert sys.modules['ages'].seenAges == pytest.approx([0.0, 0.05, 0.10]) +def test_feedspeed_holds_the_feeds_speed_while_speed_goes_live(tmp_path): + body = ("seen = []\n" + "def control(ego, dt):\n" + " seen.append((ego.feedSpeed, ego.speed))\n" + " ego.set(speedDesired=5.0, steerAngleDesired=0.0)\n") + backend = _mockWithEgo(speed=7.25) + ctl = loadController(controllerFile(tmp_path, body, name='feedspeed.py')) + ego = makeEgo(speed=6.0) + runController(backend, ctl, ego, 0.05, True, 0.7) # the feed + runController(backend, ctl, ego, 0.05, False, 0.7) # sub-step + assert sys.modules['feedspeed'].seen == [(6.0, 7.25), (6.0, 7.25)] + + def test_written_is_cleared_between_steps(tmp_path): """Without this _written only grows, so a controller that commanded once would look like it was still commanding ten steps later."""