def publish_relative_frames(self, data):
tmp_dict = {}
for segment in data.segments:
if(segment.is_quaternion):
tmp_dict[segment.id] = (segment.position,segment.quat)
else:
tmp_dict[segment.id] = (segment.position,segment.euler)
for idx in tmp_dict.keys():
p_idx = xsens_client.get_segment_parent_id(idx)
child_data = tmp_dict[idx]
if(p_idx==-1):
continue
elif(p_idx==0):
new_quat = tft.quaternion_multiply(child_data[1],tft.quaternion_inverse((1,1,1,1)))#if(segment.is_quaternion): TODO Handle Euler
#self.sendTransform(child_data[0],
self.sendTransform(child_data[0],
child_data[1],
#(1.0,0,0,0),#FIXME
rospy.Time.now(),
xsens_client.get_segment_name(idx),
self.ref_frame)
else:
parent_data = tmp_dict[p_idx]
parent_transform = tf.Transform(parent_data[1],parent_data[0])
child_transform = tf.Transform(child_data[1],child_data[0])
new_trans = parent_transform.inversetimes(child_transform)
new_quat = tft.quaternion_multiply(child_data[1],tft.quaternion_inverse(parent_data[1]))
tmp_pos = (child_data[0][0]-parent_data[0][0],child_data[0][1]-parent_data[0][1] ,child_data[0][2]-parent_data[0][2] )
#tmp_pos = (child_data[0][0] - parent_data[0][0],child_data[0][1] - parent_data[0][1],child_data[0][2] - parent_data[0][2])
#self.sendTransform(qv_mult(parent_data[1],tmp_pos),
#self.sendTransform(tmp_pos,
# new_quat,
#child_data[1],
self.sendTransform(new_trans.getOrigin(), new_trans.getRotation(),rospy.Time.now(),xsens_client.get_segment_name(idx),xsens_client.get_segment_name(p_idx))
def publish_relative_frames2(self, data):
tmp_dict = {}
for segment in data.segments:
if(segment.is_quaternion):
tmp_dict[segment.id] = (segment.position,segment.quat,self.tr.fromTranslationRotation(segment.position,segment.quat))
else:
tmp_dict[segment.id] = (segment.position,segment.euler)
for idx in tmp_dict.keys():
p_idx = xsens_client.get_segment_parent_id(idx)
child_data = tmp_dict[idx]
if(p_idx==-1):
continue
elif(p_idx==0):
helper = tft.quaternion_about_axis(1,(-1,0,0))
new_quat = tft.quaternion_multiply(tft.quaternion_inverse(helper),child_data[1])#if(segment.is_quaternion): TODO Handle Euler
#self.sendTransform(child_data[0],
self.sendTransform(child_data[0],
child_data[1],
#(1.0,0,0,0),#FIXME
rospy.Time.now(),
xsens_client.get_segment_name(idx),
self.ref_frame)
elif(p_idx>0):
parent_data = tmp_dict[p_idx]
new_quat = tft.quaternion_multiply(tft.quaternion_inverse(parent_data[1]),child_data[1])
tmp_trans = (parent_data[0][0] - child_data[0][0],parent_data[0][1] - child_data[0][1],parent_data[0][2] - child_data[0][2])
new_trans = qv_mult(parent_data[1],tmp_trans)
self.sendTransform(
new_trans,
new_quat,
rospy.Time.now(),
xsens_client.get_segment_name(idx),
xsens_client.get_segment_name(p_idx))
else:
parent_data = tmp_dict[p_idx]
this_data = np.multiply(tft.inverse_matrix(child_data[2]) , parent_data[2])
self.sendTransform(
tft.translation_from_matrix(this_data),
tft.quaternion_from_matrix(this_data),
rospy.Time.now(),
xsens_client.get_segment_name(idx),
xsens_client.get_segment_name(p_idx))
def publish_global_frames(self, data):
for segment in data.segments:
if(segment.is_quaternion):
self.sendTransform(segment.position,
segment.quat,
rospy.Time.now(),
xsens_client.get_segment_name(segment.id),
xsens_client.get_segment_parent_name(1,self.ref_frame))
else:
print "euler"
self.sendTransform(segment.position,
tf.transformations.quaternion_from_euler(segment.euler),
rospy.Time.now(),
xsens_client.get_segment_name_by_index(segment.id),
xsens_client.get_segment_parent_name_by_index(1,self.ref_frame))