Exemple #1
0
 def __init__(self, shape, calc_time, resolution, distance):
     ou.setLogLevel(ou.LOG_WARN)
     self._calc_time = calc_time
     self._space = ob.RealVectorStateSpace(2)
     bounds = ob.RealVectorBounds(2)
     bounds.setLow(0.0)
     bounds.setHigh(0, shape[0] - 1)
     bounds.setHigh(1, shape[1] - 1)
     self._space.setBounds(bounds)
     self._si = ob.SpaceInformation(self._space)
     self._validity_checker = OmplPlannerImpl.ValidityChecker(self._si)
     self._si.setStateValidityChecker(self._validity_checker)
     self._si.setStateValidityCheckingResolution(resolution)
     self._si.setup()
     self._start = ob.State(self._space)
     self._goal = ob.State(self._space)
     self._pdef = ob.ProblemDefinition(self._si)
     objective = ob.MultiOptimizationObjective(self._si)
     objective.addObjective(OmplPlannerImpl.MapCostObjective(self._si), 0.5)
     # ## setting length objective
     length_objective = ob.PathLengthOptimizationObjective(self._si)
     objective.addObjective(length_objective, 0.1)
     # objective.setCostThreshold(1000.0)
     self._pdef.setOptimizationObjective(objective)
     self._planner = og.RRTstar(self._si)
     self._planner.setRange(distance)
     self._planner.setup()
Exemple #2
0
def planning_evaluation(outdir: pathlib.Path,
                        planners_params: List[Tuple[str, Dict]],
                        trials: List[int],
                        logfile_name: Optional[str],
                        start_idx: int = 0,
                        stop_idx: int = -1,
                        skip_on_exception: Optional[bool] = False,
                        verbose: int = 0,
                        record: bool = False,
                        no_execution: bool = False,
                        timeout: Optional[int] = None,
                        test_scenes_dir: Optional[pathlib.Path] = None,
                        save_test_scenes_dir: Optional[pathlib.Path] = None,
                        ):
    ou.setLogLevel(ou.LOG_ERROR)

    if logfile_name is None:
        logfile_name = pathlib.Path(tempfile.gettempdir()) / f'planning-evaluation-log-file-{time()}'

    job_chunker = JobChunker(logfile_name=logfile_name)

    rospy.loginfo(Fore.CYAN + "common output directory: {}".format(outdir))
    if not outdir.is_dir():
        rospy.loginfo(Fore.YELLOW + "Creating output directory: {}".format(outdir))
        outdir.mkdir(parents=True)

    # NOTE: if method names are not unique, we would overwrite results. Very bad!
    assert_method_names_are_unique(planners_params)

    for comparison_idx, (method_name, planner_params) in enumerate(planners_params):
        if comparison_idx < start_idx:
            continue
        if stop_idx != -1 and comparison_idx >= stop_idx:
            break

        job_chunker.setup_key(method_name)
        sub_job_chunker = job_chunker.sub_chunker(method_name)

        rospy.loginfo(Fore.GREEN + f"Running method {method_name}")
        comparison_root_dir = outdir / method_name

        conditional_try(skip_on_exception,
                        evaluate_planning_method,
                        planner_params=planner_params,
                        job_chunker=sub_job_chunker,
                        trials=trials,
                        comparison_root_dir=comparison_root_dir,
                        verbose=verbose,
                        record=record,
                        no_execution=no_execution,
                        timeout=timeout,
                        test_scenes_dir=test_scenes_dir,
                        save_test_scenes_dir=save_test_scenes_dir,
                        )

        rospy.loginfo(f"Results written to {outdir}")

    return outdir
