Python tf.TransformListener类代码示例

def subscriber_callback(data):
	occupancyMap = transformation(data.data, data.info.width, data.info.height)
	way_points = find_goals(occupancyMap, data.info.width, data.info.height)
	result = random.choice(way_points)

	try:
		planMaker = rospy.ServiceProxy('move_base/make_plan', nav_srv.GetPlan)
		listener = TransformListener()
		listener.waitForTransform("base_link", "map", rospy.Time(), rospy.Duration(4.0))
		t = listener.getLatestCommonTime("base_link", "map")
		position, quaternion = listener.lookupTransform("base_link", "map", t)
		pos_x = position[0]
		pos_y = position[1]
		pos_z = position[2]
		goal_robot = create_goal((result[1]-2000)*data.info.resolution,(result[0]-2000)*data.info.resolution)
		#Make plan with 0.5 meter flexibility, from target pose and current pose (with same header)
		start_pose = create_message(pos_x,pos_y,pos_z,quaternion)

		plan = planMaker(
			start_pose,
			goal_robot.target_pose,
			0.5)
		action_server = actionlib.SimpleActionClient('move_base', mb_msg.MoveBaseAction);
		action_server.wait_for_server()
		send_goal(goal_robot, action_server)

	except rospy.ServiceException, e:
		print "plan service call failed: %s"%e

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

class FTPNode:
    def __init__(self, *args):
        print("init")
        self.tf = TransformListener()
        self.tt = Transformer()
        rospy.Subscriber("/trackedHumans", TrackedHumans, self.pos_callback)
        self.publisher = rospy.Publisher("directionmarker_array", MarkerArray)


    def pos_callback(self, data):
        rospy.loginfo("on updated pos, face robot towards guy...")
        print("hi")
        if (len(data.trackedHumans) > 0):
            print(data.trackedHumans[0].location.point.x)
            try:
                self.tf.waitForTransform(data.trackedHumans[0].location.header.frame_id, "/base_link", rospy.Time.now(), rospy.Duration(2.0))
                pp = self.tf.transformPoint("/base_link", data.trackedHumans[0].location)
                print "Original:"
                print [data.trackedHumans[0].location.point]
                print "Transform:"
                print pp.point

                phi = math.atan2(pp.point.y, pp.point.x)
                sss.move_base_rel("base", [0,0,phi])
                print phi*180/math.pi
                
                markerArray = MarkerArray()
                marker = Marker()
                marker.header.stamp = rospy.Time();    
                marker.ns = "my_namespace";
                marker.id = 0;  
                marker.header.frame_id = "/base_link"
                marker.type = marker.ARROW
                marker.action = marker.ADD
                marker.scale.x = .1
                marker.scale.y = .1
                marker.scale.z = .1
                marker.color.a = 1.0
                marker.color.r = 1.0
                marker.color.g = 1.0
                marker.color.b = 0.0
                p1 = Point()
                p1.x = 0
                p1.y = 0
                p1.z = 0
                p2 = Point()
                p2.x = pp.point.x
                p2.y = pp.point.y
                p2.z = 0
                marker.points = [p1,p2]
                #marker.pose.position.x = 1
                #marker.pose.position.y = 0
                #marker.pose.position.z = 1
                #marker.pose.orientation.w = 1
                markerArray.markers.append(marker)
                self.publisher.publish(markerArray)
                print "try ended"
            except Exception as e:
                print e

def calculate_distance_to_rows():
    tflisten = TransformListener()
    dist = []
    for i in range(0,n_rows):
        (veh_trans,veh_rot) = tflisten.lookupTransform("odom","base_footprint",rospy.Time(0))
        
        
    pass

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

