Python TransformListener.frameExists方法代码示例

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import frameExists [as 别名]
class ChallengeProblemLogger(object):
  _knownObstacles = {}
  _placedObstacle = False
  _lastgzlog = 0.0
  _tf_listener = None
  
  def __init__(self,name):
    self._name = name;

    self._experiment_started = False
    self._tf_listener = TransformListener()

    # Subscribe to robot pose ground truth from gazebo
    rospy.Subscriber("/gazebo/model_states", ModelStates, callback=self.gazebo_model_monitor,
                     queue_size=1)

  # Whenever we get a report from Gazebo, map the gazebo coordinate to map coordinates and
  # log this
  # Only do this every second - this should be accurate enough
  # TODO: model is assumed to be the third in the list. Need to make this based
  #       on the array to account for obstacles (maybe)
  def gazebo_model_monitor(self, data):
    if (len(data.pose))<=2:
      return
    data_time = rospy.get_rostime().to_sec()
    if ((self._lastgzlog == 0.0) | (data_time - self._lastgzlog >= 1)):
      # Only do this every second

      # Get the turtlebot model state information (assumed to be indexed at 2)    
      tb_pose = data.pose[2]
      tb_position = tb_pose.position
      self._lastgzlog = data_time

      # Do this only if the transform exists
      if self._tf_listener.frameExists("/base_link") and self._tf_listener.frameExists("/map"):
        self._tf_listener.waitForTransform("/map", "/base_link", rospy.Time(0), rospy.Duration(1))
        (trans,rot) = self._tf_listener.lookupTransform("/map", "/base_link",rospy.Time(0))
        rospy.loginfo("BRASS | Turtlebot | {},{}".format(trans[0], trans[1]))
      
      # Log any obstacle information, but do it only once. This currently assumes one obstacle
      # TODO: test this
      if len(data.name) > 3:
        addedObstacles = {}
        removedObstacles = self._knownObstacles.copy()
        for obs in range(3, len(data.name)-1):
          if (data.name[obs] not in self._knownObstacles):
            addedObstacles[data.name[obs]] = obs
          else:
             self._knownObstacles[data.name[obs]] = obs
 	     del removedObstacles[data.name[obs]]
        
        for key, value in removedObstacles.iteritems():
           rospy.logInfo("BRASS | Obstacle {} | removed".format(key))
           del self._knownObstacles[key]

        for key, value in addedObstacles.iteritems():
	   obs_pos = data.pose[value].position
           rospy.logInfo ("BRASS | Obstacle {} | {},{}".format(key, obs_pos.x, obs_pos.y))
	   self._knownObstacles[key] = value

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import frameExists [as 别名]
class TfTest(RosTestCase):

    def setUpEnv(self):
        
        robot = ATRV()

        odometry = Odometry()
        robot.append(odometry)
        odometry.add_stream('ros')

        motion = Teleport()
        robot.append(motion)
        motion.add_stream('socket')
        
        robot2 = ATRV()
        robot2.translate(0,1,0)
        odometry2 = Odometry()
        robot2.append(odometry2)
        odometry2.add_stream('ros', frame_id="map", child_frame_id="robot2")

        env = Environment('empty', fastmode = True)
        env.add_service('socket')

    def _check_pose(self, frame1, frame2, position, quaternion, precision = 0.01):

        t = self.tf.getLatestCommonTime(frame1, frame2)
        observed_position, observed_quaternion = self.tf.lookupTransform(frame1, frame2, t)

        for a,b in zip(position, observed_position):
            self.assertAlmostEqual(a, b, delta = precision)
        for a,b in zip(quaternion, observed_quaternion):
            self.assertAlmostEqual(a, b, delta = precision)



    def test_tf(self):

        rospy.init_node('morse_ros_tf_test')

        self.tf = TransformListener()

        sleep(1)
        self.assertTrue(self.tf.frameExists("/odom"))
        self.assertTrue(self.tf.frameExists("/base_footprint"))
        self.assertTrue(self.tf.frameExists("/map"))
        self.assertTrue(self.tf.frameExists("/robot2"))

        self._check_pose("odom", "base_footprint", [0,0,0], [0,0,0,1])
        self._check_pose("map", "robot2", [0,0,0], [0,0,0,1])

        with Morse() as morse:
            teleport = morse.robot.motion

            teleport.publish({'x' : 2, 'y' : 0, 'z' : 0, \
                              'yaw' : 0, 'pitch' : 0.0, 'roll' : 0.0})
            morse.sleep(0.1)

        self._check_pose("odom", "base_footprint", [2,0,-0.1], [0,0,0,1])

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import frameExists [as 别名]
class TransformNode:
    def __init__(self, *args):
        self.tf = TransformListener()
        rospy.Subscriber("/tf", TFMessage, transform, queue_size=1)

    def transform(self, msg):
        if self.tf.frameExists("/base_link") and self.tf.frameExists("/map"):
            t = self.tf.getLatestCommonTime("/base_link", "/map")
            position, quaternion = self.tf.lookupTransform("/base_link", "/map", t)
            print position, quaternion

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import frameExists [as 别名]
class KinectNiteHelp:
    def __init__(self, name="kinect_listener", user=1):
        # rospy.init_node(name, anonymous=True)
        self.tf = TransformListener()
        self.user = user

    def getLeftHandPos(self):
        if self.tf.frameExists(BASE_FRAME) and self.tf.frameExists("left_hand_1"):
            print " Inside TF IF Get LEft HAnd POS "
            t = self.tf.getLatestCommonTime(BASE_FRAME, "left_hand_1")
            (leftHandPos, quaternion) = self.tf.lookupTransform(BASE_FRAME, "left_hand_1", t)
            return leftHandPos

    def getRightHandPos(self):
        if self.tf.frameExists(BASE_FRAME) and self.tf.frameExists("right_hand_1"):
            t = self.tf.getLatestCommonTime(BASE_FRAME, "right_hand_1")
            (rightHandPos, quaternion) = self.tf.lookupTransform(BASE_FRAME, "right_hand_1", t)
            return rightHandPos

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import frameExists [as 别名]
class Transformation():
	def __init__(self):
		pub = 0
		self.tf = TransformListener()
		self.tf1 = TransformerROS()
		self.fdata = geometry_msgs.msg.TransformStamped()
		self.fdata_base = geometry_msgs.msg.TransformStamped()
		self.transform = tf.TransformBroadcaster()
		self.wrench = WrenchStamped()
		self.wrench_bl = WrenchStamped()
		
	def wrench_cb(self,msg):
		self.wrench = msg.wrench
		self.transform.sendTransform((0,0,-0.025),quaternion_from_euler(3.14, 0, 3.665195102, 'rxyz'),rospy.Time.now(),'/ft_debug_link','/arm_7_link')
		self.fdata.transform.translation.x = self.wrench.force.x
		self.fdata.transform.translation.y = self.wrench.force.y
		self.fdata.transform.translation.z = self.wrench.force.z
		
		try:
			if self.tf.frameExists("/dummy_link") and self.tf.frameExists("ft_debug_link"):
				t = self.tf.getLatestCommonTime("/dummy_link", "/ft_debug_link")
				(transform_ee_base_position,transform_ee_base_quaternion) = self.tf.lookupTransform("/dummy_link", '/ft_debug_link', t)
	   				#print transform_ee_base_position
	   				#print transform_ee_base_quaternion
	   				#print self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)
		except(tf.LookupException,tf.ConnectivityException):
			print("TRANSFORMATION ERROR")
			sss.say(["error"])	
			#return 'failed'
		
		self.fdata_base.transform.translation.x =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,3]))
		
		self.fdata_base.transform.translation.y =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,3]))

		self.fdata_base.transform.translation.z =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,3]))	
		
		self.wrench_bl.wrench.force.y = self.fdata_base.transform.translation.y
		self.wrench_bl.wrench.force.x = self.fdata_base.transform.translation.x
		self.wrench_bl.wrench.force.z = self.fdata_base.transform.translation.z
		self.wrench_bl.header.stamp = rospy.Time.now();
		self.pub.publish(self.wrench_bl)

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import frameExists [as 别名]
class MergePoses:

    def __init__(self):

        self.avg_pose = None
        self.tl = TransformListener()

        self.topics = rospy.get_param('~poses',[])
        print self.topics
        if len(self.topics) == 0:
            rospy.logerr('Please provide a poses list as input parameter')
            return
        self.output_pose = rospy.get_param('~output_pose','ar_avg_pose')
        self.output_frame = rospy.get_param('~output_frame', 'rf_map')
        
        self.subscribers = []
        for i in self.topics:
            subi = rospy.Subscriber(i, PoseStamped, self.callback, queue_size=1)
            self.subscribers.append(subi)

        self.pub = rospy.Publisher(self.output_pose, PoseStamped, queue_size=1)
            
        self.mutex_avg = threading.Lock()
        self.mutex_t = threading.Lock()
        
        self.transformations = {}
        
    def callback(self, pose_msg):

        # get transformation to common frame
        position = None
        quaternion = None
        if self.transformations.has_key(pose_msg.header.frame_id):
            position, quaternion = self.transformations[pose_msg.header.frame_id]
        else:
            if self.tl.frameExists(pose_msg.header.frame_id) and \
            self.tl.frameExists(self.output_frame):
                t = self.tl.getLatestCommonTime(self.output_frame, pose_msg.header.frame_id)
                position, quaternion = self.tl.lookupTransform(self.output_frame,
                                                               pose_msg.header.frame_id, t)
                self.mutex_t.acquire()
                self.transformations[pose_msg.header.frame_id] = (position, quaternion)
                self.mutex_t.release()
                rospy.loginfo("Got static transform for %s" % pose_msg.header.frame_id)

        # transform pose
        framemat = self.tl.fromTranslationRotation(position, quaternion)

        posemat = self.tl.fromTranslationRotation([pose_msg.pose.position.x,
                                                   pose_msg.pose.position.y,
                                                   pose_msg.pose.position.z],
                                                  [pose_msg.pose.orientation.x,
                                                   pose_msg.pose.orientation.y,
                                                   pose_msg.pose.orientation.z,
                                                   pose_msg.pose.orientation.w])

        newmat = numpy.dot(framemat,posemat)
        
        xyz = tuple(translation_from_matrix(newmat))[:3]
        quat = tuple(quaternion_from_matrix(newmat))

        tmp_pose = PoseStamped()
        tmp_pose.header.stamp = pose_msg.header.stamp
        tmp_pose.header.frame_id = self.output_frame
        tmp_pose.pose = Pose(Point(*xyz),Quaternion(*quat))
        
        tmp_angles = euler_from_quaternion([tmp_pose.pose.orientation.x,
                                            tmp_pose.pose.orientation.y,
                                            tmp_pose.pose.orientation.z,
                                            tmp_pose.pose.orientation.w])
        
        # compute average
        self.mutex_avg.acquire()
        
        if self.avg_pose == None or (pose_msg.header.stamp - self.avg_pose.header.stamp).to_sec() > 0.5:
            self.avg_pose = tmp_pose
        else:            
            self.avg_pose.header.stamp = pose_msg.header.stamp
            a = 0.7
            b = 0.3
            self.avg_pose.pose.position.x = a*self.avg_pose.pose.position.x + b*tmp_pose.pose.position.x
            self.avg_pose.pose.position.y = a*self.avg_pose.pose.position.y + b*tmp_pose.pose.position.y
            self.avg_pose.pose.position.z = a*self.avg_pose.pose.position.z + b*tmp_pose.pose.position.z

            angles = euler_from_quaternion([self.avg_pose.pose.orientation.x,
                                            self.avg_pose.pose.orientation.y,
                                            self.avg_pose.pose.orientation.z,
                                            self.avg_pose.pose.orientation.w])
            angles = list(angles)
            angles[0] = avgAngles(angles[0], tmp_angles[0], 0.7, 0.3)
            angles[1] = avgAngles(angles[1], tmp_angles[1], 0.7, 0.3)
            angles[2] = avgAngles(angles[2], tmp_angles[2], 0.7, 0.3)

            q = quaternion_from_euler(angles[0], angles[1], angles[2])
            
            self.avg_pose.pose.orientation.x = q[0]
            self.avg_pose.pose.orientation.y = q[1]
            self.avg_pose.pose.orientation.z = q[2]
            self.avg_pose.pose.orientation.w = q[3]

