def execute(self,userdata):
"""
- move head to the right place
- get a point cloud
returns: success, failure
"""
global HANDLES
HANDLES=[]
# Globals.pr2.head.set_pan_tilt(0, HEAD_TILT)
Globals.pr2.larm.goto_posture('side')
Globals.pr2.rarm.goto_posture('side')
Globals.pr2.join_all()
if HUMAN_GET_ROPE:
xyz,frame = human_get_rope()
xyz = ros_utils.transform_points(xyz, Globals.pr2.tf_listener, "base_footprint", frame)
pose_array = conversions.array_to_pose_array(xyz,"base_footprint")
HANDLES.append(Globals.rviz.draw_curve(pose_array,id=3250864,rgba=(0,0,1,1)))
xyz = curves.unif_resample(xyz, 100,tol=.01)
else:
msg = rospy.wait_for_message("/preprocessor/points", sensor_msgs.msg.PointCloud2)
xyz, rgb = ros_utils.pc2xyzrgb(msg)
xyz = xyz.reshape(-1,3)
xyz = ros_utils.transform_points(xyz, Globals.pr2.tf_listener, "base_footprint", msg.header.frame_id)
userdata.points = xyz
return "success"
def human_get_rope():
point_cloud = rospy.wait_for_message("/camera/depth_registered/points", sm.PointCloud2)
xyz, bgr = ros_utils.pc2xyzrgb(point_cloud)
xys = roi.get_polyline(bgr,'draw curve')
uvs = np.int32(xys)[:,::-1]
us,vs = uvs.T
xyzs = xyz[us,vs]
xyzs_good = xyzs[np.isfinite(xyzs).all(axis=1)]
print "%i out of %i labeled points are good"%(len(xyzs_good), len(xyzs))
xyzs_unif = curves.unif_resample(xyzs_good,100,tol=.002)
return xyzs_unif
def initialize_rope(xyzs, plotting=False):
pdists = ssd.squareform(ssd.pdist(xyzs))
for (i_from, row) in enumerate(pdists):
to_inds = np.flatnonzero(row[:i_from] < MAX_DIST)
for i_to in to_inds:
G.add_edge(i_from, i_to, weight = pdists[i_from, i_to])
A = nx.floyd_warshall_numpy(G)
A[np.isinf(A)] = 0
(i_from_long, i_to_long) = np.unravel_index(A.argmax(), A.shape)
path = nx.shortest_path(G, source=i_from_long, target=i_to_long)
xyz_path = xyz[path,:]
xyzs_unif = curves.unif_resample(total_path,N_CTRL_PTS,tol=.002)
labels = np.ones(len(xyzs_unif),'int')
labels[[1,-1]] = 2
return xyzs_unif, labels
def initialize_rope(label_img, xyz,bgr, plotting=False):
rope_mask = (label_img==1) | ndi.morphology.binary_dilation(label_img==2,np.ones((5,5)))
rope_mask = ndi.binary_opening(rope_mask, np.ones((3,3)))
if plotting:
cv2.imshow('bgr',bgr.copy())
cv2.imshow('labels',label_img.astype('uint8')*50)
cv2.waitKey(5)
start_end = (uv_start, uv_end) = get_cc_centers(label_img==2,2)
skel = mip.skeletonize(rope_mask)
skel = skeletons.removeSpurs1(skel,10)
if plotting: cv2.imshow('skel',skel*100)
paths2d = skeletons.get_paths_2d(skel)
endpts2d = []
for path in paths2d:
endpts2d.append(path[0])
endpts2d.append(path[-1])
i_start = nearest_neighbor(uv_start, endpts2d)
i_end = nearest_neighbor(uv_end, endpts2d)
print "start,end",i_start,i_end
paths3d = [to3d(path,xyz) for path in paths2d]
if plotting:
starts_ends = []
for path in paths2d:
starts_ends.append(path[0])
starts_ends.append(path[-1])
plot_colorskel(paths2d,bgr,start_end,starts_ends)
C = skeletons.make_cost_matrix(paths3d)
PG = skeletons.make_path_graph(C, [len(path) for path in paths3d])
(_,nodes) = skeletons.longest_path_between(PG,i_start, set([]), i_end)
if nodes is None: raise Exception("longest path failed")
print nodes
total_path = []
for node in nodes[::2]:
if node%2 == 0:
total_path.extend(paths3d[node//2])
else:
total_path.extend(paths3d[node//2][::-1])
total_path = np.array(total_path)
xyzs_unif = curves.unif_resample(total_path,N_SEGS+1,tol=.0025)
#us,vs = xyz2uv(xyzs_unif).T
#labels = label_img[us,vs]
labels = np.ones(xyzs_unif.shape[0]-1,'int')
labels[0] = labels[-1] = 2
if plotting:
xs,ys,zs = xyzs_unif.T
us_skel, vs_skel = np.nonzero(skel)
xs_skel, ys_skel, zs_skel = to3d(np.c_[us_skel, vs_skel], xyz).T
import matplotlib.pyplot as plt, enthought.mayavi.mlab as mlab
mlab.plot3d(xs,ys,zs,np.arange(len(xs)),tube_radius=.0025, colormap='spectral')
mlab.points3d(xs_skel, ys_skel, zs_skel, scale_factor=.02, color=(1,1,1),opacity=.1)
for (i,path) in enumerate(paths3d):
mlab.text3d(path[0,0],path[0,1],path[0,2],str(2*i),scale=.01,color=(0,0,0))
mlab.text3d(path[-1,0],path[-1,1],path[-1,2],str(2*i+1),scale=.01,color=(0,0,0))
mlab.show()
return xyzs_unif, labels
#!/usr/bin/env python
from image_proc import interactive_roi as roi
import os,cv2
from os.path import dirname, abspath,join
from image_proc import curves, pcd_io
import numpy as np
import argparse
parser = argparse.ArgumentParser()
parser.add_argument('pcdfile')
parser.add_argument('outfile')
args = parser.parse_args()
xyz,bgr = pcd_io.load_xyzrgb(args.pcdfile)
print xyz.shape, bgr.shape
cv2.namedWindow('draw curve',cv2.cv.CV_WINDOW_NORMAL)
xys = roi.get_polyline(bgr,'draw curve')
uvs = np.int32(xys)[:,::-1]
us,vs = uvs.T
xyzs = xyz[us,vs]
xyzs_good = xyzs[np.isfinite(xyzs).all(axis=1)]
print "%i out of %i labeled points are good"%(len(xyzs_good), len(xyzs))
xyzs_unif = curves.unif_resample(xyzs_good,100,tol=.002)
#if bool(args.outfile) and not os.path.exists(dirname(args.outfile)): os.mkdir(dirname(args.outfile))
print "writing",args.outfile
np.savetxt(args.outfile,xyzs_unif)
import image_proc.interactive_roi as roi
import os,cv2
from os.path import dirname, abspath,join
from image_proc import curves
import numpy as np
X = np.zeros((100,100),'uint8')+255
poly0 = roi.get_polyline(X,"curves")
poly1 = roi.get_polyline(X,"curves")
curve0 = curves.unif_resample(poly0,100)
curve1 = curves.unif_resample(poly1,100)
np.savetxt("curve0.txt",curve0)
np.savetxt("curve1.txt",curve1)
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("outfile")
args = parser.parse_args()
assert os.path.exists(os.path.dirname(args.outfile))
table_corners = np.loadtxt("/home/joschu/bulletsim/data/knots/table_corners.txt");
dx_table = table_corners[1] - table_corners[0]
dy_table = table_corners[3] - table_corners[0]
z = table_corners[0,2]
nrows = 300
length_per_pix = norm(dx_table) / nrows
ncols = norm(dy_table) / length_per_pix
img = np.zeros((nrows, ncols),'uint8')+255
nout = 100
poly = roi.get_polyline(img,"curves")
curve = curves.unif_resample(poly,nout,tol=0)
curve3d = (table_corners[0]+np.r_[0,0,.03])[None,:] + np.c_[curve * length_per_pix, np.zeros((nout,1))]
print curve3d
np.savetxt(args.outfile,curve3d,fmt="%.3f")
