[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 @@ FloatValue = struct.Struct( '<f' )
 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 @@ class NatNetClient:
         # 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 @@ class NatNetClient:
         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 )
@@ -127,10 +134,6 @@ class NatNetClient:
                     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] )
             offset += 4
@@ -167,6 +170,7 @@ class NatNetClient:
 
         data = memoryview( data )
         offset = 0
+        self.rigidBodyList = []
         
         # Frame number (4 bytes)
         frameNumber = int.from_bytes( data[offset:offset+4], byteorder='little' )
@@ -299,6 +303,11 @@ class NatNetClient:
                         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 @@ class NatNetClient:
             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 @@ if args.ac is None:
 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,17 +126,18 @@ 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 receiveRigidBodyList( rigidBodyList, stamp ):
-def receiveRigidBodyFrame( ac_id, pos, quat ):
+    for (ac_id, pos, quat) in rigidBodyList:
-    t = time()
         i = str(ac_id)
         if i not in id_dict.keys():
-      return
+            continue
 
-    store_track(i, pos, t)
+        store_track(i, pos, stamp)
-    if abs(t - timestamp[i]) < period:
+        if timestamp[i] is None or abs(stamp - timestamp[i]) < period:
-        return # too early for next message
+            if timestamp[i] is None:
-    timestamp[i] = t
+                timestamp[i] = stamp
+            continue # too early for next message
+        timestamp[i] = stamp
 
         msg = PprzMessage("datalink", "REMOTE_GPS_LOCAL")
         msg['ac_id'] = id_dict[i]
@@ -146,7 +149,7 @@ def receiveRigidBodyFrame( ac_id, pos, quat ):
         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 ?
+        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])
@@ -157,7 +160,7 @@ def receiveRigidBodyFrame( ac_id, pos, quat ):
 # 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 @@ except (KeyboardInterrupt, SystemExit):
     print("Shutting down ivy and natnet interfaces...")
     natnet.stop()
     ivy.shutdown()
+except OSError:
+    print("Natnet connection error")
+    natnet.stop()
+    ivy.stop()
+    exit(-1)