Beispiel #1
0
 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))
Beispiel #2
0
  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))
Beispiel #3
0
  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))