class ForceFromGravity(object):
    def __init__(self):
        self.tf_l = TransformListener()
        self.last_wrench = None
        self.wrench_sub = rospy.Subscriber('/left_wrist_ft',
                                           WrenchStamped,
                                           self.wrench_cb,
                                           queue_size=1)
    def wrench_cb(self, data):
        self.last_wrench = data

    def compute_forces(self):
        qs = self.get_orientation()
        o = qs.quaternion
        r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
        rospy.loginfo("wrist_left_ft_link rpy vs world: " + str( (round(r, 3), round(p, 3), round(y, 3)) ))
        rospy.loginfo("in degrees: " + str( (round(degrees(r), 3), round(degrees(p), 3), round(degrees(y), 3)) ))
        hand_total_force = 10 # 10 Newton, near to a Kg
        fx = hand_total_force * cos(r) * -1.0 # hack
        fy = hand_total_force * cos(p)
        fz = hand_total_force * cos(y)
        #total = abs(fx) + abs(fy) + abs(fz)
        #rospy.loginfo("fx, fy, fz, total:")
        #rospy.loginfo( str( (round(fx, 3), round(fy, 3), round(fz, 3), round(total, 3)) ) )
        return fx, fy, fz

    def get_last_forces(self):
        f = self.last_wrench.wrench.force
        return f.x, f.y, f.z

    def get_orientation(self, from_frame="wrist_left_ft_link"):
        qs = QuaternionStamped()
        qs.quaternion.w = 1.0
        qs.header.stamp = self.tf_l.getLatestCommonTime("world", from_frame)
        qs.header.frame_id = "/" + from_frame
        transform_ok = False
        while not transform_ok and not rospy.is_shutdown():
            try:
                world_q = self.tf_l.transformQuaternion("/world", qs)
                transform_ok = True
                return world_q
            except ExtrapolationException as e:
                rospy.logwarn(
                    "Exception on transforming pose... trying again \n(" + str(e) + ")")
                rospy.sleep(0.05)
                qs.header.stamp = self.tf_l.getLatestCommonTime(
                    "world", from_frame)

    def run(self):
        r = rospy.Rate(1)
        while not rospy.is_shutdown():
            cx, cy, cz = self.compute_forces()
            c_total = abs(cx) + abs(cy) + abs(cz)
            fx, fy, fz = self.get_last_forces()
            f_total = abs(fx) + abs(fy) + abs(fz)
            rospy.loginfo("Computed Forces:" + str(round(c_total, 3)) + "\n" + str( (round(cx, 3), round(cy, 3), round(cz, 3) )))
            rospy.loginfo("Real Forces    :" + str(round(f_total, 3)) + "\n" + str( (round(fx, 3), round(fy, 3), round(fz, 3) )))
            r.sleep()

class TfFilter:
    def __init__(self, buffer_size):
        self.tf = TransformListener(True, rospy.Duration(5))
        self.target = ''
        self.buffer = np.zeros((buffer_size, 1))
        self.buffer_ptr = 0
        self.buffer_size = buffer_size
        self.tf_sub = rospy.Subscriber('tf', tfMessage, self.tf_cb)
        self.tf_pub = rospy.Publisher('tf', tfMessage)
        self.srv = rospy.Service(SERVICE, PublishGoal, self.publish_goal_cb)

    def tf_cb(self, msg):
        for t in msg.transforms:
            if t.child_frame_id == self.target:
                time = self.tf.getLatestCommonTime(self.source, self.target)
                p, q = self.tf.lookupTransform(self.source, self.target, time)
                rm = tfs.quaternion_matrix(q)
                # assemble a new coordinate frame that has z-axis aligned with
                # the vertical direction and x-axis facing the z-axis of the
                # original frame
                z = rm[:3, 2]
                z[2] = 0
                axis_x = tfs.unit_vector(z)
                axis_z = tfs.unit_vector([0, 0, 1])
                axis_y = np.cross(axis_x, axis_z)
                rm = np.vstack((axis_x, axis_y, axis_z)).transpose()
                # shift the pose along the x-axis
                self.position = p + np.dot(rm, self.d)[:3]
                self.buffer[self.buffer_ptr] = tfs.euler_from_matrix(rm)[2]
                self.buffer_ptr = (self.buffer_ptr + 1) % self.buffer_size
                self.publish_filtered_tf(t.header)

    def publish_filtered_tf(self, header):
        yaw = np.median(self.buffer)
        q = tfs.quaternion_from_euler(0, 0, yaw - math.pi)
        ts = TransformStamped()
        ts.header = header
        ts.header.frame_id = self.source
        ts.child_frame_id = self.goal
        ts.transform.translation.x = self.position[0]
        ts.transform.translation.y = self.position[1]
        ts.transform.translation.z = 0
        ts.transform.rotation.x = q[0]
        ts.transform.rotation.y = q[1]
        ts.transform.rotation.z = q[2]
        ts.transform.rotation.w = q[3]
        msg = tfMessage()
        msg.transforms.append(ts)
        self.tf_pub.publish(msg)

    def publish_goal_cb(self, r):
        rospy.loginfo('Received [%s] request. Will start publishing %s frame' %
                      (SERVICE, r.goal_frame_id))
        self.source = r.source_frame_id
        self.target = r.target_frame_id
        self.goal = r.goal_frame_id
        self.d = [r.displacement.x, r.displacement.y, r.displacement.z]
        return []

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