Exemple #3
0
    def _get_path(
        self,
        is_state_valid: Callable[[np.ndarray], bool],
        start_js: np.ndarray,
        robot_targ: RobotTarget,
        use_sim: MpSim,
        mp_space: MpSpace,
        timeout: int,
    ):
        """
        Does the low-level path planning with OMPL.
        """
        if not self._should_render:
            ou.setLogLevel(ou.LOG_ERROR)
        dim = mp_space.get_state_dim()
        space = ob.RealVectorStateSpace(dim)
        bounds = ob.RealVectorBounds(dim)

        lims = mp_space.get_state_lims()
        for i, lim in enumerate(lims):
            bounds.setLow(i, lim[0])
            bounds.setHigh(i, lim[1])
        space.setBounds(bounds)

        si = ob.SpaceInformation(space)
        si.setStateValidityChecker(ob.StateValidityCheckerFn(is_state_valid))
        si.setup()

        pdef = ob.ProblemDefinition(si)
        mp_space.set_problem(pdef, space, si, start_js, robot_targ)

        planner = mp_space.get_planner(si)
        planner.setProblemDefinition(pdef)
        planner.setup()
        if mp_space.get_range() is not None:
            planner.setRange(mp_space.get_range())

        solved = planner.solve(timeout)

        if not solved:
            self._log("Could not find plan")
            return None
        objective = pdef.getOptimizationObjective()
        if objective is not None:
            cost = (pdef.getSolutionPath().cost(
                pdef.getOptimizationObjective()).value())
        else:
            cost = np.inf

        self._log("Got a path of length %.2f and cost %.2f" %
                  (pdef.getSolutionPath().length(), cost))

        path = pdef.getSolutionPath()
        joint_plan = mp_space.convert_sol(path)
        self._is_approx_sol = pdef.hasApproximateSolution()

        return joint_plan
Exemple #4
0
    parser.add_argument('-pdf', '--pdfile', default=None, \
        help='(Optional) Specify an output path for the planner data.')
    parser.add_argument('-pd', '--plotData', type=bool, default=False, help=\
        '(Optional) Specify if plot the planner data and path.')
    parser.add_argument('-i', '--info', type=int, default=0, choices=[0, 1, 2], \
        help='(Optional) Set the OMPL log level. 0 for WARN, 1 for INFO, 2 for DEBUG.' \
        ' Defaults to WARN.')

    # Parse the arguments
    args = parser.parse_args()

    # Check that time is positive
    if args.runtime <= 0:
        raise argparse.ArgumentTypeError(
            "argument -t/--runtime: invalid choice: %r (choose a positive number greater than 0)" \
            % (args.runtime,))

    # Set the log level
    if args.info == 0:
        ou.setLogLevel(ou.LOG_WARN)
    elif args.info == 1:
        ou.setLogLevel(ou.LOG_INFO)
    elif args.info == 2:
        ou.setLogLevel(ou.LOG_DEBUG)
    else:
        ou.OMPL_ERROR("Invalid log-level integer.")

    # Solve the planning problem
    plan(args.runtime, args.planner, args.objective, args.plotData, args.file,
         args.pdfile)
Exemple #5
0
    def testGeometric_LBKPIECE1(self):
        planner = LBKPIECE1Test()
        (success, avgruntime, avglength) = self.runPlanTest(planner)
        self.assertTrue(success >= 99.0)
        self.assertTrue(avgruntime < 0.1)
        self.assertTrue(avglength < 100.0)

    def testGeometric_EST(self):
        planner = ESTTest()
        (success, avgruntime, avglength) = self.runPlanTest(planner)
        self.assertTrue(success >= 99.0)
        self.assertTrue(avgruntime < 0.1)
        self.assertTrue(avglength < 100.0)

    def testGeometric_PRM(self):
        planner = PRMTest()
        (success, avgruntime, avglength) = self.runPlanTest(planner)
        self.assertTrue(success >= 99.0)
        self.assertTrue(avgruntime < 2.0)
        self.assertTrue(avglength < 100.0)


def suite():
    suites = (unittest.makeSuite(PlanTest))
    return unittest.TestSuite(suites)


if __name__ == '__main__':
    setLogLevel(LogLevel.LOG_ERROR)
    unittest.main()
