def find_separators():
rospy.init_node('find_separators', anonymous=False)
rate = rospy.Rate(0.3)
dataHandler = DataHandler()
# Callbacks to save images and keyframes
rospy.Subscriber("left/image_rect",
Image, dataHandler.save_image_l)
rospy.Subscriber("right/image_rect", Image, dataHandler.save_image_r)
rospy.Subscriber("rgb/image_rect", Image, dataHandler.save_image_rgb)
rospy.Subscriber("odom_info", OdomInfo, dataHandler.get_keyframes)
# Initialize services
s_find_matches = rospy.Service(
'find_matches_compute', FindMatches, dataHandler.find_matches_service)
s_receive_separators = rospy.Service(
'receive_separators_py', ReceiveSeparators, dataHandler.receive_separators_service)
# Find services that need to be called
s_ans_est_transform = rospy.ServiceProxy(
'estimate_transformation', EstTransform)
s_find_matches_query = rospy.ServiceProxy(
'find_matches_query', FindMatches)
s_ans_rec_sep = rospy.ServiceProxy(
'found_separators_query', ReceiveSeparators)
i = 0
while not rospy.is_shutdown():
# main loop
i += 1
# Compute descriptors
dataHandler.compute_descriptors()
service_list = rosservice.get_service_list()
# Check if other robot is visible
if '/robot_'+str(dataHandler.other_robot_id)+'/find_matches_query' in service_list:
if i % 20 and (len(dataHandler.local_descriptors) > 0):
# Call service to find matches and corresponding keypoints and geometric descriptors
try:
# Only send descriptors which havent been sent yet
descriptors_to_send = dataHandler.local_descriptors[dataHandler.nb_descriptors_already_sent:]
flatten_desc = [
item for sublist in list(descriptors_to_send) for item in sublist]
res_matches = s_find_matches_query(flatten_desc)
except rospy.ServiceException, e:
print "Service call to find matches query failed: %s" % e
break
dataHandler.nb_descriptors_already_sent += len(descriptors_to_send)
# If no matches are found, do nothing
if not res_matches.kf_ids_computing_robot:
continue
# If matches are found, compute the separators, send them back, and add them to the pose graph
frames_kept_ids_from_kept = collections.deque()
frames_kept_ids_to_kept = collections.deque()
kf_ids_to_kept = collections.deque()
transform_est_success = collections.deque()
separators_found = collections.deque()
pose_estimates_to_kept = collections.deque()
for i in range(len(res_matches.kf_ids_computing_robot)):
# Compute geometric features of the local frames that were matched. Use matched_ids_other since it is from the point of view of the other robot.
local_features_and_desc = dataHandler.get_geom_features(
res_matches.frames_kept_ids_querying_robot[i])
try:
# Transform computed FROM local_features_and_desc TO res_matches (other robot)
res_transform = s_ans_est_transform(
local_features_and_desc.descriptors, res_matches.descriptors_vec[i], local_features_and_desc.kpts3D, res_matches.kpts3D_vec[i], local_features_and_desc.kpts, res_matches.kpts_vec[i])
except rospy.ServiceException, e:
print "Service call estimate_transformation failed: %s" % e
continue
# Check if transform was successfully computed
if res_transform.success:
transform_est_success.append(True)
else:
transform_est_success.append(False)
frames_kept_ids_from_kept.append(
res_matches.frames_kept_ids_querying_robot[i])
frames_kept_ids_to_kept.append(res_matches.frames_kept_ids_computing_robot[i])
kf_ids_to_kept.append(
res_matches.kf_ids_computing_robot[i])
separators_found.append(res_transform.poseWithCov)
if dataHandler.send_estimates_of_poses:
pose_estimates_to_kept.append(res_matches.pose_estimates[i])
kf_ids_from_kept = dataHandler.get_kf_ids_from_frames_kept_ids(
frames_kept_ids_from_kept)
if dataHandler.send_estimates_of_poses:
try:
pose_estimates_from_kept = dataHandler.s_get_pose_estimates(
kf_ids_from_kept).pose_estimates
except rospy.ServiceException, e:
print "Service call pose_estimates failed: %s" % e
else:
pose_estimates_from_kept = collections.deque()
# Add the separator to the factor graph and save it
dataHandler.found_separators_local(kf_ids_from_kept, kf_ids_to_kept, frames_kept_ids_from_kept, frames_kept_ids_to_kept, pose_estimates_from_kept, pose_estimates_to_kept, transform_est_success, separators_found)
# Send the separator found to the other robot concerned
try:
s_ans_rec_sep(dataHandler.local_robot_id, dataHandler.other_robot_id,kf_ids_from_kept,
kf_ids_to_kept, frames_kept_ids_from_kept, frames_kept_ids_to_kept, pose_estimates_from_kept, pose_estimates_to_kept, transform_est_success, separators_found)
except rospy.ServiceException, e:
print "Service call found_separators_query failed: %s" % e
