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()
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
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
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)
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()
# 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
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()
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))