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,69 +110,69 @@ 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 = marker_data.model_name.decode('utf-8')
+        if re.fullmatch(args.name, name) is not None:
 
-    # check if name is matching regexp
+            # check if message should be sent (first time or period)
-    name = name.decode('utf-8')
+            send = False
-    if re.fullmatch(args.name, name) is not None:
+            now = time()
-
+            if name in markerset:
-        # check if message should be sent (first time or period)
+                dt = now - markerset[name]['time']
-        send = False
+                dt_refresh = now - markerset[name]['time_refresh']
-        now = time()
+                if dt >= period:
-        if name in markerset:
+                    # period elapsed, check if moved
-            dt = now - markerset[name]['time']
+                    markerset[name]['time'] = now
-            dt_refresh = now - markerset[name]['time_refresh']
+                    moved = is_moving(markerset[name]['pos'], marker_data.marker_pos_list)
-            if dt >= period:
+                    if moved:
-                # period elapsed, check if moved
+                        send = True
-                markerset[name]['time'] = now
+                        markerset[name]['pos'] = marker_data.marker_pos_list
-                moved = is_moving(markerset[name]['pos'], posList)
+                if dt_refresh >= args.refresh_period:
-                if moved:
+                    # refresh period elapsed, send anyway
                     send = True
-                    markerset[name]['pos'] = posList
-            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 }
-            current_index += 1
-
-        if args.very_verbose:
-            print(name, posList, time)
-
-        if send:
-            if args.verbose and (not args.very_verbose):
-                print(name, posList, now)
-
-            # build list of 2D points and compute convex hull
-            points = [(pos[X_AXIS], Y_SIGN*pos[Y_AXIS]) for pos in posList]
-            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 ]
-            longitudes = [ int(1e7 * (long0 + np.rad2deg(points[i][0] / R_cos_lat0))) for i in hull.vertices ]
-
-            # send SHAPE message
-            shape = PprzMessage("ground", "SHAPE")
-            shape['id'] = markerset[name]['id']
-            shape['linecolor'] = '"{}"'.format(args.color)
-            shape['fillcolor'] = '"{}"'.format(args.color)
-            shape['opacity'] = 1 # light
-            shape['shape'] = 1 # polygon
-            shape['status'] = 0 # create or update
-            shape['latarr'] = latitudes
-            shape['lonarr'] = longitudes
-            shape['radius'] = 0. # not relevant
-            if args.show_name:
-                shape['text'] = name
             else:
-                shape['text'] = '" "'
+                send = True
-            ivy.send(shape)
+                markerset[name] = {'time_refresh': now, 'time': now, 'id': current_index, 'pos': marker_data.marker_pos_list }
-            markerset[name]['time_refresh'] = now
+                current_index += 1
-            sleep(0.01)
+
+            if args.very_verbose:
+                print(name, marker_data.marker_pos_list, time)
+
+            if send:
+                if args.verbose and (not args.very_verbose):
+                    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 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 ]
+                longitudes = [ int(1e7 * (long0 + np.rad2deg(points[i][0] / R_cos_lat0))) for i in hull.vertices ]
+
+                # send SHAPE message
+                shape = PprzMessage("ground", "SHAPE")
+                shape['id'] = markerset[name]['id']
+                shape['linecolor'] = '"{}"'.format(args.color)
+                shape['fillcolor'] = '"{}"'.format(args.color)
+                shape['opacity'] = 1 # light
+                shape['shape'] = 1 # polygon
+                shape['status'] = 0 # create or update
+                shape['latarr'] = latitudes
+                shape['lonarr'] = longitudes
+                shape['radius'] = 0. # not relevant
+                if args.show_name:
+                    shape['text'] = name
+                else:
+                    shape['text'] = '" "'
+                ivy.send(shape)
+                markerset[name]['time_refresh'] = now
+                sleep(0.01)
 
 def check_timeout():
     '''
@@ -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,15 +290,18 @@ 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:
@@ -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)