from brett2 import ros_utils
import os
import h5py
from snazzy_msgs.srv import *
from jds_utils import conversions
import rope_vision.rope_initialization as ri
from jds_image_proc.clouds import voxel_downsample
rospy.init_node("manually_segment_point_cloud")
listener = tf.TransformListener()
seg_svc = rospy.ServiceProxy("interactive_segmentation", ProcessCloud)
if args.infile.endswith("npz"):
f = np.load(args.infile)
xyz0, rgb0 = f["xyz"], f["bgr"]
pc = ros_utils.xyzrgb2pc(xyz0, rgb0, "base_footprint")
else:
raise NotImplementedError
if args.objs is None:
object_names = raw_input("type object names separated by spaces\n").split()
else:
object_names = args.objs
outfilename = os.path.splitext(args.infile)[0] + ".seg.h5"
if os.path.exists(outfilename): os.remove(outfilename)
outfile = h5py.File(outfilename, "w")
def array_contains(arr, pt):
return np.any(np.all((pt[None,:] == arr), axis=1))
def callback(req):
xyz, rgb = pc2xyzrgb(req.cloud_in)
rgb = rgb.copy()
gc = GetClick()
cv2.setMouseCallback("rgb", gc.callback)
while not gc.done:
cv2.imshow("rgb", rgb)
cv2.waitKey(10)
gm = GetMouse()
xy_corner1 = np.array(gc.xy)
cv2.setMouseCallback("rgb", gm.callback)
while not gm.done:
img = rgb.copy()
if gm.xy is not None:
xy_corner2 = np.array(gm.xy)
cv2.rectangle(img, tuple(xy_corner1), tuple(xy_corner2), (0,255, 0))
cv2.imshow("rgb",img)
cv2.waitKey(10)
xy_tl = np.array([xy_corner1, xy_corner2]).min(axis=0)
xy_br = np.array([xy_corner1, xy_corner2]).max(axis=0)
#mask = np.zeros(xyz.shape[:2],dtype='uint8')
#rectangle = gm.xy + tuple(2*(center - np.r_[gm.xy]))
#tmp1 = np.zeros((1, 13 * 5))
#tmp2 = np.zeros((1, 13 * 5))
#result = cv2.grabCut(rgb,mask,rectangle,tmp1, tmp2,10,mode=cv2.GC_INIT_WITH_RECT)
#from cv2 import cv
#mask[mask == cv.GC_BGD] = 0
#mask[mask == cv.GC_PR_BGD] = 63
#mask[mask == cv.GC_FGD] = 255
#mask[mask == cv.GC_PR_FGD] = 192
#cv2.imshow('mask',mask)
#cv2.waitKey(10)
#table_height = get_table_height(xyz)
xl, yl = xy_tl
w, h = xy_br - xy_tl
mask = np.zeros((h,w),dtype='uint8')
mask.fill(cv2.GC_PR_BGD)
if args.init == "height":
initial_height_thresh = stats.scoreatpercentile(xyz[yl:yl+h, xl:xl+w,2].flatten(), 50)
mask[xyz[yl:yl+h, xl:xl+w,2] > initial_height_thresh] = cv2.GC_PR_FGD
elif args.init == "middle":
print int(yl+h/4),int(yl+3*h/4),int(xl+w/4),int(xl+3*w/4)
mask[h//4:3*h//4, w//4:3*w//4] = cv2.GC_PR_FGD
print mask.mean()
tmp1 = np.zeros((1, 13 * 5))
tmp2 = np.zeros((1, 13 * 5))
cv2.grabCut(rgb[yl:yl+h, xl:xl+w, :],mask,(0,0,0,0),tmp1, tmp2,10,mode=cv2.GC_INIT_WITH_MASK)
contours = cv2.findContours(mask.astype('uint8')%2,cv2.RETR_LIST,cv2.CHAIN_APPROX_NONE)[0]
cv2.drawContours(rgb[yl:yl+h, xl:xl+w, :],contours,-1,(0,255,0))
cv2.imshow('rgb', rgb)
cv2.waitKey(10)
zsel = xyz[yl:yl+h, xl:xl+w, 2]
mask = (mask%2==1) & np.isfinite(zsel)# & (zsel - table_height > -1)
resp = ProcessCloudResponse()
xyz_sel = xyz[yl:yl+h, xl:xl+w,:][mask.astype('bool')]
rgb_sel = rgb[yl:yl+h, xl:xl+w,:][mask.astype('bool')]
resp.cloud_out = xyzrgb2pc(xyz_sel, rgb_sel, req.cloud_in.header.frame_id)
return resp
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]:
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 __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]:
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()
from brett2 import ros_utils
import os
import h5py
from snazzy_msgs.srv import *
from jds_utils import conversions
import rope_vision.rope_initialization as ri
from jds_image_proc.clouds import voxel_downsample
rospy.init_node("manually_segment_point_cloud")
listener = tf.TransformListener()
seg_svc = rospy.ServiceProxy("interactive_segmentation", ProcessCloud)
if args.infile.endswith("npz"):
f = np.load(args.infile)
xyz0, rgb0 = f["xyz"], f["bgr"]
pc = ros_utils.xyzrgb2pc(xyz0, rgb0, "base_footprint")
else:
raise NotImplementedError
if args.objs is None:
object_names = raw_input("type object names separated by spaces\n").split()
else:
object_names = args.objs
outfilename = os.path.splitext(args.infile)[0] + ".seg.h5"
if os.path.exists(outfilename): os.remove(outfilename)
outfile = h5py.File(outfilename, "w")
def array_contains(arr, pt):
return np.any(np.all((pt[None, :] == arr), axis=1))
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("xyz",type=float,nargs=3)
args = parser.parse_args()
import rospy
import numpy as np
import roslib; roslib.load_manifest("verb_msgs")
from brett2.ros_utils import xyzrgb2pc
from verb_msgs.srv import *
rospy.init_node("call_push_service")
rospy.wait_for_service("/push")
proxy = rospy.ServiceProxy("/push",Push)
request = PushRequest()
xyz = np.array([args.xyz])
rgb = np.array([[0,0,0]])
pc = xyzrgb2pc(xyz,rgb, 'base_link')
request.point_cloud = pc
proxy.call(request)
def callback(req):
xyz, rgb = pc2xyzrgb(req.cloud_in)
rgb = rgb.copy()
gc = GetClick()
cv2.setMouseCallback("rgb", gc.callback)
while not gc.done:
cv2.imshow("rgb", rgb)
cv2.waitKey(10)
gm = GetMouse()
xy_corner1 = np.array(gc.xy)
cv2.setMouseCallback("rgb", gm.callback)
while not gm.done:
img = rgb.copy()
if gm.xy is not None:
xy_corner2 = np.array(gm.xy)
cv2.rectangle(img, tuple(xy_corner1), tuple(xy_corner2), (0,255, 0))
cv2.imshow("rgb",img)
cv2.waitKey(10)
xy_tl = np.array([xy_corner1, xy_corner2]).min(axis=0)
xy_br = np.array([xy_corner1, xy_corner2]).max(axis=0)
#mask = np.zeros(xyz.shape[:2],dtype='uint8')
#rectangle = gm.xy + tuple(2*(center - np.r_[gm.xy]))
#tmp1 = np.zeros((1, 13 * 5))
#tmp2 = np.zeros((1, 13 * 5))
#result = cv2.grabCut(rgb,mask,rectangle,tmp1, tmp2,10,mode=cv2.GC_INIT_WITH_RECT)
#from cv2 import cv
#mask[mask == cv.GC_BGD] = 0
#mask[mask == cv.GC_PR_BGD] = 63
#mask[mask == cv.GC_FGD] = 255
#mask[mask == cv.GC_PR_FGD] = 192
#cv2.imshow('mask',mask)
#cv2.waitKey(10)
#table_height = get_table_height(xyz)
if args.grabcut:
rospy.loginfo("using grabcut")
xl, yl = xy_tl
w, h = xy_br - xy_tl
mask = np.zeros((h,w),dtype='uint8')
mask.fill(cv2.GC_PR_BGD)
if args.init == "height":
initial_height_thresh = stats.scoreatpercentile(xyz[yl:yl+h, xl:xl+w,2].flatten(), 50)
mask[xyz[yl:yl+h, xl:xl+w,2] > initial_height_thresh] = cv2.GC_PR_FGD
elif args.init == "middle":
mask[h//4:3*h//4, w//4:3*w//4] = cv2.GC_PR_FGD
tmp1 = np.zeros((1, 13 * 5))
tmp2 = np.zeros((1, 13 * 5))
cv2.grabCut(rgb[yl:yl+h, xl:xl+w, :],mask,(0,0,0,0),tmp1, tmp2,10,mode=cv2.GC_INIT_WITH_MASK)
else:
rospy.loginfo("using table height")
table_height = rospy.get_param("table_height")
xl, yl = xy_tl
w, h = xy_br - xy_tl
mask = np.zeros((h,w),dtype='uint8')
mask[np.isfinite(xyz[yl:yl+h, xl:xl+w,2]) &
(xyz[yl:yl+h, xl:xl+w,2] > table_height+.02)] = 1
mask = mask % 2
if (args.erode > 0):
mask = ndi.binary_erosion(mask, utils_images.disk(args.erode)).astype('uint8')
contours = cv2.findContours(mask,cv2.RETR_LIST,cv2.CHAIN_APPROX_NONE)[0]
cv2.drawContours(rgb[yl:yl+h, xl:xl+w, :],contours,-1,(0,255,0),thickness=2)
cv2.imshow('rgb', rgb)
cv2.waitKey(10)
zsel = xyz[yl:yl+h, xl:xl+w, 2]
mask = (mask%2==1) & np.isfinite(zsel)# & (zsel - table_height > -1)
resp = ProcessCloudResponse()
xyz_sel = xyz[yl:yl+h, xl:xl+w,:][mask.astype('bool')]
rgb_sel = rgb[yl:yl+h, xl:xl+w,:][mask.astype('bool')]
resp.cloud_out = xyzrgb2pc(xyz_sel, rgb_sel, req.cloud_in.header.frame_id)
if (args.plotting):
pose_array = conversions.array_to_pose_array(xyz_sel, req.cloud_in.header.frame_id)
Globals.handles = [Globals.rviz.draw_curve(pose_array, rgba = (1,1,0,1), type=Marker.CUBE_LIST, width=.001, ns="segmentation")]
return resp
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)
print "ready"
rospy.spin()