class CameraPointer(object):
    def __init__(self, side, camera_ik):
        self.side = side
        self.camera_ik = camera_ik
        self.joint_names = self.joint_angles_act = self.joint_angles_des = None
        self.tfl = TransformListener()
        self.joint_state_sub = rospy.Subscriber('/{0}_arm_controller/state'.format(self.side), JointTrajectoryControllerState, self.joint_state_cb)
        self.joint_traj_pub = rospy.Publisher('/{0}_arm_controller/command'.format(self.side), JointTrajectory)
        # Wait for joint information to become available
        while self.joint_names is None and not rospy.is_shutdown():
            rospy.sleep(0.5)
            rospy.loginfo("[{0}] Waiting for joint state from arm controller.".format(rospy.get_name()))
        #Set rate limits on a per-joint basis
        self.max_vel_rot = [np.pi]*len(self.joint_names)
        self.target_sub = rospy.Subscriber('{0}/lookat_ik/goal'.format(rospy.get_name()), PointStamped, self.goal_cb)
        rospy.loginfo("[{0}] Ready.".format(rospy.get_name()))

    def joint_state_cb(self, jtcs):
        if self.joint_names is None:
            self.joint_names = jtcs.joint_names
        self.joint_angles_act = jtcs.actual.positions
        self.joint_angles_des = jtcs.desired.positions

    def goal_cb(self, pt_msg):
        rospy.loginfo("[{0}] New LookatIK goal received.".format(rospy.get_name()))
        if (pt_msg.header.frame_id != self.camera_ik.base_frame):
            try:
                now = pt_msg.header.stamp
                self.tfl.waitForTransform(pt_msg.header.frame_id,
                                          self.camera_ik.base_frame,
                                          now, rospy.Duration(1.0))
                pt_msg = self.tfl.transformPoint(self.camera_ik.base_frame, pt_msg)
            except (LookupException, ConnectivityException, ExtrapolationException):
                rospy.logwarn("[{0}] TF Error tranforming point from {1} to {2}".format(rospy.get_name(),
                                                                                        pt_msg.header.frame_id,
                                                                                        self.camera_ik.base_frame))
        target = np.array([pt_msg.point.x, pt_msg.point.y, pt_msg.point.z])
        # Get IK Solution
        current_angles = list(copy.copy(self.joint_angles_act))
        iksol = self.camera_ik.lookat_ik(target, current_angles[:len(self.camera_ik.arm_indices)])
        # Start with current angles, then replace angles in camera IK with IK solution
        # Use desired joint angles to avoid sagging over time
        jtp = JointTrajectoryPoint()
        jtp.positions = list(copy.copy(self.joint_angles_des))
        for i, joint_name in enumerate(self.camera_ik.arm_joint_names):
            jtp.positions[self.joint_names.index(joint_name)] = iksol[i]
        deltas = np.abs(np.subtract(jtp.positions, current_angles))
        duration = np.max(np.divide(deltas, self.max_vel_rot))
        jtp.time_from_start = rospy.Duration(duration)
        jt = JointTrajectory()
        jt.joint_names = self.joint_names
        jt.points.append(jtp)
        rospy.loginfo("[{0}] Sending Joint Angles.".format(rospy.get_name()))
        self.joint_traj_pub.publish(jt)

