Add HostVehicleData support to OSITrace

Closes #817.

Signed-off-by: Pierre R. Mai <[email protected]>

Author: pmai

doc/architecture/architecture_overview.adoc

@@ -8,7 +8,7 @@ OSI contains an object-based environment description that uses the message forma
 Google developed and maintains the Protocol Buffer library.
 OSI defines top-level messages that are used to exchange data between separate models.
 Top-level messages define the `GroundTruth` interface, the `SensorData` interface, and – since OSI version 3.0.0 – the interfaces `SensorView` and `SensorViewConfiguration`.
-Further top-level messages that were added in later versions of OSI are `TrafficCommand`, `TrafficCommandUpdate`, `TrafficUpdate`, `MotionRequest`, and `StreamingUpdate`.
+Further top-level messages that were added in later versions of OSI are `HostVehicleData`, `TrafficCommand`, `TrafficCommandUpdate`, `TrafficUpdate`, `MotionRequest`, and `StreamingUpdate`.
 
 The following figure shows the interfaces and models involved in modeling a sensor.
 

osi3trace/osi_trace.py

@@ -7,6 +7,7 @@
 
 from osi3.osi_sensorview_pb2 import SensorView
 from osi3.osi_groundtruth_pb2 import GroundTruth
+from osi3.osi_hostvehicledata_pb2 import HostVehicleData
 from osi3.osi_sensordata_pb2 import SensorData
 from osi3.osi_sensorviewconfiguration_pb2 import SensorViewConfiguration
 from osi3.osi_trafficupdate_pb2 import TrafficUpdate
@@ -19,6 +20,7 @@
 MESSAGES_TYPE = {
     "SensorView": SensorView,
     "GroundTruth": GroundTruth,
+    "HostVehicleData": HostVehicleData,
     "SensorData": SensorData,
     "SensorViewConfiguration": SensorViewConfiguration,
     "TrafficUpdate": TrafficUpdate,

tests/test_osi_trace.py

@@ -5,6 +5,7 @@
 from osi3trace.osi_trace import OSITrace
 from osi3.osi_sensorview_pb2 import SensorView
 from osi3.osi_groundtruth_pb2 import GroundTruth
+from osi3.osi_hostvehicledata_pb2 import HostVehicleData
 from osi3.osi_sensordata_pb2 import SensorData
 from osi3.osi_sensorviewconfiguration_pb2 import SensorViewConfiguration
 from osi3.osi_trafficupdate_pb2 import TrafficUpdate
@@ -51,6 +52,23 @@ def test_osi_trace_gt(self):
 
             self.assertTrue(os.path.exists(path_output))
 
+    def test_osi_trace_hvd(self):
+        with tempfile.TemporaryDirectory() as tmpdirname:
+            path_output = os.path.join(tmpdirname, "output_hvd.txth")
+            path_input = os.path.join(tmpdirname, "input_hvd.osi")
+            create_sample_hvd(path_input)
+
+            trace = OSITrace(path_input, "HostVehicleData")
+            with open(path_output, "wt") as f:
+                for message in trace:
+                    self.assertIsInstance(message, HostVehicleData)
+                    f.write(str(message))
+
+            self.assertEqual(len(trace.retrieve_offsets()), 10)
+            trace.close()
+
+            self.assertTrue(os.path.exists(path_output))
+
     def test_osi_trace_sd(self):
         with tempfile.TemporaryDirectory() as tmpdirname:
             path_output = os.path.join(tmpdirname, "output_sd.txth")
@@ -280,6 +298,44 @@ def create_sample_gt(path):
     f.close()
 
 
+def create_sample_hvd(path):
+    f = open(path, "ab")
+    hostvehicledata = HostVehicleData()
+
+    hostvehicledata.version.version_major = 3
+    hostvehicledata.version.version_minor = 0
+    hostvehicledata.version.version_patch = 0
+
+    hostvehicledata.timestamp.seconds = 0
+    hostvehicledata.timestamp.nanos = 0
+
+    hostvehicledata.host_vehicle_id.value = 114
+
+    # Generate 10 OSI messages for 9 seconds
+    for i in range(10):
+        # Increment the time
+        hostvehicledata.timestamp.seconds += 1
+        hostvehicledata.timestamp.nanos += 100000
+
+        hostvehicledata.location.dimension.length = 5
+        hostvehicledata.location.dimension.width = 2
+        hostvehicledata.location.dimension.height = 1
+
+        hostvehicledata.location.position.x = 0.0 + i
+        hostvehicledata.location.position.y = 0.0
+        hostvehicledata.location.position.z = 0.0
+
+        hostvehicledata.location.orientation.roll = 0.0
+        hostvehicledata.location.orientation.pitch = 0.0
+        hostvehicledata.location.orientation.yaw = 0.0
+
+        """Serialize"""
+        bytes_buffer = hostvehicledata.SerializeToString()
+        f.write(struct.pack("<L", len(bytes_buffer)) + bytes_buffer)
+
+    f.close()
+
+
 def create_sample_sd(path):
     f = open(path, "ab")
     sensordata = SensorData()