Пример #1
0
def delete_demo(demo_type, demo_name):
    
    assert demo_type != '', 'DEMO TYPE needs to be valid.'
    assert demo_name != '', 'DEMO NAME needs to be valid.'
    
    demo_type_dir = osp.join(demo_files_dir, demo_type)
    demo_dir= osp.join(demo_type_dir, demo_name)
    
    if not osp.exists(demo_dir):
        redprint('%s does not exist! Invalid demo.'%demo_dir)
        return
    
    yellowprint('This cannot be undone!')
    if not yes_or_no('Are you still sure you want to delete %s?'%demo_name):
        return
    
    # If it was the last demo recorded, update the latest_demo.txt file
    # Otherwise don't bother
    latest_demo_file = osp.join(demo_type_dir, latest_demo_name)
    with open(latest_demo_file,'r') as fh: demo_num = int(fh.read())
    if demo_name == demo_names.base_name%demo_num:
        with open(latest_demo_file,'w') as fh: fh.write(str(demo_num-1))
    
    # Delete contents of demo_dir
    shutil.rmtree(demo_dir)
    
    # Update master_file
    master_file = osp.join(demo_type_dir, master_name)
    with open(master_file,"r") as fh: master_lines = fh.readlines()
    
    fh = open(master_file,"w")
    for line in master_lines:
      if demo_name not in line: fh.write(line)
    fh.close()
        yaml.dump(demos, fh)

