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)