def do_segmentation(obj_name):
seg_svc = rospy.ServiceProxy("/interactive_segmentation", ProcessCloud)
pc = rospy.wait_for_message("/drop/points", sm.PointCloud2)
pc_tf = ru.transformPointCloud2(pc, exec_verb_traj.Globals.pr2.tf_listener, "base_footprint", pc.header.frame_id)
print "select the %s" % (obj_name)
pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in = pc_tf)).cloud_out
return pc_sel
def do_segmentation(obj_name):
seg_svc = rospy.ServiceProxy("/interactive_segmentation", ProcessCloud)
pc = rospy.wait_for_message("/drop/points", sm.PointCloud2)
pc_tf = ros_utils.transformPointCloud2(pc, exec_verb_traj.Globals.pr2.tf_listener, "base_footprint", pc.header.frame_id)
print "select the %s" % (obj_name)
pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in = pc_tf)).cloud_out
return pc_sel
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("out")
parser.add_argument("--cloud_in",default="/drop/points",nargs = "?")
parser.add_argument("--dont_transform", action="store_true")
args = parser.parse_args()
from brett2 import ros_utils
from jds_utils import conversions
import roslib
roslib.load_manifest('tf')
import tf
import rospy
import numpy as np
import sensor_msgs.msg as sm
from brett2.ros_utils import transformPointCloud2
rospy.init_node("get_point_cloud")
listener = tf.TransformListener()
rospy.sleep(.3)
pc = rospy.wait_for_message(args.cloud_in, sm.PointCloud2)
if args.dont_transform:
pc_tf = pc
else:
pc_tf = transformPointCloud2(pc, listener, "base_footprint", pc.header.frame_id)
xyz, bgr = ros_utils.pc2xyzrgb(pc_tf)
np.savez(args.out, xyz=xyz, bgr=bgr)
pr2.lgrip.open()
pr2.rarm.goto_posture('side')
pr2.larm.goto_posture('side')
verb = select_from_list(verb_list + ["swap-tool"])
if verb in verb_info:
arg_names = F[verb]["arg_names"]
print "arguments: %s"%" ".join(arg_names)
pr2.join_all()
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])
pr2.rgrip.open()
pr2.lgrip.open()
pr2.rarm.goto_posture('side')
pr2.larm.goto_posture('side')
pr2.join_all()
demo_info = verbs.get_demo_info(demo_name)
demo_data = verbs.get_demo_data(demo_name)
make_req = MakeTrajectoryRequest()
make_req.verb = demo_info["verb"]
if not args.test:
seg_svc = rospy.ServiceProxy("/interactive_segmentation", ProcessCloud)
pc = rospy.wait_for_message("/drop/points", sm.PointCloud2)
pc_tf = ru.transformPointCloud2(pc, exec_verb_traj.Globals.pr2.tf_listener,
"base_footprint", pc.header.frame_id)
for obj_name in demo_info["args"]:
print "select the %s" % obj_name
pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in=pc_tf)).cloud_out
make_req.object_clouds.append(pc_sel)
else:
object_clouds = [
demo_data["object_clouds"][obj_name]["xyz"]
for obj_name in demo_data["object_clouds"].keys()
]
for i in xrange(len(object_clouds)):
cloud = object_clouds[i].reshape(-1, 3)
#translation = np.random.randn(1,3) * np.array([[.1,.1,0]])
#cloud += translation
pr2.rgrip.open()
pr2.lgrip.open()
pr2.rarm.goto_posture('side')
pr2.larm.goto_posture('side')
verb = select_from_list(verb_list + ["swap-tool"])
if verb in verb_info:
arg_names = F[verb]["arg_names"]
print "arguments: %s" % " ".join(arg_names)
pr2.join_all()
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"]:
pr2.lgrip.open()
pr2.rarm.goto_posture('side')
pr2.larm.goto_posture('side')
pr2.join_all()
demo_info = verbs.get_demo_info(demo_name)
demo_data = verbs.get_demo_data(demo_name)
make_req = MakeTrajectoryRequest()
make_req.verb = demo_info["verb"]
if not args.test:
seg_svc = rospy.ServiceProxy("/interactive_segmentation", ProcessCloud)
pc = rospy.wait_for_message("/drop/points", sm.PointCloud2)
pc_tf = ru.transformPointCloud2(pc, exec_verb_traj.Globals.pr2.tf_listener, "base_footprint", pc.header.frame_id)
for obj_name in demo_info["args"]:
print "select the %s"%obj_name
pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in = pc_tf)).cloud_out
make_req.object_clouds.append(pc_sel)
else:
object_clouds = [demo_data["object_clouds"][obj_name]["xyz"]
for obj_name in demo_data["object_clouds"].keys()]
for i in xrange(len(object_clouds)):
cloud = object_clouds[i].reshape(-1,3)
#translation = np.random.randn(1,3) * np.array([[.1,.1,0]])
#cloud += translation
center = cloud.mean(axis=0)
from numpy import cos,sin,pi
import sensor_msgs.msg as sm
import bulletsim_msgs.msg as bm
import bulletsim_msgs.srv as bs
from brett2 import ros_utils
from jds_utils import conversions
import roslib; roslib.load_manifest('tf'); import tf
import numpy as np
sub = rospy.Subscriber("/preprocessor/points", sm.PointCloud2)
service = rospy.ServiceProxy('/initialization', bs.Initialization)
rospy.init_node("loop_initialization")
listener = tf.TransformListener()
#rviz = ros_utils.RvizWrapper()
while not rospy.is_shutdown():
cloud = rospy.wait_for_message("/preprocessor/points", sm.PointCloud2)
cloud_tf = ros_utils.transformPointCloud2(cloud, listener, "ground", cloud.header.frame_id)
req = bs.InitializationRequest()
req.cloud = cloud_tf
resp = service.call(req)
#pose_array = gm.PoseArray()
#pose_array.header = cloud.header
#pose_array.poses = [gm.Pose(position=point, orientation=gm.Quaternion(0,0,0,1)) for point in resp.objectInit.rope.nodes]
#rviz.draw_curve(pose_array, 0)
#rospy.sleep(1)
parser = argparse.ArgumentParser()
parser.add_argument("out")
parser.add_argument("--cloud_in", default="/drop/points", nargs="?")
parser.add_argument("--dont_transform", action="store_true")
args = parser.parse_args()
from brett2 import ros_utils
from jds_utils import conversions
import roslib
roslib.load_manifest('tf')
import tf
import rospy
import numpy as np
import sensor_msgs.msg as sm
from brett2.ros_utils import transformPointCloud2
rospy.init_node("get_point_cloud")
listener = tf.TransformListener()
rospy.sleep(.3)
pc = rospy.wait_for_message(args.cloud_in, sm.PointCloud2)
if args.dont_transform:
pc_tf = pc
else:
pc_tf = transformPointCloud2(pc, listener, "base_footprint",
pc.header.frame_id)
xyz, bgr = ros_utils.pc2xyzrgb(pc_tf)
np.savez(args.out, xyz=xyz, bgr=bgr)
