Python tf 模块，ExtrapolationException() 实例源码

def robot_listener(self):
        '''
        rospy.wait_for_service('spawn')
        spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
        spawner(4, 2, 0, 'turtle2')
        '''
        robot_vel_pub = rospy.Publisher('robot_0/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)

        rate = rospy.Rate(10.0)
        while not rospy.is_shutdown():
            try:
                (trans,rot) = self.listener.lookupTransform('/robot_3', '/robot_0', rospy.Time(0))
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue
            angular = 4 * math.atan2(trans[1], trans[0])
            linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
            cmd = geometry_msgs.msg.Twist()
            cmd.linear.x = linear
            cmd.angular.z = angular
            robot_vel_pub.publish(cmd)

            rate.sleep()

def printJointStates(self):
        try:
            # now = rospy.Time.now()
            # self.tf.waitForTransform(self.frame, 'r_gripper_tool_frame', now, rospy.Duration(10.0))
            self.tf.waitForTransform(self.frame, 'r_gripper_tool_frame', rospy.Time(), rospy.Duration(10.0))
            currentRightPos, currentRightOrient = self.tf.lookupTransform(self.frame, 'r_gripper_tool_frame', rospy.Time(0))

            msg = rospy.wait_for_message('/r_arm_controller/state', JointTrajectoryControllerState)
            currentRightJointPositions = msg.actual.positions
            print 'Right positions:', currentRightPos, currentRightOrient
            print 'Right joint positions:', currentRightJointPositions

            # now = rospy.Time.now()
            # self.tf.waitForTransform(self.frame, 'l_gripper_tool_frame', now, rospy.Duration(10.0))
            currentLeftPos, currentLeftOrient = self.tf.lookupTransform(self.frame, 'l_gripper_tool_frame', rospy.Time(0))

            msg = rospy.wait_for_message('/l_arm_controller/state', JointTrajectoryControllerState)
            currentLeftJointPositions = msg.actual.positions
            print 'Left positions:', currentLeftPos, currentLeftOrient
            print 'Left joint positions:', currentLeftJointPositions

            # print 'Right gripper state:', rospy.wait_for_message('/r_gripper_controller/state', JointControllerState)
        except tf.ExtrapolationException:
            print 'No transformation available! Failing to record this time step.'

def markerCB(self, msg):
        try:
            self.marker_lock.acquire()
            if not self.initialize_poses:
                return
            self.initial_poses = {}
            for marker in msg.markers:
                if marker.name.startswith("EE:goal_"):
                    # resolve tf
                    if marker.header.frame_id != self.frame_id:
                        ps = PoseStamped(header=marker.header, pose=marker.pose)
                        try:
                            transformed_pose = self.tf_listener.transformPose(self.frame_id, ps)
                            self.initial_poses[marker.name[3:]] = transformed_pose.pose
                        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, e):
                            rospy.logerr("tf error when resolving tf: %s" % e)
                    else:
                        self.initial_poses[marker.name[3:]] = marker.pose   #tf should be resolved
        finally:
            self.marker_lock.release()

def goalCB(self, poseStamped):
        # reset timestamp because of bug: https://github.com/ros/geometry/issues/82
        poseStamped.header.stamp = rospy.Time(0)
        try:
            robotToTarget1 = self.listener.transformPose("/base_footprint", poseStamped)
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
            rospy.logerr("Error while transforming pose: %s", str(e))
            return
        quat = robotToTarget1.pose.orientation
        (roll,pitch,yaw) = euler_from_quaternion((quat.x, quat.y, quat.z, quat.w))
        self.motionProxy.moveTo(robotToTarget1.pose.position.x, robotToTarget1.pose.position.y, yaw)

def transform_pose(self, pose):
        try:
            new_pose = self.listener.transformPose(self.frame_id, pose)
            # now new_pose should be the pose of the tag in the odom_meas frame.
            # Let's ignore the height, and rotations not about the z-axis
            new_pose.pose.position.z = 0
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            return None
        return new_pose

