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
parser.add_argument("infile")
parser.add_argument("outfile")
args = parser.parse_args()
assert args.outfile.endswith("npz") or args.outfile.endswith(".h5")
from point_clouds import tabletop
import matplotlib.pyplot as plt, numpy as np
import rospy
rospy.init_node("segment_point_cloud")
plt.ion()
f = np.load(args.infile)
clusters = tabletop.segment_tabletop_scene(f["xyz"], plotting2d_all=True, plotting2d=True, plotting3d=True)
ids_str = raw_input("type wanted cluster ids in order and then press enter")
object_clusters = []
for num_str in ids_str.split():
object_clusters.append(clusters[int(num_str)])
if args.outfile.endswith("npz"):
np.save(args.outfile, np.array(object_clusters, dtype=object))
else:
import h5py
out = h5py.File(args.outfile, "w")
for clu in object_clusters:
out[str(clu)] = clu
out.close()
parser.add_argument("infile")
parser.add_argument("outfile")
args = parser.parse_args()
assert args.outfile.endswith("npz") or args.outfile.endswith(".h5")
from point_clouds import tabletop
import matplotlib.pyplot as plt, numpy as np
import rospy
rospy.init_node("segment_point_cloud")
plt.ion()
f = np.load(args.infile)
clusters = tabletop.segment_tabletop_scene(f["xyz"],
plotting2d_all=True,
plotting2d=True,
plotting3d=True)
ids_str = raw_input("type wanted cluster ids in order and then press enter")
object_clusters = []
for num_str in ids_str.split():
object_clusters.append(clusters[int(num_str)])
if args.outfile.endswith("npz"):
np.save(args.outfile, np.array(object_clusters, dtype=object))
else:
import h5py
out = h5py.File(args.outfile, "w")
for clu in object_clusters:
out[str(clu)] = clu
out.close()
import geometry_msgs.msg as gm
import sensor_msgs.msg as sm
import numpy as np
if __name__ == "__main__":
if rospy.get_name() == "/unnamed":
rospy.init_node("test_tabletop", disable_signals=True)
pr2 = PR2.create()
rviz = ros_utils.RvizWrapper.create()
rospy.sleep(1)
point_cloud = rospy.wait_for_message("/drop/points", sm.PointCloud2)
xyz, bgr = ros_utils.pc2xyzrgb(point_cloud)
xyz = ros_utils.transform_points(xyz, pr2.tf_listener, "base_footprint",
point_cloud.header.frame_id)
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))
from verb_msgs.srv import *
from brett2.ros_utils import xyzrgb2pc
import geometry_msgs.msg as gm
import sensor_msgs.msg as sm
import numpy as np
if __name__ == "__main__":
if rospy.get_name() == "/unnamed": rospy.init_node("test_tabletop", disable_signals=True)
pr2 = PR2.create()
rviz = ros_utils.RvizWrapper.create()
rospy.sleep(1)
point_cloud = rospy.wait_for_message("/drop/points", sm.PointCloud2)
xyz, bgr = ros_utils.pc2xyzrgb(point_cloud)
xyz = ros_utils.transform_points(xyz, pr2.tf_listener, "base_footprint", point_cloud.header.frame_id)
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]:
response.success = True
else:
rospy.logerr("could not execute trajectory because not enough points are reachable")
response.success = False
return response
if args.test:
from point_clouds import tabletop
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,plotting3d=1,plotting2d=0)
clusters = sorted(clusters, key = len)
req = PourRequest()
_xyz0 = np.array([clusters[0]])
_xyz1 = np.array([clusters[1]])
_rgb0 = np.zeros(_xyz0.shape,'uint8')
_rgb1 = np.zeros(_xyz1.shape,'uint8')
req.object_clouds.append(xyzrgb2pc(_xyz0, _rgb0,'base_footprint'))
req.object_clouds.append(xyzrgb2pc(_xyz1, _rgb1,'base_footprint'))
success = callback(req)
print success
else:
rospy.Service("pour", Pour, callback)
