#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