Improve template, copy vehicle from esmini

diff --git a/car.py b/car.py
new file mode 100644 (file)
index 0000000..c7fd294
--- /dev/null
+++ b/car.py
@@ -0,0 +1,87 @@
+"""Vehicle methods copied from esmini."""
+from math import atan, cos, fmod, pi, sin, tan
+
+
+class Car:
+    max_sp = 70.0
+    max_acc = 20.0
+    max_dec = 20.0
+    st_rate = 5.0
+    st_return_factor = 4.0
+    st_max_angle = (60.0 * pi / 180)
+    w_rad = 0.35
+    length = 5.0
+
+    def __init__(self, x, y, h, sp, wa):
+        """Create new `Car` object.
+
+        :param x: ``x`` coordinate.
+        :param y: ``y`` coordinate.
+        :param h: The heading.
+        :param sp: The speed.
+        :param wa: Wheel angle.
+        """
+        self.x = x
+        self.y = y
+        self.h = h
+        self.sp = sp
+        self.wa = wa
+        self.wr = 0.0  # wheel rotation
+        self.va = 0.0  # angular velocity
+        self.vx = 0.0  # velocity in x
+        self.vy = 0.0  # velocity in y
+
+    def ctrl(self, dt=0, throttle=0, steering=0):
+        """Corresponds to vehicle's analog control.
+
+        :param dt: GetSimTimeStep.
+        :param throttle: Longitudinal control
+            -1: maximum brake
+            0: no acceleration
+            +1: maximum acceleration
+        :param steering: Lateral control
+            -1: max left
+            0: straight
+            +1: max right
+        """
+        acc = self.max_dec if throttle < 0 else self.max_acc
+        self.sp += acc * throttle * dt
+
+        if abs(throttle) < 1e-3:
+            # Apply drag
+            self.sp *= (1 - 1e-3)
+        self.sp = min(self.max_sp, max(0, self.sp))
+
+        # Make steering slightly wheel speed dependent
+        st_scale = 1.0 / (1 + 0.005 * self.sp**2)
+        self.wa += st_scale * self.st_rate * steering * dt
+
+        # Self-align
+        self.wa *= (1.0 - self.st_return_factor * dt)
+
+        # Limit wheel angle
+        ma = st_scale * self.st_max_angle
+        self.wa = min(ma, max(-ma, self.wa))
+
+        self.update(dt)
+
+    def update(self, dt=0):
+        # Calculate wheel rot: https://en.wikipedia.org/wiki/Arc_(geometry)
+        self.wr = fmod(self.wr + self.sp * dt / self.w_rad, 2 * pi)
+
+        # Calculate vehicle kinematics according to simple bicycle model, see
+        # http://www.me.berkeley.edu/~frborrel/pdfpub/IV_KinematicMPC_jason.pdf
+
+        var = atan(0.15 * tan(self.wa))
+        self.va = var + self.h
+        hd = self.sp * sin(var) / (self.length * 0.5)
+
+        self.vx = self.sp * cos(self.va)
+        self.vy = self.sp * sin(self.va)
+
+        self.h = fmod(self.h + dt * hd, 2 * pi)
+        while self.h < 0:
+            self.h += 2 * pi
+
+        self.x += dt * self.vx
+        self.y += dt * self.vy

diff --git a/esmini-player.py b/esmini-player.py
index ffa1047baf5f7e3ef95dcd41c21250801bea958e..1809e0382a0c032c876980a94091113f6f0dbc04 100755 (executable)
--- a/esmini-player.py
+++ b/esmini-player.py
@@ -2,6 +2,10 @@
 import ctypes
 import sys
 
+from ctypes import c_int, c_float, c_void_p, c_bool
+
+from car import Car
+
 if sys.platform == "linux" or sys.platform == "linux2":
     se = ctypes.CDLL("./libesminiLib.so")
 elif sys.platform == "darwin":