def go_to_pose(self, name, T, timeout):
        msg = convert_to_trans_message(T)
        self.update_marker(T)
        start_time = time.time()
        done = False
        while not done and not rospy.is_shutdown():
            self.pub_cmd.publish(msg)
            try:
                (trans,rot) = self.listener.lookupTransform('/world_link','lwr_arm_7_link',
                                                            rospy.Time(0))
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                print "TF Exception!"
                continue

            TR = numpy.dot(tf.transformations.translation_matrix(trans), 
                           tf.transformations.quaternion_matrix(rot))

            if (is_same(T, TR)): 
                print name + ": Reached desired pose"
                done = True

            if (time.time() - start_time > timeout) :
                print name + ": Robot took too long to reach desired pose"
                done = True
            else:
                rospy.sleep(0.1)

def goto_pose(self, name, T, timeout):
        self.server.setPose("move_arm_marker", convert_to_message(T))
        self.server.applyChanges()
        self.pub_command.publish(convert_to_trans_message(T))
        print 'Goal published'        
        start_time = time.time()
        done = False
        while not done and not rospy.is_shutdown():

            self.mutex.acquire()
            last_joint_state = deepcopy(self.joint_state)
            self.mutex.release()
            if not self.check_validity(last_joint_state):
                print 'COLLISION!'

            try:
                (trans,rot) = self.listener.lookupTransform('/world_link','/lwr_arm_7_link',
                                                            rospy.Time(0))
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                print "TF Exception!"
                continue

            TR = numpy.dot(tf.transformations.translation_matrix(trans), 
                           tf.transformations.quaternion_matrix(rot))

            if (is_same(T, TR)): 
                print name + ": Reached goal"
                done = True

            if (time.time() - start_time > timeout) :
                done = True
                print name + ": Robot took too long to reach goal. Grader timed out"
            else:
                rospy.sleep(0.05)

def establish_tfs(self, relations):
        """
        Perform a one-time look up of all the requested
        *static* relations between frames (available via /tf)
        """

        # Init node and tf listener
        rospy.init_node(self.__class__.__name__, disable_signals=True)
        tf_listener = tf.TransformListener()

        # Create tf decoder
        tf_dec = TfDecoderAndPublisher(channel='/tf')

        # Establish tf relations
        print('{} :: Establishing tfs from ROSBag'.format(self.__class__.__name__))
        for self.idx, (channel, msg, t) in enumerate(self.log.read_messages(topics='/tf')): 
            tf_dec.decode(msg)
            for (from_tf, to_tf) in relations:  

                # If the relations have been added already, skip
                if (from_tf, to_tf) in self.relations_map_: 
                    continue

                # Check if the frames exist yet
                if not tf_listener.frameExists(from_tf) or not tf_listener.frameExists(to_tf): 
                    continue

                # Retrieve the transform with common time
                try:
                    tcommon = tf_listener.getLatestCommonTime(from_tf, to_tf)
                    (trans,rot) = tf_listener.lookupTransform(from_tf, to_tf, tcommon)
                    self.relations_map_[(from_tf,to_tf)] = RigidTransform(tvec=trans, xyzw=rot)
                    # print('\tSuccessfully received transform: {:} => {:} {:}'
                    #       .format(from_tf, to_tf, self.relations_map_[(from_tf,to_tf)]))
                except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
                    # print e
                    pass

            # Finish up once we've established all the requested tfs
            if len(self.relations_map_) == len(relations): 
                break

        try: 
            tfs = [self.relations_map_[(from_tf,to_tf)] for (from_tf, to_tf) in relations] 
            for (from_tf, to_tf) in relations: 
                print('\tSuccessfully received transform:\n\t\t {:} => {:} {:}'
                      .format(from_tf, to_tf, self.relations_map_[(from_tf,to_tf)]))

        except: 
            raise RuntimeError('Error concerning tf lookup')
        print('{} :: Established {:} relations\n'.format(self.__class__.__name__, len(tfs)))

        return tfs