def main():
    
    rospy.init_node('tf_test')
    rospy.loginfo("Node for testing actionlib server")
 
    #from_link = '/head_color_camera_l_link'
    #to_link = '/base_link'
    
    from_link = '/base_link'
    to_link = '/map'
    
    tfl = TransformListener()
    
    rospy.sleep(5)
    
    
    t = rospy.Time(0)
    
    mpose = PoseStamped()
    
    mpose.pose.position.x = 1
    mpose.pose.position.y = 0
    mpose.pose.position.z = 0
    
    mpose.pose.orientation.x = 0
    mpose.pose.orientation.y = 0
    mpose.pose.orientation.z = 0
    mpose.pose.orientation.w = 0
    
    mpose.header.frame_id = from_link
    mpose.header.stamp = rospy.Time.now()
    
    rospy.sleep(5)
    
    mpose_transf = None
    
    rospy.loginfo('Waiting for transform for some time...')
    
    tfl.waitForTransform(to_link,from_link,t,rospy.Duration(5))
    
    if tfl.canTransform(to_link,from_link,t):
      
      mpose_transf = tfl.transformPose(to_link,mpose)
      
      print mpose_transf
      
    else:
      
      rospy.logerr('Transformation is not possible!')
      sys.exit(0)

 def __init__(self):
     self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1)
     self.listener = TransformListener()
     rospy.Subscriber("joy", Joy, self._joyChanged)
     rospy.Subscriber("cmd_vel_telop", Twist, self._cmdVelTelopChanged)
     self.cmd_vel_telop = Twist()
     #self.pidX = PID(20, 12, 0.0, -30, 30, "x")
     #self.pidY = PID(-20, -12, 0.0, -30, 30, "y")
     #self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z")
     #self.pidYaw = PID(50.0, 0.0, 0.0, -200.0, 200.0, "yaw")
     self.pidX = PID(20, 12, 0.0, -30, 30, "x")
     self.pidY = PID(-20, 12, 0.0, -15, 15, "y")
     #50000  800
     self.pidZ = PID(15000, 3000.0,  1500.0, 10000, 60000, "z")
     self.pidYaw = PID(50.0, 0.0, 0.0, -100.0, 100.0, "yaw")
     self.state = Controller.Manual
     #Target Values
     self.pubtarX = rospy.Publisher('target_x', Float32, queue_size=1)
     self.pubtarY = rospy.Publisher('target_y', Float32, queue_size=1)
     self.pubtarZ = rospy.Publisher('target_z', Float32, queue_size=1)
     self.targetZ = 0.5
     self.targetX = 0.0
     self.targetY = 0.0
     self.des_angle = 0.0
     #self.power = 50000.0
     #Actual Values
     self.pubrealX = rospy.Publisher('real_x', Float32, queue_size=1)
     self.pubrealY = rospy.Publisher('real_y', Float32, queue_size=1)
     self.pubrealZ = rospy.Publisher('real_z', Float32, queue_size=1)
     self.lastJoy = Joy()

    def __init__(self, use_dummy_transform=False):
        print "init"
        rospy.init_node("star_center_positioning_node")
        if use_dummy_transform:
            self.odom_frame_name = ROBOT_NAME + "_odom_dummy"  # identify this odom as ROBOT_NAME's
        else:
            self.odom_frame_name = ROBOT_NAME + "_odom"  # identify this odom as ROBOT_NAME's

        self.marker_locators = {}

        self.add_marker_locator(MarkerLocator(0, (-6 * 12 * 2.54 / 100.0, 0), 0))
        self.add_marker_locator(MarkerLocator(1, (0.0, 0.0), pi))
        self.add_marker_locator(MarkerLocator(2, (0.0, 10 * 12 * 2.54 / 100.0), 0))
        self.add_marker_locator(MarkerLocator(3, (-6 * 12 * 2.54 / 100.0, 6 * 12 * 2.54 / 100.0), 0))

        self.pose_correction = rospy.get_param("~pose_correction", 0.0)

        self.marker_sub = rospy.Subscriber("ar_pose_marker", ARMarkers, self.process_markers)
        self.odom_sub = rospy.Subscriber(
            ROBOT_NAME + "/odom", Odometry, self.process_odom, queue_size=10
        )  # publish and subscribe to the correct robot's topics
        self.star_pose_pub = rospy.Publisher(ROBOT_NAME + "/STAR_pose", PoseStamped, queue_size=10)
        self.continuous_pose = rospy.Publisher(ROBOT_NAME + "/STAR_pose_continuous", PoseStamped, queue_size=10)
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()

 def __init__(self, goals):
     rospy.init_node("demo", anonymous=True)
     self.frame = rospy.get_param("~frame")
     self.pubGoal = rospy.Publisher("goal", PoseStamped, queue_size=1)
     self.listener = TransformListener()
     self.goals = goals
     self.goalIndex = 0

	def __init__(self):
		cv2.namedWindow("map")
		cv2.namedWindow("past_map")
		rospy.init_node("run_mapping")	
		#create map properties, helps to make ratio calcs
		self.origin = [-10,-10]
		self.seq = 0
		self.resolution = 0.1
		self.n = 200

		self.pose = []
		self.dyn_obs=[]
		self.obstacle = []
		self.rapid_appear = set()
		self.counter=0

		#Giving initial hypotheses to the system
		self.p_occ = 0.5*np.ones((self.n, self.n)) #50-50 chance of being occupied
		self.odds_ratio_hit = 3.0 #this is arbitrary, can re-assign
		self.odds_ratio_miss = 0.3 #this is arbitrary, can reassign

		#calculates odds based upon hit to miss, equal odds to all grid squares
		self.odds_ratios = (self.p_occ)/(1-self.p_occ)*np.ones((self.n, self.n))
		#calculate initial past odds_ratio
		self.past_odds_ratios = (self.p_occ)/(1-self.p_occ)*np.ones((self.n, self.n))

		#write laser pubs and subs
		rospy.Subscriber("scan", LaserScan, self.scan_received, queue_size=1)
		self.pub = rospy.Publisher("map", OccupancyGrid)

		#note - in case robot autonomy is added back in
		self.tf_listener = TransformListener()	

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