Update natnet SDK. (#3249)

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

@@ -38,6 +38,8 @@ import argparse
 
 # import NatNet client
 from NatNetClient import NatNetClient
+import DataDescriptions
+import MoCapData
 
 # if PAPARAZZI_HOME not set, then assume the tree containing this
 # file is a reasonable substitute
@@ -108,14 +110,14 @@ def is_moving(old_pos, new_pos):
         # if failing, update position
         return True
 
-def receiveMarkerSet(name, posList):
+def receiveMarkerSet(data: MoCapData.MarkerSetData):
     '''
     callback for markerset with name and marker position as input
     '''
     global current_index
-
+    for marker_data in data.marker_data_list:
         # check if name is matching regexp
-    name = name.decode('utf-8')
+        name = marker_data.model_name.decode('utf-8')
         if re.fullmatch(args.name, name) is not None:
 
             # check if message should be sent (first time or period)
@@ -127,27 +129,27 @@ def receiveMarkerSet(name, posList):
                 if dt >= period:
                     # period elapsed, check if moved
                     markerset[name]['time'] = now
-                moved = is_moving(markerset[name]['pos'], posList)
+                    moved = is_moving(markerset[name]['pos'], marker_data.marker_pos_list)
                     if moved:
                         send = True
-                    markerset[name]['pos'] = posList
+                        markerset[name]['pos'] = marker_data.marker_pos_list
                 if dt_refresh >= args.refresh_period:
                     # refresh period elapsed, send anyway
                     send = True
             else:
                 send = True
-            markerset[name] = {'time_refresh': now, 'time': now, 'id': current_index, 'pos': posList }
+                markerset[name] = {'time_refresh': now, 'time': now, 'id': current_index, 'pos': marker_data.marker_pos_list }
                 current_index += 1
 
             if args.very_verbose:
-            print(name, posList, time)
+                print(name, marker_data.marker_pos_list, time)
 
             if send:
                 if args.verbose and (not args.very_verbose):
-                print(name, posList, now)
+                    print(name, marker_data.marker_pos_list, now)
 
                 # build list of 2D points and compute convex hull
-            points = [(pos[X_AXIS], Y_SIGN*pos[Y_AXIS]) for pos in posList]
+                points = [(pos[X_AXIS], Y_SIGN*pos[Y_AXIS]) for pos in marker_data.marker_pos_list]
                 hull = ConvexHull(points)
                 # build lists of polygon corners to display in lat long
                 latitudes = [ int(1e7 * (lat0 + np.rad2deg(points[i][1] / R_earth))) for i in hull.vertices ]
@@ -196,16 +198,14 @@ def check_timeout():
 
 
 # start natnet interface
-natnet_version = (3,0,0,0)
+natnet = NatNetClient()
-if args.old_natnet:
+natnet.set_server_address(args.server)
-    natnet_version = (2,9,0,0)
+natnet.set_client_address('0.0.0.0')
-natnet = NatNetClient(
+natnet.marker_set_listener = receiveMarkerSet
-        server=args.server,
+if args.verbose:
-        markerSetListener=receiveMarkerSet,
+    natnet.set_print_level(1) # print all frames
-        dataPort=args.data_port,
+else:
-        commandPort=args.command_port,
+    natnet.set_print_level(0)
-        verbose=args.very_verbose,
-        version=natnet_version)
 
 
 print("Starting Object Display interface at %s" % (args.server))
@@ -218,11 +218,11 @@ try:
         check_timeout()
 except (KeyboardInterrupt, SystemExit):
     print("Shutting down ivy and natnet interfaces...")
-    natnet.stop()
+    natnet.shutdown()
     ivy.shutdown()
 except OSError:
     print("Natnet connection error")
-    natnet.stop()
+    natnet.shutdown()
     ivy.stop()
     exit(-1)
 

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

@@ -156,6 +156,8 @@ import argparse
 
 # import NatNet client
 from NatNetClient import NatNetClient
+import DataDescriptions
+import MoCapData
 
 # if PAPARAZZI_HOME not set, then assume the tree containing this
 # file is a reasonable substitute
@@ -288,16 +290,19 @@ def performTransformation( pos, vel, quat ):
 
     return pos, vel, quat
 
-def receiveRigidBodyList( rigidBodyList, stamp ):
+def receiveRigidBodyList( rigid_body_data, stamp ):
-    for (ac_id, pos, quat, valid) in rigidBodyList:
+    for rigid_body in rigid_body_data.rigid_body_list:
-        if not valid:
+        if not rigid_body.tracking_valid:
             # skip if rigid body is not valid
             continue
 
-        i = str(ac_id)
+        i = str(rigid_body.id_num)
         if i not in id_dict.keys():
             continue
         
+        pos = rigid_body.pos
+        quat = rigid_body.rot
+
         store_track(i, pos, stamp)
         if timestamp[i] is None or abs(stamp - timestamp[i]) < period:
             if timestamp[i] is None:
@@ -380,32 +385,44 @@ if not run_test_cases:
         ivy = IvyMessagesInterface("natnet2ivy")
 
     # start natnet interface
-    natnet_version = (3,0,0,0)
+    natnet = NatNetClient()
-    if args.old_natnet:
+    natnet.set_server_address(args.server)
-        natnet_version = (2,9,0,0)
+    natnet.set_client_address('0.0.0.0')
-    natnet = NatNetClient(
+    natnet.rigid_body_list_listener = receiveRigidBodyList
-            server=args.server,
-            rigidBodyListListener=receiveRigidBodyList,
-            dataPort=args.data_port,
-            commandPort=args.command_port,
-            verbose=args.verbose,
-            version=natnet_version)
 
+    if args.verbose:
+        natnet.set_print_level(1) # print all frames
+    else:
+        natnet.set_print_level(0)
+    if args.old_natnet:
+        natnet.set_nat_net_version(2,9)
     print("Starting Natnet3.x to Ivy interface at %s" % (args.server))
     try:
         # Start up the streaming client.
         # This will run perpetually, and operate on a separate thread.
         id_dict, timestamp, period, track, q_total, q_nose_correction = process_args(args)
-        natnet.run()
+        is_running = natnet.run()
+        if not is_running:
+            print("Natnet error: Could not start streaming client.")
+            exit(-1)
+
+        sleep(1)
+        if not natnet.connected():
+            print("Natnet error: Fail to connect to natnet")
+            exit(-1)
+
+        if args.verbose:
+            print_configuration(natnet)
+            
         while True:
             sleep(1)
     except (KeyboardInterrupt, SystemExit):
         print("Shutting down ivy and natnet interfaces...")
-        natnet.stop()
+        natnet.shutdown()
         ivy.shutdown()
     except OSError:
         print("Natnet connection error")
-        natnet.stop()
+        natnet.shutdown()
         ivy.stop()
         exit(-1)