def create_data():
grasps = grasp_proxy.detect_grasps_whole_cloud(cloud, grasp_offsets)
grasps = filter(
lambda g: (grasp_space[0][0] < g.bottom[0] < grasp_space[0][
1] and grasp_space[1][0] < g.bottom[1] < grasp_space[1][1] and
grasp_space[2][0] < g.bottom[2] < grasp_space[2][1]),
grasps)
print 'grasps after filter: ' + str(len(grasps))
rviz_node.cloud_pub.publish(cloud_proxy.convert_to_point_cloud2(cloud))
rviz_node.all_grasps_pub.publish(
plot.createGraspsMarkerArray(grasps, rgba=[1, 0, 0, 0.5]))
rviz_node.grasp_space_pub.publish(plot.createGraspsPosCube(grasp_space))
image_3d = []
image_2d = []
for grasp in grasps:
hand_des_3d = HandDescriptor(
HandDescriptor.T_from_approach_axis_center(
grasp.approach, grasp.axis, (grasp.bottom + grasp.top) / 2),
32)
hand_des_3d.generate_3d_binary_image(cloud)
image_3d.append(hand_des_3d.image)
hand_des_2d = HandDescriptor(
HandDescriptor.T_from_approach_axis_center(
grasp.approach, grasp.axis, (grasp.bottom + grasp.top) / 2))
hand_des_2d.generate_depth_image(cloud)
image_2d.append(hand_des_2d.image)
image_3d = np.stack(image_3d, 0)
image_2d = np.stack(image_2d, 0)
f_3d = os.path.dirname(__file__) + '/../data/normal_shelf1_3d.npy'
f_2d = os.path.dirname(__file__) + '/../data/normal_shelf1_2d.npy'
try:
old_3d = np.load(f_3d)
if old_3d.shape[0] != 0:
image_3d = np.vstack([image_3d, old_3d])
except:
pass
try:
old_2d = np.load(f_2d)
if old_2d.shape[0] != 0:
image_2d = np.vstack([image_2d, old_2d])
except:
pass
finally:
raw_input('write?')
np.save(f_3d, image_3d)
print 'current 3d data: ' + str(image_3d.shape[0])
np.save(f_2d, image_2d)
print 'current 2d data: ' + str(image_2d.shape[0])
image_3d = []
for grasp in grasps:
hand_des_3d = HandDescriptor(
HandDescriptor.T_from_approach_axis_center(
grasp.approach, grasp.axis, (grasp.bottom + grasp.top) / 2), 32)
hand_des_3d.generate_3d_binary_image(cloud)
image_3d.append(hand_des_3d.image)
cnn.eval()
image_3d = np.stack(image_3d, 0)
inputs = torch.FloatTensor(image_3d).unsqueeze(1).cuda()
inputs = Variable(inputs, volatile=True)
outputs = cnn(inputs)
prediction = outputs.data.max(1, keepdim=True)[1]
prediction = prediction.squeeze().tolist()
edge_grasp = []
non_edge_grasp = []
for i in range(len(grasps)):
if prediction[i] == 1:
edge_grasp.append(grasps[i])
else:
non_edge_grasp.append(grasps[i])
rviz_node.edge_grasp_pub.publish(
plot.createGraspsMarkerArray(edge_grasp, rgba=[1, 0, 0, 0.5]))
rviz_node.non_edge_grasp_pub.publish(
plot.createGraspsMarkerArray(non_edge_grasp, rgba=[0, 0, 1, 0.5]))
rviz_node.cloud_pub.publish(cloud_proxy.convert_to_point_cloud2(cloud))
print 'edge grasp: {}'.format(len(edge_grasp))
print 'non-edge grasp: {}'.format(len(non_edge_grasp))
