def main():
'''Runs each of the functions in this module that begin with the name Test. Prints pass/fail info.'''
try:
X = point_cloud.LoadPcd("test.pcd")
# point_cloud.Plot(X)
GREEN = '\033[92m'
RED = '\033[91m'
END = '\033[0m'
except Exception as e:
print("Failed to initialize testing.")
print str(e)
return
items = globals()
for item in items:
if len(item) > 4 and item[:4] == "Test":
try:
result = eval(item + "(X)")
except Exception as e:
print str(e)
result = False
if result: print("[" + GREEN +"PASS" + END + "] " + item[4:])
else: print("[" + RED +"FAIL" + END + "] " + item[4:])
import point_cloud
workspace = [(-0.5, 1.0), (0, 1.5), (-0.50, 0.50)]
grasp_offsets = [0.23, 0, 0.16]
grasp_offsets[1] = grasp_offsets[2] + (grasp_offsets[0] -
grasp_offsets[2]) / 2.0
rospy.init_node("edge_grasp")
cloud_proxy = CloudProxy()
grasp_proxy = GraspProxy()
rviz_node = RvizNode()
plot_rviz = PlotRviz()
plot_rviz.initRos()
cloud = point_cloud.LoadPcd(
os.path.dirname(__file__) + '/../data/pcd/cloud5.pcd')
cloud = point_cloud_util.filter_workspace(workspace, cloud)
def create_data():
grasps = grasp_proxy.detect_grasps_whole_cloud(cloud, grasp_offsets)
# cloud 2
# 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)
grasps = filter(lambda g: g.bottom[1] < 0.8, grasps)
print 'grasps after filter: ' + str(len(grasps))
rviz_node.cloud_pub.publish(cloud_proxy.convert_to_point_cloud2(cloud))
cnn = torch.load(os.path.dirname(__file__) + '/../model/cnn3d')
workspace = [(-0.5, 1.0), (0, 1.5), (-0.50, 0.50)]
grasp_offsets = [0.23, 0, 0.16]
grasp_offsets[1] = grasp_offsets[2] + (grasp_offsets[0] -
grasp_offsets[2]) / 2.0
rospy.init_node("edge_grasp")
cloud_proxy = CloudProxy()
grasp_proxy = GraspProxy()
rviz_node = RvizNode()
plot_rviz = PlotRviz()
plot_rviz.initRos()
cloud = point_cloud.LoadPcd(
os.path.dirname(__file__) + '/../data/pcd/' + cloud_name)
cloud = point_cloud_util.filter_workspace(workspace, cloud)
grasps = grasp_proxy.detect_grasps_whole_cloud(cloud, grasp_offsets)
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()
def TestSavePcd(X):
initCount = X.shape[0]
point_cloud.SavePcd("save.pcd", X)
X = point_cloud.LoadPcd("save.pcd")
return X.shape[0] == initCount and X.shape[1] == 3
T[0:3, 1] = [0, 1, 0]
T[0:3, 2] = [-1, 0, 0]
cloud = None
T1 = eye(4)
T1[0:3, 0] = [1, 0, 1]
T1[0:3, 2] = [-1, 0, 1]
T = T.dot(T1)
T1 = eye(4)
# T1[0:3, 2] = [0, -1, 1]
# T1[0:3, 1] = [0, 1, 1]
# T1[0:3, 2] = [0, 1, 1]
# T1[0:3, 1] = [0, 1, -1]
T = T.dot(T1)
file = os.getcwd() + "/../data/pcd/shelf_s.pcd"
cloud = point_cloud.LoadPcd(file)
for y in range(0, 20):
y *= -0.1
T[0:3, 3] = [1, y, 1.5]
simulator.MoveSensorToPose(T)
cloud1 = simulator.GetCloud(workspace)
if cloud is None:
cloud = cloud1
else:
cloud = vstack([cloud, cloud1])
file = os.getcwd() + "/../data/pcd/testpcd.pcd"
point_cloud.SavePcd(file, cloud)
print cloud