@@ -12,12 +16,98 @@ else:
     print("Unsupported platform: {}".format(sys.platform))
     quit()
 
+
+class SEScenarioObjectState(ctypes.Structure):
+    """Definition of SE_ScenarioObjectState struct."""
+    _fields_ = [
+        ("id", c_int),
+        ("model_id", c_int),
+        ("control", c_int),
+        ("timestamp", c_float),
+        ("x", c_float),
+        ("y", c_float),
+        ("z", c_float),
+        ("h", c_float),
+        ("p", c_float),
+        ("r", c_float),
+        ("roadId", c_int),
+        ("junctionId", c_int),
+        ("t", c_float),
+        ("laneId", c_int),
+        ("laneOffset", c_float),
+        ("s", c_float),
+        ("speed", c_float),
+        ("centerOffsetX", c_float),
+        ("centerOffsetY", c_float),
+        ("centerOffsetZ", c_float),
+        ("width", c_float),
+        ("length", c_float),
+        ("height", c_float),
+        ("objectType", c_int),
+        ("objectCategory", c_int)]
+
+
+class SERoadInfo(ctypes.Structure):
+    """Definition of SE_RoadInfo struct."""
+    _fields_ = [
+        ("global_pos_x", c_float),
+        ("global_pos_y", c_float),
+        ("global_pos_z", c_float),
+        ("local_pos_x", c_float),
+        ("local_pos_y", c_float),
+        ("local_pos_z", c_float),
+        ("angle", c_float),
+        ("road_heading", c_float),
+        ("road_pitch", c_float),
+        ("road_roll", c_float),
+        ("trail_heading", c_float),
+        ("curvature", c_float),
+        ("speed_limit", c_float),
+        ("roadId", c_int),
+        ("junctionId", c_int),
+        ("laneId", c_int),
+        ("laneOffset", c_float),
+        ("s", c_float),
+        ("t", c_float)]
+
+
 if __name__ == "__main__":
     if (len(sys.argv) < 2):
         print('Usage: {} <xosc file>'.format(sys.argv[0]))
         exit(-1)
 
     se.SE_Init(sys.argv[1].encode('ascii'), 0, 1, 0, 0)
+    print("#objs in scenario is {}".format(se.SE_GetNumberOfObjects()))
+
+    ego = SEScenarioObjectState()
+    se.SE_GetObjectState(se.SE_GetId(0), ctypes.byref(ego))
+    c = Car(ego.x, ego.y, ego.h, ego.speed, 0)
+
+    target = SEScenarioObjectState()
+    ri = SERoadInfo()
+
+    se.SE_GetSimTimeStep.restype = c_float
+    se.SE_StepDT.argtypes = [c_float]
+    se.SE_GetRoadInfoAtDistance.argtypes = [
+        c_int, c_float, c_void_p, c_int, c_bool]
+    se.SE_ReportObjectPosXYH.argtypes = [
+        c_int, c_float, c_float, c_float, c_float, c_float]
 
     while not se.SE_GetQuitFlag():
-        se.SE_Step()
+        dt = se.SE_GetSimTimeStep()
+
+        se.SE_GetObjectState(se.SE_GetId(0), ctypes.byref(ego))
+        se.SE_GetObjectState(se.SE_GetId(1), ctypes.byref(target))
+        se.SE_GetRoadInfoAtDistance(
+            se.SE_GetId(0), 5.0, ctypes.byref(ri), 0, True)
+
+        if ego.laneId == target.laneId and abs(target.s - ego.s) < 25.0:
+            sp = -1  # emergency braking
+        else:
+            sp = 0.1 if c.sp < 30 else -0.1
+        st = ri.angle
+
+        c.ctrl(dt, sp, st)
+        se.SE_ReportObjectPosXYH(se.SE_GetId(0), 0, c.x, c.y, c.h, c.sp)
+
+        se.SE_StepDT(dt)