我们从Python开源项目中，提取了以下28个代码示例，用于说明如何使用tf.ConnectivityException()。
def robot_listener(self): ''' rospy.wait_for_service('spawn') spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) spawner(4, 2, 0, 'turtle2') ''' robot_vel_pub = rospy.Publisher('robot_0/cmd_vel', geometry_msgs.msg.Twist, queue_size=1) rate = rospy.Rate(10.0) while not rospy.is_shutdown(): try: (trans,rot) = self.listener.lookupTransform('/robot_3', '/robot_0', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue angular = 4 * math.atan2(trans[1], trans[0]) linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2) cmd = geometry_msgs.msg.Twist() cmd.linear.x = linear cmd.angular.z = angular robot_vel_pub.publish(cmd) rate.sleep()
def markerCB(self, msg): try: self.marker_lock.acquire() if not self.initialize_poses: return self.initial_poses = {} for marker in msg.markers: if marker.name.startswith("EE:goal_"): # resolve tf if marker.header.frame_id != self.frame_id: ps = PoseStamped(header=marker.header, pose=marker.pose) try: transformed_pose = self.tf_listener.transformPose(self.frame_id, ps) self.initial_poses[marker.name[3:]] = transformed_pose.pose except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, e): rospy.logerr("tf error when resolving tf: %s" % e) else: self.initial_poses[marker.name[3:]] = marker.pose #tf should be resolved finally: self.marker_lock.release()
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_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 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 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 transform_pose(self, pose): try: new_pose = self.listener.transformPose(self.frame_id, pose) # now new_pose should be the pose of the tag in the odom_meas frame. # Let's ignore the height, and rotations not about the z-axis new_pose.pose.position.z = 0 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return None return new_pose
def get_position(self): 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 get_odom_angle(self): 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 "succeeded" return self.quat_to_angle(Quaternion(*rot))
def get_odom_angle(self): 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 self.quat_to_angle(Quaternion(*rot))
def record_tf(self, event): if self.active: try: self.listener.waitForTransform(self.root_frame, self.measured_frame, rospy.Time(0), rospy.Duration.from_sec(1 / (2*self.tf_sampling_freq))) (trans, rot) = self.listener.lookupTransform(self.root_frame, self.measured_frame, rospy.Time(0)) except (tf.Exception, tf.LookupException, tf.ConnectivityException) as e: rospy.logwarn(e) pass else: if self.first_value: self.trans_old = trans self.rot_old = rot self.first_value = False return #print "transformations: \n", "trans[0]", trans[0], "self.trans_old[0]",self.trans_old[0], "trans[1]", trans[1], "self.trans_old[1]",self.trans_old[1], "trans[2]",trans[2], "self.trans_old[2]",self.trans_old[2], "\n ------------------------------------------------ " path_increment = math.sqrt((trans[0] - self.trans_old[0]) ** 2 + (trans[1] - self.trans_old[1]) ** 2 + (trans[2] - self.trans_old[2]) ** 2) if(path_increment < 1): #rospy.logwarn("Transformation: %s, Path Increment: %s",str(trans), str(path_increment)) self.path_length += path_increment else: rospy.logwarn("Transformation Failed! \n Transformation: %s, Path Increment: %s",str(trans), str(path_increment)) self.trans_old = trans self.rot_old = rot
def go_to_pose(self, name, T, timeout): msg = convert_to_trans_message(T) self.update_marker(T) start_time = time.time() done = False while not done and not rospy.is_shutdown(): self.pub_cmd.publish(msg) try: (trans,rot) = self.listener.lookupTransform('/world_link','lwr_arm_7_link', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print "TF Exception!" continue TR = numpy.dot(tf.transformations.translation_matrix(trans), tf.transformations.quaternion_matrix(rot)) if (is_same(T, TR)): print name + ": Reached desired pose" done = True if (time.time() - start_time > timeout) : print name + ": Robot took too long to reach desired pose" done = True else: rospy.sleep(0.1)
def goto_pose(self, name, T, timeout): self.server.setPose("move_arm_marker", convert_to_message(T)) self.server.applyChanges() self.pub_command.publish(convert_to_trans_message(T)) print 'Goal published' start_time = time.time() done = False while not done and not rospy.is_shutdown(): self.mutex.acquire() last_joint_state = deepcopy(self.joint_state) self.mutex.release() if not self.check_validity(last_joint_state): print 'COLLISION!' try: (trans,rot) = self.listener.lookupTransform('/world_link','/lwr_arm_7_link', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print "TF Exception!" continue TR = numpy.dot(tf.transformations.translation_matrix(trans), tf.transformations.quaternion_matrix(rot)) if (is_same(T, TR)): print name + ": Reached goal" done = True if (time.time() - start_time > timeout) : done = True print name + ": Robot took too long to reach goal. Grader timed out" else: rospy.sleep(0.05)
def establish_tfs(self, relations): """ Perform a one-time look up of all the requested *static* relations between frames (available via /tf) """ # Init node and tf listener rospy.init_node(self.__class__.__name__, disable_signals=True) tf_listener = tf.TransformListener() # Create tf decoder tf_dec = TfDecoderAndPublisher(channel='/tf') # Establish tf relations print('{} :: Establishing tfs from ROSBag'.format(self.__class__.__name__)) for self.idx, (channel, msg, t) in enumerate(self.log.read_messages(topics='/tf')): tf_dec.decode(msg) for (from_tf, to_tf) in relations: # If the relations have been added already, skip if (from_tf, to_tf) in self.relations_map_: continue # Check if the frames exist yet if not tf_listener.frameExists(from_tf) or not tf_listener.frameExists(to_tf): continue # Retrieve the transform with common time try: tcommon = tf_listener.getLatestCommonTime(from_tf, to_tf) (trans,rot) = tf_listener.lookupTransform(from_tf, to_tf, tcommon) self.relations_map_[(from_tf,to_tf)] = RigidTransform(tvec=trans, xyzw=rot) # print('\tSuccessfully received transform: {:} => {:} {:}' # .format(from_tf, to_tf, self.relations_map_[(from_tf,to_tf)])) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: # print e pass # Finish up once we've established all the requested tfs if len(self.relations_map_) == len(relations): break try: tfs = [self.relations_map_[(from_tf,to_tf)] for (from_tf, to_tf) in relations] for (from_tf, to_tf) in relations: print('\tSuccessfully received transform:\n\t\t {:} => {:} {:}' .format(from_tf, to_tf, self.relations_map_[(from_tf,to_tf)])) except: raise RuntimeError('Error concerning tf lookup') print('{} :: Established {:} relations\n'.format(self.__class__.__name__, len(tfs))) return tfs
def __init__(self): # Give the node a name rospy.init_node('quat_to_angle', anonymous=False) # Publisher to control the robot's speed self.turtlebot_angle = rospy.Publisher('/turtlebot_angle', Float64, queue_size=5) self.turtlebot_posex = rospy.Publisher('/turtlebot_posex', Float64, queue_size=5) self.turtlebot_posey = rospy.Publisher('/turtlebot_posey', Float64, queue_size=5) #goal.target_pose.pose = Pose(Point(float(data["point"]["x"]), float(data["point"]["y"]), float(data["point"]["z"])), Quaternion(float(data["quat"]["x"]), float(data["quat"]["y"]), float(data["quat"]["z"]), float(data["quat"]["w"]))) # How fast will we update the robot's movement? rate = 20 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Initialize the tf listener self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer rospy.sleep(2) # Set the odom frame self.odom_frame = '/odom' # Find out if the robot uses /base_link or /base_footprint try: self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_footprint' except (tf.Exception, tf.ConnectivityException, tf.LookupException): try: self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_link' except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") rospy.signal_shutdown("tf Exception") # Initialize the position variable as a Point type position = Point() while not rospy.is_shutdown(): (position, rotation) = self.get_odom() print(position) self.turtlebot_angle.publish(rotation) #print(str(position).split('x: ')[1].split('\ny:')[0]) x = float(str(position).split('x: ')[1].split('\ny:')[0]) y = float(str(position).split('y: ')[1].split('\nz:')[0]) self.turtlebot_posex.publish(x) self.turtlebot_posey.publish(y) #print(rotation) rospy.sleep(5)
