beta_v0.9.0: promote dev_v0.9.0 (a robot driver with delay and accel mode, speed and acceleration references, fixs.driver(xil=, control=), XilSetup.SpeedTo*, the (vRef, aRef) pair)

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

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

CommonLib/ConfigHelper.cpp

@@ -1,5 +1,6 @@
 #include "ConfigHelper.h"
 #include "PlatformCompat.h"   // #65: windows.h/shlwapi live behind this now
+#include <algorithm>
 
 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();
 
 }
 

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;
+
 };
 
 

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

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

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('<f')
+
+
+def _f32(x):
+    """x rounded to the wire's float32."""
+    return _F32.unpack(_F32.pack(x))[0]
+
+
+def speedFromAcceleration(speed, accelerationDesired, step):
+    """(m/s, m/s^2, s) -> 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