Ejemplo n.º 1
0
    #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