def do_set_table_params(args):
if args.manual_val:
rospy.set_param("table_bounds", args.manual_val)
else:
pc = rospy.wait_for_message(args.input_cloud, sm.PointCloud2)
pc_tf = ros_utils.transformPointCloud2(pc, pr2.tf_listener, "base_footprint", pc.header.frame_id)
xyz,rgb = ros_utils.pc2xyzrgb(pc_tf)
table_bounds = list(tabletop.get_table_dimensions(xyz))
rospy.set_param("table_height", float(table_bounds[-1]))
table_bounds[-1] -= args.downshift
table_bounds[-2] -= args.downshift
print "table bounds", table_bounds
rospy.set_param("table_bounds", " ".join([str(x) for x in table_bounds]))
def create_obstacle_env(env):
clone=env.CloneSelf(1)
point_cloud = rospy.wait_for_message("/drop/points", sm.PointCloud2)
xyz, bgr = ros_utils.pc2xyzrgb(point_cloud)
xyz = ros_utils.transform_points(xyz, Globals.pr2.tf_listener, "base_footprint", point_cloud.header.frame_id)
#xyz = xyz[250:, 250:]
clusters = tabletop.segment_tabletop_scene(xyz,'base_footprint')
bounds = tabletop.get_table_dimensions(xyz)
kinbodies.create_box_from_bounds(clone,bounds,"table")
print len(clusters),"clusters"
for (i_clu,clu) in enumerate(clusters):
x,y,z,r,h = tabletop.get_cylinder(clu)
#print kinbodies.create_cylinder(clone, (x,y,z),r,h, name="cylinder%i"%i_clu)
return clone
def do_set_table_params(args):
if args.manual_val:
rospy.set_param("table_bounds", args.manual_val)
else:
pc = rospy.wait_for_message(args.input_cloud, sm.PointCloud2)
pc_tf = ros_utils.transformPointCloud2(pc, pr2.tf_listener,
"base_footprint",
pc.header.frame_id)
xyz, rgb = ros_utils.pc2xyzrgb(pc_tf)
table_bounds = list(tabletop.get_table_dimensions(xyz))
rospy.set_param("table_height", float(table_bounds[-1]))
table_bounds[-1] -= args.downshift
table_bounds[-2] -= args.downshift
print "table bounds", table_bounds
rospy.set_param("table_bounds",
" ".join([str(x) for x in table_bounds]))
#!/usr/bin/env python
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("--downshift",type=float, default=0)
parser.add_argument("--input_cloud",type=str, default="/camera/depth_registered/points")
parser.add_argument("--manual_val", type=str, default="")
args = parser.parse_args()
from brett2.PR2 import PR2, ros_utils
import rospy
from point_clouds import tabletop
import sensor_msgs.msg as sm
rospy.init_node("set_table_params")
if args.manual_val:
rospy.set_param("table_bounds", args.manual_val)
else:
pr2 = PR2()
#pr2.rarm.goto_posture('side')
#pr2.larm.goto_posture('side')
pc = rospy.wait_for_message(args.input_cloud, sm.PointCloud2)
pc_tf = ros_utils.transformPointCloud2(pc, pr2.tf_listener, "base_footprint", pc.header.frame_id)
xyz,rgb = ros_utils.pc2xyzrgb(pc_tf)
table_bounds = list(tabletop.get_table_dimensions(xyz))
rospy.set_param("table_height", float(table_bounds[-1]))
table_bounds[-1] -= args.downshift
table_bounds[-2] -= args.downshift
print "table bounds", table_bounds
rospy.set_param("table_bounds", " ".join([str(x) for x in table_bounds]))
push_svc = rospy.ServiceProxy("push", Push)
req = PushRequest()
xyz0 = clusters[np.argmax(map(len, clusters))].reshape(1, -1, 3)
rgb0 = np.zeros(xyz0.shape, 'uint8')
point_cloud = xyzrgb2pc(xyz0, rgb0, "base_footprint")
req.point_cloud = point_cloud
push_svc.call(req)
if False:
pour_srv = rospy.ServiceProxy("pour", Pour)
req = PourRequest()
sort_inds = np.argsort(map(len, clusters))
for i in sort_inds[:2]:
xyz0 = clusters[i].reshape(1, -1, 3)
rgb0 = np.zeros(xyz0.shape, 'uint8')
cloud = xyzrgb2pc(xyz0, rgb0, "base_footprint")
req.object_clouds.append(cloud)
pour_srv.call(req)
if True:
env = pr2.robot.GetEnv()
if env.GetViewer() is None: env.SetViewer('qtcoin')
bounds = tabletop.get_table_dimensions(xyz)
from kinematics import kinbodies
kinbodies.create_box_from_bounds(env, bounds, "table")
for (i_clu, clu) in enumerate(clusters):
x, y, z, r, h = tabletop.get_cylinder(clu)
print kinbodies.create_cylinder(env, (x, y, z),
r,
h,
name="cylinder%i" % i_clu)
clusters = tabletop.segment_tabletop_scene(xyz,"base_footprint",plotting2d=True, plotting3d=True)
if False:
push_svc = rospy.ServiceProxy("push",Push)
req = PushRequest()
xyz0 = clusters[np.argmax(map(len,clusters))].reshape(1,-1,3)
rgb0 = np.zeros(xyz0.shape,'uint8')
point_cloud = xyzrgb2pc(xyz0, rgb0, "base_footprint")
req.point_cloud = point_cloud
push_svc.call(req)
if False:
pour_srv = rospy.ServiceProxy("pour", Pour)
req = PourRequest()
sort_inds = np.argsort(map(len,clusters))
for i in sort_inds[:2]:
xyz0 = clusters[i].reshape(1,-1,3)
rgb0 = np.zeros(xyz0.shape,'uint8')
cloud = xyzrgb2pc(xyz0, rgb0, "base_footprint")
req.object_clouds.append(cloud)
pour_srv.call(req)
if True:
env = pr2.robot.GetEnv()
if env.GetViewer() is None: env.SetViewer('qtcoin')
bounds = tabletop.get_table_dimensions(xyz)
from kinematics import kinbodies
kinbodies.create_box_from_bounds(env,bounds,"table")
for (i_clu,clu) in enumerate(clusters):
x,y,z,r,h = tabletop.get_cylinder(clu)
print kinbodies.create_cylinder(env, (x,y,z),r,h, name="cylinder%i"%i_clu)
