#obstacleCloud = asus.filterWorkspace(obstacleCloud, obstacleCloudWorkspace) #if obstacleCloud.shape[0] == 0: # raw_input("Asus cloud has no points! Press any key to continue...") #Get the obstacle cloud in the base frame, publish it for debugging, and #filter it to only the workspace we want obstacleCloud = mergedCloud.getCloudInBaseFrame(node) node.cloudRvizPub.publish(mergedCloud.convertToPointCloud2(obstacleCloud)) obstacleCloud = mergedCloud.filterWorkspace(obstacleCloud, obstacleCloudWorkspace) if obstacleCloud.shape[0] == 0: raw_input( "No points in obstacleCloud after filtering! Press enter to continue..." ) arm.addCloudToEnvironment(obstacleCloud, obstacleCubeSize) obstacleCloudTree = cKDTree(obstacleCloud) # Planning: ====================================================================================== didMove = False for planAttemptIdx in xrange(maxPlanningAttempts): # sample viewpoints sample, configs = planner.sampleFirstRandomViewpoint(\ laserPoint, sensorKeepout, nSphereSamples, minViewDownAngle, obstacleCloudTree, arm) if sample is None: print("No IK solutions after testing {} samples.".format( nSphereSamples)) continue
maxLaserPointSigma) sphereMarker = plot.createSphereWithCenterMarker("laserPoint", [1, 0, 1], centroid, graspSearchRadius * 2) node.sphereMarker.publish(sphereMarker) # request base cloud node.hasBaseCloud = False node.hasGrasps = False node.controlAsus(START_CAMERA) print("Waiting for base cloud...") while not node.hasBaseCloud: rospy.sleep(0.01) node.controlAsus(STOP_CAMERA) arm.addCloudToEnvironment(node.baseCloud, obstacleCubeSize) baseCloudTree = cKDTree(node.baseCloud) # Planning: ====================================================================================== didMove = False for planAttemptIdx in xrange(maxPlanningAttempts): # sample a pair samplePair, sampleData = planner.sampleViewpoints(centroid, sensorKeepout+sphereRelief, \ nSphereSamples, sampleSeparation, sampleSeparationTolerance, maxViewDownAngle, arm, \ viewEffector, baseCloudTree) if samplePair is None: print("No sample pair found.") continue