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)