Exemple #6
0
    # Create an argument parser
    parser = argparse.ArgumentParser(description='Optimal motion planning demo program.')

    # Add a filename argument
    parser.add_argument('-t', '--runtime', type=float, default=1.0, help='(Optional) Specify the runtime in seconds. Defaults to 1 and must be greater than 0.')
    parser.add_argument('-p', '--planner', default='RRTstar', choices=['BITstar', 'FMTstar', 'BFMTstar', 'InformedRRTstar', 'RRTstarsmart', 'PRMstar', 'RRTstar'], help='(Optional) Specify the optimal planner to use, defaults to RRTstar if not given.') # Alphabetical order
    parser.add_argument('-o', '--objective', default='PathLength', choices=['PathClearance', 'PathLength', 'ThresholdPathLength', 'WeightedLengthAndClearanceCombo'], help='(Optional) Specify the optimization objective, defaults to PathLength if not given.') # Alphabetical order
    parser.add_argument('-f', '--file',  default=None, help='(Optional) Specify an output path for the found solution path.')
    parser.add_argument('-i', '--info', type=int, default=0, choices=[0, 1, 2], help='(Optional) Set the OMPL log level. 0 for WARN, 1 for INFO, 2 for DEBUG. Defaults to WARN.')

    # Parse the arguments
    args = parser.parse_args()

    # Check that time is positive
    if args.runtime <= 0:
        raise argparse.ArgumentTypeError("argument -t/--runtime: invalid choice: %r (choose a positive number greater than 0)"%(args.runtime,))

    # Set the log level
    if args.info == 0:
        ou.setLogLevel(ou.LOG_WARN)
    elif args.info == 1:
        ou.setLogLevel(ou.LOG_INFO)
    elif args.info == 2:
        ou.setLogLevel(ou.LOG_DEBUG)
    else:
        OMPL_ERROR("Invalid log-level integer.");

    # Solve the planning problem
    plan(args.runtime, args.planner, args.objective, args.file)

## @endcond
Exemple #7
0
        self.assertTrue(avglength < 100.0)

    def testGeometric_LBKPIECE1(self):
        planner = LBKPIECE1Test()
        (success, avgruntime, avglength) = self.runPlanTest(planner)
        self.assertTrue(success >= 99.0)
        self.assertTrue(avgruntime < 0.1)
        self.assertTrue(avglength < 100.0)

    def testGeometric_EST(self):
        planner = ESTTest()
        (success, avgruntime, avglength) = self.runPlanTest(planner)
        self.assertTrue(success >= 99.0)
        self.assertTrue(avgruntime < 0.1)
        self.assertTrue(avglength < 100.0)

    def testGeometric_PRM(self):
        planner = PRMTest()
        (success, avgruntime, avglength) = self.runPlanTest(planner)
        self.assertTrue(success >= 99.0)
        self.assertTrue(avgruntime < 2.0)
        self.assertTrue(avglength < 100.0)

def suite():
    suites = ( unittest.makeSuite(PlanTest) )
    return unittest.TestSuite(suites)

if __name__ == '__main__':
    setLogLevel(LogLevel.LOG_ERROR)
    unittest.main()
