def run(self): rospy.init_node('calibrate_bundle') next_pub = rospy.Publisher('next_poses', String, queue_size = 1) tf_b = tf.TransformBroadcaster() tf_l = tf.TransformListener() rate = rospy.Rate(10) rospy.Service('commit_next_poses', CommitNextPoses, self.commit_next_poses ) rospy.Service('save_known_poses', SaveKnownPoses, self.save_known_poses ) main_frame_avg = self.main_marker + '_avg' main_frame_bundle = self.main_marker + '_bundle' while not rospy.is_shutdown(): self.next_poses = {} for bundle_marker in self.bundle_markers: marker_avg_frame = bundle_marker + '_avg' marker_bundle_frame = bundle_marker + '_bundle' if marker_bundle_frame not in self.known_poses.keys(): try: now = rospy.Time.now() if (now - tf_l.getLatestCommonTime(main_frame_avg, marker_avg_frame)) < TIME_THRESH: T,R = tf_l.lookupTransform(main_frame_avg, marker_avg_frame, rospy.Time(0)) tfs = TransformStamped() tfs.header.stamp = now tfs.header.frame_id = main_frame_bundle tfs.child_frame_id = marker_bundle_frame tfs.transform = numpy_to_tf_msg(T,R) self.next_poses[tfs.child_frame_id] = tfs except ( tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e : continue tf_b.sendTransform([0,0,0],[0,0,0,1],now, main_frame_bundle, main_frame_avg) for kp in self.known_poses.values(): base_frame = kp.header.frame_id child_frame = kp.child_frame_id T,R = tf_msg_to_numpy(kp) tf_b.sendTransform(T,R,now,child_frame,base_frame) next_pub.publish(str(self.next_poses.keys())) rate.sleep()
def __init__(self, base_frame='usb_cam', bundle_bagfile=DEFAULT_BAGFILE): with rosbag.Bag(bundle_bagfile) as bundle_bag: tf_msg = [msg for _,msg,_ in bundle_bag.read_messages()][0] self.base_frame = base_frame # build dictionary mapping marker_filt to inverse transform necessary to get to main marker self.bundle_tfs = {} for bundle_tf in tf_msg.transforms: marker_name = bundle_tf.child_frame_id[:-7] + '_filt' T,R = tf_msg_to_numpy(bundle_tf) H = quaternion_matrix(R) H[0:3,3] = T self.bundle_tfs[marker_name] = inverse_matrix(H) self.main_marker = tf_msg.transforms[0].header.frame_id[:-7] self.bundle_tfs[self.main_marker + '_filt'] = numpy.eye(4)