def update(self):
if not self.dsock:
return
# print('NatnetReader.update')
data = None
try:
data = self.dsock.recv(rx.MAX_PACKETSIZE)
if self.connection_status != None:
self.connection_status = None
self.connectionStatusUpdateEvent(self)
except Exception as e:
# error: [Errno 35] Resource temporarily unavailable
# print('socket receive err: ', e.strerror)
if self.connection_status != e.strerror:
self.connection_status = e.strerror
self.connectionStatusUpdateEvent(self)
if data:
packet = rx.unpack(data, version=self.version)
if type(packet) is rx.SenderData:
self.setVersion(packet.natnet_version)
self._parse(packet)
def main(paths):
for path in paths:
with open(path, "rb") as input:
print("Reading %s" % path)
data = input.read()
packet = rx.unpack(data, version=(2,9,0,0))
print(packet)
def update(self):
if not self.dsock:
return
# print('NatnetReader.update')
data = None
try:
data = self.dsock.recv(rx.MAX_PACKETSIZE)
if self.connection_status != None:
self.connection_status = None
self.connectionStatusUpdateEvent(self)
except Exception as e:
# error: [Errno 35] Resource temporarily unavailable
# print('socket receive err: ', e.strerror)
if self.connection_status != e.strerror:
self.connection_status = e.strerror
self.connectionStatusUpdateEvent(self)
if data:
packet = rx.unpack(data, version=self.version)
if type(packet) is rx.SenderData:
self.setVersion(packet.natnet_version)
self._parse(packet)
def test_unpack_frame_of_data_natnet2700():
expected_rb = [
(-0.053690, 0.099419, -1.398518),
(0.047905, 0.115714, -1.436263),
(0.023839, 0.072290, -1.388070)]
expected_om = [
(-0.254053, 0.055445, -1.432309),
(-0.281266, 0.049510, -1.421349)]
for i in range(1, 1 + 2):
with open("test/data/frame-motive-1.7.2-%03d.bin" % i, "rb") as f:
binary = f.read()
parsed = rx.unpack(binary, (2, 7, 0, 0))
print(parsed)
assert_is(type(parsed), rx.FrameOfData)
assert_in(parsed.frameno, [411213, 411214, 411215])
assert_in(b"all", parsed.sets)
assert_in(b"Rigid Body 1", parsed.sets)
assert_almost_equal(parsed.sets[b"Rigid Body 1"], expected_rb, 4)
assert_equal(parsed.rigid_bodies[0].mrk_ids, (1, 2, 3))
assert_equal(len(parsed.other_markers), 2)
assert_almost_equal(parsed.other_markers, expected_om, 3)
assert_equal(parsed.skeletons, [])
assert_equal(len(parsed.labeled_markers), 3)
def recv_data():
#def recv_date(vehicle_name)
if sys.argv[2:]:
version = tuple(map(int, sys.argv[2]))
else:
version = (2, 7, 0, 0) # the latest SDK version
dsock = rx.mkdatasock()
count = 0
#while count < max_count:
data = dsock.recv(rx.MAX_PACKETSIZE)
packet = rx.unpack(data, version=version)
if type(packet) is rx.SenderData:
version = packet.natnet_version
#print("NatNet version received:", version)
if type(packet) in [rx.SenderData, rx.ModelDefs, rx.FrameOfData]:
packet_dict = packet._asdict()
all_bodies = packet_dict['rigid_bodies']
"""
@todo: ought to be able to specify which rigid body we're looking for, i.e. all_bodies['name_of_helicopter']
Take a second look at Optitrack data (get rigid body by name), then return data for just the helicopter
"""
for body in all_bodies:
contortion = body._asdict()['orientation']
euler = Quat([elem for elem in contortion]).equatorial
#print(dumps(packet._asdict(), indent=4))
#count += 1
return contortion, euler
def main(paths):
for path in paths:
with open(path, "rb") as input:
print("Reading %s" % path)
data = input.read()
packet = rx.unpack(data, version=(2, 9, 0, 0))
print(packet)
def read_motive_packet(socket):
data = socket.recv(rx.MAX_PACKETSIZE)
# parse packet and store it locally
packet = rx.unpack(data, motiveConfig['natnetsdk_version'])
if type(packet) is rx.SenderData:
motiveConfig['natnetsdk_version'] = packet.natnet_version
return packet
def getdata(): #coloca el codigo para capturar datos del optitrack en una funcion, la cual puede ser llamada posteriormente
dsock = rx.mkdatasock()
version = (2, 7, 0, 0) # NatNet version to use
data = dsock.recv(rx.MAX_PACKETSIZE)
packet = rx.unpack(data, version=version)
if type(packet) is rx.SenderData:
version = packet.natnet_version
return packet
def update(self):
if not self.dsock:
return
# print('NatnetReader.update')
data = self.dsock.recv(rx.MAX_PACKETSIZE)
packet = rx.unpack(data, version=self.version)
if type(packet) is rx.SenderData:
setVersion(packet.natnet_version)
self._parse(packet)
def update(self):
if not self.dsock:
return
# print('NatnetReader.update')
data = self.dsock.recv(rx.MAX_PACKETSIZE)
packet = rx.unpack(data, version=self.version)
if type(packet) is rx.SenderData:
setVersion(packet.natnet_version)
self._parse(packet)
def recv_data():
# def recv_date(vehicle_name)
# the second optional command line argument
# is the version string of the NatNet server;
# may be necessary to receive data without
# the initial SenderData packet
if sys.argv[2:]:
version = tuple(map(int, sys.argv[2]))
else:
version = (2, 7, 0, 0) # the latest SDK version
# "Create a data socket."
PORT = 1511
MULTICAST_ADDRESS = '239.255.42.99'
SOCKET_BUFSIZE = 1024
datasock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM,
socket.IPPROTO_UDP)
datasock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
datasock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
datasock.bind((MULTICAST_ADDRESS, PORT))
# join a multicast group
mreq = struct.pack("4sl", socket.inet_aton(MULTICAST_ADDRESS),
socket.INADDR_ANY)
datasock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 32)
datasock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1)
datasock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, SOCKET_BUFSIZE)
datasock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
datasock.settimeout(3)
print(datasock.gettimeout())
# dsock = rx.mkdatasock(ip_address='', multicast_address='239.255.42.99', port=1511)
while 1:
print("looping")
data = datasock.recv(rx.MAX_PACKETSIZE)
packet = rx.unpack(data, version=version)
if type(packet) is rx.SenderData:
version = packet.natnet_version
# print("NatNet version received:", version)
if type(packet) in [rx.SenderData, rx.ModelDefs, rx.FrameOfData]:
packet_dict = packet._asdict()
all_bodies = packet_dict['rigid_bodies']
"""
@todo: ought to be able to specify which rigid body we're looking for, i.e. all_bodies['name_of_helicopter']
Take a second look at Optitrack data (get rigid body by name), then return data for just the helicopter
"""
for body in all_bodies:
contortion = body._asdict()['orientation']
euler = Quat([elem for elem in contortion]).equatorial
print(euler)
# print(dumps(packet._asdict(), indent=4))
# count += 1
return contortion
def processFrameData(self, data):
# notify about new (raw, binary) frame data
self.frameDataEvent(data, self)
# unpack
packet = rx.unpack(data, version=self._natnet_version)
# change natnet version if necessary
if type(packet) is rx.SenderData:
self._natnet_version = packet.natnet_version
# store
self.frame = packet
# notify about new (unpacked) frame
self.frameEvent(packet, self)
def processFrameData(self, data):
# notify about new (raw, binary) frame data
self.frameDataEvent(data, self)
# unpack
packet = rx.unpack(data, version=self._natnet_version)
# change natnet version if necessary
if type(packet) is rx.SenderData:
self._natnet_version = packet.natnet_version
# store
self.frame = packet
# notify about new (unpacked) frame
self.frameEvent(packet, self)
def recv_data():
# def recv_date(vehicle_name)
# the second optional command line argument
# is the version string of the NatNet server;
# may be necessary to receive data without
# the initial SenderData packet
if sys.argv[2:]:
version = tuple(map(int, sys.argv[2]))
else:
version = (2, 7, 0, 0) # the latest SDK version
# "Create a data socket."
PORT = 1511
MULTICAST_ADDRESS = '239.255.42.99'
SOCKET_BUFSIZE = 1024
datasock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
datasock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
datasock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
datasock.bind((MULTICAST_ADDRESS, PORT))
# join a multicast group
mreq = struct.pack("4sl", socket.inet_aton(MULTICAST_ADDRESS), socket.INADDR_ANY)
datasock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 32)
datasock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1)
datasock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, SOCKET_BUFSIZE)
datasock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
datasock.settimeout(3)
print(datasock.gettimeout())
# dsock = rx.mkdatasock(ip_address='', multicast_address='239.255.42.99', port=1511)
while 1:
print("looping")
data = datasock.recv(rx.MAX_PACKETSIZE)
packet = rx.unpack(data, version=version)
if type(packet) is rx.SenderData:
version = packet.natnet_version
# print("NatNet version received:", version)
if type(packet) in [rx.SenderData, rx.ModelDefs, rx.FrameOfData]:
packet_dict = packet._asdict()
all_bodies = packet_dict['rigid_bodies']
"""
@todo: ought to be able to specify which rigid body we're looking for, i.e. all_bodies['name_of_helicopter']
Take a second look at Optitrack data (get rigid body by name), then return data for just the helicopter
"""
for body in all_bodies:
contortion = body._asdict()['orientation']
euler = Quat([elem for elem in contortion]).equatorial
print(euler)
# print(dumps(packet._asdict(), indent=4))
# count += 1
return contortion
def optitrack_loop(rigid_body_id):
global version
while 1:
data = None
while len(select.select([dsock], [], [], 0)[0]) > 0:
#print('got pack')
data = dsock.recv(rx.MAX_PACKETSIZE)
if data is not None:
packet = rx.unpack(data, version=version)
for pack in packet[3]:
if pack.id == rigid_body_id:
yield pack
else:
yield
def run(self):
self.kill = False
while True and not self.kill:
try:
data = self.dsock.recv(rx.MAX_PACKETSIZE)
packet = rx.unpack(data, version=self.version)
if type(packet) is rx.SenderData:
setVersion(packet.natnet_version)
self.parse(packet)
self.updated()
except:
print bcolors.FAIL + "Disconnected from server." + bcolors.ENDC
raise
self.disconnect()
break;
def test_unpack_sender_data_all_versions():
files = ["test/data/frame-motive-1.5.0-000.bin",
"test/data/frame-motive-1.7.2-000.bin"]
versions = [(2, 5, 0, 0), (2, 7, 0, 0)]
for fname, version in zip(files, versions):
with open(fname, "rb") as f:
binary = f.read()
parsed = rx.unpack(binary)
expected = rx.SenderData(appname=b"NatNetLib",
version=version,
natnet_version=version)
print("parsed:\n", parsed)
print("expected:\n", expected)
assert_equal(parsed, expected)
def update_feedback(rigid_body_ids=None, mode='Euler'):
def process_state(body, mode):
location = body['position']
if mode is 'quaternion':
orientation = [elem for elem in body['orientation']]
else:
orientation = Quat([elem for elem in body['orientation']]).equatorial # Euler Angles
return list(location), (orientation).tolist()
# the second optional command line argument
# is the version string of the NatNet server;
# may be necessary to receive data without
# the initial SenderData packet
if sys.argv[2:]:
version = tuple(map(int, sys.argv[2]))
else:
version = (2, 7, 0, 0) # the latest SDK version
# @todo: really shoudln't be making a socket each time
dsock = rx.mkdatasock()
try:
data = dsock.recv(rx.MAX_PACKETSIZE)
packet = rx.unpack(data, version=version)
except:
pass
if type(packet) is rx.SenderData:
version = packet.natnet_version
print("NatNet version received:", version)
if type(packet) in [rx.SenderData, rx.ModelDefs, rx.FrameOfData]:
all_bodies = packet[RIGID_BODIES] # elem 3 contains info on all rigid bodies
output = {}
for body in all_bodies:
body = body._asdict()
if rigid_body_ids is not None: # check if we care about this rigid body
if body['id'] in rigid_body_ids:
location, orientation = process_state(body, mode)
else:
continue
else:
location, orientation = process_state(body, mode)
output[str(body['id'])] = {'location': location, 'orientation': orientation}
# time.sleep(.5)
return location, orientation
def poll(self):
"""Poll the mocap receiver port and return True if new data is available."""
try:
data = self.receiver.recv(optirx.MAX_PACKETSIZE)
except:
return False
packet = optirx.unpack(data, version=self.sdk_version)
if type(packet) is optirx.SenderData:
version = packet.natnet_version
print "NatNet version received:", version
elif type(packet) is optirx.FrameOfData:
nbodies = len(packet.rigid_bodies)
# print "Received frame data with %d rigid bodies." % nbodies
# print "Received FrameOfData with sets:", packet.sets
# There appears to be one marker set per rigid body plus 'all'.
# print "Received FrameOfData with names:", packet.sets.keys()
# print "First marker of first marker set:", packet.sets.values()[0][0]
# print "Received FrameOfData with rigid body IDs:", [body.id for body in packet.rigid_bodies]
# print "First marker of first rigid body:", packet.rigid_bodies[0].markers[0]
# print "First tracking flag of first rigid body:", packet.rigid_bodies[0].tracking_valid
# compare markers to associate the numbered rigid bodies with the named marker sets
mapping = self._identify_rigid_bodies( packet.sets, packet.rigid_bodies)
# print "Body identification:", mapping
if nbodies > 0:
# print packet.rigid_bodies[0]
# rotate the coordinates into Rhino conventions and save them in the object instance as Python lists
self.positions = [ rotated_point(body.position) if body.tracking_valid else None for body in packet.rigid_bodies]
self.rotations = [ rotated_orientation(body.orientation) for body in packet.rigid_bodies]
self.bodynames = [ mapping.get(body.id, '<Missing>') for body in packet.rigid_bodies]
# return a new data indication
return True
elif type(packet) is optirx.ModelDefs:
print "Received ModelDefs:", packet
else:
print "Received unhandled NatNet packet type:", packet
# else return a null result
return False
def demo_recv_data():
# pretty-printer for parsed
try:
from simplejson import dumps, encoder
encoder.FLOAT_REPR = lambda o: ("%.4f" % o)
except ImportError:
from json import dumps, encoder
encoder.FLOAT_REPR = lambda o: ("%.4f" % o)
# the first optional command line argument:
# if given, the number of packets to dump
if sys.argv[1:]:
max_count = int(sys.argv[1])
else:
max_count = float("inf")
# the second optional command line argument
# is the version string of the NatNet server;
# may be necessary to receive data without
# the initial SenderData packet
if sys.argv[2:]:
version = tuple(map(int, sys.argv[2]))
else:
version = (2, 7, 0, 0) # the latest SDK version
dsock = rx.mkdatasock()
count = 0
while count < max_count:
data = dsock.recv(rx.MAX_PACKETSIZE)
packet = rx.unpack(data, version=version)
if type(packet) is rx.SenderData:
version = packet.natnet_version
print("NatNet version received:", version)
if type(packet) in [rx.SenderData, rx.ModelDefs, rx.FrameOfData]:
packet_dict = packet._asdict()
all_bodies = packet_dict['rigid_bodies']
for body in all_bodies:
body = body._asdict()
location = body['position']
contortion = body['orientation']
euler = Quat([elem for elem in contortion]).equatorial
#print(dumps(packet._asdict(), indent=4))
count += 1
def demo_recv_data():
# pretty-printer for parsed
try:
from simplejson import dumps, encoder
encoder.FLOAT_REPR = lambda o: ("%.4f" % o)
except ImportError:
from json import dumps, encoder
encoder.FLOAT_REPR = lambda o: ("%.4f" % o)
# the first optional command line argument:
# if given, the number of packets to dump
if sys.argv[1:]:
max_count = int(sys.argv[1])
else:
max_count = float("inf")
# the second optional command line argument
# is the version string of the NatNet server;
# may be necessary to receive data without
# the initial SenderData packet
if sys.argv[2:]:
version = tuple(map(int, sys.argv[2]))
else:
version = (2, 7, 0, 0) # the latest SDK version
dsock = rx.mkdatasock()
count = 0
while count < max_count:
data = dsock.recv(rx.MAX_PACKETSIZE)
packet = rx.unpack(data, version=version)
if type(packet) is rx.SenderData:
version = packet.natnet_version
print("NatNet version received:", version)
if type(packet) in [rx.SenderData, rx.ModelDefs, rx.FrameOfData]:
packet_dict = packet._asdict()
all_bodies = packet_dict['rigid_bodies']
for body in all_bodies:
body = body._asdict()
location = body['position']
contortion = body['orientation']
euler = Quat([elem for elem in contortion]).equatorial
#print(dumps(packet._asdict(), indent=4))
count += 1
def recv_data(self, mode='Euler', rigid_body_ids=None):
"""
This function retrieves packets from the NatNet stream originating from Motive, the stream server
for the optitrack motion capture server.
:return: position and orientation, where position is in [x, y, z] in meters and orientation is given
by the rigid body's Euler angles, or optionally, a rotation from the world to body frames via quaternion
"""
def process_state(body, mode):
location = body['position']
if mode is 'quaternion':
orientation = [elem for elem in body['orientation']]
else:
orientation = Quat([elem for elem in body['orientation']]).equatorial # Euler Angles
return location, orientation
# the second optional command line argument
# is the version string of the NatNet server;
# may be necessary to receive data without
# the initial SenderData packet
data = self.dsock.recv(rx.MAX_PACKETSIZE)
packet = rx.unpack(data, version=self.version)
if type(packet) in [rx.SenderData, rx.ModelDefs, rx.FrameOfData]:
all_bodies = packet[RIGID_BODIES] # elem 3 contains info on all rigid bodies
output = {}
for body in all_bodies:
body = body._asdict()
if rigid_body_ids is not None: # check if we care about this rigid body
if body['id'] in rigid_body_ids:
location, orientation = process_state(body, mode)
else:
continue
else:
location, orientation = process_state(body, mode)
output[str(body['id'])] = {'location': location, 'orientation': orientation}
# time.sleep(.5)
return output
def main():
# create a multicast UDP receiver socket
receiver = rx.mkdatasock()
# create a unicast UDP sender socket
sender = make_udp_sender()
# the hostname running Grasshopper, assumed to be the same machine
host = gethostip()
while True:
data = receiver.recv(rx.MAX_PACKETSIZE)
packet = rx.unpack(data, version=sdk_version)
if type(packet) is rx.SenderData:
version = packet.natnet_version
print("NatNet version received:", version)
if type(packet) == rx.FrameOfData:
print("Received frame data.")
records = [body_record(body) for body in packet.rigid_bodies]
msg = "".join(records)
print("Sending", msg)
sender.sendto(msg, (host, RHINO_PORT))
def test_unpack_frame_of_data_natnet2500():
expected_rb = [
(-0.3015673756599426, 0.08478303998708725, 1.1143304109573364),
(-0.23079043626785278, 0.04755447059869766, 1.1353150606155396),
(-0.25711703300476074, -0.014958729967474937, 1.1209092140197754)]
expected_om = [
(-0.24560749530792236, 0.1687806248664856, 1.2753326892852783),
(-0.11109362542629242, 0.1273186355829239, 1.2400494813919067)]
for i in range(1, 1 + 2):
with open("test/data/frame-motive-1.5.0-%03d.bin" % i, "rb") as f:
binary = f.read()
parsed = rx.unpack(binary)
assert_is(type(parsed), rx.FrameOfData)
assert_in(parsed.frameno, [92881, 92882])
assert_in(b"all", parsed.sets)
assert_in(b"Rigid Body 1", parsed.sets)
assert_almost_equal(parsed.sets[b"Rigid Body 1"], expected_rb, 4)
assert_equal(parsed.rigid_bodies[0].mrk_ids, (1, 2, 3))
assert_equal(len(parsed.other_markers), 2)
assert_almost_equal(parsed.other_markers, expected_om, 3)
assert_equal(parsed.skeletons, [])
assert_equal(len(parsed.labeled_markers), 3)
data = dsock.recv(rx.MAX_PACKETSIZE)
if data is not None:
packet = rx.unpack(data, version=version)
for pack in packet[3]:
if pack.id == rigid_body_id:
yield pack
else:
yield
if __name__ == '__main__':
if sys.argv[1] == 'd':
last_pos = None
while True:
data = dsock.recv(rx.MAX_PACKETSIZE)
packet = rx.unpack(data, version=version)
if type(packet) is rx.SenderData:
version = packet.natnet_version
#try manually identify the relevant rigid body
#the list of rigid bodies is in packet 3
pos = ' '.join(
['%d: %.2f' % (pk.id, pk.position[1]) for pk in packet[3]])
if pos != last_pos:
print(pos)
last_pos = pos
#import pdb;pdb.set_trace()
else:
l = optitrack_loop(int(sys.argv[1]))
while 1:
pack = l.__next__()
def readDataFrame(self):
data = self.dsock.recv(rx.MAX_PACKETSIZE)
packet = rx.unpack(data, version=self.version)
if type(packet) is rx.SenderData:
setVersion(packet.natnet_version)
self.parse(packet)
import optirx as rx
print bcolors.OKGREEN + "Libraries imported correctly" + bcolors.ENDC
except ImportException:
print bcolors.FAIL + "Error importing library, please install optirx by running: sudo pip install optirx" + bcolors.ENDC
if len(sys.argv) == 4:
print bcolors.OKGREEN + "connecting to IP address %s / multicast address %s with port %s" % (
sys.argv[1],
sys.argv[2],
sys.argv[3],
) + bcolors.ENDC
dsock = rx.mkdatasock(ip_address=sys.argv[1], multicast_address=sys.argv[2], port=int(sys.argv[3]))
elif len(sys.argv) == 3:
print bcolors.OKGREEN + "connecting to IP address %s with port %s" % (sys.argv[1], sys.argv[2]) + bcolors.ENDC
dsock = rx.mkdatasock(ip_address=sys.argv[1], port=int(sys.argv[2]))
else:
print bcolors.OKGREEN + "connecting to localhost" + bcolors.ENDC
dsock = rx.mkdatasock()
version = (2, 7, 0, 0) # NatNet version to use
while True:
data = dsock.recv(rx.MAX_PACKETSIZE)
packet = rx.unpack(data, version=version)
if type(packet) is rx.SenderData:
version = packet.natnet_version
for r in packet.skeletons[0].rigid_bodies:
sendBone(r)
def __init__(self):
# Read parameters to configure the node
tf_publish_rate = read_parameter('~tf_publish_rate', 50.)
tf_period = 1. / tf_publish_rate if tf_publish_rate > 0 else float(
'inf')
parent_frame = read_parameter('~parent_frame', 'world')
optitrack_frame = read_parameter('~optitrack_frame', 'optitrack')
rigid_bodies = read_parameter('~rigid_bodies', dict())
names = []
ids = []
for name, value in rigid_bodies.items():
names.append(name)
ids.append(value)
# Setup Publishers
pose_pub = rospy.Publisher('/optitrack/rigid_bodies',
RigidBodyArray,
queue_size=3)
# Setup TF listener and broadcaster
tf_listener = tf.TransformListener()
tf_broadcaster = tf.TransformBroadcaster()
# Connect to the optitrack system
iface = read_parameter('~iface', 'eth1')
version = (2, 9, 0, 0) # the latest SDK version
optitrack = rx.mkdatasock(ip_address=get_ip_address(iface))
rospy.loginfo('Successfully connected to optitrack')
# Get transformation from the world to optitrack frame
(parent, child) = (parent_frame, optitrack_frame)
try:
now = rospy.Time.now() + rospy.Duration(1.0)
tf_listener.waitForTransform(parent, child, now,
rospy.Duration(5.0))
(pos, rot) = tf_listener.lookupTransform(parent, child, now)
wTo = PoseConv.to_homo_mat(pos, rot)
except (tf.Exception, tf.LookupException, tf.ConnectivityException):
rospy.logwarn('Failed to get transformation from %r to %r frame' %
(parent, child))
parent_frame = optitrack_frame
wTo = np.eye(4)
# Track up to 24 rigid bodies
prevtime = np.ones(24) * rospy.get_time()
while not rospy.is_shutdown():
try:
data = optitrack.recv(rx.MAX_PACKETSIZE)
except socket.error:
if rospy.is_shutdown(): # exit gracefully
return
else:
rospy.logwarn('Failed to receive packet from optitrack')
packet = rx.unpack(data, version=version)
if type(packet) is rx.SenderData:
version = packet.natnet_version
rospy.loginfo('NatNet version received: ' + str(version))
if type(packet) in [rx.SenderData, rx.ModelDefs, rx.FrameOfData]:
# Optitrack gives the position of the centroid.
array_msg = RigidBodyArray()
for i, rigid_body in enumerate(packet.rigid_bodies):
body_id = rigid_body.id
pos_opt = np.array(rigid_body.position)
rot_opt = np.array(rigid_body.orientation)
oTr = PoseConv.to_homo_mat(pos_opt, rot_opt)
# Transformation between world frame and the rigid body
wTr = np.dot(wTo, oTr)
array_msg.header.stamp = rospy.Time.now()
array_msg.header.frame_id = parent_frame
body_msg = RigidBody()
pose = Pose()
pose.position = Point(*wTr[:3, 3])
pose.orientation = Quaternion(
*tr.quaternion_from_matrix(wTr))
body_msg.id = body_id
body_msg.tracking_valid = rigid_body.tracking_valid
body_msg.mrk_mean_error = rigid_body.mrk_mean_error
body_msg.pose = pose
for marker in rigid_body.markers:
# TODO: Should transform the markers as well
body_msg.markers.append(Point(*marker))
array_msg.bodies.append(body_msg)
# Control the publish rate for the TF broadcaster
if rigid_body.tracking_valid and (
rospy.get_time() - prevtime[body_id] >= tf_period):
body_name = 'rigid_body_%d' % (body_id)
if body_id in ids:
idx = ids.index(body_id)
body_name = names[idx]
tf_broadcaster.sendTransform(pos_opt, rot_opt,
rospy.Time.now(),
body_name,
optitrack_frame)
prevtime[body_id] = rospy.get_time()
pose_pub.publish(array_msg)
def __init__(self):
# Setup Publishers
mocap_pub = rospy.Publisher('/optitrack/rigid_bodies',
RigidBodyArray,
queue_size=3)
mc_to_px4 = rospy.Publisher('/optitrack/mc_to_px4',
PoseStamped,
queue_size=3)
# Connect to the optitrack system
version = (2, 10, 0, 0
) # the compatible SDK version for Motive 1.10.5 in DASL
optitrack = rx.mkdatasock(
'239.255.42.99'
) #Type the Multicast Ip Address from Motion capture system
rospy.loginfo('Successfully connected to optitrack')
while not rospy.is_shutdown():
try:
data = optitrack.recv(rx.MAX_PACKETSIZE)
except socket.error:
if rospy.is_shutdown():
return
else:
rospy.logwarn('Failed to receive packet from optitrack')
packet = rx.unpack(data, version=version)
if type(packet) is rx.SenderData:
version = packet.natnet_version
rospy.loginfo('NatNet version received: ' + str(version))
if type(packet) in [rx.SenderData, rx.ModelDefs, rx.FrameOfData]:
# Optitrack gives the position of the centroid.
total_msg = RigidBodyArray()
for i, rigid_body in enumerate(packet.rigid_bodies):
total_msg.header.stamp = rospy.Time.now()
total_msg.header.frame_id = 'Optitrack'
body_msg = RigidBody()
body_msg.id = rigid_body.id
body_msg.tracking_valid = rigid_body.tracking_valid
body_msg.mrk_mean_error = rigid_body.mrk_mean_error
oripose = Pose()
oripose.position = Point(*rigid_body.position)
oripose.orientation = Quaternion(*rigid_body.orientation)
body_msg.pose = oripose
px4_pose = PoseStamped()
px4_pose.header.frame_id = ''
px4_pose.header.stamp = rospy.Time.now()
px4_pose.pose.position.x = oripose.position.x #ENU frame for px4 position
px4_pose.pose.position.y = oripose.position.y
px4_pose.pose.position.z = oripose.position.z - 0.120
px4_pose.pose.orientation.w = oripose.orientation.w #base_link frame for px4 orientation
px4_pose.pose.orientation.x = oripose.orientation.x
px4_pose.pose.orientation.y = oripose.orientation.y
px4_pose.pose.orientation.z = oripose.orientation.z
for marker in rigid_body.markers:
body_msg.markers.append(Point(*marker))
total_msg.bodies.append(body_msg)
mocap_pub.publish(total_msg)
mc_to_px4.publish(px4_pose)
def __init__(self):
# Read parameters to configure the node
tf_publish_rate = read_parameter('~tf_publish_rate', 50.)
tf_period = 1. / tf_publish_rate if tf_publish_rate > 0 else float(
'inf')
parent_frame = read_parameter('~parent_frame', 'world')
optitrack_frame = read_parameter('~optitrack_frame', 'optitrack')
rigid_bodies = read_parameter('~rigid_bodies', dict())
names = []
ids = []
for name, value in rigid_bodies.items():
names.append(name)
ids.append(value)
# Setup Publishers
pose_pub = rospy.Publisher('/optitrack/rigid_bodies',
RigidBodyArray,
queue_size=3)
# Setup TF listener and broadcaster
tf_listener = tf.TransformListener()
tf_broadcaster = tf.TransformBroadcaster()
# Connect to the optitrack system
iface = read_parameter('~iface', 'eth0') #('~iface', 'eth1')
version = (2, 7, 0, 0) # the latest SDK version
#IPython.embed()
#optitrack = rx.mkdatasock(ip_address=get_ip_address(iface))
#Modified by Nikhil
# optitrack = rx.mkdatasock(ip_address=iface)
optitrack = rx.mkcmdsock(ip_address=iface)
msg = struct.pack("I", rx.NAT_PING)
server_address = '10.0.0.1' #'192.168.1.205'
result = optitrack.sendto(msg, (server_address, rx.PORT_COMMAND))
rospy.loginfo('Successfully connected to optitrack')
# Get transformation from the world to optitrack frame
(parent, child) = (parent_frame, optitrack_frame)
try:
now = rospy.Time.now() + rospy.Duration(1.0)
tf_listener.waitForTransform(parent, child, now,
rospy.Duration(5.0))
(pos, rot) = tf_listener.lookupTransform(parent, child, now)
wTo = PoseConv.to_homo_mat(pos, rot)
except (tf.Exception, tf.LookupException, tf.ConnectivityException):
rospy.logwarn('Failed to get transformation from %r to %r frame' %
(parent, child))
parent_frame = optitrack_frame
wTo = np.eye(4)
# Track up to 24 rigid bodies
prevtime = np.ones(24) * rospy.get_time()
# IPython.embed()
rospy.loginfo('Successfully got transformation')
while not rospy.is_shutdown():
try:
data = optitrack.recv(rx.MAX_PACKETSIZE)
# data, address = optitrack.recvfrom(rx.MAX_PACKETSIZE + 4)
except socket.error:
if rospy.is_shutdown(): # exit gracefully
return
else:
rospy.logwarn('Failed to receive packet from optitrack')
msgtype, packet = rx.unpack(data, version=version)
if type(packet) is rx.SenderData:
version = packet.natnet_version
rospy.loginfo('NatNet version received: ' + str(version))
if type(packet) in [rx.SenderData, rx.ModelDefs, rx.FrameOfData]:
# Optitrack gives the position of the centroid.
array_msg = RigidBodyArray()
# IPython.embed()
if msgtype == rx.NAT_FRAMEOFDATA:
# IPython.embed()
for i, rigid_body in enumerate(packet.rigid_bodies):
body_id = rigid_body.id
pos_opt = np.array(rigid_body.position)
rot_opt = np.array(rigid_body.orientation)
oTr = PoseConv.to_homo_mat(pos_opt, rot_opt)
# Transformation between world frame and the rigid body
wTr = np.dot(wTo, oTr)
array_msg.header.stamp = rospy.Time.now()
array_msg.header.frame_id = parent_frame
body_msg = RigidBody()
pose = Pose()
pose.position = Point(*wTr[:3, 3])
pose.orientation = Quaternion(
*tr.quaternion_from_matrix(wTr))
body_msg.id = body_id
body_msg.tracking_valid = rigid_body.tracking_valid
body_msg.mrk_mean_error = rigid_body.mrk_mean_error
body_msg.pose = pose
for marker in rigid_body.markers:
# TODO: Should transform the markers as well
body_msg.markers.append(Point(*marker))
array_msg.bodies.append(body_msg)
# Control the publish rate for the TF broadcaster
if rigid_body.tracking_valid and (
rospy.get_time() - prevtime[body_id] >=
tf_period):
body_name = 'rigid_body_%d' % (body_id)
if body_id in ids:
idx = ids.index(body_id)
body_name = names[idx]
tf_broadcaster.sendTransform(
pos_opt, rot_opt, rospy.Time.now(), body_name,
optitrack_frame)
prevtime[body_id] = rospy.get_time()
logFile = open("optitrack_position", "a")
strX = str(array_msg.bodies[0].pose.position.x).decode('utf-8')
strX = strX[2:-2]
strY = str(array_msg.bodies[0].pose.position.y).decode('utf-8')
strY = strY[2:-2]
strZ = str(array_msg.bodies[0].pose.position.z).decode('utf-8')
strZ = strZ[2:-2]
logFile.write(strX)
logFile.write(", ")
logFile.write(strY)
logFile.write(", ")
logFile.write(strZ)
logFile.write("\n")
logFile.close()
pose_pub.publish(array_msg)
try:
fltX = float(strX)
fltY = float(strY)
fltZ = float(strZ)
rospy.loginfo("x: %.4lf | y: %.4lf | z: %.4lf" %
(fltX, fltY, fltZ))
except:
rospy.logwarn('didn\'t read correctly')
logFile = open("optitrack_orientation", "a")
q1 = str(
array_msg.bodies[0].pose.orientation.x).decode('utf-8')
q1 = q1[2:-2]
q2 = str(
array_msg.bodies[0].pose.orientation.y).decode('utf-8')
q2 = q2[2:-2]
q3 = str(
array_msg.bodies[0].pose.orientation.z).decode('utf-8')
q3 = q3[2:-2]
q4 = str(
array_msg.bodies[0].pose.orientation.w).decode('utf-8')
q4 = q4[2:-2]
logFile.write(q1)
logFile.write(", ")
logFile.write(q2)
logFile.write(", ")
logFile.write(q3)
logFile.write(", ")
logFile.write(q4)
logFile.write("\n")
logFile.close()
pose_pub.publish(array_msg)
def __init__(self):
# Read parameters to configure the node
tf_publish_rate = read_parameter('~tf_publish_rate', 50.)
tf_period = 1. / tf_publish_rate if tf_publish_rate > 0 else float(
'inf')
parent_frame = read_parameter('~parent_frame', 'world')
optitrack_frame = read_parameter('~optitrack_frame', 'optitrack')
rigid_bodies = read_parameter('~rigid_bodies', dict())
names = []
ids = []
for name, value in rigid_bodies.items():
names.append(name)
ids.append(value)
# Setup Publishers
pose_pub = rospy.Publisher('/optitrack/rigid_bodies',
RigidBodyArray,
queue_size=3)
# Setup TF listener and broadcaster
tf_listener = tf.TransformListener()
tf_broadcaster = tf.TransformBroadcaster()
# Connect to the optitrack system
iface = read_parameter('~iface', 'eth1')
version = (2, 7, 0, 0) # the latest SDK version
#IPython.embed()
#optitrack = rx.mkdatasock(ip_address=get_ip_address(iface))
#Modified by Nikhil
# optitrack = rx.mkdatasock(ip_address=iface)
optitrack = rx.mkcmdsock(ip_address=iface)
msg = struct.pack("I", rx.NAT_PING)
server_address = '192.168.1.205'
result = optitrack.sendto(msg, (server_address, rx.PORT_COMMAND))
rospy.loginfo('Successfully connected to optitrack')
# Get transformation from the world to optitrack frame
(parent, child) = (parent_frame, optitrack_frame)
try:
now = rospy.Time.now() + rospy.Duration(1.0)
tf_listener.waitForTransform(parent, child, now,
rospy.Duration(5.0))
(pos, rot) = tf_listener.lookupTransform(parent, child, now)
wTo = PoseConv.to_homo_mat(pos, rot) # return 4x4 numpy mat
except (tf.Exception, tf.LookupException, tf.ConnectivityException):
rospy.logwarn('Failed to get transformation from %r to %r frame' %
(parent, child))
parent_frame = optitrack_frame
wTo = np.eye(4)
# Track up to 24 rigid bodies
prevtime = np.ones(24) * rospy.get_time()
# IPython.embed()
while not rospy.is_shutdown():
try:
# data = optitrack.recv(rx.MAX_PACKETSIZE)
data, address = optitrack.recvfrom(rx.MAX_PACKETSIZE + 4)
except socket.error:
if rospy.is_shutdown(): # exit gracefully
return
else:
rospy.logwarn('Failed to receive packet from optitrack')
msgtype, packet = rx.unpack(data, version=version)
if type(packet) is rx.SenderData:
version = packet.natnet_version
rospy.loginfo('NatNet version received: ' + str(version))
if type(packet) in [rx.SenderData, rx.ModelDefs, rx.FrameOfData]:
# Optitrack gives the position of the centroid.
array_msg = RigidBodyArray()
# IPython.embed()
if msgtype == rx.NAT_FRAMEOFDATA:
# IPython.embed()
for i, rigid_body in enumerate(packet.rigid_bodies):
body_id = rigid_body.id
pos_opt = np.array(rigid_body.position)
rot_opt = np.array(rigid_body.orientation)
# IPython.embed()
oTr = PoseConv.to_homo_mat(pos_opt, rot_opt)
# Transformation between world frame and the rigid body
WTr = np.dot(wTo, oTr)
wTW = np.array([[0, -1, 0, 0], [1, 0, 0, 0],
[0, 0, 1, 0], [0, 0, 0, 1]])
wTr = np.dot(wTW, WTr)
## New Change ##
# Toffset = np.array( [[0, 1, 0, 0],[0, 0, 1, 0],[1, 0, 0, 0],[0, 0, 0, 1]] )
# tf_wTr = np.dot(oTr,Toffset)
# tf_pose = Pose()
# tf_pose.position = Point(*tf_wTr[:3,3])
# tf_pose.orientation = Quaternion(*tr.quaternion_from_matrix(tf_wTr))
# IPython.embed()
array_msg.header.stamp = rospy.Time.now()
array_msg.header.frame_id = parent_frame
body_msg = RigidBody()
pose = Pose()
pose.position = Point(*wTr[:3, 3])
# OTr = np.dot(oTr,Toffset)
# pose.orientation = Quaternion(*tr.quaternion_from_matrix(oTr))
# change 26 Feb. 2017
# get the euler angle we want then compute the new quaternion
euler = tr.euler_from_quaternion(rot_opt)
quaternion = tr.quaternion_from_euler(
euler[2], euler[0], euler[1])
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]
body_msg.id = body_id
body_msg.tracking_valid = rigid_body.tracking_valid
body_msg.mrk_mean_error = rigid_body.mrk_mean_error
body_msg.pose = pose
for marker in rigid_body.markers:
# TODO: Should transform the markers as well
body_msg.markers.append(Point(*marker))
array_msg.bodies.append(body_msg)
# Control the publish rate for the TF broadcaster
if rigid_body.tracking_valid and (
rospy.get_time() - prevtime[body_id] >=
tf_period):
body_name = 'rigid_body_%d' % (body_id)
if body_id in ids:
idx = ids.index(body_id)
body_name = names[idx]
## no change ##
# tf_broadcaster.sendTransform(pos_opt, rot_opt, rospy.Time.now(), body_name, optitrack_frame)
# change 1 ##
# pos2 = np.array([tf_pose.position.x, tf_pose.position.y, tf_pose.position.z])
# rot2 = np.array([tf_pose.orientation.x, tf_pose.orientation.y, tf_pose.orientation.z, tf_pose.orientation.w])
# tf_broadcaster.sendTransform(pos2, rot2, rospy.Time.now(), body_name, optitrack_frame)
## change 2 ## <fail>
# pos2 = np.array([-pose.position.y, pose.position.x, pose.position.z])
# rot2 = np.array([-pose.orientation.y, pose.orientation.x, pose.orientation.z, pose.orientation.w])
# tf_broadcaster.sendTransform(pos2, rot2, rospy.Time.now(), body_name, optitrack_frame)
## change 3 ## <fail>
# pos2 = np.array([-pos_opt[1],pos_opt[0],pos_opt[2]])
# rot2 = np.array([-rot_opt[1],rot_opt[0],rot_opt[2],rot_opt[3]])
# tf_broadcaster.sendTransform(pos2, rot2, rospy.Time.now(), body_name, optitrack_frame)
## change 4 ## <>
pos2 = np.array([
pose.position.x, pose.position.y,
pose.position.z
])
rot2 = np.array([
pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w
])
tf_broadcaster.sendTransform(
pos2, rot2, rospy.Time.now(), body_name,
parent_frame)
prevtime[body_id] = rospy.get_time()
pose_pub.publish(array_msg)