Python tf.ConnectivityException() Examples

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 

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 

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 

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 

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

def _lookup_transform_tf2(self, target_frame, source_frame, time, timeout):
        try:
            return self.tf_listener.lookup_transform(
                target_frame, source_frame, time, timeout)
        except (tf2_ros.LookupException,
                tf2_ros.ConnectivityException,
                tf2_ros.ExtrapolationException,
                tf2_ros.TransformException,
                rospy.exceptions.ROSTimeMovedBackwardsException):
            return False 

def _wait_for_transform_tf1(self,
                                target_frame, source_frame,
                                time, timeout):
        try:
            self.tf_listener.waitForTransform(
                target_frame, source_frame, time, timeout)
            return True
        except (tf.LookupException,
                tf.ConnectivityException,
                tf.ExtrapolationException,
                rospy.exceptions.ROSTimeMovedBackwardsException):
            return False 

def get_trans(self, target):
        try:
            (trans, rot) = self.listener.lookupTransform('/fssim_map', '/fssim/vehicle/' + target, rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            # rospy.logwarn("Could not find transform from %s to %s", 'fssim_map', 'fluela/base_link')
            rospy.sleep(1)
            return [0.0, 0.0]
        return trans 

def get_trans(self, target):
        try:
            (trans, rot) = self.listener.lookupTransform('/fssim_map', '/fssim/vehicle/' + target, rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            # rospy.logwarn("Could not find transform from %s to %s", 'fssim_map', 'fluela/base_link')
            rospy.sleep(1)
            return [0.0, 0.0]
        return trans 

def dock(self, target_bin):
    self.target_bin = target_bin
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
      marker_frame = "ar_marker_%d_up" % self.target_bin
      try:
        t = self.tf_listener.getLatestCommonTime("/base_link", marker_frame)
        print "age: %.6f" % (rospy.Time.now() - t).to_sec()
        if (rospy.Time.now() - t).to_sec() > 0.2:
          rospy.sleep(0.1)
          continue
        (marker_translation, marker_orient) = self.tf_listener.lookupTransform('/base_link',marker_frame,t)
        print "marker: " + str(marker_translation)
        target_translation = Vector3(1.3, 0, 0.5)
        if (abs(marker_translation[0]) + abs(marker_translation[1])) < 0.15:
          print "close enough!"
          break;
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = marker_frame
        goal.target_pose.pose.position.x = 0
        goal.target_pose.pose.position.y = -1.5
        orient = Quaternion(*quaternion_from_euler(0, 0, 1.57))
        goal.target_pose.pose.orientation = orient
        self.move_base.send_goal(goal)
        self.move_base.wait_for_result()
        result = self.move_base.get_result()
        nav_state = self.move_base.get_state()
        if nav_state == 3:
          print "move success! waiting to calm down before looking again..."
          rospy.sleep(1) # wait for things to calm down a bit before looking
          self.point_head_forwards()
          rospy.sleep(0.5)
          print "done waiting."
        else:
          print "move failure!"

      except(tf.Exception, tf.LookupException, 
             tf.ConnectivityException, tf.ExtrapolationException):
        rate.sleep() # not yet in view 

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

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

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 

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 

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

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 

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 

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 

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 

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

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) 

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

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

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

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) 

def update_info_text(self):
        """
        update the displayed info text
        """
        if not self._show_info:
            return
        try:
            (position, quaternion) = self.tf_listener.lookupTransform(
                '/map', self.role_name, rospy.Time())
            _, _, yaw = tf.transformations.euler_from_quaternion(quaternion)
            yaw = -math.degrees(yaw)
            x = position[0]
            y = -position[1]
            z = position[2]
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            x = 0
            y = 0
            z = 0
            yaw = 0
        heading = 'N' if abs(yaw) < 89.5 else ''
        heading += 'S' if abs(yaw) > 90.5 else ''
        heading += 'E' if 179.5 > yaw > 0.5 else ''
        heading += 'W' if -0.5 > yaw > -179.5 else ''
        fps = 0
        if self.carla_status.fixed_delta_seconds:
            fps = 1 / self.carla_status.fixed_delta_seconds
        self._info_text = [
            'Frame: % 22s' % self.carla_status.frame,
            'Simulation time: % 12s' % datetime.timedelta(
                seconds=int(rospy.get_rostime().to_sec())),
            'FPS: % 24.1f' % fps,
            '',
            'Vehicle: % 20s' % ' '.join(self.vehicle_info.type.title().split('.')[1:]),
            'Speed:   % 15.0f km/h' % (3.6 * self.vehicle_status.velocity),
            u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (yaw, heading),
            'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (x, y)),
            'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (self.latitude, self.longitude)),
            'Height:  % 18.0f m' % z,
            '']
        self._info_text += [
            ('Throttle:', self.vehicle_status.control.throttle, 0.0, 1.0),
            ('Steer:', self.vehicle_status.control.steer, -1.0, 1.0),
            ('Brake:', self.vehicle_status.control.brake, 0.0, 1.0),
            ('Reverse:', self.vehicle_status.control.reverse),
            ('Hand brake:', self.vehicle_status.control.hand_brake),
            ('Manual:', self.vehicle_status.control.manual_gear_shift),
            'Gear:        %s' % {-1: 'R', 0: 'N'}.get(self.vehicle_status.control.gear,
                                                      self.vehicle_status.control.gear),
            '']
        self._info_text += [('Manual ctrl:', self.manual_control)]
        if self.carla_status.synchronous_mode:
            self._info_text += [('Sync mode running:', self.carla_status.synchronous_mode_running)]
        self._info_text += ['', '', 'Press <H> for help']