def run (self):
"""
Publishes the transforms stored.
"""
while True:
if self.publish_pc and self.cameras is not None:
data=self.cameras.get_RGBD()
for i,rgbd in data.items():
if rgbd['depth'] is None or rgbd['rgb'] is None:
print 'wut'
continue
camera_frame = 'camera%d_depth_optical_frame'%(i+1)
xyz = clouds.depth_to_xyz(rgbd['depth'], asus_xtion_pro_f)
pc = ru.xyzrgb2pc(xyz, rgbd['rgb'], camera_frame)
ar_cam = gmt.get_ar_marker_poses(None, None, pc=pc)
self.pc_pubs[i].publish(pc)
marker_frame = 'ar_marker%d_camera%d'
for marker in ar_cam:
trans, rot = conversions.hmat_to_trans_rot(ar_cam[marker])
self.tf_broadcaster.sendTransform(trans, rot,
rospy.Time.now(),
marker_frame%(marker,i+1),
camera_frame)
if self.ready:
for parent, child in self.transforms:
trans, rot = self.transforms[parent, child]
self.tf_broadcaster.sendTransform(trans, rot,
rospy.Time.now(),
child, parent)
time.sleep(1/self.rate)
def process_observation(self, n_avg=5):
"""
Get an observation and update transform list.
"""
yellowprint("Please hold still for a few seconds.")
self.observation_info = {i:[] for i in xrange(self.num_cameras)}
self.observed_ar_transforms = {i:{} for i in xrange(self.num_cameras)}
# Get RGBD observations
for i in xrange(n_avg):
print colorize("Transform %d out of %d for averaging."%(i,n_avg),'yellow',False)
data = self.cameras.get_RGBD()
for j,cam_data in data.items():
self.observation_info[j].append(cam_data)
# Find AR transforms from RGBD observations and average out transforms.
for i in self.observation_info:
for obs in self.observation_info[i]:
ar_pos = gmt.get_ar_marker_poses (obs['rgb'], obs['depth'])
for marker in ar_pos:
if self.observed_ar_transforms[i].get(marker) is None:
self.observed_ar_transforms[i][marker] = []
self.observed_ar_transforms[i][marker].append(ar_pos[marker])
for marker in self.observed_ar_transforms[i]:
self.observed_ar_transforms[i][marker] = utils.avg_transform(self.observed_ar_transforms[i][marker])
for i in xrange(1,self.num_cameras):
transform = self.find_transform_between_cameras_from_obs(0, i)
if transform is None:
redprint("Did not find a transform between cameras 0 and %d"%i)
continue
if self.transform_list.get(0,i) is None:
self.transform_list[0,i] = []
self.transform_list[0,i].append(transform)
