[natnet] fix some bugs in natnet python

- add a callback to receive all the regid bodies of a frame in a single
  list together with the correct timestamp
- use the timestamp instead of system time to compute dt
- remove the padding offset (yes, there is a bug in the code provided by
  optitrack...)
- tested and working in real fligts

sw/ground_segment/python/natnet3.x/NatNetClient.py

@@ -13,7 +13,7 @@
 DoubleValue = struct.Struct( '<d' )
 
 class NatNetClient:
-    def __init__( self, server="127.0.0.1", multicast="239.255.42.99", commandPort=1510, dataPort=1511, rigidBodyListener=None, newFrameListener=None, verbose=False ):
+    def __init__( self, server="127.0.0.1", multicast="239.255.42.99", commandPort=1510, dataPort=1511, rigidBodyListener=None, newFrameListener=None, rigidBodyListListener=None, verbose=False ):
         # IP address of the NatNet server.
         self.serverIPAddress = server
 
@@ -32,6 +32,10 @@ def __init__( self, server="127.0.0.1", multicast="239.255.42.99", commandPort=1
         # Set this to a callback method of your choice to receive data at each frame.
         self.newFrameListener = newFrameListener
 
+        # Set this to a callback method of your choice to receive rigid-body data list and timestamp at each frame.
+        self.rigidBodyListListener = rigidBodyListListener
+        self.rigidBodyList = []
+
         # NatNet stream version. This will be updated to the actual version the server is using during initialization.
         self.__natNetStreamVersion = (3,0,0,0)
 
@@ -96,6 +100,9 @@ def __unpackRigidBody( self, data ):
         offset += 16
         self.__trace( "\tOrientation:", rot[0],",", rot[1],",", rot[2],",", rot[3] )
 
+        # Store data
+        self.rigidBodyList.append((id, pos, rot))
+
         # Send information to any listener.
         if self.rigidBodyListener is not None:
             self.rigidBodyListener( id, pos, rot )
@@ -126,10 +133,6 @@ def __unpackRigidBody( self, data ):
                     size = FloatValue.unpack( data[offset:offset+4] )
                     offset += 4
                     self.__trace( "\tMarker Size", i, ":", size[0] )
-
-        # Skip padding inserted by the server
-        if( self.__natNetStreamVersion[0] > 2 ):
-            offset += 4
 
         if( self.__natNetStreamVersion[0] >= 2 ):
             markerError, = FloatValue.unpack( data[offset:offset+4] )
@@ -167,6 +170,7 @@ def __unpackMocapData( self, data ):
 
         data = memoryview( data )
         offset = 0
+        self.rigidBodyList = []
 
         # Frame number (4 bytes)
         frameNumber = int.from_bytes( data[offset:offset+4], byteorder='little' )
@@ -298,7 +302,12 @@ def __unpackMocapData( self, data ):
                         deviceChannelVal = int.from_bytes( data[offset:offset+4], byteorder='little' )
                         offset += 4
                         self.__trace( "\t\t", deviceChannelVal )
-
+
+        # software latency (removed in version 3.0)
+        if self.__natNetStreamVersion[0] < 3:
+            latency = FloatValue.unpack( data[offset:offset+4] )
+            offset += 4
+
         # Timecode            
         timecode = int.from_bytes( data[offset:offset+4], byteorder='little' )
         offset += 4
@@ -333,6 +342,10 @@ def __unpackMocapData( self, data ):
             self.newFrameListener( frameNumber, markerSetCount, unlabeledMarkersCount, rigidBodyCount, skeletonCount,
                                   labeledMarkerCount, timecode, timecodeSub, timestamp, isRecording, trackedModelsChanged )
 
+        # Send rigid body list and timestamp
+        if self.rigidBodyListListener is not None:
+            self.rigidBodyListListener( self.rigidBodyList, timestamp )
+
     # Unpack a marker set description packet
     def __unpackMarkerSetDescription( self, data ):
         offset = 0

sw/ground_segment/python/natnet3.x/natnet2ivy.py

@@ -80,7 +80,7 @@
 id_dict = dict(args.ac)
 
 # initial time per AC
-timestamp = dict([(ac_id, time()) for ac_id in id_dict.keys()])
+timestamp = dict([(ac_id, None) for ac_id in id_dict.keys()])
 period = 1. / args.freq
 
 # initial track per AC
@@ -112,6 +112,8 @@ def compute_velocity(ac_id):
                 t1 = t2
             else:
                 dt = t2 - t1
+                if dt < 1e-5:
+                    continue
                 vel[0] = (p2[0] - p1[0]) / dt
                 vel[1] = (p2[1] - p1[1]) / dt
                 vel[2] = (p2[2] - p1[2]) / dt
@@ -124,40 +126,41 @@ def compute_velocity(ac_id):
     return vel
 
 
-# This is a callback function that gets connected to the NatNet client. It is called once per rigid body per frame
-def receiveRigidBodyFrame( ac_id, pos, quat ):
-    t = time()
-    i = str(ac_id)
-    if i not in id_dict.keys():
-      return
-
-    store_track(i, pos, t)
-    if abs(t - timestamp[i]) < period:
-        return # too early for next message
-    timestamp[i] = t
-
-    msg = PprzMessage("datalink", "REMOTE_GPS_LOCAL")
-    msg['ac_id'] = id_dict[i]
-    msg['pad'] = 0
-    msg['enu_x'] = pos[0]
-    msg['enu_y'] = pos[1]
-    msg['enu_z'] = pos[2]
-    vel = compute_velocity(i)
-    msg['enu_xd'] = vel[0]
-    msg['enu_yd'] = vel[1]
-    msg['enu_zd'] = vel[2]
-    msg['tow'] = int(timestamp[i]) # TODO convert to GPS itow ?
-    # convert quaternion to psi euler angle
-    dcm_0_0 = 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])
-    dcm_1_0 = 2.0 * (quat[0] * quat[1] - quat[3] * quat[2])
-    msg['course'] = 180. * np.arctan2(dcm_1_0, dcm_0_0) / 3.14
-    ivy.send(msg)
+def receiveRigidBodyList( rigidBodyList, stamp ):
+    for (ac_id, pos, quat) in rigidBodyList:
+        i = str(ac_id)
+        if i not in id_dict.keys():
+            continue
+
+        store_track(i, pos, stamp)
+        if timestamp[i] is None or abs(stamp - timestamp[i]) < period:
+            if timestamp[i] is None:
+                timestamp[i] = stamp
+            continue # too early for next message
+        timestamp[i] = stamp
+
+        msg = PprzMessage("datalink", "REMOTE_GPS_LOCAL")
+        msg['ac_id'] = id_dict[i]
+        msg['pad'] = 0
+        msg['enu_x'] = pos[0]
+        msg['enu_y'] = pos[1]
+        msg['enu_z'] = pos[2]
+        vel = compute_velocity(i)
+        msg['enu_xd'] = vel[0]
+        msg['enu_yd'] = vel[1]
+        msg['enu_zd'] = vel[2]
+        msg['tow'] = int(stamp) # TODO convert to GPS itow ?
+        # convert quaternion to psi euler angle
+        dcm_0_0 = 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])
+        dcm_1_0 = 2.0 * (quat[0] * quat[1] - quat[3] * quat[2])
+        msg['course'] = 180. * np.arctan2(dcm_1_0, dcm_0_0) / 3.14
+        ivy.send(msg)
 
 
 # start natnet interface
 natnet = NatNetClient(
         server=args.server,
-        rigidBodyListener=receiveRigidBodyFrame,
+        rigidBodyListListener=receiveRigidBodyList,
         dataPort=args.data_port,
         commandPort=args.command_port,
         verbose=args.verbose)
@@ -174,4 +177,9 @@ def receiveRigidBodyFrame( ac_id, pos, quat ):
     print("Shutting down ivy and natnet interfaces...")
     natnet.stop()
     ivy.shutdown()
+except OSError:
+    print("Natnet connection error")
+    natnet.stop()
+    ivy.stop()
+    exit(-1)