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