def get_tfm(self, master_node, child_node, angle):
# angle in degrees
mtfm = self.masterG.edge[master_node]["cor"]['tfm']
angle = utils.rad_angle(angle)
rot = np.eye(4)
rot[0:3,0:3] = utils.rotation_matrix(np.array([0,0,1]), angle*self.childN["angle_scale"])
ctfm = self.childN["graph"].edge[self.childN["primary"]][child_node]['tfm']
return mtfm.dot(rot).dot(self.gpTfm).dot(ctfm)
def get_tfm(master_node, child_node, angle):
# angle in degrees
mprimary_node = mG_opt.node[master]["primary"]
#print masterG.edge[master_node][mprimary_node]
mtfm = masterG.edge[master_node][mprimary_node]['tfm']
angle = utils.rad_angle(angle)
rot = np.eye(4)
rot[0:3,0:3] = utils.rotation_matrix(np.array([0,0,1]), angle*mG_opt.node[group]["angle_scale"])
cprimary_node = mG_opt.node[group]["primary"]
ctfm = mG_opt.node[group]["graph"].edge[cprimary_node][child_node]['tfm']
return mtfm.dot(rot).dot(mG_opt.edge[master][group]['tfm']).dot(ctfm)
def f_objective (X):
"""
Objective function to make transforms close to average transforms.
Sum of the norms of matrix differences between each Tij and
"""
obj = 0
zaxis = np.array([0,0,1])
for g1,g2 in mG.edges_iter():
for angle in mG.edge[g1][g2]['avg_tfm']:
t1 = np.r_[get_mat_from_node(X,g1),np.array([[0,0,0,1]])]
t2 = np.r_[get_mat_from_node(X,g2),np.array([[0,0,0,1]])]
rad = utils.rad_angle(angle)
tot_angle = rad*(-mG.node[g1]["angle_scale"]+mG.node[g2]["angle_scale"])
rot = np.eye(4)
rot[0:3,0:3] = utils.rotation_matrix(zaxis, tot_angle)
tfm = t1.dot(rot).dot(nlg.inv(t2))
obj += nlg.norm(tfm - mG.edge[g1][g2]['avg_tfm'][angle])
return obj
def test_gc2 (n, add_noise=False):
##
# Markers 1,2,3 are on l finger
# Markers 4,5,6 are on r finger
# 7 and 8 are on the middle
##
### Initializing everything
# create calib info
calib_info = {"master": {"ar_markers":[7,8],
"master_group":True,
"angle_scale":0},
"l": {"ar_markers":[1,2,3],
"angle_scale":1},
"r": {"ar_markers":[4,5,6],
"angle_scale":-1}}
masterGraph = nx.DiGraph()
# Setup the groups
for group in calib_info:
masterGraph.add_node(group)
masterGraph.node[group]["graph"] = nx.Graph()
masterGraph.node[group]["angle_scale"] = calib_info[group]['angle_scale']
if calib_info[group].get("ar_markers") is None:
calib_info[group]["ar_markers"] = []
if calib_info[group].get("hydras") is None:
calib_info[group]["hydras"] = []
if calib_info[group].get("ps_markers") is None:
calib_info[group]["ps_markers"] = []
if calib_info[group].get("master_group") is not None:
masterGraph.node[group]["master"] = 1
masterGraph.node[group]['angle_scale'] = 0
masterGraph.node[group]["markers"] = calib_info[group]["ar_markers"] + calib_info[group]["hydras"] + calib_info[group]["ps_markers"]
masterGraph.node[group]["calib_info"] = calib_info[group]
### Done initializing.
T12 = make_obs()
T23 = make_obs()
T45 = make_obs()
T56 = make_obs()
T78 = make_obs()
T7cor = make_obs()
Tcor1 = make_obs()
Tcor4 = make_obs()
zaxis = np.array([0,0,1])
for _ in range(n):
theta = -15 + np.random.random()*30
theta2 = utils.rad_angle(theta)
T7 = make_obs()
R1 = np.eye(4)
R1[0:3,0:3] = rotation_matrix(zaxis,theta2)
R4 = np.eye(4)
R4[0:3,0:3] = rotation_matrix(zaxis,-theta2)
if add_noise:
Tcor = T7.dot(T7cor)
noise = make_obs(0.01,0.01)
T1 = Tcor.dot(R1).dot(Tcor1).dot(noise)
noise = make_obs(0.01,0.01)
T2 = T1.dot(T12).dot(noise)
noise = make_obs(0.01,0.01)
T3 = T2.dot(T23).dot(noise)
noise = make_obs(0.01,0.01)
T4 = Tcor.dot(R4).dot(Tcor4).dot(noise)
noise = make_obs(0.01,0.01)
T5 = T4.dot(T45).dot(noise)
noise = make_obs(0.01,0.01)
T6 = T5.dot(T56).dot(noise)
noise = make_obs(0.01,0.01)
T8 = T7.dot(T78).dot(noise)
else:
noise = np.eye(4)
Tcor = T7.dot(T7cor)
T1 = Tcor.dot(R1).dot(Tcor1).dot(noise)
T2 = T1.dot(T12).dot(noise)
T3 = T2.dot(T23).dot(noise)
T4 = Tcor.dot(R4).dot(Tcor4).dot(noise)
T5 = T4.dot(T45).dot(noise)
T6 = T5.dot(T56).dot(noise)
T8 = T7.dot(T78).dot(noise)
# Randomly sample a few until done maybe?
tfms = {1:T1,2:T2,3:T3,4:T4,5:T5,6:T6,7:T7,8:T8}
gc.update_groups_from_observations(masterGraph, tfms, theta)
init = np.zeros(36)
init[0:12] = nlg.inv(T78).dot(T7cor)[0:3,:].reshape(12,order='F')
init[12:24] = nlg.inv(Tcor4)[0:3,:].reshape(12,order='F')
init[24:36] = nlg.inv(Tcor1)[0:3,:].reshape(12,order='F')
G_opt = gc.compute_relative_transforms(masterGraph, 5)#, init)
# 7,8 markers
group = G_opt.node["master"]
G = group["graph"]
print "T78"
print "Original:"
print T78
print "Calculated:"
print G.edge[7][8]['tfm'], '\n'
# 1,2,3 markers
group = G_opt.node["l"]
G = group["graph"]
print "T12"
print "Original:"
print T12
print "Calculated:"
print G.edge[1][2]['tfm'], '\n'
print "T23"
print "Original:"
print T23
print "Calculated:"
print G.edge[2][3]['tfm'], '\n'
print "T31"
print "Original:"
print nlg.inv(T12.dot(T23))
print "Calculated:"
print G.edge[3][1]['tfm'], '\n'
# 4,5,6 markers
group = G_opt.node["r"]
G = group["graph"]
print "T45"
print "Original:"
print T45
print "Calculated:"
print G.edge[4][5]['tfm'], '\n'
print "T56"
print "Original:"
print T56
print "Calculated:"
print G.edge[5][6]['tfm'], '\n'
print "T64"
print "Original:"
print nlg.inv(T45.dot(T56))
print "Calculated:"
print G.edge[6][4]['tfm'], '\n'
# Let's try random angles:
theta = -15 + np.random.random()*30
theta2 = utils.rad_angle(theta)
R1 = np.eye(4)
R1[0:3,0:3] = rotation_matrix(zaxis,theta2)
print "T71"
print "Original:"
print T7cor.dot(R1).dot(Tcor1)
print "Calculated:"
print G_opt.edge["master"]["l"]['tfm_func'](7,1,theta), '\n'
print "norm diff:", nlg.norm(G_opt.edge["master"]["l"]['tfm_func'](7,1,theta)-T7cor.dot(R1).dot(Tcor1)), '\n'
print "Difference between cors"
T7cor_calib = G_opt.node["master"]["graph"].edge[7]["cor"]["tfm"]
print nlg.inv(T7cor).dot(T7cor_calib)
def test_gc3 (n, add_noise=False):
##
# Markers 1,2,3 are on l finger
# Markers 4,5,6 are on r finger
# 7 and 8 are on the middle
##
### Initializing everything
# create calib info
calib_info = {"master": {"ar_markers":[7,8],
"master_group":True,
"angle_scale":0},
"l": {"ar_markers":[1,2,3],
"angle_scale":1},
"r": {"ar_markers":[4,5,6],
"angle_scale":-1}}
gripper_calib = gc.GripperCalibrator(None, calib_info=calib_info)
gripper_calib.initialize_calibration(fake_data=True)
T12 = make_obs()
T23 = make_obs()
T45 = make_obs()
T56 = make_obs()
T78 = make_obs()
T7cor = make_obs()
Tcor1 = make_obs()
Tcor4 = make_obs()
zaxis = np.array([0,0,1])
for _ in range(n):
theta = -15 + np.random.random()*30
theta2 = utils.rad_angle(theta)
T7 = make_obs()
R1 = np.eye(4)
R1[0:3,0:3] = rotation_matrix(zaxis,theta2)
R4 = np.eye(4)
R4[0:3,0:3] = rotation_matrix(zaxis,-theta2)
if add_noise:
Tcor = T7.dot(T7cor)
noise = make_obs(0.01,0.01)
T1 = Tcor.dot(R1).dot(Tcor1).dot(noise)
noise = make_obs(0.01,0.01)
T2 = T1.dot(T12).dot(noise)
noise = make_obs(0.01,0.01)
T3 = T2.dot(T23).dot(noise)
noise = make_obs(0.01,0.01)
T4 = Tcor.dot(R4).dot(Tcor4).dot(noise)
noise = make_obs(0.01,0.01)
T5 = T4.dot(T45).dot(noise)
noise = make_obs(0.01,0.01)
T6 = T5.dot(T56).dot(noise)
noise = make_obs(0.01,0.01)
T8 = T7.dot(T78).dot(noise)
else:
noise = np.eye(4)
Tcor = T7.dot(T7cor)
T1 = Tcor.dot(R1).dot(Tcor1).dot(noise)
T2 = T1.dot(T12).dot(noise)
T3 = T2.dot(T23).dot(noise)
T4 = Tcor.dot(R4).dot(Tcor4).dot(noise)
T5 = T4.dot(T45).dot(noise)
T6 = T5.dot(T56).dot(noise)
T8 = T7.dot(T78).dot(noise)
# Randomly sample a few until done maybe?
tfms = {1:T1,2:T2,3:T3,4:T4,5:T5,6:T6,7:T7,8:T8}
gc.update_groups_from_observations(gripper_calib.masterGraph, tfms, theta)
init = np.zeros(36)
init[0:12] = nlg.inv(T78).dot(T7cor)[0:3,:].reshape(12,order='F')
init[12:24] = nlg.inv(Tcor4)[0:3,:].reshape(12,order='F')
init[24:36] = nlg.inv(Tcor1)[0:3,:].reshape(12,order='F')
gripper_calib.calibrated = gripper_calib.finish_calibration()
G_opt = gripper_calib.transform_graph
# 7,8 markers
group = G_opt.node["master"]
G = group["graph"]
print "T78"
print "Original:"
print T78
print "Calculated:"
print G.edge[7][8]['tfm'], '\n'
# 1,2,3 markers
group = G_opt.node["l"]
G = group["graph"]
print "T12"
print "Original:"
print T12
print "Calculated:"
print G.edge[1][2]['tfm'], '\n'
print "T23"
print "Original:"
print T23
print "Calculated:"
print G.edge[2][3]['tfm'], '\n'
print "T31"
print "Original:"
print nlg.inv(T12.dot(T23))
print "Calculated:"
print G.edge[3][1]['tfm'], '\n'
# 4,5,6 markers
group = G_opt.node["r"]
G = group["graph"]
print "T45"
print "Original:"
print T45
print "Calculated:"
print G.edge[4][5]['tfm'], '\n'
print "T56"
print "Original:"
print T56
print "Calculated:"
print G.edge[5][6]['tfm'], '\n'
print "T64"
print "Original:"
print nlg.inv(T45.dot(T56))
print "Calculated:"
print G.edge[6][4]['tfm'], '\n'
# Let's try random angles:
theta = -15 + np.random.random()*30
theta2 = utils.rad_angle(theta)
R1 = np.eye(4)
R1[0:3,0:3] = rotation_matrix(zaxis,theta2)
print "Theta: ",theta
print "T71"
print "Original:"
print T7cor.dot(R1).dot(Tcor1)
print "Calculated:"
print G_opt.edge["master"]["l"]['tfm_func'](7,1,theta), '\n'
print "norm diff:", nlg.norm(G_opt.edge["master"]["l"]['tfm_func'](7,1,theta)-T7cor.dot(R1).dot(Tcor1)), '\n'
theta = -15 + np.random.random()*30
theta2 = utils.rad_angle(theta)
R1 = np.eye(4)
R1[0:3,0:3] = rotation_matrix(zaxis,theta2)
print "Theta: ",theta
print "T71"
print "Original:"
print T7cor.dot(R1).dot(Tcor1)
print "Calculated:"
print G_opt.edge["master"]["l"]['tfm_func'](7,1,theta), '\n'
print "norm diff:", nlg.norm(G_opt.edge["master"]["l"]['tfm_func'](7,1,theta)-T7cor.dot(R1).dot(Tcor1)), '\n'
print "Difference between cors"
T7cor_calib = G_opt.node["master"]["graph"].edge[7]["cor"]["tfm"]
print nlg.inv(T7cor).dot(T7cor_calib)
return gripper_calib
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()