Python tf2_ros.LookupException方法代碼示例

# 需要導入模塊: import tf2_ros [as 別名]
# 或者: from tf2_ros import LookupException [as 別名]
def convert_pose(pose, from_frame, to_frame):
    """
    Convert a pose or transform between frames using tf.
        pose            -> A geometry_msgs.msg/Pose that defines the robots position and orientation in a reference_frame
        from_frame      -> A string that defines the original reference_frame of the robot
        to_frame        -> A string that defines the desired reference_frame of the robot to convert to
    """
    global tfBuffer, listener

    # Create Listener objet to recieve and buffer transformations
    trans = None

    try:
        trans = tfBuffer.lookup_transform(to_frame, from_frame, rospy.Time(0), rospy.Duration(1.0))
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException), e:
        print(e)
        rospy.logerr('FAILED TO GET TRANSFORM FROM %s to %s' % (to_frame, from_frame))
        return None 

# 需要導入模塊: import tf2_ros [as 別名]
# 或者: from tf2_ros import LookupException [as 別名]
def convert_pose(pose, from_frame, to_frame):
    """
    Convert a pose or transform between frames using tf.
        pose            -> A geometry_msgs.msg/Pose that defines the robots position and orientation in a reference_frame
        from_frame      -> A string that defines the original reference_frame of the robot
        to_frame        -> A string that defines the desired reference_frame of the robot to convert to
    """
    global tfBuffer, listener

    if tfBuffer is None or listener is None:
        _init_tf()

    try:
        trans = tfBuffer.lookup_transform(to_frame, from_frame, rospy.Time(0), rospy.Duration(1.0))
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException), e:
        print(e)
        rospy.logerr('FAILED TO GET TRANSFORM FROM %s to %s' % (to_frame, from_frame))
        return None 

# 需要導入模塊: import tf2_ros [as 別名]
# 或者: from tf2_ros 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 tf2_ros [as 別名]
# 或者: from tf2_ros 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 tf2_ros [as 別名]
# 或者: from tf2_ros import LookupException [as 別名]
def run(self):
        msg_count = 0
        try:
            bag = rosbag.Bag(self.bagfile, mode='a' if self.append else 'w')
            rate = rospy.Rate(self.lookup_frequency)
            last_stamp = rospy.Time()
            while not rospy.is_shutdown():
                try:
                    transform = self.tf_buffer.lookup_transform(
                        self.parent_frame, self.child_frame, rospy.Time())
                    rate.sleep()
                except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                        tf2_ros.ExtrapolationException):
                    rate.sleep()
                    continue
                if last_stamp == transform.header.stamp:
                    continue
                pose = transformstamped_to_posestamped(transform)
                bag.write(self.output_topic, pose, t=pose.header.stamp)
                msg_count += 1
                last_stamp = transform.header.stamp
                rospy.loginfo_throttle(
                    10, "Recorded {} PoseStamped messages.".format(msg_count))

        except rospy.ROSInterruptException:
            pass
        finally:
            bag.close()
            rospy.loginfo("Finished recording.") 

# 需要導入模塊: import tf2_ros [as 別名]
# 或者: from tf2_ros import LookupException [as 別名]
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 

# 需要導入模塊: import tf2_ros [as 別名]
# 或者: from tf2_ros import LookupException [as 別名]
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 

# 需要導入模塊: import tf2_ros [as 別名]
# 或者: from tf2_ros import LookupException [as 別名]
def __debug_loop(self):
        """
        Debug loop that will run in a separate thread.
        (tfBuffer should be threadsafe)
        """
        broadcaster = tf2_ros.TransformBroadcaster()
        rviz_marker_pub = rospy.Publisher('/visualization_marker', Marker,
                                          queue_size=1000)
        rate = rospy.Rate(5)
        while not self.__debug_stop_event.is_set() and not rospy.is_shutdown():
            if self.__debug_current_ws is None:
                print "Could not publish debug tf, no workspace set."
                rate.sleep()
                continue

            try:
                broadcaster.sendTransform(
                    self.__tf_buffer.lookup_transform(
                        "base_link", self.__debug_current_ws.name,
                        rospy.Time(0))
                )
                broadcaster.sendTransform(
                    self.__tf_buffer.lookup_transform(
                        self.__debug_current_ws.name, "object_base",
                        rospy.Time(0))
                )
                broadcaster.sendTransform(
                    self.__tf_buffer.lookup_transform(
                        "object_base", "tool_link_target", rospy.Time(0))
                )
            except tf2_ros.LookupException as e:
                print "Could not publish debug tf: ", e

            for i in range(4):  # Iterate over the 4 markers defining the workspace
                msg = Marker()
                msg.header.frame_id = "base_link"
                msg.id = i
                msg.type = 2  # It correspond to a sphere which will be drawn

                msg.pose.position.x = self.__debug_current_ws.points[i][0]
                msg.pose.position.y = self.__debug_current_ws.points[i][1]
                msg.pose.position.z = self.__debug_current_ws.points[i][2]

                msg.scale.x = 0.005
                msg.scale.y = 0.005
                msg.scale.z = 0.005

                msg.color.r = 1.0 if i == 0 or i == 3 else 0.0
                msg.color.g = 1.0 if i == 1 or i == 3 else 0.0
                msg.color.b = 1.0 if i == 2 or i == 3 else 0.0
                msg.color.a = 1.0

                rviz_marker_pub.publish(msg)

        rate.sleep()