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_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()
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 rviz_kalman(demo_dir, bag_file, data_file, calib_file, freq, use_rgbd, use_smoother, use_spline, customized_shift, single_camera): ''' For rgbd, data_file and bag_file are redundant Otherwise, demo_dir is redundant ''' if use_rgbd: bag_file = osp.join(demo_dir, 'demo.bag') rgbd1_dir = osp.join(demo_dir, 'camera_#1') rgbd2_dir = osp.join(demo_dir, 'camera_#2') data_file = osp.join(demo_dir, 'demo.data') bag = rosbag.Bag(bag_file) else: bag = rosbag.Bag(bag_file) dat = cp.load(open(data_file)) grippers = dat.keys() pub = rospy.Publisher('/point_cloud1', PointCloud2) pub2= rospy.Publisher('/point_cloud2', PointCloud2) c1_tfm_pub = {} c2_tfm_pub = {} hydra_tfm_pub = {} T_filt = {} ang_strm = {} ar1_strm = {} ar2_strm = {} hy_strm = {} smooth_hy = {} ## publishers for unfiltered-data: for lr in grippers: c1_tfm_pub[lr] = rospy.Publisher('/%s_ar1_estimate'%(lr), PoseStamped) c2_tfm_pub[lr] = rospy.Publisher('/%s_ar2_estimate'%(lr), PoseStamped) hydra_tfm_pub[lr] = rospy.Publisher('/%s_hydra_estimate'%(lr), PoseStamped) _, _, _, ar1_strm[lr], ar2_strm[lr], hy_strm = relative_time_streams(data_file, lr, freq, single_camera) ## run the kalman filter: nsteps, tmin, F_means, S,A,R = run_kalman_filter(data_file, lr, freq, use_spline, True, single_camera) ## run the kalman smoother: S_means, _ = smoother(A, R, F_means, S) X_kf = np.array(F_means) X_kf = np.reshape(X_kf, (X_kf.shape[0], X_kf.shape[1])).T X_ks = np.array(S_means) X_ks = np.reshape(X_ks, (X_ks.shape[0], X_ks.shape[1])).T # Shifting between filter and smoother: if customized_shift != None: shift = customized_shift else: shift = correlation_shift(X_kf, X_ks) X_ks = np.roll(X_ks,shift,axis=1) X_ks[:,:shift] = X_ks[:,shift][:,None] if use_smoother: T_filt[lr] = state_to_hmat(list(X_ks.T)) else: T_filt[lr] = state_to_hmat(list(X_kf.T)) ## load the potentiometer-angle stream: pot_data = cp.load(open(data_file))[lr]['pot_angles'] ang_ts = np.array([tt[1] for tt in pot_data]) ## time-stamps ang_vals = [tt[0] for tt in pot_data] ## angles # plt.plot(ang_vals) # plt.show() ang_vals = [0*open_frac(x) for x in ang_vals] ang_strm[lr] = streamize(ang_vals, ang_ts, freq, lambda x : x[-1], tmin) if use_spline: smooth_hy[lr] = (t for t in fit_spline_to_stream(hy_strm, nsteps)) else: smooth_hy[lr] = hy_strm ## get the point-cloud stream cam1_frame_id = '/camera1_rgb_optical_frame' cam2_frame_id = '/camera2_rgb_optical_frame' if use_rgbd: pc1_strm = streamize_rgbd_pc(rgbd1_dir, cam1_frame_id, freq, tmin) if single_camera: pc2_strm = streamize_rgbd_pc(None, cam2_frame_id, freq, tmin) else: pc2_strm = streamize_rgbd_pc(rgbd2_dir, cam2_frame_id, freq, tmin) else: pc1_strm = streamize_pc(bag, '/camera1/depth_registered/points', freq, tmin) if single_camera: pc2_strm = streamize_pc(bag, None, freq, tmin) else: pc2_strm = streamize_pc(bag, '/camera2/depth_registered/points', freq, tmin) ## get the relative-transforms between the cameras: cam_tfm = get_cam_transform(calib_file) publish_static_tfm(cam1_frame_id, cam2_frame_id, cam_tfm) ## frame of the filter estimate: sleeper = rospy.Rate(freq) T_far = np.eye(4) T_far[0:3,3] = [10,10,10] handles = [] prev_ang = {'l': 0, 'r': 0} for i in xrange(nsteps): #raw_input("Hit next when ready.") print "Kalman ts: ", tmin+(0.0+i)/freq ## show the point-cloud: found_pc = False try: pc = pc1_strm.next() if pc is not None: print "pc1 ts:", pc.header.stamp.to_sec() pc.header.stamp = rospy.Time.now() pub.publish(pc) found_pc = True else: print "pc1 ts:",None except StopIteration: print "pc1 ts: finished" #print "no more point-clouds" pass try: pc2 = pc2_strm.next() if pc2 is not None: #print "pc2 not none" print "pc2 ts:", pc2.header.stamp.to_sec() pc2.header.stamp = rospy.Time.now() pub2.publish(pc2) found_pc = True else: print "pc2 ts:", None except StopIteration: print "pc2 ts: finished" pass ang_vals = [] T_filt_lr = [] for lr in grippers: ang_val = soft_next(ang_strm[lr]) if ang_val != None: prev_ang[lr] = ang_val ang_val = ang_val else: ang_val = prev_ang[lr] ang_vals.append(ang_val) T_filt_lr.append(T_filt[lr][i]) handles = draw_trajectory(cam1_frame_id, T_filt_lr, color=(1,1,0,1), open_fracs=ang_vals) # draw un-filtered estimates: for lr in grippers: ar1_est = soft_next(ar1_strm[lr]) if ar1_est != None: c1_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(ar1_est), cam1_frame_id)) else: c1_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(T_far), cam1_frame_id)) ar2_est = soft_next(ar2_strm[lr]) if ar2_est != None: c2_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(ar2_est), cam1_frame_id)) else: c2_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(T_far), cam1_frame_id)) hy_est = soft_next(smooth_hy[lr]) if hy_est != None: hydra_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(hy_est), cam1_frame_id)) else: hydra_tfm_pub[lr].publish(pose_to_stamped_pose(hmat_to_pose(T_far), cam1_frame_id)) sleeper.sleep()