def plan(samplerIndex):
# construct the state space we are planning in
space = ob.RealVectorStateSpace(3)
# set the bounds
bounds = ob.RealVectorBounds(3)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# define a simple setup class
ss = og.SimpleSetup(space)
# set state validity checking for this space
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# create a start state
start = ob.State(space)
start[0] = 0
start[1] = 0
start[2] = 0
# create a goal state
goal = ob.State(space)
goal[0] = 0
goal[1] = 0
goal[2] = 1
# set the start and goal states;
ss.setStartAndGoalStates(start, goal)
# set sampler (optional; the default is uniform sampling)
si = ss.getSpaceInformation()
if samplerIndex == 1:
# use obstacle-based sampling
si.setValidStateSamplerAllocator(
ob.ValidStateSamplerAllocator(allocOBValidStateSampler))
elif samplerIndex == 2:
# use my sampler
si.setValidStateSamplerAllocator(
ob.ValidStateSamplerAllocator(allocMyValidStateSampler))
# create a planner for the defined space
planner = og.PRM(si)
ss.setPlanner(planner)
# attempt to solve the problem within ten seconds of planning time
solved = ss.solve(10.0)
if solved:
print("Found solution:")
# print the path to screen
print(ss.getSolutionPath())
else:
print("No solution found")
c = Constraint()
ss.setStateValidityChecker(ob.StateValidityCheckerFn(c.isStateValid))
start = ob.State(space)
start()[0] = 0
start()[1] = 0
goal = ob.State(space)
goal()[0] = 2
goal()[1] = 1.5
ss.setStartAndGoalStates(start, goal)
si = ss.getSpaceInformation()
si.setValidStateSamplerAllocator(
ob.ValidStateSamplerAllocator(allocMyValidStateSampler))
planner = og.RRTstar(si)
planner = og.PRM(si)
ss.setPlanner(planner)
solved = ss.solve(0.05)
c.draw()
if solved:
ss.simplifySolution()
pd = ob.PlannerData(ss.getSpaceInformation())
ss.getPlannerData(pd)
pd.computeEdgeWeights()
def plan(self, start_tup, goal_tup, turning_radius=10, planning_time=30.0):
def isStateValid(state):
y = int(state.getY())
x = int(state.getX())
if y < 0 or x < 0 or y >= self.map_data.shape[
0] or x >= self.map_data.shape[1]:
return False
return bool(self.map_data[y, x] == 0)
space = ob.DubinsStateSpace(turningRadius=turning_radius)
# Set State Space bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(0, 0)
bounds.setHigh(0, self.map_data.shape[1])
bounds.setLow(1, 0)
bounds.setHigh(1, self.map_data.shape[0])
space.setBounds(bounds)
si = ob.SpaceInformation(space)
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
si.setStateValidityCheckingResolution(
self.params["validity_resolution"]
) # Set based on thinness of walls in map
si.setValidStateSamplerAllocator(
ob.ValidStateSamplerAllocator(
ob.ObstacleBasedValidStateSampler(si)))
si.setup()
# Set Start and Goal states
start = ob.State(space)
start().setX(start_tup[0])
start().setY(start_tup[1])
start().setYaw(start_tup[2])
goal = ob.State(space)
goal().setX(goal_tup[0])
goal().setY(goal_tup[1])
goal().setYaw(goal_tup[2])
# Set Problem Definition
pdef = ob.ProblemDefinition(si)
pdef.setStartAndGoalStates(start, goal)
optimObj = ob.PathLengthOptimizationObjective(si)
pdef.setOptimizationObjective(optimObj)
# Set up planner
optimizingPlanner = og.BITstar(si)
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
solved = optimizingPlanner.solve(planning_time)
def solution_path_to_tup(solution_path):
result = []
states = solution_path.getStates()
for state in states:
x = state.getX()
y = state.getY()
yaw = state.getYaw()
result.append((x, y, yaw))
return result
solutionPath = None
if solved:
solutionPath = pdef.getSolutionPath()
solutionPath.interpolate(self.params['interpolation_density'])
solutionPath = solution_path_to_tup(solutionPath)
return bool(solved), solutionPath