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):
# 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)
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)
#!/usr/bin/env python
import rospy, os
import socket
import optirx as rx
from struct import pack
from optitrack.utils import get_ip_address, read_parameter
if __name__ == '__main__':
# Connect to the optitrack system
rospy.init_node("recorder")
iface = read_parameter('~iface', 'wlp3s0')
version = (2, 10, 0, 0) # the latest SDK version
optitrack = rx.mkcmdsock(ip_address=get_ip_address(iface), port=1512)
#optitrack = rx.mkcmdsock(ip_address="192.168.0.77", port=5050)
# start_string="<CaptureStart>\
# <Name VALUE=\"RemoteTriggerTest_take01\"/>\
# <SessionName VALUE=\"SessionName\" />\
# <Notes VALUE=\"\"/>\
# <Assets VALUE=\"RigidBody1\" />\
# <Description VALUE=\"\" />\
# <DatabasePath VALUE=\"\"/>\
# <TimeCode VALUE=\"00:00:00:00\"/>\
# <PacketID VALUE=\"0\"/>\
# </CaptureStart>"
#header_b = "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\" ?>"
#start_string = "<CaptureStart><TimeCode VALUE=\"12 13 14 15 0 0 1 1\"/><Name VALUE=\"RemoteTriggerTest_take01\"/><Notes VALUE=""/><Description VALUE=""/><DatabasePath VALUE=\"C:Users\\tracking\\Downloads\"/><PacketID VALUE=\"0\"/><ProcessID VALUE=\"" +str(os.getpid()) +"\"/></CaptureStart>"
