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
Example #3
0
    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()
Example #4
0
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()
Example #5
0
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))
Example #6
0
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
Example #8
0
            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()