Exemple #8
0
def createPath(
    state,
    runtimeSeconds=None,
    numRuns=None,
    resetSpeedupDuringPlan=False,
    maxAllowableRuntimeSeconds=MAX_ALLOWABLE_PATHFINDING_TOTAL_RUNTIME_SECONDS
):
    '''Create a Path from state.position to state.globalWaypoint. Runs OMPL pathfinding multiple times and returns
    the best path.

    Args:
       state (BoatState): Current state of the boat, which contains all necessary information to perform pathfinding
       runtimeSeconds (float): Number of seconds that the each pathfinding attempt should run.
                               If None, read from rospy param "runtime_seconds"
       numRuns (int): Number of pathfinding attempts that are run in normal case
                      If None, read from rospy param "num_runs"
       resetSpeedupDuringPlan (bool): Decides if pathfinding should set the speedup value to 1.0 during the pathfinding
       maxAllowableRuntimeSeconds (double): Maximum total time that this method should take to run. Will take longer
                                            than runtimeSeconds*numRuns only if pathfinding is unable to find a path.

    Returns:
       Path object representing the path from state.position to state.globalWaypoint
       Returns None if cannot find a single valid solution in the given maxAllowableRuntimeSeconds
    '''

    # Helper methods
    def getXYLimits(start, goal, extraLengthFraction=0.6):
        # Calculate extra length to allow wider solution space
        width = math.fabs(goal[0] - start[0])
        height = math.fabs(goal[1] - start[1])
        extraKm = extraLengthFraction * max(width, height)

        xMin = min(start[0], goal[0]) - extraKm
        yMin = min(start[1], goal[1]) - extraKm
        xMax = max(start[0], goal[0]) + extraKm
        yMax = max(start[1], goal[1]) + extraKm
        return [xMin, yMin, xMax, yMax]

    def isValidSolution(solution, referenceLatlon, state):
        if not solution.haveExactSolutionPath():
            return False
        return True

    def plotPathfindingProblem(globalWindDirectionDegrees, dimensions, start,
                               goal, obstacles, headingDegrees):
        # Clear plot if already there
        plt.cla()

        # Create plot with start and goal
        x_min, y_min, x_max, y_max = dimensions
        markersize = min(x_max - x_min, y_max - y_min) / 2
        plt.ion()
        axes = plt.gca()
        goalPlot, = axes.plot(goal[0],
                              goal[1],
                              marker='*',
                              color='y',
                              markersize=markersize)  # Yellow star
        # Red triangle with correct heading. The (-90) is because the triangle
        # default heading 0 points North, but this heading has 0 be East.
        startPlot, = axes.plot(start[0],
                               start[1],
                               marker=(3, 0, headingDegrees - 90),
                               color='r',
                               markersize=markersize)

        # Setup plot xy limits and labels
        axes.set_xlim(x_min, x_max)
        axes.set_ylim(y_min, y_max)
        axes.set_aspect('equal')
        plt.grid(True)
        axes.set_xlabel('X distance to position (km)')
        axes.set_ylabel('Y distance to position (km)')
        axes.set_title('Setup of pathfinding problem')

        # Add boats and wind speed arrow
        for obstacle in obstacles:
            obstacle.addPatch(axes)

        arrowLength = min(dimensions[2] - dimensions[0],
                          dimensions[3] - dimensions[1]) / 15
        arrowCenter = (dimensions[0] + 1.5 * arrowLength,
                       dimensions[3] - 1.5 * arrowLength)
        arrowStart = (arrowCenter[0] - 0.5 * arrowLength *
                      math.cos(math.radians(globalWindDirectionDegrees)),
                      arrowCenter[1] - 0.5 * arrowLength *
                      math.sin(math.radians(globalWindDirectionDegrees)))
        windDirection = patches.FancyArrow(
            arrowStart[0],
            arrowStart[1],
            arrowLength * math.cos(math.radians(globalWindDirectionDegrees)),
            arrowLength * math.sin(math.radians(globalWindDirectionDegrees)),
            width=arrowLength / 4)
        axes.add_patch(windDirection)

        # Draw plot
        plt.draw()
        plt.pause(0.001)

    def setAverageDistanceBetweenWaypoints(solutionPath):
        # Set the average distance between waypoints
        localPathLengthKm = solutionPath.length()
        numberOfLocalWaypoints = int(
            localPathLengthKm / utils.AVG_DISTANCE_BETWEEN_LOCAL_WAYPOINTS_KM)
        solutionPath.interpolate(numberOfLocalWaypoints)

    def findBestSolution(validSolutions, invalidSolutions):
        # If no valid solutions found, use the best invalid one. Do not perform any path simplifying on invalid paths.
        if len(validSolutions) == 0:
            # Set the average distance between waypoints. Must be done before cost calculation and comparison
            for solution in invalidSolutions:
                setAverageDistanceBetweenWaypoints(solution.getSolutionPath())

            bestSolution = min(invalidSolutions,
                               key=lambda x: x.getSolutionPath().cost(
                                   x.getOptimizationObjective()).value())
            bestSolutionPath = bestSolution.getSolutionPath()
            minCost = bestSolutionPath.cost(
                bestSolution.getOptimizationObjective()).value()
        else:
            # Set the average distance between waypoints. Must be done before cost calculation and comparison
            for solution in validSolutions:
                setAverageDistanceBetweenWaypoints(solution.getSolutionPath())

            # Find path with minimum cost. Can be either simplified or unsimplified path.
            # Need to recheck that simplified paths are valid before using
            minCost = sys.maxsize
            bestSolution = None
            bestSolutionPath = None
            for solution in validSolutions:
                # Check unsimplified path
                unsimplifiedPath = og.PathGeometric(solution.getSolutionPath())
                unsimplifiedCost = unsimplifiedPath.cost(
                    solution.getOptimizationObjective()).value()
                if unsimplifiedCost < minCost:
                    bestSolution = solution
                    bestSolutionPath = unsimplifiedPath
                    minCost = unsimplifiedCost

        return bestSolution, bestSolutionPath, minCost

    if runtimeSeconds is None:
        runtimeSeconds = rospy.get_param('runtime_seconds', default=0.125)
    if numRuns is None:
        numRuns = rospy.get_param('num_runs', default=8)

    ou.setLogLevel(ou.LOG_WARN)
    # Set speedup to 1.0 during planning
    speedupBeforePlan = rospy.get_param('speedup', default=1.0)
    if resetSpeedupDuringPlan:
        speedupDuringPlan = 1.0
        rospy.loginfo(
            "Setting speedup to this value during planning = {}".format(
                speedupDuringPlan))
        rospy.set_param('speedup', speedupDuringPlan)

    # Get setup parameters from state for ompl plan()
    # Convert all latlons to NE in km wrt referenceLatlon
    referenceLatlon = state.globalWaypoint
    start = utils.latlonToXY(state.position, referenceLatlon)
    goal = utils.latlonToXY(state.globalWaypoint, referenceLatlon)
    dimensions = getXYLimits(start, goal)
    obstacles = obs.getObstacles(state, referenceLatlon)
    stateSampler = rospy.get_param('state_sampler', default='grid')

    # Run the planner multiple times and find the best one
    rospy.loginfo(
        "Running createLocalPathSS. runtimeSeconds: {}. numRuns: {}. Total time: {} seconds"
        .format(runtimeSeconds, numRuns, runtimeSeconds * numRuns))
    rospy.loginfo("Using stateSampler = {}".format(stateSampler)
                  if len(stateSampler) > 0 else "Using default state sampler")

    # Create non-blocking plot showing the setup of the pathfinding problem.
    # Useful to understand if the pathfinding problem is invalid or impossible
    shouldPlot = rospy.get_param('plot_pathfinding_problem', False)
    if shouldPlot:
        plotPathfindingProblem(state.globalWindDirectionDegrees, dimensions,
                               start, goal, obstacles, state.headingDegrees)

    # Take screenshot
    shouldTakeScreenshot = rospy.get_param('screenshot', False)
    if shouldTakeScreenshot:
        utils.takeScreenshot()

    # Look for solutions
    validSolutions = []
    invalidSolutions = []
    plannerType = rospy.get_param('planner_type', 'lazyprmstar')
    for i in range(numRuns):
        # TODO: Incorporate globalWindSpeed into pathfinding?
        rospy.loginfo("Starting path-planning run number: {}".format(i))
        solution = plan(runtimeSeconds, plannerType,
                        state.globalWindDirectionDegrees, dimensions, start,
                        goal, obstacles, state.headingDegrees, stateSampler)
        if isValidSolution(solution, referenceLatlon, state):
            validSolutions.append(solution)
        else:
            invalidSolutions.append(solution)

    # If no validSolutions found, re-run with larger runtime
    totalRuntimeSeconds = numRuns * runtimeSeconds
    while len(validSolutions) == 0:
        rospy.logwarn("No valid solutions found in {} seconds runtime".format(
            runtimeSeconds))
        runtimeSeconds *= INCREASE_RUNTIME_FACTOR
        totalRuntimeSeconds += runtimeSeconds
        incrementTempInvalidSolutions()

        # If valid solution can't be found for large runtime, then stop searching
        if totalRuntimeSeconds >= maxAllowableRuntimeSeconds:
            rospy.logerr(
                "No valid solution can be found in under {} seconds. Returning None."
                .format(maxAllowableRuntimeSeconds))
            incrementCountInvalidSolutions()
            return None

        rospy.logwarn(
            "Attempting to rerun with longer runtime: {} seconds".format(
                runtimeSeconds))
        solution = plan(runtimeSeconds, plannerType,
                        state.globalWindDirectionDegrees, dimensions, start,
                        goal, obstacles, state.headingDegrees, stateSampler)

        if isValidSolution(solution, referenceLatlon, state):
            validSolutions.append(solution)
        else:
            invalidSolutions.append(solution)

    # Find best solution
    bestSolution, bestSolutionPath, minCost = findBestSolution(
        validSolutions, invalidSolutions)

    # Close plot if it was started
    plt.close()

    # Reset speedup back to original value
    if resetSpeedupDuringPlan:
        rospy.loginfo(
            "Setting speedup back to its value before planning = {}".format(
                speedupBeforePlan))
        rospy.set_param('speedup', speedupBeforePlan)

    return Path(OMPLPath(bestSolution, bestSolutionPath, referenceLatlon))