Python tf.LookupException方法代码示例

# 需要导入模块: import tf [as 别名]
# 或者: from tf import LookupException [as 别名]
def tick(self):
        try:
            (trans,rot) = self.listener.lookupTransform('/map', '/base_link', rospy.Time(0))
            msg = self.get_link_state(link_name="base_link")
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            return False

        self.pose = msg.link_state.pose
        self.trans, self.rot = trans, rot
        #quaternion = (rot[0], rot[1], rot[2], rot[3])
        orientation = self.pose.orientation
        quaternion = (orientation.x, orientation.y, orientation.z,
                orientation.w)
        euler = tf.transformations.euler_from_quaternion(quaternion)
        self.roll = euler[0]
        self.pitch = euler[1]
        self.yaw = euler[2]
        return True 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import LookupException [as 别名]
def _wait_for_transform_tf2(self,
                                target_frame, source_frame,
                                time, timeout):
        try:
            ret = self.tf_listener.can_transform(
                target_frame, source_frame, time, timeout, True)
            if ret[0] > 0:
                return True
            else:
                raise Exception(ret[1])
        except (tf2_ros.LookupException,
                tf2_ros.ConnectivityException,
                tf2_ros.ExtrapolationException,
                tf2_ros.TransformException,
                rospy.exceptions.ROSTimeMovedBackwardsException):
            return False 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import LookupException [as 别名]
def _lookup_transform_tf1(self, target_frame, source_frame, time, timeout):
        self._wait_for_transform_tf1(target_frame, source_frame, time, timeout)
        try:
            res = self.tf_listener.lookupTransform(
                target_frame, source_frame, time)
            if time.is_zero():
                time = self.tf_listener.getLatestCommonTime(
                    target_frame, source_frame)
        except (tf.LookupException,
                tf.ConnectivityException,
                tf.ExtrapolationException,
                rospy.exceptions.ROSTimeMovedBackwardsException):
            return False
        ret = geometry_msgs.msg.TransformStamped()
        ret.header.frame_id = target_frame
        ret.header.stamp = time
        ret.child_frame_id = source_frame
        ret.transform.translation.x = res[0][0]
        ret.transform.translation.y = res[0][1]
        ret.transform.translation.z = res[0][2]
        ret.transform.rotation.x = res[1][0]
        ret.transform.rotation.y = res[1][1]
        ret.transform.rotation.z = res[1][2]
        ret.transform.rotation.w = res[1][3]
        return ret 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import LookupException [as 别名]
