def _connect(self):
try:
if self.host is None:
self.dsock = rx.mkdatasock() #Connecting to localhost
elif self.multicast is not None and self.multicast is not '' and self.port is not None:
ColorTerminal().blue(
"Connecting to natnet on %s@%s (multicast: %s)" %
(self.host, self.port, self.multicast))
self.dsock = rx.mkdatasock(
ip_address=self.host,
multicast_address=self.multicast,
port=int(self.port)) #Connecting to multicast address
else:
ColorTerminal().blue("Connecting to natnet on %s@%s" %
(self.host, self.port))
self.dsock = rx.mkdatasock(
ip_address=self.host,
port=int(self.port)) # Connecting to IP address
self.dsock.setblocking(0)
self.connected = True
self.connectEvent(self)
ColorTerminal().green("Connected")
except:
ColorTerminal().red("There was an error connecting")
self._disconnect()
return self.connected
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 connect(self):
print(bcolors.OKBLUE + "Connecting to %s : %s" % (self.host, self.port) + bcolors.ENDC)
try:
if self.host is None:
self.dsock = rx.mkdatasock() #Connecting to localhost
elif self.multicast is not None and self.port is not None:
self.dsock = rx.mkdatasock(ip_address=self.host, multicast_address=self.multicast, port=int(self.port)) #Connecting to multicast address
else:
self.dsock = rx.mkdatasock(ip_address=self.host, port=int(self.port)) # Connecting to IP address
self.connected = True
except:
self.connected = False
print(bcolors.FAIL +"There was an error connecting"+ bcolors.ENDC)
raise
return self.connected
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 _connect(self):
ColorTerminal().blue("Connecting to %s : %s" % (self.host, self.port))
try:
if self.host is None:
self.dsock = rx.mkdatasock() # Connecting to localhost
elif self.multicast is not None and self.port is not None:
self.dsock = rx.mkdatasock(
ip_address=self.host, multicast_address=self.multicast, port=int(self.port)
) # Connecting to multicast address
else:
self.dsock = rx.mkdatasock(ip_address=self.host, port=int(self.port)) # Connecting to IP address
self.connected = True
ColorTerminal().green("Connected")
except:
ColorTerminal().red("There was an error connecting")
self._disconnect()
return self.connected
def _connect(self):
try:
if self.host is None:
self.dsock = rx.mkdatasock() #Connecting to localhost
elif self.multicast is not None and self.multicast is not '' and self.port is not None:
ColorTerminal().blue("NatnetReader connecting to natnet @ %s:%s (multicast: %s)" % (self.host, self.port, self.multicast))
self.dsock = rx.mkdatasock(ip_address=self.host, multicast_address=self.multicast, port=int(self.port)) #Connecting to multicast address
else:
ColorTerminal().blue("NatnetReader connecting to natnet @ %s:%s" % (self.host, self.port))
self.dsock = rx.mkdatasock(ip_address=self.host, port=int(self.port)) # Connecting to IP address
self.dsock.setblocking(0)
self.connected = True
self.connectEvent(self)
ColorTerminal().green("NatnetReader Connected")
except:
ColorTerminal().red("NatnetReader encountered an error while connecting")
self._disconnect()
return self.connected
def main():
version, max_count = sys.argv[1:]
max_count = int(max_count)
dsock = rx.mkdatasock()
count = 0
while count < max_count:
data = dsock.recv(rx.MAX_PACKETSIZE)
fname = os.path.join("test", "data", "frame-motive-%s-%03d.bin" % (version, count))
with open(fname, "wb") as dumpfile:
dumpfile.write(bytes(data))
print("dumped", fname)
count += 1
def _connect(self):
ColorTerminal().blue("Connecting to %s : %s" % (self.host, self.port))
try:
if self.host is None:
self.dsock = rx.mkdatasock() #Connecting to localhost
elif self.multicast is not None and self.port is not None:
self.dsock = rx.mkdatasock(
ip_address=self.host,
multicast_address=self.multicast,
port=int(self.port)) #Connecting to multicast address
else:
self.dsock = rx.mkdatasock(
ip_address=self.host,
port=int(self.port)) # Connecting to IP address
self.connected = True
ColorTerminal().green("Connected")
except:
ColorTerminal().red("There was an error connecting")
self._disconnect()
return self.connected
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 __init__(self, version_string, ip_address=None):
# The version string should be of the form "2900" and should match the SDK version of the Motive software.
# E.g. Motive 1.9 == SDK 2.9.0.0 == "2900"
# Motive 1.8 == SDK 2.8.0.0 == "2800"
self.sdk_version = tuple(map(int,version_string)) # e.g. result is (2,9,0,0)
# create a multicast UDP receiver socket
self.receiver = optirx.mkdatasock(ip_address=ip_address)
# set non-blocking mode so the socket can be polled
self.receiver.setblocking(0)
# Keep track of the most recent results. These are stored as normal Python list structures, but
# already rotated into Rhino coordinate conventions.
self.positions = list() # list of Point3d objects
self.rotations = list() # list of [x,y,z,w] quaternions as Python list of numbers
self.bodynames = list() # list of name strings associated with the bodies
return
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 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))
try:
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:
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, ip_address='204.102.224.2', version=None):
self.dsock = rx.mkdatasock(ip_address)
if version is None:
version = (2, 7, 0, 0) # the latest SDK version
self.version = version
pass
# vim: tabstop=8 expandtab shiftwidth=4 softtabstop=4
#run those two lines
#sudo route del default gw 192.168.0.1
#sudo route add -net 224.0.0.0 netmask 240.0.0.0 enp1s0
import optirx as rx
import select
import sys
version = (3, 0, 0, 0) # NatNet version to use
#dsock = rx.mkdatasock(\
dsock = rx.mkdatasock(ip_address='0.0.0.0',
multicast_address='239.255.42.99',
port=1511)
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 make_motive_sock():
return rx.mkdatasock(ip_address=motiveConfig['ip'],
multicast_address=motiveConfig['multicast_address'],
port=motiveConfig['port'])
