Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 20:36:06 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[natnet] fix some bugs in natnet python

Browse Source 
- 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
This commit is contained in:
Gautier Hattenberger
2017-12-14 21:33:13 +01:00
parent ef144a21e7
commit 4eb5ab1f13
2 changed files with 55 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/ground_segment/python/natnet3.x/NatNetClient.py
+19 -6
View File
@@ -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 )
@@ -126,10 +133,6 @@ class NatNetClient:
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 @@ class NatNetClient:
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 @@ class NatNetClient:
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 @@ 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
+36 -28
View File
@@ -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,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
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, t)
if abs(t - timestamp[i]) < period:
return # too early for next message
timestamp[i] = t
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(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)
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 @@ 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)