def search_and_broadcast(frames_req, attempts, wait, br_freq, record_file): rospy.loginfo( "Waiting for service 'frame_to_frame'. Did you initialize tf_finder?") rospy.wait_for_service('frame_to_frame') rospy.loginfo("Service 'frame_to_frame' found!") frame_srv = rospy.ServiceProxy('frame_to_frame', CoordinateTransform) tfs_found = [] #List of transforms found in the search frames_found = [] #List of frame pairs found in the search for trial in range(attempts): for frame_pair in [ pair for pair in frames_req if not pair in frames_found ]: src_frame = frame_pair[0] dest_frame = frame_pair[1] rospy.loginfo("Looking for transform from " + src_frame + " to " + dest_frame) try: response = frame_srv(frame_src=src_frame, frame_dest=dest_frame) if (response == None): rospy.logwarn("Service returned no transform from " + src_frame + " to " + dest_frame + " during attempt " + str(trial + 1)) else: rospy.loginfo("Found transform from " + src_frame + " to " + dest_frame + " during attempt " + str(trial + 1)) tfs_found.append(response.transform ) #Stores the stamped transform object frames_found.append(frame_pair) #Stores the frame pair except (rospy.ServiceException): rospy.logwarn("Couldn't find a transform from " + src_frame + " to " + dest_frame + " during attempt " + str(trial + 1)) if (len(frames_found) == len(frames_req)): rospy.loginfo("All transforms found at trial " + str(trial + 1)) break #Stops looking for transforms else: rospy.logwarn("Sleeping " + str(wait) + " before next attempt.") rospy.sleep(wait) if (record_file): rospy.loginfo("Writing output to " + record_file) cat_pickle.write_pickle_file(tfs_found, record_file) else: broadcast_tf(tfs_found, br_freq)
def search_and_broadcast(frames_req,attempts,wait,br_freq,record_file): rospy.loginfo("Waiting for service 'frame_to_frame'. Did you initialize tf_finder?") rospy.wait_for_service('frame_to_frame') rospy.loginfo("Service 'frame_to_frame' found!") frame_srv = rospy.ServiceProxy('frame_to_frame',CoordinateTransform) tfs_found = [] #List of transforms found in the search frames_found = [] #List of frame pairs found in the search for trial in range(attempts): for frame_pair in [pair for pair in frames_req if not pair in frames_found]: src_frame = frame_pair[0] dest_frame = frame_pair[1] rospy.loginfo("Looking for transform from "+src_frame+" to "+dest_frame) try: response = frame_srv(frame_src=src_frame,frame_dest=dest_frame) if(response == None): rospy.logwarn("Service returned no transform from "+src_frame+" to "+dest_frame+" during attempt "+str(trial+1)) else: rospy.loginfo("Found transform from "+src_frame+" to "+dest_frame+" during attempt "+str(trial+1)) tfs_found.append(response.transform) #Stores the stamped transform object frames_found.append(frame_pair) #Stores the frame pair except (rospy.ServiceException): rospy.logwarn("Couldn't find a transform from "+src_frame+" to "+dest_frame+" during attempt "+str(trial+1)) if(len(frames_found)==len(frames_req)): rospy.loginfo("All transforms found at trial "+str(trial+1)) break #Stops looking for transforms else: rospy.logwarn("Sleeping "+str(wait)+" before next attempt.") rospy.sleep(wait) if(record_file): rospy.loginfo("Writing output to "+record_file) cat_pickle.write_pickle_file(tfs_found,record_file) else: broadcast_tf(tfs_found,br_freq)
try: rospy.loginfo("Loading graph from file "+args.graph_file) frame_graph = pickle.load(open(args.graph_file,'r')) rospy.loginfo("Graph successfully loaded.") except IOError: print "File "+args.graph_file+" could not be opened." sys.exit(0) else: #Empty graph frame_graph = Graph() #Dictionary used to determine if a given pair of coordinate frames #has been consistently detected detected_pairs={} #List of validated frame pairs validated_pairs=[] tf_listener = tf.TransformListener() #Global TF listener subs = rospy.Subscriber("/tf",tfMessage,tf_callback) rospy.Service('frame_to_frame',CoordinateTransform,service_handler) # Sits idle waiting for service calls rospy.spin() if(record_to_file): filename = "frame_graph.pickle" rospy.loginfo("Writing frame graph to pickle file "+filename) cat_pickle.write_pickle_file(frame_graph,filename)
if args.graph_file: try: rospy.loginfo("Loading graph from file " + args.graph_file) frame_graph = pickle.load(open(args.graph_file, 'r')) rospy.loginfo("Graph successfully loaded.") except IOError: print "File " + args.graph_file + " could not be opened." sys.exit(0) else: #Empty graph frame_graph = Graph() #Dictionary used to determine if a given pair of coordinate frames #has been consistently detected detected_pairs = {} #List of validated frame pairs validated_pairs = [] tf_listener = tf.TransformListener() #Global TF listener subs = rospy.Subscriber("/tf", tfMessage, tf_callback) rospy.Service('frame_to_frame', CoordinateTransform, service_handler) # Sits idle waiting for service calls rospy.spin() if (record_to_file): filename = "frame_graph.pickle" rospy.loginfo("Writing frame graph to pickle file " + filename) cat_pickle.write_pickle_file(frame_graph, filename)