Python tf.ExtrapolationException方法代码示例

# 需要导入模块: import tf [as 别名]
# 或者: from tf import ExtrapolationException [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 ExtrapolationException [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 ExtrapolationException [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 ExtrapolationException [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 ExtrapolationException [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 ExtrapolationException [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 ExtrapolationException [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 ExtrapolationException [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 ExtrapolationException [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 ExtrapolationException [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 ExtrapolationException [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 ExtrapolationException [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 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import ExtrapolationException [as 别名]
def update(self,maxt=0.01):
        '''
        Look up what the world should look like, based on the provided arguments.
        "objs" should be a list of all possible objects that we might want to
        aggregate. They'll be saved in the "objs" dictionary. It's the role of the
        various options/policies to choose and use them intelligently.

        Parameters:
        -----------
        maxt: max duration used when waiting for transforms

        Returns:
        --------
        n/a

        Access via the self.observation member or the getPose() function.
        '''
        self.observation = {}
        for obj in self.objects_to_track:
            try:
                self.tf_listener.waitForTransform(self.base_link, obj, rospy.Time.now(), rospy.Duration(maxt))
                (trans, rot) = self.tf_listener.lookupTransform(
                    self.base_link, obj, rospy.Time(0.))
                self.observation[obj] = pm.fromTf((trans, rot))

            except (tf.LookupException,
                    tf.ConnectivityException,
                    tf.ExtrapolationException,
                    tf2.TransformException) as e:
                self.observation[obj] = None

        # Update 
        for actor in self.actors:
            actor.state = actor.getState()
            actor.state.t = rospy.Time.now().to_sec() 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import ExtrapolationException [as 别名]
def goto(kdl_kin, pub, listener, trans, rot): 

  try:
    T = pm.fromTf((trans, rot))

    q0 = [-1.0719114121799995, -1.1008140645600006, 1.7366724169200003,
            -0.8972388608399999, 1.25538042294, -0.028902652380000227,]

    # DEFAULT
    objt, objr = ((0.470635159016, 0.0047549889423, -0.428045094013),(0,0,0,1))
    T_orig = pm.fromTf((objt,objr))
    # MOVEd
    objt, objr = ((0.52, 0.00, -0.43),(0,0,0,1))
    T_new = pm.fromTf((objt,objr))

    T_pose = pm.toMatrix(T)
    Q = kdl_kin.inverse(T_pose, q0)

    print "----------------------------"
    print "[ORIG] Closest joints =", Q

    msg = JointState(name=CONFIG['joints'], position=Q)
    pub.publish(msg)
    rospy.sleep(0.2)

    T_goal = T_orig.Inverse() * T
    T2 = T_new * T_goal
    T2_pose = pm.toMatrix(T2)
    Q = kdl_kin.inverse(T2_pose, q0)
    print "[NEW] Closest joints =", Q
    msg = JointState(name=CONFIG['joints'], position=Q)
    pub.publish(msg)
    rospy.sleep(0.2)

  except  (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e:
    pass 

# 需要导入模块: import tf [as 别名]
# 或者: from tf import ExtrapolationException [as 别名]
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)