def get_position(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return Point(*trans) 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import LookupException [as 别名]
def get_odom(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans), quat_to_angle(Quaternion(*rot))) 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import LookupException [as 别名]
def get_odom_angle(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return
        
        # Convert the rotation from a quaternion to an Euler angle
        return quat_to_angle(Quaternion(*rot)) 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import LookupException [as 别名]
def _image_callback(self, image_msg):
        try:
            self.quad_to_obj_pos, self.quad_to_obj_rot = self.listener.lookupTransform('bebop', 'car', image_msg.header.stamp)
            self.quad_pos, self.quad_rot = self.listener.lookupTransform('world', 'bebop', image_msg.header.stamp)
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            return
        self.image = self.cv_bridge.imgmsg_to_cv2(image_msg) 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import LookupException [as 别名]
def getErrorToGoal(self):
        # Look up flie position, convert to 2d, publish, return error

        if self.wandControl:
            #update goal using wand
            try:
                (t,r) = self.lookupTransformInWorld(frame='/wand', forceRealtime=True)
                e = tf.transformations.euler_from_quaternion(r)
                self.goal= [t[0], t[1], t[2], degrees(e[2]-e[0])]
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                rospy.loginfo('Could not find wand')


        # Publish GOAL from world frame
        q = tf.transformations.quaternion_from_euler(0, 0, radians(self.goal[3]))
        self.pub_tf.sendTransform((self.goal[0],self.goal[1],self.goal[2]), q, rospy.Time.now(), "/goal", "/world")


        # Look up 6D flie pose, publish 3d+yaw pose
        (trans,rot) = self.lookupTransformInWorld(frame='/cf_gt', poseNoiseMeters=0.0)
        euler = tf.transformations.euler_from_quaternion(rot)
        q = tf.transformations.quaternion_from_euler(0, 0, euler[2])
        self.pub_tf.sendTransform(trans, q, rospy.Time.now(), "/cf_gt2d", "/world")

        # Look up error goal->cf
        (trans,rot) = self.sub_tf.lookupTransform('/cf_gt2d', '/goal', rospy.Time(0))
        #self.pub_tf.sendTransform(trans, rot, rospy.Time.now(), "/goalCF", "/cf_gt2d")
        euler = tf.transformations.euler_from_quaternion(rot)
        return trans[0], trans[1], trans[2], degrees(euler[2]) 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import LookupException [as 别名]
def setGoalFromCurrent(self,config={}):
        try:
            (trans,rot) = self.lookupTransformInWorld(frame='/cf_gt', forceRealtime=True)
            euler = tf.transformations.euler_from_quaternion(rot)
            config['x'],config['y'],config['z'],config['rz']= trans[0], trans[1], trans[2], degrees(euler[2])
            rospy.loginfo('Updated goal state: [%.2f %.2f %.2f %.1f]', trans[0], trans[1], trans[2], degrees(euler[2]))
            self.goal = [config['x'],config['y'],config['z'],config['rz']]
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            rospy.logwarn('Could not look up transform world -> cf_gt')
        return config 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import LookupException [as 别名]
def getCameraGoal(self):
        """ Set the goal in camera coordinates from world coordinates """
        now = rospy.Time.now()
        try:
            self.sub_tf.waitForTransform("/camera_depth_optical_frame","/goal", now, rospy.Duration(0.1))
            (self.goal, rot) = self.sub_tf.lookupTransform("/camera_depth_optical_frame","/goal", now)
            self.goal = list(self.goal)
            #self.pub_tf.sendTransform(self.goal, rot, now, "/goalCam", "/camera_depth_optical_frame")#TODO: debugging only
            return self.goal
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            rospy.logwarn('Could not get Goal position in the camera frame')
            pass 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import LookupException [as 别名]
def getGTDist(self):
        now = rospy.Time.now()
        try:
            self.sub_tf.waitForTransform("/camera_depth_optical_frame","/cf_gt", now, rospy.Duration(0.1))
            (t, r) = self.sub_tf.lookupTransform("/camera_depth_optical_frame","/cf_gt", now )
            return list(t)
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            rospy.logwarn('Could not get ground truth position in the camera frame')
            pass 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import LookupException [as 别名]
def getKinectPoint(self):
        now = rospy.Time.now()
        try:
            self.sub_tf.waitForTransform("/world","/camera_depth_optical_frame", now, rospy.Duration(0.1))
            (t, r) = self.sub_tf.lookupTransform("/world","/camera_depth_optical_frame", now)
            return list(t), list(r)
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            rospy.logwarn('Could not get ground kinect position in the world')
            pass 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import LookupException [as 别名]
def get_robot_pose(self, event):
        try:
            (pos, rot) = self.tf_listener.lookupTransform('base_link', 'tool_link', rospy.Time(0))
            self.position = pos
            self.rpy = get_rpy_from_quaternion(rot)
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            rospy.loginfo("TF fail") 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import LookupException [as 别名]
def _get_tf_transform(self, tf_listener, tgt_frame, src_frame):
        """
        Uses ROS TF to lookup the current transform from tgt_frame to src_frame,
        If the returned transform is applied to data, it will transform data in
        the src_frame into the tgt_frame

        :param tgt_frame: target frame
        :param src_frame: source frame
        :type tgt_frame: string
        :type src_frame: string

        :returns: trans, translation (x,y,z)
        :rtype: tuple (of floats)
        :returns: quat, rotation as a quaternion (x,y,z,w)
        :rtype: tuple (of floats)
        """
        try:
            tf_listener.waitForTransform(
                tgt_frame, src_frame, rospy.Time(0), rospy.Duration(3)
            )
            (trans, quat) = tf_listener.lookupTransform(
                tgt_frame, src_frame, rospy.Time(0)
            )
        except (
            tf.LookupException,
            tf.ConnectivityException,
            tf.ExtrapolationException,
        ):
            raise RuntimeError(
                "Cannot fetch the transform from"
                " {0:s} to {1:s}".format(tgt_frame, src_frame)
            )
        return trans, quat 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import LookupException [as 别名]
def get_tf_transform(tf_listener, tgt_frame, src_frame):
    """
    Uses ROS TF to lookup the current transform from tgt_frame to src_frame,
    If the returned transform is applied to data, it will transform data in
    the src_frame into the tgt_frame

    :param tgt_frame: target frame
    :param src_frame: source frame
    :type tgt_frame: string
    :type src_frame: string

    :returns: trans, translation (x,y,z)
    :rtype: tuple (of floats)
    :returns: quat, rotation as a quaternion (x,y,z,w)
    :rtype: tuple (of floats)
    """
    try:
        tf_listener.waitForTransform(
            tgt_frame, src_frame, rospy.Time(0), rospy.Duration(3)
        )
        (trans, quat) = tf_listener.lookupTransform(tgt_frame, src_frame, rospy.Time(0))
    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
        raise RuntimeError(
            "Cannot fetch the transform from"
            " {0:s} to {1:s}".format(tgt_frame, src_frame)
        )
    return trans, quat