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))