with open(demo_master_file, 'r') as fh: demos_info = yaml.load(fh) if args.demo_name == '': for demo in demos_info["demos"]: demo_dir = osp.join(demo_type_dir, demo["demo_name"]) # Wait until current demo is done recording, if so. while osp.isfile(osp.join(demo_dir, demo_names.record_demo_temp)): time.sleep(1) # Some other node is extracting data currently. if osp.isfile(osp.join(demo_dir, demo_names.extract_hydra_data_temp)): yellowprint("Another node seems to be extracting hydra data already for %s."%demo["demo_name"]) continue # Check if data file already exists if not osp.isfile(osp.join(demo_dir, demo_names.hydra_data_name)): ed.save_hydra_only(args.demo_type, demo["demo_name"]) else: yellowprint("Hydra data file exists for %s. Not overwriting."%demo["demo_name"]) else: demo_dir = osp.join(demo_type_dir, args.demo_name) if osp.exists(demo_dir): # Wait until current demo is done recording, if so. while osp.isfile(osp.join(demo_dir, demo_names.record_demo_temp)): time.sleep(1) # Check if some other node is extracting data currently. if not osp.isfile(osp.join(demo_dir, demo_names.extract_hydra_data_temp)): # Check if data file already exists if osp.isfile(osp.join(demo_dir, demo_names.hydra_data_name)): if yes_or_no('Hydra data file already exists for this demo. Overwrite?'):
def view_hydra_demo_on_rviz (demo_type, demo_name, freq, speed, prompt=False, verbose=False): """ Uses hydra_only.data for the segment to quickly visualize the demo. @demo_type, @demo_name: demo identification. @freq: basically measure of fine-ness of timesteps. @speed: how fast to replay demo. @prompt: does the user hit enter after each time step? """ demo_dir = osp.join(demo_files_dir, demo_type, demo_name) bag_file = osp.join(demo_dir, demo_names.bag_name) data_file = osp.join(demo_dir, demo_names.hydra_data_name) calib_file = osp.join(demo_dir, demo_names.calib_name) with open(osp.join(demo_dir, demo_names.camera_types_name),'r') as fh: cam_types = yaml.load(fh) if not osp.isfile(data_file): yellowprint("%s does not exist for this demo. Extracting now."%demo_names.hydra_data_name) ed.save_hydra_only(demo_type, demo_name) with open(data_file, 'r') as fh: dat = cp.load(fh) # get grippers used grippers = [key for key in dat.keys() if key in 'lr'] # data rgbd_dirs = {cam:osp.join(demo_dir,demo_names.video_dir%cam) for cam in cam_types if cam_types[cam] == 'rgbd'} cam_frames = {cam:'/camera%i_rgb_optical_frame'%cam for cam in rgbd_dirs} tfm_pubs = {} hydra_dat = {} pot_dat = {} _, hydra_dat['l'], pot_dat['l'] = load_data(data_file, 'l', freq, speed, hydra_only=True) _, hydra_dat['r'], pot_dat['r'] = load_data(data_file, 'r', freq, speed, hydra_only=True) tmin, _, nsteps = relative_time_streams(hydra_dat.values() + pot_dat.values(), freq, speed) if rospy.get_name() == "/unnamed": rospy.init_node("visualize_demo") ## publishers for unfiltered-data: for lr in grippers: tfm_pubs[lr] = rospy.Publisher('/%s_hydra_estimate'%(lr), PoseStamped) ## get the point-cloud stream pc_strms = {cam:streamize_rgbd_pc(rgbd_dirs[cam], cam_frames[cam], freq, tstart=tmin,speed=speed,verbose=verbose) for cam in rgbd_dirs} pc_pubs = {cam:rospy.Publisher('/point_cloud%i'%cam, PointCloud2) for cam in rgbd_dirs} cam_tfms = get_cam_transforms (calib_file, len(cam_types)) for cam in rgbd_dirs: if cam != 1: publish_static_tfm(cam_frames[1], cam_frames[cam], cam_tfms[cam]) sleeper = rospy.Rate(freq) T_far = np.eye(4) T_far[0:3,3] = [10,10,10] handles = [] dat_snext = {lr:{} for lr in grippers} for lr in grippers: dat_snext[lr]['h'] = stream_soft_next(hydra_dat[lr]) dat_snext[lr]['pot'] = stream_soft_next(pot_dat[lr]) prev_ang = {'l': 0, 'r': 0} for i in xrange(nsteps): if prompt: raw_input("Hit enter when ready.") if verbose: print "Time stamp: ", tmin+(0.0+i*speed)/freq ## show the point-cloud: for cam in pc_strms: try: pc = pc_strms[cam].next() if pc is not None: if verbose: print "pc%i ts:"%cam, pc.header.stamp.to_sec() pc.header.stamp = rospy.Time.now() pc_pubs[cam].publish(pc) else: if verbose: print "pc%i ts:"%cam,None except StopIteration: pass ests = {} tfms = [] ang_vals = [] for lr in grippers: ests[lr] = dat_snext[lr]['h']() ang_val = dat_snext[lr]['pot']() if ang_val != None and not np.isnan(ang_val): prev_ang[lr] = ang_val ang_val = ang_val else: ang_val = prev_ang[lr] ang_val *= 2 if ests[lr] is None: tfms.append(T_far) else: tfms.append(ests[lr]) ang_vals.append(rad_angle(ang_val)) handles = draw_trajectory(cam_frames[1], tfms, color=(1,1,0,1), open_fracs=ang_vals) for lr in grippers: if ests[lr] is not None: tfm_pubs[lr].publish(conversions.pose_to_stamped_pose(conversions.hmat_to_pose(ests[lr]), cam_frames[1])) sleeper.sleep() empty_cloud = PointCloud2() for cam in pc_pubs: pc_pubs[cam].publish(empty_cloud)