bag_utils.py 文件源码

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