#.........这里部分代码省略.........

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import frameExists [as 别名]
    def run(self, aut_path=None):
        """Intialize all NL components."""
        # pylint: disable=W0603
        global WORLD_KNOWLEDGE
    
        # Start the NL pipeline
        if not aut_path:
            print "Starting NL pipeline..."
            init_pipes()
        else:
            print "Skipping loading nlpipeline because an automaton was loaded"
     
        # Start the ROS node
        print "Initializing ROS node..."
        rospy.init_node('nlproxy')
    
        # Set up the state mgr and its callbacks
        print "Starting state manager..."
        self.state_mgr = StateManager(self)
        self.state_mgr.set_basedir(LTLGEN_BASE_DIR)
    
        # Load the automaton if needed
        if aut_path:
            self.state_mgr.load_test_automaton(aut_path, False)
    
        # Create world knowledge
        print "Starting knowledge..."
        WORLD_KNOWLEDGE = Knowledge.Knowledge(self)
        self.state_mgr.world_knowledge = WORLD_KNOWLEDGE
        
        # Wait a little for ROS to avoid timing startup issues.
        print "Waiting for ROS node to settle..."
        time.sleep(1)
        
        # Set up ROS listening
        print "Subscribing to notifications..."
        # Set up state manager's sending and listening
        pub = add_ltl_publisher()
        self.state_mgr.set_publisher(pub)
        add_subscriber(LTL_ENVIRONMENT_TOPIC, self.state_mgr.process_sensor_data)
        add_subscriber(LTL_ENVIRONMENT_TOPIC, WORLD_KNOWLEDGE.process_sensor_data)

        # Create the input comm_proxy and iPad connections
        print "Starting comm_proxy..."
        self.comm_proxy = CallbackSocket(self.text_port)
        self.comm_proxy.register_callback(self.process_text)
        self.ipad_conn = iPadConnection(self.map_port)
        self.ipad_conn.register_rename_callback(Knowledge.rename_entity)
        self.ipad_conn.register_text_callback(self.process_text)
        # TODO Add highlight callback
        self.map_proxy = MapProxy(self.ipad_conn)
        add_subscriber(LTL_ENVIRONMENT_TOPIC, self.ipad_conn.add_icons)        
        WORLD_KNOWLEDGE.map_proxy = self.map_proxy

        # Set up odometry forwarding to the ipad
        tf = TransformListener()
        while not tf.frameExists("/map"):
            rospy.logwarn("Not ready for transforms yet")
            rospy.sleep(1.0)
        position_proxy = RobotPositionProxy(self.ipad_conn,tf)
        rospy.Subscriber(ODOM_TOPIC, Odometry, position_proxy.forward_position)

        # Set up getting directions
        direction_proxy = DirectionProxy()
        rospy.Subscriber(LOCATION_TOPIC, String, direction_proxy.set_location)
        WORLD_KNOWLEDGE.direction_proxy = direction_proxy
        
        print "NLMaster startup complete!"
        print "*" * 80
    
        # Finally, wait for input before quitting
        try:
            while True:
                text = raw_input("").strip()
                if text == "q":
                    break
        except (KeyboardInterrupt, EOFError):
            pass
        except:
            raise
        finally:
            # Only shutdown the pipeline if we actually were taking language input
            if not aut_path:
                print "Shutting down NL pipeline..."
                close_pipes()
            self.comm_proxy.shutdown()
            self.ipad_conn.shutdown()
    
        sys.exit()

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import frameExists [as 别名]
class Rotate(smach.State):
  #class handles the rotation until program is stopped
  def __init__(self):
    self.tf = TransformListener()
    smach.State.__init__(self,
      outcomes=['finished','failed'],
      input_keys=['base_pose','stop_rotating','id'],
      output_keys=['detected'])
    rospy.Subscriber("/cob_people_detection/detection_tracker/face_position_array",DetectionArray, self.callback)
    self.stop_rotating=False
    self.detections=      list()
    self.false_detections=list()

  def callback(self,msg):
    # go through list of detections and append them to detection list
    if len(msg.detections) >0:
      #clear detection list
      del self.detections[:]
      for i in xrange( len(msg.detections)):
        self.detections.append(msg.detections[i].label)
    return

  def execute(self, userdata):
    sss.say(["I am going to take a look around now."])

    # get position from tf
    if self.tf.frameExists("/base_link") and self.tf.frameExists("/map"):
        t = self.tf.getLatestCommonTime("/base_link", "/map")
        position, quaternion = self.tf.lookupTransform("/base_link", "/map", t)
  # calculate angles from quaternion
	[r,p,y]=euler_from_quaternion(quaternion)
	#print r
	#print p
	#print y
  #print position
    else:
        print "No transform available"
        return "failed"

    time.sleep(1)
    self.stop_rotating=False
    # create relative pose - x,y,theta
    curr_pose=list()
    curr_pose.append(0)
    curr_pose.append(0)
    curr_pose.append(0.1)

    while not rospy.is_shutdown() and self.stop_rotating==False and curr_pose[2]< 3.14:
      handle_base = sss.move_base_rel("base", curr_pose)

      #check in detection and react appropriately
      for det in self.detections:
        # right person is detected
        if det == userdata.id:
          self.stop_rotating=True
          sss.say(['I have found you, %s! Nice to see you.'%str(det)])
        elif det in self.false_detections:
        # false person is detected
          print "Already in false detections"
       #  person detected is unknown - only react the first time
        elif det == "Unknown":
          print "Unknown face detected"
          sss.say(['Hi! Nice to meet you, but I am still searching for %s.'%str(userdata.id)])
          self.false_detections.append("Unknown")
      # wrong face is detected the first time
        else:
          self.false_detections.append(det)
          print "known - wrong face detected"
          sss.say(['Hello %s! Have you seen %s.'%(str(det),str(userdata.id))])
      #clear detection list, so it is not checked twice
      del self.detections[:]
      time.sleep(2)

    print "-->stop rotating"
    return 'finished'

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import frameExists [as 别名]
class Bag2UosConverter:
    def __init__(self, rootdir, scale_factor, transform_pitch_angle):
	rospy.init_node('bad_to_uos_convert', anonymous=False)
	rospy.Subscriber('velotime_points', PointCloud2, self.processMsg)
	self.counter = 0
	self.rootdir = rootdir
	self.scale_factor = scale_factor
	self.tf = TransformListener()
	self.init = True
	self.transform_pitch_angle = math.radians(transform_pitch_angle)
	pass
    def transformPoint(self,x ,y , z):
	yy = y
	xx =  math.cos(self.transform_pitch_angle) * x + math.sin(self.transform_pitch_angle) * z
	zz = -math.sin(self.transform_pitch_angle) * x + math.cos(self.transform_pitch_angle) * z
	return xx, yy, zz
    def run(self):
	rospy.spin()
	pass
    def processMsg(self, msg):
	print 'data arrived'
	# print list(cloud)
	# print self.tf.frameExists("velodyne") , self.tf.frameExists("odom")

	if self.tf.frameExists("world") and self.tf.frameExists("odom"):
	    t = self.tf.getLatestCommonTime("world", "odom")
	    position, quat= self.tf.lookupTransform("world", "odom", t)
	    print position, quat
	    euler = tf.transformations.euler_from_quaternion(quat)
	    # heading_rad = math.degrees(euler[2])
	    heading_rad = euler[2]
	    print heading_rad
	    cloud = pc2.read_points(msg, skip_nans=True)
	    self.saveData(list(cloud))
	    self.savePose(position[0],position[1],0,0,0,heading_rad)

	    self.counter = self.counter + 1
	if self.init:
	    self.init = False
	    cloud = pc2.read_points(msg, skip_nans=True)
	    self.saveData(list(cloud))
	    self.savePose(0,0,0,0,0,0)
	    self.counter = self.counter + 1

    def saveData(self, cloud):
	file_string = 'scan'+format(self.counter,'03d')
	fullfilename_data = self.rootdir + file_string + '.3d'
	fh_data  = open(fullfilename_data, 'w')
	for (x,y,z,intensity,ring) in cloud:
	    # print x,y,z
	    x, y, z = self.transformPoint(x, y, z)
	    x = x*self.scale_factor
	    y = y*self.scale_factor
	    z = z*self.scale_factor
	    str_ = format(x, '.1f')    + ' ' + format(z, '.1f')     + ' ' + format(y, '.1f')   + '\n'
	    fh_data.write(str_)
	fh_data.close()

    def savePose(self, x, y, z, roll, pitch, yaw):
	file_string = 'scan'+format(self.counter,'03d')
	fullfilename_pose = self.rootdir + file_string + '.pose'
	fh_pose  = open(fullfilename_pose, 'w')
	x = x*self.scale_factor
	y = y*self.scale_factor
	z = z*self.scale_factor
	roll  = roll*180/math.pi
	yaw   = yaw*180/math.pi
	pitch = pitch*180/math.pi
	str_first_line = format(x, '.1f')    + ' ' + format(-z, '.1f')     + ' ' + format(y, '.1f')   + '\n'
	str_2nd_line   = format(roll, '.1f') + ' ' + format(-yaw, '.1f') + ' ' + format(pitch, '.1f') + '\n'
	print str_first_line+str_2nd_line
	fh_pose.writelines([str_first_line, str_2nd_line])
	fh_pose.close()

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import frameExists [as 别名]
class calib:
    def __init__(self, *args):
        self.tf = TransformListener()
        self.detector_service = rospy.ServiceProxy("/fiducials/get_fiducials", DetectObjects)

        self.frame_camera_mount = rospy.get_param('~frame_camera_mount')
        self.frame_marker_mount = rospy.get_param('~frame_marker_mount')
        self.frame_marker = rospy.get_param('~frame_marker', "/marker")

    def compute(self):
        
        x_values = []
        y_values = []
        z_values = []
        roll_values = []
        pitch_values = []
        yaw_values = []
        
        count_success = 0
        count_failed = 0
        while count_success <= 15 and count_failed <= 100:
            try: 
                rospy.wait_for_service("/fiducials/get_fiducials", 3.0)
                res = self.detector_service(DetectObjectsRequest())
                
                if self.tf.frameExists(self.frame_camera_mount) and self.tf.frameExists(self.frame_marker_mount):
                    t = self.tf.getLatestCommonTime(self.frame_camera_mount, self.frame_marker_mount)
                    position, quaternion = self.tf.lookupTransform(self.frame_camera_mount, self.frame_marker_mount, t)
                    euler = tf.transformations.euler_from_quaternion(quaternion)
                    print '<origin xyz="'+str(position[0])+" "+str(position[1])+" "+str(position[2])+'" rpy="'+str(euler[0])+" "+str(euler[1])+" "+str(euler[2])+'" />'
                    x_values.append(position[0])
                    y_values.append(position[1])
                    z_values.append(position[2])
                    roll_values.append(euler[0])
                    pitch_values.append(euler[1])
                    yaw_values.append(euler[2])
                else:
                    print "tf does not exist!", self.tf.frameExists(self.frame_camera_mount), self.tf.frameExists(self.frame_marker_mount)
            except:
                print "did not detect marker."
                print "count_success = ", count_success
                print "count_failed = ", count_failed
                count_failed += 1
                continue
            count_success += 1
        
        if len(x_values) < 5:
            print "to few detections, aborting"
            return
                
        x_avg_value = (float)(sum(x_values))/len(x_values)
        y_avg_value = (float)(sum(y_values))/len(y_values)
        z_avg_value = (float)(sum(z_values))/len(z_values)
        roll_avg_value = (float)(sum(roll_values))/len(roll_values)
        pitch_avg_value = (float)(sum(pitch_values))/len(pitch_values)
        yaw_avg_value = (float)(sum(yaw_values))/len(yaw_values)

        print "The average values (without hardcoding) <xyz> are : \n\n" + '<origin xyz="' + str(x_avg_value) +" "+ str(-y_avg_value) +" "+  str(z_avg_value) + '" rpy="' + str(roll_avg_value) + " " + str(pitch_avg_value) + " " + str(yaw_avg_value) + '" />\n'
        
        # HACK set some DOf to fixed values
        z_avg_value = 1.80
        roll_avg_value = -3.1415926
        pitch_avg_value = 0
        yaw_avg_value = -3.1415926
        
        print "The average values<xyz> are : \n\n" + '<origin xyz="' + str(x_avg_value) +" "+ str(-y_avg_value) +" "+  str(z_avg_value) + '" rpy="' + str(roll_avg_value) + " " + str(pitch_avg_value) + " " + str(yaw_avg_value) + '" />\n'

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import frameExists [as 别名]
class i2oNode(object):
    def __init__(self, *args, **kwargs): #pylint: disable=unused-argument
        rospy.init_node('i2oNode', anonymous=True)

        self.tf = TransformListener()
        # print str(dir(self.tf))

        self.imu_to_baselink = None

        # PUB/SUB Setup
        self.pub = rospy.Publisher('imu2odom', Odometry, queue_size=10)
        self.init_sub = rospy.Subscriber('initialize_localization', Bool, queue_size=10)
        self.sub = rospy.Subscriber('imu', Imu, self.imu_callback)

        # state model
        self.baselink_model = State2D(0,0,0)
        self.imu_model = State2D(0,0,0)
        self.imu_zero = State2D(0,0,0)
        self.init_state = False

    # MAIN FUNCTION

    def listal(self): # listen-talk

        # run the node
        rate = rospy.Rate(10) # 10hz
        while not rospy.is_shutdown():
            rate.sleep()
        return 0

    # === TRANSITION FUNCTION ===

    def update_state_imu(self, state, imu_msg):
        # update state with new imu_information
        
        # Renamed state vars
        #    (time - time) => duration
        dt = (imu_msg.header.stamp - state.timestamp()).to_sec()
        final_orientation = self.quat_to_euler(imu_msg.orientation)
        # jerk
        twerk = self.twerk(state.theta(), final_orientation, state.omega(), state.alpha(), dt)
        
        # intermediate vars
        avg_heading = self.theta_avg(twerk, state.theta(), state.omega(), state.alpha(), dt)
        ang_acc = self.ang_acc(twerk, state.alpha(), dt)
        # lin_vel((a_measured, v0, dt))
        lin_vel = self.lin_vel(imu_msg.linear_acceleration.x, state.v(), dt)

        # assign final state
        state.stamp = imu_msg.header.timestamp
        state.point = self.linear_shift(state.x(), state.y(), avg_heading, state.v(), imu_msg.linear_acceleration.x, dt) # includes x, y
        state.heading = final_orientation # includes heading
        state.ang_acc = ang_acc # includes alpha
        state.ang_vel = Vector.from_Vector3(imu_msg.angular_velocity) # includes omega
        state.lin_vel = lin_vel # includes v
        state.lin_acc = Vector.from_Vector3(imu_msg.linear_acceleration) # includes a

        return state

    def update_state_init(self, state, imu_msg):
        # average state with new imu_information
        
        # Renamed state vars
        #    (time - time) => duration
        dt = (imu_msg.header.stamp - state.timestamp()).to_sec()
        final_orientation = self.quat_to_euler(imu_msg.orientation)
        # jerk
        twerk = self.twerk(state.theta(), final_orientation, state.omega(), state.alpha(), dt)
        
        # intermediate vars
        avg_heading = self.theta_avg(twerk, state.theta(), state.omega(), state.alpha(), dt)
        ang_acc = self.ang_acc(twerk, state.alpha(), dt)
        # lin_vel((a_measured, v0, dt))
        lin_vel = self.lin_vel(imu_msg.linear_acceleration.x, state.v(), dt)

        # average final state
        # so the values collected during the init are a moving average
        # this all should be pretty stable around 0, because the robot should be stopped
        # also, all state models should use this to zero-out their models
        state.point = Vector.average(state.point, self.linear_shift(state.x(), state.y(), avg_heading, state.v(), imu_msg.linear_acceleration.x, dt)) # includes x, y
        state.heading = state.heading/2 + final_orientation/2 # includes heading
        state.ang_acc = Vector.average(state.ang_acc, ang_acc) # includes alpha
        state.ang_vel = Vector.average(state.ang_vel, Vector.from_Vector3(imu_msg.angular_velocity)) # includes omega
        state.lin_vel = Vector.average(state.lin_vel, lin_vel) # includes v
        state.lin_acc = Vector.average(state.lin_acc, Vector.from_Vector3(imu_msg.linear_acceleration)) # includes a

        return state

    # DATA CALLBACK

    def imu_callback(self, imu_msg):
        # TODO(buckbaskin): implement initialization/zeroing, not necessarily here

        if self.init_state:
            # do initializations
            self.imu_zero = self.update_state_init(self.imu_zero, imu_msg)

        else:
            # lookup Transformation here because of unknown imu frame until callback
            if self.tf.frameExists('/base_link') and self.tf.frameExists(imu_msg.header.frame_id): #pylint: disable=no-member
