Python tf.Exception方法代碼示例

# 需要導入模塊: import tf [as 別名]
# 或者: from tf import Exception [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 Exception [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 Exception [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 Exception [as 別名]
def tick(self):

        #TODO: figure out why this is bad
        #if not self.listener.frameExists(self.root):
        #    rospy.logerr("missing root: %s"%self.root)
        #    return
        self.t = rospy.Time.now()

        for name, urdf in self.urdfs.items():
            if self.objs[name] == None:
                operation = CollisionObject.ADD
            else:
                operation = CollisionObject.MOVE
            if not self.listener.frameExists(name):
                #rospy.logwarn("Frame %s does not exist"%name)
                continue
            try:
                t = self.listener.getLatestCommonTime(self.root, name)
            except tf.Exception as e:
                rospy.logerr(str(e))
                continue
            dt = (self.t - t).to_sec()
            if dt > self.max_dt:
                rospy.logwarn("object %s has not been observed in the last %f seconds"%(name, dt))
                continue
            pose = self.listener.lookupTransform(self.root, name, t)
            pose = pm.fromTf(pose)
            co = _getCollisionObject(name, urdf, pose, operation)
            co.header.frame_id = self.root
            self.objs[name] = co
            self.co_pub.publish(co) 

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