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)
