diff --git a/Carla/templates/driver_template.py b/Carla/templates/driver_template.py index aa624ef4..06616ed8 100644 --- a/Carla/templates/driver_template.py +++ b/Carla/templates/driver_template.py @@ -36,10 +36,10 @@ # --------------------------------------------------------------------------- # # import fixs.xil # -# dyno = fixs.xil.dyno(vehicle={'mass_kg': 2100.0}) # or your own rig +# dyno = fixs.xil.dynosim(vehicle={'mass_kg': 2100.0}) # or your own rig # # def exchange(vref, dt): -# return dyno.exchange(vref, dt) # a speed in, reached out +# return dyno.exchange(vref, 0.0, dt) # speed and accel in, reached out # --------------------------------------------------------------------------- # @@ -51,7 +51,7 @@ # 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, dt)`. +# 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. diff --git a/CommonLib/ConfigHelper.py b/CommonLib/ConfigHelper.py index fcdf8a97..7d28f6ae 100644 --- a/CommonLib/ConfigHelper.py +++ b/CommonLib/ConfigHelper.py @@ -89,26 +89,13 @@ def getConfig(self, configName): xil_node = config.get("XilSetup", {}) self.Xil_setup["EnableXil"] = self.parserFlag(xil_node, "EnableXil", False) self.Xil_setup["VehicleSubscription"] = self.parseVehicleSubscription(xil_node, "VehicleSubscription", []) - # How the XIL plant is reached. 'inprocess' simulates it here (no - # hardware, and the same code path as a cell), 'udp' and 'tcp' put it on - # the wire at the VehicleSubscription's ip and port. + # 'inprocess': FIXS's simulated dyno. 'udp': the application talks to its + # own rig at the VehicleSubscription's ip. 'tcp': reserved, not built yet. self.Xil_setup["Transport"] = self.parserString(xil_node, "Transport", "inprocess").strip().lower() if self.Xil_setup["Transport"] not in ("inprocess", "udp", "tcp"): raise SystemExit( "ERROR: XilSetup.Transport must be one of inprocess|udp|tcp, got '%s'" % self.Xil_setup["Transport"]) - # The bench itself: what vehicle is on it, and what the dyno does. Both - # are passed to CommonLib.xil by name, so the yaml names the parameter - # rather than restating a list that would then have to be kept in step. - # An unknown one is refused there, not ignored. - self.Xil_setup["Vehicle"] = dict(xil_node.get("Vehicle") or {}) - self.Xil_setup["Dyno"] = dict(xil_node.get("Dyno") or {}) - # The ROBOT DRIVER on the bench. Its own block because it is not the - # vehicle and not the dyno: on a real cell it is the one of the three - # that is yours to set. Capping its pedal is how a bench is held to an - # acceleration envelope without pretending the vehicle has less torque - # than it has (#24). - self.Xil_setup["Driver"] = dict(xil_node.get("Driver") or {}) # Carla Setup carla_node = config.get("CarlaSetup", {}) diff --git a/CommonLib/fixs/__init__.py b/CommonLib/fixs/__init__.py index b192ba97..a6274759 100644 --- a/CommonLib/fixs/__init__.py +++ b/CommonLib/fixs/__init__.py @@ -113,6 +113,15 @@ def driver(exchange=None, **options): return _driver(exchange, **options) +def _driverSet(**options): + """fixs.driver.set(startInGear=True): options for every driver built after this call.""" + from CommonLib.fixs._driver import setDefaults + setDefaults(**options) + + +driver.set = _driverSet + + #: Full-lock front road-wheel angle [rad]. `steerAngleDesired` is an ANGLE on #: the wire, where a CARLA agent's VehicleControl.steer is normalised [-1, 1]; #: the plant divides by this same constant, so multiplying by it here makes the @@ -1079,7 +1088,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'): + if name in ('sumo', 'carla', 'config'): 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 ce9d8688..ec6d67a5 100644 --- a/CommonLib/fixs/_driver.py +++ b/CommonLib/fixs/_driver.py @@ -21,9 +21,10 @@ def exchange(vref, dt): 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, pass the one FIXS already has:: +the declaration. For the simulated dyno, wrap the one FIXS already has:: - Controller = fixs.driver(fixs.xil.exchange) + dyno = fixs.xil.dynosim() + Controller = fixs.driver(lambda vref, dt: dyno.exchange(vref, 0.0, dt)) which is a function like any other and holds no privilege over yours. @@ -276,17 +277,32 @@ def __repr__(self): +#: The options fixs.driver(**options) and fixs.driver.set(**options) take. +_OPTION_NAMES = frozenset({'shape', 'loop', 'tuning', 'limits', 'idealSpeedTracking', + 'startInGear'}) +#: Set by fixs.driver.set(); every driver built afterwards starts from these. +_DEFAULTS = {} + + +def setDefaults(**defaults): + """fixs.driver.set(name=value): options for every driver built after this call.""" + unknown = set(defaults) - _OPTION_NAMES + if unknown: + raise TypeError('fixs.driver.set(): unknown option(s) %s -- known: %s' + % (', '.join(sorted(unknown)), ', '.join(sorted(_OPTION_NAMES)))) + _DEFAULTS.update(defaults) + + def _options(config, overrides=None): """Three layers, nearest the run wins: the module constants below are the defaults, fixs.driver(**options) overrides them, and what the SCENARIO wrote after the controller's path overrides both -- because the scenario is the thing an operator edits without touching code.""" over = dict(overrides or {}) - known = {'shape', 'loop', 'tuning', 'limits', 'idealSpeedTracking'} - unknown = set(over) - known + unknown = set(over) - _OPTION_NAMES if unknown: raise TypeError('fixs.driver(): unknown option(s) %s -- known: %s' - % (', '.join(sorted(unknown)), ', '.join(sorted(known)))) + % (', '.join(sorted(unknown)), ', '.join(sorted(_OPTION_NAMES)))) p = argparse.ArgumentParser(prog='fixs.driver', add_help=False) p.add_argument('--command-shape', choices=('speed', 'pedals'), @@ -312,6 +328,7 @@ def _options(config, overrides=None): % type(opt.limits).__name__) opt.idealSpeedTracking = bool( over.get('idealSpeedTracking', IDEAL_SPEED_TRACKING)) + opt.startInGear = bool(over.get('startInGear', False)) return opt @@ -333,6 +350,10 @@ def __init__(self, config, egoId): #: How close it comes to a bar or a leader. Policy, not tuning. self.limits = opt.limits self.idealSpeedTracking = opt.idealSpeedTracking + #: Put the ego in first gear on the first step: CARLA's autobox leaves a + #: spawned car in neutral below ~15% throttle, so a creeping pedal never moves it. + self.startInGear = opt.startInGear + self._inGear = False self.dt = float(config.get('CarlaTimeStep') or 0.1) self.fallbackSpeed = float(config.get('EgoTargetSpeed') or 8.33) self.useAdvisory = USE_ADVISORY @@ -358,8 +379,8 @@ def __init__(self, config, egoId): #: 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 #: caller's, so passing a function IS the declaration and there is no - #: flag here to disagree with it. fixs.xil.exchange is one such - #: function, for the simulated cell; a rig's own is another. + #: flag here to disagree with it. A wrapper around fixs.xil.dynosim() is + #: one such function, for the simulated cell; a rig's own is another. self._exchange = self._EXCHANGE self.benchInLoop = self._exchange is not None #: Ticks the cell did not answer usefully. Counted here, not asked of @@ -369,31 +390,9 @@ def __init__(self, config, egoId): #: What the bench last achieved. None until it has answered once. self.vDyno = None - # EVERYTHING THAT DOES NOT NEED THE EGO IS BUILT HERE, not in _build. - # - # _build runs on the first controlled tick. With a warm-up that tick is - # also the one where the whole network arrives in CARLA at once, so it - # is the single worst tick in the run to add work to -- and a bench in - # the loop feels it as a freeze the instant it starts being commanded. - # Measured inside that tick on MLK eco-driving: BehaviorAgent(...) - # 0.734 s, of which ~0.58 s was its own world.get_map(), plus 0.157 s - # for the route snap. FIXS#373. - # - # This constructor runs at bridge start-up, right before the bridge - # blocks in recv for the whole warm-up, so the same work costs nothing. - # The agent itself still cannot be built here: it is constructed on - # carla.ego, and the traffic simulator has not inserted the ego yet. - # Only when a bridge is actually behind this module. The law below is - # exercised on its own -- tests construct the driver with no backend - # registered -- and carla.map REFUSES rather than inventing a world, so - # asking unconditionally would make the driver unusable without CARLA. - # None is what BasicAgent already treats as "make your own", so the - # no-bridge path is exactly the old behaviour. - #: ... and not on a passive rung, where there is no agent to hand them - #: to. _build never runs there (control() returns before it), so the map - #: and the planner would be built, paid for, and never read. FIXS#373 - #: moved this work off the first controlled tick because it cost 0.73 s - #: there; on this rung it costs that at start-up for nothing at all. + # Map and route planner are built at start-up, not on the first + # controlled tick (FIXS#373). Skipped without a bridge (unit tests) and + # on a passive rung, which has no agent to use them. self._map, self._grp = None, None if not self.passive and carla.available(): self._map = carla.map # FIXS caches it; the agent reuses it @@ -501,6 +500,13 @@ def control(self, ego, dt): 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 + return + if self.agent is None: self._build() @@ -841,8 +847,8 @@ def driver(exchange=None, usercontrol=None, **options): 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, pass - ``fixs.xil.exchange``; for yours, pass yours:: + Omit it and nothing is in the loop. For the simulated cell, wrap + ``fixs.xil.dynosim().exchange(vref, aref, dt)``; for yours, pass yours:: Controller = fixs.driver(exchange) @@ -856,8 +862,9 @@ def driver(exchange=None, usercontrol=None, **options): scenario still says which FILE -- FIXS has nothing to import otherwise -- but nothing inside it has to be spelled a particular way. - ``options`` override the module defaults, and the scenario's own - ``--command-shape`` still wins over both, being nearer the run. + ``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)): if fn is not None and not callable(fn): @@ -874,7 +881,7 @@ def driver(exchange=None, usercontrol=None, **options): dict(_EXCHANGE=staticmethod(exchange) if exchange else None, _USERCONTROL=staticmethod(usercontrol) if usercontrol else None, - _OPTIONS=dict(options))) + _OPTIONS=dict(_DEFAULTS, **options))) _BUILT.append(cls) return cls diff --git a/CommonLib/fixs/config.py b/CommonLib/fixs/config.py new file mode 100644 index 00000000..b3173687 --- /dev/null +++ b/CommonLib/fixs/config.py @@ -0,0 +1,100 @@ +"""fixs.config -- the scenario yaml this run is using, read-only. + + xil = fixs.config.get('xil') + xil['transport'] # 'inprocess' | 'udp' | 'tcp' + xil['ip'] # the section's address, see get() + +Names are the yaml's, lowercased: a section drops its ``Setup`` suffix and +every name goes to snake_case, so ``XilSetup.EnableXil`` is +``get('xil')['enable_xil']`` and ``CarlaSetup.CarlaTimeStep`` is +``get('carla')['carla_time_step']``. +""" + +import copy +import os +import re +import types + +from CommonLib.ConfigHelper import ConfigHelper + +from . import FixsError + +__all__ = ['get'] + +#: yaml section -> the ConfigHelper attribute holding its parsed values. +_PARSED = { + 'SimulationSetup': 'simulation_setup', + 'SumoSetup': 'Sumo_setup', + 'ApplicationSetup': 'application_setup', + 'XilSetup': 'Xil_setup', + 'EgoSetup': 'Ego_setup', + 'CarlaSetup': 'Carla_setup', + 'CarMakerSetup': 'CarMaker_setup', + 'DataLogSetup': 'DataLog_setup', +} + +_cache = {} + + +def _snake(name): + """CarlaTimeStep -> carla_time_step, TrafficSimulatorIP -> traffic_simulator_ip.""" + return re.sub(r'(?<=[a-z0-9])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])', '_', name).lower() + + +def _section(name): + """XilSetup -> xil, DataLogSetup -> data_log.""" + return _snake(name[:-len('Setup')] if name.endswith('Setup') else name) + + +def _configPath(configPath): + from . import _connectedConfigPath # set by fixs.connect(), read at call time + path = (configPath or _connectedConfigPath + or os.environ.get('FIXS_CONFIG_YAML')) + if not path or not os.path.isfile(path): + raise FixsError( + 'cannot read the scenario: ' + + ('$FIXS_CONFIG_YAML is not set' if not path else '%s does not exist' % path) + + '. Pass the scenario yaml, or set $FIXS_CONFIG_YAML to the one this run is using.') + return path + + +def _load(path): + config = ConfigHelper() + config.getConfig(path) + sections = {} + for yamlName in list(config.raw) + [n for n in _PARSED if n not in config.raw]: + written = config.raw.get(yamlName) or {} + if not isinstance(written, dict): + continue + # What the author wrote, then what ConfigHelper parsed over it (defaults, validation). + merged = dict(written) + if yamlName in _PARSED: + merged.update(getattr(config, _PARSED[yamlName])) + values = {_snake(k): v for k, v in merged.items()} + subs = values.get('vehicle_subscription') + if subs: + # The same rule TrafficLayer uses for a section's address. + values.setdefault('ip', (subs[0].get('ip') or [None])[0]) + values.setdefault('port', (subs[0].get('port') or [None])[0]) + sections[_section(yamlName)] = values + return sections + + +def get(section, configPath=None): + """(str) -> read-only dict -- one section of this run's scenario yaml. + + Values are parsed: defaults applied and validated the way FIXS reads them. + A section with a ``vehicle_subscription`` also answers ``ip`` and ``port``, + the first subscription's first ip and port -- the address TrafficLayer + takes for that section. + + Changing the result changes nothing; each call hands back its own copy. + """ + path = _configPath(configPath) + if path not in _cache: + _cache[path] = _load(path) + sections = _cache[path] + if section not in sections: + raise FixsError('%s has no %r section. It has: %s' + % (path, section, ', '.join(sorted(sections)) or 'none')) + return types.MappingProxyType(copy.deepcopy(sections[section])) diff --git a/CommonLib/fixs/xil.py b/CommonLib/fixs/xil.py index a748ba83..7d30556e 100644 --- a/CommonLib/fixs/xil.py +++ b/CommonLib/fixs/xil.py @@ -1,167 +1,66 @@ -"""fixs.xil -- the dynamometer this scenario declares, if it declares one. +"""fixs.xil -- the simulated dynamometer FIXS ships. -A submodule, like fixs.carla, because a plant is not part of the wire API and a -controller that is not driving one has no reason to load it. + import fixs.xil - from fixs import xil + if fixs.xil.enabled(): + dyno = fixs.xil.dynosim(vehicle={'mass_kg': 2100.0}) + vRef = dyno.exchange(vRef, aRef, dt) - self.dyno = xil.dyno() # None unless XilSetup.EnableXil - ... - if self.dyno is not None: - vRef = self.dyno.exchange(vRef, dt) - -What this saves a controller is reading XilSetup itself -- which yaml, which -transport, which endpoint, and the bench's own parameters. Everything it builds -is CommonLib.xil, which a controller with its own ideas can build directly. +FIXS ships the simulated dyno and nothing else. Which dyno answers a run is +the application's choice: ``Transport: udp`` means it talks to a rig of its +own, at ``fixs.config.get('xil')['ip']`` and ``['port']``. """ -import os - -from CommonLib.ConfigHelper import ConfigHelper - -from . import FixsError - -__all__ = ['enabled', 'dyno'] +from . import config - -def _scenario(configPath): - """The scenario this run is using, read the same way fixs.connect() reads it.""" - configPath = configPath or os.environ.get('FIXS_CONFIG_YAML') - if not configPath or not os.path.exists(configPath): - raise FixsError( - 'cannot tell whether this scenario declares a dynamometer: ' - + ('$FIXS_CONFIG_YAML is not set' if not configPath - else '%s does not exist' % configPath) - + '. Pass the scenario yaml, or set $FIXS_CONFIG_YAML to the one ' - 'this run is using.') - config = ConfigHelper() - config.getConfig(configPath) - return config.Xil_setup +__all__ = ['enabled', 'dynosim'] def enabled(configPath=None): """Is a dynamometer in this run's loop? -> bool - Ask this rather than testing whether :func:`dyno` returned something. The - two are different questions: this one is about the SCENARIO, and it stays - true for a controller that brings its own cell client instead of the one - below. A rig with its own dyno software still answers the same flag, so the - control flow that depends on a bench being present does not have to care - who talks to it. + True for every transport, including a rig the application talks to itself. """ - return bool(_scenario(configPath)['EnableXil']) - - -def dyno(configPath=None, vehicle=None, dyno=None, driver=None): - """The dynamometer this scenario declares, or None when it declares none. - - A bench is not a plant that owns the ego. The virtual environment still - integrates position, heading and everything lateral; the bench answers one - question, and it is the longitudinal one: *you asked for this speed -- here - is what a real vehicle did with it*. A controller sends its command through - the bench and commands the simulator with what came back, so the vehicle's - mass and the time its torque takes to arrive are in the loop rather than - beside it. - - Reads ``XilSetup`` from the yaml TrafficLayer is running -- ``EnableXil`` - switches it on, ``Transport`` picks inprocess/udp/tcp, and the endpoint is - the ``VehicleSubscription``'s ip and port. Same source as :func:`connect`, - for the same reason: two yamls that disagree are two different scenarios. - - self.dyno = fixs.dyno() # None unless EnableXil - ... - if self.dyno is not None: - vRef = self.dyno.exchange(vRef, dt) - - ``inprocess`` runs CommonLib.xil here in this process, so the coupling can - be exercised with no hardware and over the same call the cell takes. What - vehicle is on the bench, and what the dyno does, come from the yaml too: - - XilSetup: - Vehicle: {mass_kg: 2100.0, torque_bandwidth_Hz: 5.0} - Dyno: {road_A_N: 111.0, roller_inertia_kgm2: 40.0} - Driver: {max_throttle: 0.28, max_brake: 0.30} - - ``Driver`` is the ROBOT on the bench, and the one of the three a real cell - lets you set: the vehicle's torque is the vehicle's, but how hard the robot - presses is yours. Capping its pedal is how a bench is held inside an - acceleration envelope without pretending the car has less torque than it - has -- which matters when the traffic simulator moving the ego has an - envelope of its own and the two must describe the same vehicle. - - Forwarded by name, so the yaml names the parameter rather than restating - a list -- and an unknown one fails the run rather than leaving a bench - silently on its defaults. + return bool(config.get('xil', configPath)['enable_xil']) + + +def dynosim(vehicle=None, dyno=None, driver=None): + """The simulated dyno, built from the vehicle, dyno and robot-driver + parameters given. Reads no scenario: whether one is in the loop is the + caller's question (:func:`enabled`, ``fixs.config.get('xil')``). + CommonLib.xil refuses a parameter it does not know, so a typo fails. """ - xil = _scenario(configPath) - if not xil['EnableXil']: - return None - - subs = xil['VehicleSubscription'] or [] - host = (subs[0].get('ip') or ['127.0.0.1'])[0] if subs else '127.0.0.1' - port = (subs[0].get('port') or [None])[0] if subs else None - # Stated in code wins over the yaml: a caller who writes the mass down - # is saying what is on the bench, and should not have to edit a scenario - # as well to be believed. - return _Dyno(xil['Transport'], host, port, - vehicle=dict(xil['Vehicle'], **(vehicle or {})), - dyno=dict(xil['Dyno'], **(dyno or {})), - driver=dict(xil.get('Driver') or {}, **(driver or {}))) + return _Dyno(vehicle=vehicle, dyno=dyno, driver=driver) class _Dyno: - """The bench, as the simulator sees it. Get one from :func:`dyno`. + """The simulated bench. ``exchange`` steps it once and returns the speed reached.""" - One call, because there is only one thing to ask. ``exchange`` is a round - trip: the reference goes out, the achieved speed comes back. - """ - - def __init__(self, transport, host='127.0.0.1', port=None, - vehicle=None, dyno=None, driver=None): - from CommonLib.xil.dynosim import Dyno, DynoSim + def __init__(self, vehicle=None, dyno=None, driver=None): from CommonLib.xil.driver import RobotDriver - from CommonLib.xil.link import LocalLink, TcpLink, UdpLink + from CommonLib.xil.dynosim import Dyno, DynoSim + from CommonLib.xil.link import LocalLink from CommonLib.xil.vehicle import Vehicle - self.transport = transport - self.sim = None + self.sim = DynoSim(Vehicle(**(vehicle or {})), Dyno(**(dyno or {})), + RobotDriver(**(driver or {}))) + self.link = LocalLink() self.misses = 0 - if transport == 'inprocess': - # Named parameters, straight through. CommonLib.xil refuses one it - # does not know rather than ignoring it, so a typo in the yaml is a - # failed run and not a bench quietly running on its defaults. - self.sim = DynoSim(Vehicle(**(vehicle or {})), Dyno(**(dyno or {})), - RobotDriver(**(driver or {}))) - self.link = LocalLink() - elif transport == 'tcp': - self.link = (TcpLink('simulator', peer_ip=host, port=port) - if port else TcpLink('simulator', peer_ip=host)) - elif transport == 'udp': - self.link = UdpLink('simulator', peer_ip=host) - else: - raise FixsError( - f'unknown XilSetup.Transport {transport!r}. ' - f'Use one of: inprocess, udp, tcp.') - - def exchange(self, speed, dt, steer=0.0): - """(float, float) -> float -- ask for a speed, get what the bench did. - - The bench's answer whenever there is one. When there is not -- the cell - has not answered yet, or has stopped -- the reference comes straight - back, which is the run behaving as though no bench were attached. That - is the only fallback that cannot invent motion; ``misses`` counts how - often it was taken, and a run that ends with a large one did not test - what it claims to have tested. + + def exchange(self, vref, aref, dt): + """Step the bench once by dt and return the speed it reached. + + vref is the speed command (m/s); aref the acceleration command (m/s^2), + read only by the robot driver's hold (``stop_speed_mps``/``stop_accel_mps2``). """ - self.link.send_reference(speed, steer) - if self.sim is not None: - reference = self.link.recv_reference() - self.sim.step(reference[0] if reference else 0.0, dt) - self.link.send_measurement(self.sim.speed) + self.link.send_reference(vref) + reference = self.link.recv_reference() + self.sim.step(reference[0] if reference else 0.0, dt, a_ref=aref) + self.link.send_measurement(self.sim.speed) got = self.link.recv_measurement() if got is None: self.misses += 1 - return speed + return vref return got[0] def age(self): diff --git a/CommonLib/xil/driver.py b/CommonLib/xil/driver.py index 5b2e5242..7a4df63d 100644 --- a/CommonLib/xil/driver.py +++ b/CommonLib/xil/driver.py @@ -26,7 +26,8 @@ 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): + max_accel_mps2=None, max_decel_mps2=None, + stop_speed_mps=0.0, stop_accel_mps2=0.0): self.kp = kp self.ki = ki self.max_throttle = max_throttle @@ -53,6 +54,10 @@ def __init__(self, kp=0.45, ki=0.25, max_throttle=1.0, max_brake=1.0, self._ramp = None self.standstill_ref_mps = standstill_ref_mps self.standstill_brake = standstill_brake + #: openpilot's hold: brake while slower than stop_speed_mps and asked for + #: 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 self.integral = 0.0 self.pedal = 0.0 @@ -66,11 +71,20 @@ def reset(self): self.pedal = 0.0 self._ramp = None - def step(self, v_ref, v_measured, dt): - """Returns (throttle, brake), each in [0, 1], never both positive.""" + def step(self, v_ref, v_measured, dt, a_ref=None): + """Returns (throttle, brake), each in [0, 1], never both positive. + + ``a_ref`` is the acceleration command, read only by the hold below. + """ if dt <= 0.0: raise ValueError('dt must be positive') + if a_ref is not None and v_measured < self.stop_speed_mps \ + and a_ref < self.stop_accel_mps2: + self.integral = 0.0 + self.pedal = -self.standstill_brake + return 0.0, min(self.max_brake, self.standstill_brake) + # THE ENVELOPE. A RAMPED setpoint, advanced from its own last value -- # not the incoming reference clamped to the measured speed. # diff --git a/CommonLib/xil/dynosim.py b/CommonLib/xil/dynosim.py index abf6970e..cf83fcbd 100644 --- a/CommonLib/xil/dynosim.py +++ b/CommonLib/xil/dynosim.py @@ -123,9 +123,10 @@ def __init__(self, vehicle=None, dyno=None, driver=None): # -- running ----------------------------------------------------------- - def step(self, v_ref, dt): - """The driver chases v_ref. Read ``throttle``/``brake`` for what it did.""" - throttle, brake = self.driver.step(v_ref, self.speed, dt) + def step(self, v_ref, dt, a_ref=None): + """The driver chases v_ref. Read ``throttle``/``brake`` for what it did. + ``a_ref``, the acceleration command, goes to the driver's hold.""" + throttle, brake = self.driver.step(v_ref, self.speed, dt, a_ref) return self.step_pedals(throttle, brake, dt) def step_pedals(self, throttle, brake, dt): diff --git a/CommonLib/xil/link.py b/CommonLib/xil/link.py index 359d8cdf..fe371a64 100644 --- a/CommonLib/xil/link.py +++ b/CommonLib/xil/link.py @@ -48,6 +48,7 @@ class LocalLink(object): """Both ends in one process. No latency, no loss, no jitter. The baseline: whatever a UDP run does differently from this is transport. + Values pass through the same float32 packet the wire carries. """ def __init__(self, clock=time.monotonic): @@ -60,7 +61,7 @@ def _put(self, which, value, stamp): # simulator end ------------------------------------------------------- def send_reference(self, speed, steer=0.0): - self._to_dyno = ((float(speed), float(steer)), self._clock()) + self._to_dyno = (unpack(pack(speed, steer)), self._clock()) def recv_measurement(self): return self._to_sim[0] if self._to_sim else None @@ -70,7 +71,7 @@ def measurement_age(self): # dyno end ------------------------------------------------------------ def send_measurement(self, speed, steer=0.0): - self._to_sim = ((float(speed), float(steer)), self._clock()) + self._to_sim = (unpack(pack(speed, steer)), self._clock()) def recv_reference(self): return self._to_dyno[0] if self._to_dyno else None diff --git a/doc/ConfigSetup.md b/doc/ConfigSetup.md index 48930e5c..6dec84fe 100644 --- a/doc/ConfigSetup.md +++ b/doc/ConfigSetup.md @@ -121,9 +121,12 @@ VehicleSubscription: | VehicleSubscription | list | [] | Same schema as the application-layer subscriptions. | | SignalSubscription | list | [] | Signal subscriptions for XIL. | | DetectorSubscription | list | [] | Detector subscriptions for XIL. | +| Transport | string | inprocess | `inprocess`: FIXS's simulated dyno (`fixs.xil.dynosim()`). `udp`: the application talks to its own rig at the first VehicleSubscription's ip and port; FIXS opens no socket. `tcp`: reserved. | When the application layer is disabled but XIL is enabled, the interface automatically reuses the XIL vehicle subscriptions to seed outbound traffic. +A controller reads this section with `fixs.config.get('xil')`: keys in snake_case (`transport`, `enable_xil`), plus `ip` and `port` from the first VehicleSubscription. + ## CarMakerSetup | Key | Type | Default | Notes | diff --git a/tests/Python/unit/test_fixs_config.py b/tests/Python/unit/test_fixs_config.py new file mode 100644 index 00000000..ad04e50f --- /dev/null +++ b/tests/Python/unit/test_fixs_config.py @@ -0,0 +1,90 @@ +"""fixs.config.get: the scenario yaml, read-only, by lowercase names. + + python -m pytest tests/Python/unit/test_fixs_config.py +""" +import os +import sys + +import pytest +import yaml + +_ROOT = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', '..', '..')) +for _p in (_ROOT, os.path.join(_ROOT, 'CommonLib')): + if _p not in sys.path: + sys.path.insert(0, _p) + +import fixs # noqa: E402 + +DOC = { + "SimulationSetup": {"EnableRealSim": True, "SimulationEndTime": 100, + "SelectedTrafficSimulator": "SUMO", + "TrafficSimulatorIP": "127.0.0.1"}, + "ApplicationSetup": { + "EnableApplicationLayer": True, + "VehicleSubscription": [{"type": "ego", "attribute": {"all": ["true"]}, + "ip": ["127.0.0.1"], "port": [4430]}]}, + "XilSetup": {"EnableXil": True, "Transport": "udp", + "VehicleSubscription": [{"type": "ego", "attribute": {"id": ["ego"]}, + "ip": ["192.168.140.24", "10.0.0.1"], + "port": [4420]}]}, + "EgoSetup": {"Id": "ego", "Dynamics": "virenv"}, + "CarlaSetup": {"EnableCosimulation": True, "CarlaTimeStep": 0.05}, +} + + +@pytest.fixture +def path(tmp_path): + p = tmp_path / "scenario.yaml" + p.write_text(yaml.safe_dump(DOC), encoding="utf-8") + return str(p) + + +def test_sections_and_keys_are_lowercase(path): + assert fixs.config.get('xil', path)['transport'] == 'udp' + assert fixs.config.get('xil', path)['enable_xil'] is True + assert fixs.config.get('carla', path)['carla_time_step'] == 0.05 + assert fixs.config.get('simulation', path)['traffic_simulator_ip'] == '127.0.0.1' + + +def test_ip_and_port_are_the_first_subscriptions(path): + xil = fixs.config.get('xil', path) + assert (xil['ip'], xil['port']) == ('192.168.140.24', 4420) + assert xil['vehicle_subscription'][0]['ip'] == ['192.168.140.24', '10.0.0.1'] + + +def test_defaults_are_applied(tmp_path): + p = tmp_path / "bare.yaml" + p.write_text(yaml.safe_dump(dict(DOC, XilSetup={"EnableXil": True})), encoding="utf-8") + assert fixs.config.get('xil', str(p))['transport'] == 'inprocess' + + +def test_an_unwritten_parsed_section_still_answers(tmp_path): + p = tmp_path / "noxil.yaml" + p.write_text(yaml.safe_dump({k: v for k, v in DOC.items() if k != 'XilSetup'}), + encoding="utf-8") + assert fixs.config.get('xil', str(p))['enable_xil'] is False + + +def test_the_result_is_read_only_and_a_copy(path): + xil = fixs.config.get('xil', path) + with pytest.raises(TypeError): + xil['transport'] = 'tcp' + xil['vehicle_subscription'][0]['ip'][0] = 'changed' + assert fixs.config.get('xil', path)['ip'] == '192.168.140.24' + + +def test_an_unknown_section_names_the_ones_there_are(path): + with pytest.raises(fixs.FixsError) as e: + fixs.config.get('nope', path) + assert 'xil' in str(e.value) + + +def test_it_reads_the_yaml_the_run_is_using(path, monkeypatch): + monkeypatch.setenv('FIXS_CONFIG_YAML', path) + assert fixs.config.get('xil')['transport'] == 'udp' + + +def test_no_scenario_is_an_error_not_a_guess(monkeypatch): + monkeypatch.delenv('FIXS_CONFIG_YAML', raising=False) + with pytest.raises(fixs.FixsError): + fixs.config.get('xil') diff --git a/tests/Python/unit/test_fixs_driver.py b/tests/Python/unit/test_fixs_driver.py index 742fedff..61b74e57 100644 --- a/tests/Python/unit/test_fixs_driver.py +++ b/tests/Python/unit/test_fixs_driver.py @@ -105,10 +105,10 @@ def test_the_simulated_dyno_is_passed_in_like_any_other(scenario): """The bench is built with its parameters, and the exchange is a function the caller writes -- the same two lines a rig owner replaces.""" from CommonLib.fixs import xil - bench = xil.dyno(vehicle={'mass_kg': 900.0}) + bench = xil.dynosim(vehicle={'mass_kg': 900.0}) def exchange(vref, dt): - return bench.exchange(vref, dt) + return bench.exchange(vref, 0.0, dt) d = _build(driver(exchange)) assert d.benchInLoop is True @@ -575,3 +575,19 @@ def test_nothing_invents_a_speed_the_eco_controller_did_not_ask_for(passive): ego = _passiveEgo(speedDesired=advisory) d.control(ego, 0.1) assert ego.speedDesired == pytest.approx(advisory, abs=1e-12), advisory + + +@pytest.mark.parametrize('scenario', [False], indirect=True) +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 + 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 + + +def test_driver_set_refuses_an_unknown_option(): + with pytest.raises(TypeError): + fixs.driver.set(startInGera=True) diff --git a/tests/Python/unit/test_xil_dyno_hookup.py b/tests/Python/unit/test_xil_dyno_hookup.py index bc21085e..8206416b 100644 --- a/tests/Python/unit/test_xil_dyno_hookup.py +++ b/tests/Python/unit/test_xil_dyno_hookup.py @@ -1,12 +1,9 @@ """XilSetup: a dynamometer in the ego controller's loop (#24). -``fixsxil.dyno()`` turns a scenario's XilSetup into the bench the controller talks -to. What is worth testing is not the bench -- test_xil_dyno.py does that -- but -the three things the hookup can get wrong without failing: - - - a scenario that declares no bench must hand back None, not a default one, - - a scenario that declares one must reach the endpoint IT names, - - a bench that never answers must not invent motion. +``fixsxil.dynosim()`` builds the simulated bench from the parameters it is given; +it reads no scenario. What the scenario decides -- whether a bench is in the loop, +and where a udp application's rig is -- comes from ``fixs.xil.enabled()`` and +``fixs.config.get('xil')``. And one refusal: a dynamometer in the controller's loop is not a plant that owns the ego, so EnableXil with Dynamics anything but virenv is two different @@ -64,53 +61,40 @@ def xilOn(transport="inprocess", port=420, ip="127.0.0.1"): # ------------------------------------------------------------ what it builds -def test_no_xilsetup_at_all_means_no_bench(tmp_path): - assert fixsxil.dyno(write(tmp_path)) is None - - -def test_enablexil_false_means_no_bench(tmp_path): - """Off is off. A default bench would silently change every run's plant.""" - assert fixsxil.dyno(write(tmp_path, dict(xilOn(), EnableXil=False))) is None - - -@pytest.mark.parametrize("transport", ["inprocess", "udp", "tcp"]) -def test_each_transport_builds(tmp_path, transport): - d = fixsxil.dyno(write(tmp_path, xilOn(transport), name=transport + ".yaml")) +def test_dynosim_needs_no_scenario(monkeypatch): + """A udp app's stand-in rig builds it with no yaml at all.""" + monkeypatch.delenv('FIXS_CONFIG_YAML', raising=False) + d = fixsxil.dynosim() try: - assert d.transport == transport - assert (d.sim is not None) == (transport == 'inprocess') + assert d.sim is not None finally: d.close() -def test_the_endpoint_comes_from_the_subscription(tmp_path): - """Not a constant here: the yaml says where the cell is.""" - port = 5399 - d = fixsxil.dyno(write(tmp_path, xilOn('udp', port=port))) - try: - assert d.link._peer[1] == port or d.link._peer[0] == '127.0.0.1' - finally: - d.close() +def test_the_rig_address_comes_from_the_subscription(tmp_path): + xil = fixs.config.get('xil', write(tmp_path, xilOn('udp', ip='192.168.140.24', port=4420))) + assert xil['transport'] == 'udp' + assert (xil['ip'], xil['port']) == ('192.168.140.24', 4420) def test_an_unknown_transport_is_refused(tmp_path): with pytest.raises(SystemExit) as e: - fixsxil.dyno(write(tmp_path, xilOn('carrier-pigeon'))) + fixs.config.get('xil', write(tmp_path, xilOn('carrier-pigeon'))) assert 'Transport' in str(e.value) # ------------------------------------------------------------ what it answers -def test_inprocess_bench_holds_the_speed_it_is_given(tmp_path): +def test_the_bench_holds_the_speed_it_is_given(): """The whole point, end to end: ask for 15 m/s and the bench gets there. And the torque it takes is the road load, which is checkable: the dyno absorbs A + B*v + C*v^2, and F*r must equal what the axles produced. """ - d = fixsxil.dyno(write(tmp_path, xilOn())) + d = fixsxil.dynosim() try: for _ in range(400): # 20 s at the CARLA step - got = d.exchange(15.0, 0.05) + got = d.exchange(15.0, 0.0, 0.05) assert got == pytest.approx(15.0, abs=0.05) assert d.misses == 0 @@ -123,20 +107,6 @@ def test_inprocess_bench_holds_the_speed_it_is_given(tmp_path): d.close() -def test_a_silent_bench_gives_the_reference_straight_back(tmp_path): - """No answer must not become no motion, and must not become invented - motion either. The reference returns unchanged -- the run behaves as though - no bench were attached -- and the miss is counted, because a run that ends - with a large count did not test what it claims to have tested.""" - d = fixsxil.dyno(write(tmp_path, xilOn('tcp', port=5398))) - try: - assert d.exchange(12.0, 0.05) == pytest.approx(12.0) - assert d.misses == 1 - assert d.age() is None - finally: - d.close() - - # ------------------------------------------ the bench, on either integrator def test_dynamics_xil_is_refused(tmp_path): @@ -181,20 +151,6 @@ def test_transport_defaults_to_inprocess(tmp_path): assert cfg.Xil_setup['Transport'] == 'inprocess' -def test_an_unreadable_scenario_is_not_read_as_no_bench(tmp_path, monkeypatch): - """The dangerous direction. Whether a bench is declared is a fact about the - scenario; a scenario that cannot be read does not answer it, and guessing - 'no' would quietly run the plant the yaml did not ask for.""" - monkeypatch.delenv('FIXS_CONFIG_YAML', raising=False) - with pytest.raises(fixs.FixsError) as e: - fixsxil.dyno() - assert 'FIXS_CONFIG_YAML' in str(e.value) - - monkeypatch.setenv('FIXS_CONFIG_YAML', str(tmp_path / 'nope.yaml')) - with pytest.raises(fixs.FixsError): - fixsxil.dyno() - - def test_the_bridge_says_which_yaml_it_is_running(tmp_path): """mainVirCarla exports its -f so a controller loaded in-process cannot read a DIFFERENT scenario than the bridge hosting it. Without this the fallback @@ -206,13 +162,12 @@ def test_the_bridge_says_which_yaml_it_is_running(tmp_path): assert "os.environ['FIXS_CONFIG_YAML'] = os.path.abspath(args.configPath)" in text -def test_the_yaml_says_what_vehicle_is_on_the_bench(tmp_path): +def test_the_caller_says_what_vehicle_is_on_the_bench(): """Otherwise every run gets the default car, and an ablation -- put a light vehicle on it and see whether the drive comes back -- cannot be run at all. """ - cfg = dict(xilOn(), Vehicle={'mass_kg': 900.0, 'torque_bandwidth_Hz': 40.0}, - Dyno={'road_A_N': 0.0, 'roller_inertia_kgm2': 0.0}) - d = fixsxil.dyno(write(tmp_path, cfg)) + d = fixsxil.dynosim(vehicle={'mass_kg': 900.0, 'torque_bandwidth_Hz': 40.0}, + dyno={'road_A_N': 0.0, 'roller_inertia_kgm2': 0.0}) try: assert d.sim.vehicle.mass_kg == 900.0 assert d.sim.vehicle.driveline.torque_bandwidth_Hz == 40.0 @@ -222,31 +177,29 @@ def test_the_yaml_says_what_vehicle_is_on_the_bench(tmp_path): d.close() -def test_a_misspelled_bench_parameter_fails_the_run(tmp_path): +def test_a_misspelled_bench_parameter_fails_the_run(): """Ignoring it would leave the bench quietly on its defaults, and the run - would look like it tested the vehicle the yaml described.""" + would look like it tested the vehicle the caller described.""" d = None try: with pytest.raises(TypeError): - d = fixsxil.dyno(write(tmp_path, dict(xilOn(), - Vehicle={'mass': 900.0}))) + d = fixsxil.dynosim(vehicle={'mass': 900.0}) finally: if d is not None: d.close() -def test_a_light_bench_reaches_its_reference_far_sooner(tmp_path): +def test_a_light_bench_reaches_its_reference_far_sooner(): """The ablation itself, in miniature: strip the mass and the torque delay and the bench stops being a plant. That is what makes it a control -- if a light bench still changes a run, the bench is not what changed it.""" - heavy = fixsxil.dyno(write(tmp_path, xilOn(), name='heavy.yaml')) - light = fixsxil.dyno(write(tmp_path, dict( - xilOn(), Vehicle={'mass_kg': 400.0, 'torque_bandwidth_Hz': 40.0}, - Dyno={'roller_inertia_kgm2': 0.0}), name='light.yaml')) + heavy = fixsxil.dynosim() + light = fixsxil.dynosim(vehicle={'mass_kg': 400.0, 'torque_bandwidth_Hz': 40.0}, + dyno={'roller_inertia_kgm2': 0.0}) try: def stepsTo(d, target): for n in range(1, 2001): - if d.exchange(target, 0.05) >= 0.95 * target: + if d.exchange(target, 0.0, 0.05) >= 0.95 * target: return n return None nHeavy, nLight = stepsTo(heavy, 10.0), stepsTo(light, 10.0) @@ -267,7 +220,9 @@ def test_enabled_answers_the_scenario_not_the_client(tmp_path): assert fixsxil.enabled(write(tmp_path, name='none.yaml')) is False -def test_enabled_refuses_to_guess_like_dyno_does(tmp_path, monkeypatch): +def test_enabled_refuses_to_guess(tmp_path, monkeypatch): + """Whether a bench is declared is a fact about the scenario; a scenario that + cannot be read does not answer it.""" monkeypatch.delenv('FIXS_CONFIG_YAML', raising=False) with pytest.raises(fixs.FixsError): fixsxil.enabled() @@ -275,21 +230,9 @@ def test_enabled_refuses_to_guess_like_dyno_does(tmp_path, monkeypatch): # -- the robot driver, capped to an envelope (#24) --------------------------- -def test_the_robot_driver_comes_from_the_yaml(tmp_path): - """Vehicle and Dyno were settable and the ROBOT was not, so a bench could - not be held inside an acceleration envelope without pretending the car had - less torque than it has. On a real cell the robot is the one of the three - that is yours to set.""" - y = dict(xilOn(), Driver={"max_throttle": 0.27, "max_brake": 0.27}) - cfg = ConfigHelper() - cfg.getConfig(write(tmp_path, y, name="driver.yaml")) - assert cfg.Xil_setup["Driver"] == {"max_throttle": 0.27, "max_brake": 0.27} - - -def test_the_yaml_cap_reaches_the_cell(tmp_path): - d = fixsxil.dyno(write(tmp_path, - dict(xilOn(), Driver={"max_throttle": 0.27}), - name="capped.yaml")) +def test_the_robot_cap_reaches_the_cell(): + """On a real cell the robot is the one of the three that is yours to set.""" + d = fixsxil.dynosim(driver={"max_throttle": 0.27}) try: assert d.sim.driver.max_throttle == 0.27 finally: @@ -342,3 +285,31 @@ def test_the_envelope_ramps_rather_than_clamping(): worst = max(worst, (v - p) / 0.1) p = v assert worst < 2.0, worst + + +# -- openpilot's hold: slow and not asked to accelerate means brake ----------- + +HOLD = {'stop_speed_mps': 0.3, 'stop_accel_mps2': 0.1} + + +def test_the_hold_is_off_by_default(): + a, b = fixsxil.dynosim(), fixsxil.dynosim() + for _ in range(40): + assert a.exchange(0.15, 0.0, 0.05) == b.exchange(0.15, 1.0, 0.05) + + +def test_the_hold_keeps_a_slow_small_command_at_rest(): + d = fixsxil.dynosim(driver=HOLD) + assert max(d.exchange(0.15, 0.0, 0.05) for _ in range(60)) == 0.0 + + +def test_the_hold_lets_go_when_asked_to_accelerate(): + d = fixsxil.dynosim(driver=HOLD) + assert [d.exchange(0.15, 1.5, 0.05) for _ in range(60)][-1] > 0.1 + + +def test_the_hold_only_applies_while_slow(): + d = fixsxil.dynosim(driver=HOLD) + for _ in range(200): + d.exchange(1.0, 1.5, 0.05) + assert d.exchange(1.0, 0.0, 0.05) > 0.9 diff --git a/tests/Python/unit/test_xil_link.py b/tests/Python/unit/test_xil_link.py index dbe19c94..6d48aeed 100644 --- a/tests/Python/unit/test_xil_link.py +++ b/tests/Python/unit/test_xil_link.py @@ -106,6 +106,13 @@ def test_local_carries_each_direction_separately(): assert link.recv_measurement() == pytest.approx((16.2, 2.5)) +def test_local_rounds_to_the_float32_the_wire_carries(): + """So an inprocess run and a run over the wire see the same numbers.""" + link = LocalLink() + link.send_reference(0.1) + assert link.recv_reference()[0] == unpack(pack(0.1))[0] != 0.1 + + def test_the_newest_value_wins(): link = LocalLink() for v in (1.0, 2.0, 3.0):