#.........这里部分代码省略.........

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import frameExists [as 别名]
class KeyPointManager(object):
    def __init__(self):
        self.tf = TransformListener()
        self.keyPointList = list()
        
    def add(self, marker):
        for i in range(len(self.keyPointList)):
            if (self.keyPointList[i].id==marker.id):
                return
        position = self.transformMarkerToWorld(marker)
        k = KeyPoint(marker.id, Point(position[0], position[1], position[2]), Quaternion(0.,  0.,  0.,  1.))
        self.keyPointList.append(k)
        self.addWaypointMarker(k)
        rospy.loginfo('Marker is added to following position')
        rospy.loginfo(k.pose)
        pass
    def getWaypoints(self):
        waypoints = list()
        for i in range(len(self.keyPointList)):
            waypoints.append(self.keyPointList[i].pose);
        return waypoints
            
    def keyPointListComplete(self):
        if (len(self.keyPointList)==5):
            self.keyPointList.sort(key=lambda x: x.id, reverse=True)
            return True
        return False
    def markerHasValidId(self, marker):
        if (marker.id>=61) and (marker.id<=65):
            return True
        return False
    def transformMarkerToWorld(self, marker):
        markerTag = "ar_marker_"+str(marker.id )
        rospy.loginfo(markerTag);
        if self.tf.frameExists("map") and self.tf.frameExists(markerTag):
            self.tf.waitForTransform("map", markerTag, rospy.Time(), rospy.Duration(4.0))
            t = self.tf.getLatestCommonTime("map", markerTag)
            position, quaternion = self.tf.lookupTransform("map", markerTag, t)
            return self.shiftPoint( position, quaternion)
    def shiftPoint(self, position, quaternion):
        try:
            euler = euler_from_quaternion(quaternion)
            yaw = euler[2]
            tf_mat = np.array([[np.cos(yaw), -np.sin(yaw), 0, position[0]],[np.sin(yaw), np.cos(yaw), 0, position[1]],[0, 0, 1, 0],[0, 0, 0, 1]])
            displacement = np.array([[1, 0, 0, 0],[0, 1, 0, 0.35],[0, 0, 1, 0],[0, 0, 0, 1]])
            point_map = np.dot(tf_mat, displacement)
            position = (point_map[0,3], point_map[1,3], 0)
        except Exception as inst:
            print type(inst)     # the exception instance
            print inst.args      # arguments stored in .args
            print inst
        return position
    def addWaypointMarker(self, keyPoint):
        rospy.loginfo('Publishing marker')
        # Set up our waypoint markers
        marker_scale = 0.2
        marker_lifetime = 0 # 0 is forever
        marker_ns = 'waypoints'
        marker_id = keyPoint.id
        marker_color = {'r': 0.0, 'g': 1.0, 'b': 0.0, 'a': 1.0}
        
        # Initialize the marker points list.
        self.waypoint_markers = Marker()
        self.waypoint_markers.ns = marker_ns
        self.waypoint_markers.id = marker_id
        self.waypoint_markers.type = Marker.CUBE_LIST
        self.waypoint_markers.action = Marker.ADD
        self.waypoint_markers.lifetime = rospy.Duration(marker_lifetime)
        self.waypoint_markers.scale.x = marker_scale
        self.waypoint_markers.scale.y = marker_scale
        self.waypoint_markers.color.r = marker_color['r']
        self.waypoint_markers.color.g = marker_color['g']
        self.waypoint_markers.color.b = marker_color['b']
        self.waypoint_markers.color.a = marker_color['a']
        
        self.waypoint_markers.header.frame_id = 'map'
        self.waypoint_markers.header.stamp = rospy.Time.now()
        self.waypoint_markers.points = list()
        p = Point(keyPoint.pose.position.x, keyPoint.pose.position.y, keyPoint.pose.position.z)
        self.waypoint_markers.points.append(p)

        # Publish the waypoint markers
        self.marker_pub = rospy.Publisher('waypoint_markers', Marker)

        self.marker_pub.publish(self.waypoint_markers)

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import frameExists [as 别名]
class GripperMarkers:

    _offset = 0.09
    def __init__(self, side_prefix):

        
        self.side_prefix = side_prefix
        self._im_server = InteractiveMarkerServer('ik_request_markers_' + self.side_prefix)
        self._tf_listener = TransformListener()
        self._menu_handler = MenuHandler()

        self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb)

        self.r_joint_names = ['r_shoulder_pan_joint',
                              'r_shoulder_lift_joint',
                              'r_upper_arm_roll_joint',
                              'r_elbow_flex_joint',
                              'r_forearm_roll_joint',
                              'r_wrist_flex_joint',
                              'r_wrist_roll_joint']
        self.l_joint_names = ['l_shoulder_pan_joint',
                              'l_shoulder_lift_joint',
                              'l_upper_arm_roll_joint',
                              'l_elbow_flex_joint',
                              'l_forearm_roll_joint',
                              'l_wrist_flex_joint',
                              'l_wrist_roll_joint']

        # Create a trajectory action client
        r_traj_contoller_name = None
        l_traj_contoller_name = None
        if self.side_prefix == 'r':
                r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
                self.r_traj_action_client = SimpleActionClient(r_traj_controller_name,
                        JointTrajectoryAction)
                rospy.loginfo('Waiting for a response from the trajectory '
                        + 'action server for RIGHT arm...')
                self.r_traj_action_client.wait_for_server()
        
        else:
                l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
                self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
                        JointTrajectoryAction)
                rospy.loginfo('Waiting for a response from the trajectory '
                        + 'action server for LEFT arm...')
                self.l_traj_action_client.wait_for_server()

        self.is_control_visible = False
        self.ee_pose = self.get_ee_pose()
        self.ik = IK(side_prefix)
        self.update_viz()
        self._menu_handler.apply(self._im_server, 'ik_target_marker_' + self.side_prefix)
        self._im_server.applyChanges()
        print self.ik

    def get_ee_pose(self):
        from_frame = 'base_link'
        to_frame = self.side_prefix + '_wrist_roll_link'
        try:
            if self._tf_listener.frameExists(from_frame) and self._tf_listener.frameExists(to_frame):
                t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
                # t = rospy.Time.now()
                (pos, rot) = self._tf_listener.lookupTransform(to_frame, from_frame, t) # Note argument order :(
            else:
                rospy.logerr('TF frames do not exist; could not get end effector pose.')
        except Exception as err:
            rospy.logerr('Could not get end effector pose through TF.')
            rospy.logerr(err)
            pos = [1.0, 0.0, 1.0]
            rot = [0.0, 0.0, 0.0, 1.0]

        return Pose(Point(pos[0], pos[1], pos[2]),
                Quaternion(rot[0], rot[1], rot[2], rot[3]))

    def move_to_pose_cb(self, feedback):
        rospy.loginfo('You pressed the `Move arm here` button from the menu.')
        '''Moves the arm to the desired joints'''
        print feedback
        time_to_joint = 2.0
        positions = self.ik.get_ik_for_ee(feedback.pose)
        velocities = [0] * len(positions)
        traj_goal = JointTrajectoryGoal()
        traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
        traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions,
                            velocities=velocities, time_from_start=rospy.Duration(time_to_joint)))
        
        if (self.side_prefix == 'r'):
            traj_goal.trajectory.joint_names = self.r_joint_names
            self.r_traj_action_client.send_goal(traj_goal)
        else:
            traj_goal.trajectory.joint_names = self.l_joint_names
            self.l_traj_action_client.send_goal(traj_goal)
        pass

    def marker_clicked_cb(self, feedback):
        if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
            self.ee_pose = feedback.pose
            self.update_viz()
            self._menu_handler.reApply(self._im_server)
            self._im_server.applyChanges()
#.........这里部分代码省略.........