if __name__=='__main__':
    parser = argparse.ArgumentParser()
    parser.add_argument("--demo_type",help="Type of demonstration")
    parser.add_argument("--demo_name",help="Name of demo", default='', type=str)
    args = parser.parse_args()
    
    if args.demo_name != '':
        if osp.isfile(osp.join(demo_type_dir, demo["demo_name"], demo_names.ann_name)):
            if yes_or_no('Annotation file already exists for this demo. Overwrite?'):
                generate_annotation(args.demo_type, args.demo_name)
        else:
            generate_annotation(args.demo_type, args.demo_name)
    else: 
        # if args.demo_name == '', run generate annotation for all the demos in the directory
        demo_type_dir = osp.join(demo_files_dir, args.demo_type)
        demo_master_file = osp.join(demo_type_dir, master_name)
        
        with open(demo_master_file, 'r') as fh:
            demos_info = yaml.load(fh)

        for demo in demos_info["demos"]:
            if not osp.isfile(osp.join(demo_type_dir, demo["demo_name"], demo_names.ann_name)):
                generate_annotation(args.demo_type, demo["demo_name"])
            else:
                yellowprint("Annotation file exists for %s. Not overwriting."%demo["demo_name"])

    greenprint("Done annotation generation.")
    
    demo_type_dir = osp.join(demo_files_dir, args.demo_type)
    demo_master_file = osp.join(demo_type_dir, master_name)
    
    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 args.last == -1:
            latest_demo_file = osp.join(demo_type_dir, latest_demo_name)
            if osp.isfile(latest_demo_file):
                with open(latest_demo_file,'r') as fh:
                    last = int(fh.read()) + 1
            else:
                redprint("No demos!")

        
        for i in xrange (first, last+1):
            demo_name = demo_names.base_name%i
            demo_dir = osp.join(demo_files_dir, args.demo_type, demo_name)
            if osp.exists(demo_dir):
                if args.prompt:
                    raw_input('Hit enter for %s.'%demo_name)
                yellowprint("Visualizing: %s"%demo_name)
                if args.use_traj:
                    view_tracking_on_rviz(demo_type=args.demo_type, demo_name=demo_name, tps_model_fname=args.tps_fname,
                                          freq=args.freq, speed=args.speed, 
                                          use_smoother=args.use_smoother, prompt=args.prompt, verbose=args.verbose)
                else:
                    if args.hydra_only:
                        view_hydra_demo_on_rviz(demo_type=args.demo_type, demo_name=demo_name, 
                                                freq=args.freq, speed=args.speed, prompt=args.prompt, verbose=args.verbose)
                    else:
                        view_demo_on_rviz(demo_type=args.demo_type, demo_name=demo_name, 
                                          freq=args.freq, speed=args.speed, 
                                          main=args.main, prompt=args.prompt, verbose=args.verbose)

                if args.prompt_delete and yes_or_no('Do you want to delete %s?'%demo_name):
                    delete_demo(args.demo_type, demo_name)
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)
def view_tracking_on_rviz(demo_type, demo_name, tps_model_fname, freq=30.0, speed=1.0, use_smoother=True, prompt=False, verbose=False):
    """
    Visualizes demo after kalman tracking/smoothing on rviz.
    @demo_type, @demo_name: demo identification.
    @freq: basically measure of fine-ness of timesteps.
    @speed: how fast to replay demo.
    @main: which sensor to display the marker for
    @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)
    traj_file = osp.join(demo_dir, demo_names.traj_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(traj_file):
        yellowprint("%s does not exist for this demo. Running kalman filter/smoother now with default args."%demo_names.traj_name)
        data_file = osp.join(demo_dir, demo_names.data_name)
        if not osp.isfile(data_file):
            yellowprint("%s does not exist for this demo. Extracting now."%demo_names.data_name)
            ed.save_observations_rgbd(demo_type, demo_name)
        filter_traj(demo_dir, tps_model_fname=tps_model_fname, save_tps=True, do_smooth=True, plot='', block=False)
    with open(traj_file, 'r') as fh: traj = cp.load(fh)
    
    # get grippers used
    grippers = traj.keys()

    if rospy.get_name() == "/unnamed":
        rospy.init_node("visualize_demo")

    # data 
    rgbd_dirs = {cam:osp.join(demo_dir,demo_names.video_dir%cam) for cam in cam_types if cam_types[cam] == 'rgbd'}
    pc_pubs = {cam:rospy.Publisher('/point_cloud%i'%cam, PointCloud2) for cam in rgbd_dirs}
    cam_frames = {cam:'/camera%i_rgb_optical_frame'%cam 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])

    # Remove segment "done", it is just a single frame
    segs = sorted(traj[grippers[0]].keys())
    if 'done' in segs: segs.remove('done')
    
    sleeper = rospy.Rate(freq)
    T_far = np.eye(4)
    T_far[0:3,3] = [10,10,10]
    
    for seg in segs:
        if prompt:
            raw_input("Press enter for segment %s."%seg)
        else:
            yellowprint("Segment %s beginning."%seg)
            time.sleep(1)
        
        # Initializing data streams:
        traj_strms = {}
        pot_strms = {}
        tfms_key = 'tfms_s' if use_smoother else 'tfms'

        for lr in grippers:
            traj_strms[lr] = streamize(traj[lr][seg][tfms_key], traj[lr][seg]['stamps'], freq, avg_transform, speed=speed)
            # HACK
            pot_strms[lr] = streamize(traj[lr][seg]['pot_angles'][:len(traj[lr][seg]['stamps'])], traj[lr][seg]['stamps'], freq, np.mean, speed=speed)
        
        tmin, tmax, nsteps = relative_time_streams(traj_strms.values() + pot_strms.values(), freq, speed)
        


        pc_strms = {cam:streamize_rgbd_pc(rgbd_dirs[cam], cam_frames[cam], freq, tstart=tmin, tend=tmax,speed=speed,verbose=verbose) for cam in rgbd_dirs}
    
        prev_ang = {'l': 0, 'r': 0}

        dat_snext = {lr:{} for lr in grippers}
        for lr in grippers:
            dat_snext[lr]['traj'] = stream_soft_next(traj_strms[lr])
            dat_snext[lr]['pot'] = stream_soft_next(pot_strms[lr])
        

        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:
            found_pc = False
            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)
                        found_pc = True
                    else:
                        if verbose:
                            print "pc%i ts:"%cam,None
                except StopIteration:
                    pass
    
            tfms = []
            ang_vals  = []
    
            for lr in grippers:
		if lr == 'r':
		    continue
                tfm = dat_snext[lr]['traj']()
                
                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 tfm is None:
                    tfms.append(T_far)
                else:
                    tfms.append(tfm)
                    
                ang_vals.append(rad_angle(ang_val))
    
    
            handles = draw_trajectory(cam_frames[cam], tfms, color=(1,0,0,1), open_fracs=ang_vals)
            
            time.sleep(1.0/freq)
def view_demo_on_rviz(demo_type, demo_name, freq, speed=1.0, main='h', prompt=False, verbose=False):
    """
    Visualizes recorded demo on rviz (without kalman filter/smoother data).
    @demo_type, @demo_name: demo identification.
    @freq: basically measure of fine-ness of timesteps.
    @speed: how fast to replay demo.
    @main: which sensor to display the marker for
    @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.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.data_name)
        ed.save_observations_rgbd(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 = {}

    cam_dat = {}
    hydra_dat = {}
    pot_dat = {}
    
    _, cam_dat['l'], hydra_dat['l'], pot_dat['l'] = load_data(data_file, 'l', freq, speed)
    _,  cam_dat['r'], hydra_dat['r'], pot_dat['r'] = load_data(data_file, 'r', freq, speed)

    all_cam_strms = []
    for lr in 'lr':
        for cam in cam_dat[lr].keys():
            all_cam_strms.append(cam_dat[lr][cam]['stream'])
    tmin, _, nsteps = relative_time_streams(hydra_dat.values() + pot_dat.values() + all_cam_strms, freq, speed)

    if rospy.get_name() == "/unnamed":
        rospy.init_node("visualize_demo")


    ## publishers for unfiltered-data:
    for lr in grippers:
        tfm_pubs[lr] = {}
        for cam in cam_types:
            tfm_pubs[lr][cam] = rospy.Publisher('/%s_ar%i_estimate'%(lr,cam), PoseStamped)
        tfm_pubs[lr]['h'] = 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}

#     import IPython
#     IPython.embed()

    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 = []
    
    prev_ang = {'l': 0, 'r': 0}
    
    
    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])
        
        for cam in cam_types:
            dat_snext[lr][cam] = stream_soft_next(cam_dat[lr][cam]['stream'])
        
    
    
    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:
        found_pc = False
        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)
                    found_pc = True
                else:
                    if verbose:
                        print "pc%i ts:"%cam,None
            except StopIteration:
                pass

        next_est = {lr:{} for lr in grippers}
        tfms = []
        ang_vals  = []
        
        if main != 'h': main = int(main)

        for lr in grippers:
            next_est[lr]['h'] = dat_snext[lr]['h']()
            for cam in cam_types:
                next_est[lr][cam] = dat_snext[lr][cam]()

            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
            
            tfm = next_est[lr][main]
            if tfm is None:
                tfms.append(T_far)
            else:
                tfms.append(tfm)
            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:
            for m,est in next_est[lr].items():
                if est != None:
                    tfm_pubs[lr][m].publish(conversions.pose_to_stamped_pose(conversions.hmat_to_pose(est), cam_frames[1]))
                else:
                    tfm_pubs[lr][m].publish(conversions.pose_to_stamped_pose(conversions.hmat_to_pose(T_far), cam_frames[1]))
        
        sleeper.sleep()