pc = rospy.wait_for_message("/drop/points", sm.PointCloud2)
pc_tf = ros_utils.transformPointCloud2(pc, pr2.tf_listener, "base_footprint", pc.header.frame_id)
n_sel = F[verb]["nargs"].value
arg_clouds = []
for i_sel in xrange(n_sel):
pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in = pc_tf)).cloud_out
xyz, rgb = ros_utils.pc2xyzrgb(pc_sel)
arg_clouds.append(xyz.reshape(-1,3))
make_and_execute_verb_traj(verb, arg_clouds)
raw_input("press enter to continue")
elif verb in ["swap-tool"]:
l_switch_posture = np.array([ 0.773, 0.595, 1.563, -1.433, 0.478, -0.862, .864])
lr = select_from_list(["left", "right"])
if lr == 'l':
arm, grip, jpos = pr2.larm, pr2.lgrip, l_switch_posture
else:
arm, grip, jpos = pr2.rarm, pr2.rgrip, PR2.mirror_arm_joints(l_switch_posture)
arm.goto_joint_positions(jpos)
grip.open()
raw_input("press enter when ready")
grip.close()
pr2.join_all()
pc.header.frame_id)
n_sel = F[verb]["nargs"].value
arg_clouds = []
for i_sel in xrange(n_sel):
pc_sel = seg_svc.call(
ProcessCloudRequest(cloud_in=pc_tf)).cloud_out
xyz, rgb = ros_utils.pc2xyzrgb(pc_sel)
arg_clouds.append(xyz.reshape(-1, 3))
make_and_execute_verb_traj(verb, arg_clouds)
raw_input("press enter to continue")
elif verb in ["swap-tool"]:
l_switch_posture = np.array(
[0.773, 0.595, 1.563, -1.433, 0.478, -0.862, .864])
lr = select_from_list(["left", "right"])
if lr == 'l':
arm, grip, jpos = pr2.larm, pr2.lgrip, l_switch_posture
else:
arm, grip, jpos = pr2.rarm, pr2.rgrip, PR2.mirror_arm_joints(
l_switch_posture)
arm.goto_joint_positions(jpos)
grip.open()
raw_input("press enter when ready")
grip.close()
pr2.join_all()
