xil: FIXS ships the simulated dyno; fixs.config.get; openpilot's hold; fixs.driver startInGear

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.

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", {})

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

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
 

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]))