better script for assigning trajectory labels from the other agents.

data-logger/proto/f1_datalogger/proto/MultiAgentLabel.proto

@@ -9,6 +9,6 @@ message MultiAgentLabel {
 	deepf1.protobuf.eigen.Pose3d ego_agent_pose = 2;
 	deepf1.protobuf.eigen.Vector3dStamped ego_agent_linear_velocity = 3;
 	deepf1.protobuf.eigen.Vector3dStamped ego_agent_angular_velocity = 4;
-	deepf1.protobuf.Trajectory ego_agent_trajectory = 5;
-	repeated deepf1.protobuf.Trajectory other_agent_trajectories = 6;		
+	repeated deepf1.protobuf.Trajectory other_agent_trajectories = 5;	
+	repeated uint32 trajectory_car_indices = 6;		
 }

data-logger/python/deepracing/__init__.py

@@ -6,4 +6,4 @@ def timestampedUdpPacketKey(packet):
             "Canada", "Britain", "Germany", "Hungary", "Belgium", "Italy",\
             "Singapore", "Japan", "Abu_Dhabi", "USA", "Brazil", "Austria",\
             "Russia", "Mexico", "Azerbaijan", "Bahrain_short", "Britan_short",\
-            "USA_short", "Japan_short"]
+            "USA_short", "Japan_short"]

data-logger/python/deepracing/exceptions/__init__.py

@@ -0,0 +1 @@
+class DeepRacingException(Exception): ...

data-logger/python/deepracing/protobuf_utils/__init__.py

@@ -11,6 +11,8 @@
 from .proto_utils import extractVelocity as extractVelocity
 from .proto_utils import extractAngularVelocity as extractAngularVelocity
 from .proto_utils import extractPosition as extractPosition
+from .proto_utils import extractRotation as extractRotation
+
 from .proto_utils import loadTrackfile as loadTrackfile
 from .proto_utils import labelPacketToNumpy as labelPacketToNumpy
 from .proto_utils import vectorFromNumpy as vectorFromNumpy

data-logger/python/deepracing/protobuf_utils/proto_utils.py

@@ -5,7 +5,7 @@
 import TimestampedImage_pb2
 import PacketMotionData_pb2
 import CarTelemetryData_pb2
-import Spline2DParams_pb2
+import Spline3D_pb2
 import Vector3d_pb2
 import Quaterniond_pb2
 import os
@@ -17,11 +17,11 @@
 from tqdm import tqdm as tqdm
 import BezierCurve_pb2
 
-def splineSciPyToPB(splineSciPy : scipy.interpolate.BSpline, tmin,tmax,Xmin,Xmax,Zmin,Zmax):
-   return Spline2DParams_pb2.Spline2DParams(XParams = splineSciPy.c[:,0], ZParams = splineSciPy.c[:,1],degree=splineSciPy.k, knots=splineSciPy.t,\
-                                                           tmin=tmin,tmax=tmax,Xmin=Xmin,Xmax=Xmax,Zmin=Zmin,Zmax=Zmax)
+def splineSciPyToPB(splineSciPy : scipy.interpolate.BSpline, tmin,tmax,Xmin,Xmax,Ymin,Ymax,Zmin,Zmax):
+   return Spline3D_pb2.Spline3D(XParams = splineSciPy.c[:,0], ZParams = splineSciPy.c[:,1],degree=splineSciPy.k, knots=splineSciPy.t,\
+                                                           tmin=tmin,tmax=tmax,Xmin=Xmin,Xmax=Xmax,Ymin=Ymin,Ymax=Ymax,Zmin=Zmin,Zmax=Zmax)
 
-def splinePBToSciPy(splinePB: Spline2DParams_pb2.Spline2DParams):
+def splinePBToSciPy(splinePB: Spline3D_pb2.Spline3D):
    return scipy.interpolate.BSpline(splinePB.knots, np.array([splinePB.XParams, splinePB.ZParams]).transpose(), splinePB.degree)
 
 def getAllTelemetryPackets(telemetry_folder: str, use_json: bool):
@@ -244,12 +244,11 @@ def extractPosition(packet , car_index = None):
    position = np.array((motion_data.m_worldPositionX, motion_data.m_worldPositionY, motion_data.m_worldPositionZ), dtype=np.float64)
    return position 
 
-def extractPose(packet : PacketMotionData_pb2.PacketMotionData, car_index = None):
+def extractRotation(packet : PacketMotionData_pb2.PacketMotionData, car_index = None):
    if car_index is None:
       idx = packet.m_header.m_playerCarIndex
    else:
       idx = car_index
-   position = extractPosition(packet, car_index=idx)
    motion_data = packet.m_carMotionData[idx]
    rightvector = np.array((motion_data.m_worldRightDirX, motion_data.m_worldRightDirY, motion_data.m_worldRightDirZ), dtype=np.float64)
    rightvector = rightvector/la.norm(rightvector)
@@ -259,7 +258,17 @@ def extractPose(packet : PacketMotionData_pb2.PacketMotionData, car_index = None
    upvector = upvector/la.norm(upvector)
    rotationmat = np.column_stack((-rightvector,upvector,forwardvector))
    quat = Rot.from_matrix(rotationmat).as_quat()
+   return quat
+
+def extractPose(packet : PacketMotionData_pb2.PacketMotionData, car_index = None):
+   if car_index is None:
+      idx = packet.m_header.m_playerCarIndex
+   else:
+      idx = car_index
+   position = extractPosition(packet, car_index=idx)
+   quat = extractRotation(packet, car_index=idx)
    return position, quat 
+
 def loadTrackfile(filepath : str):
    trackin = np.loadtxt(filepath,delimiter=",",skiprows=2)
    I = np.argsort(trackin[:,0])