def allocatePlanner(si, plannerType):
if plannerType.lower() == "bfmtstar":
return og.BFMT(si)
elif plannerType.lower() == "bitstar":
return og.BITstar(si)
elif plannerType.lower() == "fmtstar":
return og.FMT(si)
elif plannerType.lower() == "informedrrtstar":
return og.InformedRRTstar(si)
elif plannerType.lower() == "prmstar":
return og.PRMstar(si)
elif plannerType.lower() == "rrtstar":
return og.RRTstar(si)
elif plannerType.lower() == "sorrtstar":
return og.SORRTstar(si)
elif plannerType.lower() == "rrtxstatic":
return og.RRTXstatic(si)
elif plannerType.lower() == "rrtsharp":
return og.RRTsharp(si)
# multi-query
elif plannerType.lower() == "lazyprmstar":
return og.LazyPRMstar(si)
# single-query
elif plannerType.lower() == "rrtconnect":
return og.RRTConnect(si)
elif plannerType.lower() == "lbtrrt":
return og.LBTRRT(si)
elif plannerType.lower() == "lazylbtrrt":
return og.LazyLBTRRT(si)
else:
ou.OMPL_ERROR(
"Planner-type is not implemented in allocation function.")
def planBITstar(q_st, q_g, res):
# create an SE2 state space
space = ob.RealVectorStateSpace(3)
# set lower and upper bounds
bounds = ob.RealVectorBounds(3)
bounds.setLow(0)
bounds.setHigh(res)
space.setBounds(bounds)
# construct an instance of space information from this state space
si = ob.SpaceInformation(space)
# set state validity checking for this space
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# create start state
start = ob.State(space)
start[0] = q_st[0]
start[1] = q_st[1]
# create goal state
goal = ob.State(space)
goal[0] = q_g[0]
goal[1] = q_g[1]
# create a problem instance
pdef = ob.ProblemDefinition(si)
# set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# create a planner for the defined space
planner = og.BITstar(si)
# set the problem we are trying to solve for the planner
planner.setProblemDefinition(pdef)
# perform setup steps for the planner
planner.setup()
# attempt to solve the problem within one second of planning time
solved = planner.solve(1.0)
if solved:
# get the goal representation from the problem definition (not the same as the goal state)
# and inquire about the found path
path = pdef.getSolutionPath()
print("Found solution:\n")
path_arr = []
print(path.getStateCount())
for i in range(path.getStateCount()):
path_arr.append([])
path_arr[i].append(path.getState(i)[0])
path_arr[i].append(path.getState(i)[1])
return path_arr
else:
print("No solution found")
return []
def allocatePlanner(si, plannerType):
if plannerType.lower() == "bitstar":
return og.BITstar(si)
elif plannerType.lower() == "fmtstar":
return og.FMT(si)
elif plannerType.lower() == "prmstar":
return og.PRMstar(si)
elif plannerType.lower() == "rrtstar":
return og.RRTstar(si)
else:
OMPL_ERROR("Planner-type is not implemented in allocation function.")
def setPlanner_3d(self):
self.si = self.ss.getSpaceInformation()
if self.plannerType.lower() == "bitstar":
planner = og.BITstar(self.si)
elif self.plannerType.lower() == "fmtstar":
planner = og.FMT(self.si)
elif self.plannerType.lower() == "informedrrtstar":
planner = og.InformedRRTstar(self.si)
elif self.plannerType.lower() == "prmstar":
planner = og.PRMstar(self.si)
elif self.plannerType.lower() == "rrtstar":
planner = og.RRTstar(self.si)
elif self.plannerType.lower() == "sorrtstar":
planner = og.SORRTstar(self.si)
else:
print("Planner-type is not implemented in allocation function.")
planner = og.RRTstar(self.si)
self.ss.setPlanner(planner)
def allocatePlanner(self, si, plannerType):
if plannerType.lower() == "bfmtstar":
return og.BFMT(si)
elif plannerType.lower() == "bitstar":
return og.BITstar(si)
elif plannerType.lower() == "fmtstar":
return og.FMT(si)
elif plannerType.lower() == "informedrrtstar":
return og.InformedRRTstar(si)
elif plannerType.lower() == "prmstar":
return og.PRMstar(si)
elif plannerType.lower() == "rrtstar":
return og.RRTstar(si)
elif plannerType.lower() == "sorrtstar":
return og.SORRTstar(si)
else:
ou.OMPL_ERROR(
"Planner-type is not implemented in allocation function.")
def allocatePlanner(si, plannerType):
if plannerType.lower() == "bfmtstar":
return og.BFMT(si)
elif plannerType.lower() == "bitstar":
planner = og.BITstar(si)
planner.setPruning(False)
planner.setSamplesPerBatch(200)
planner.setRewireFactor(20.)
return planner
elif plannerType.lower() == "fmtstar":
return og.FMT(si)
elif plannerType.lower() == "informedrrtstar":
return og.InformedRRTstar(si)
elif plannerType.lower() == "prmstar":
return og.PRMstar(si)
elif plannerType.lower() == "rrtstar":
return og.RRTstar(si)
elif plannerType.lower() == "sorrtstar":
return og.SORRTstar(si)
elif plannerType.lower() == 'rrtconnect':
return og.RRTConnect(si)
else:
ou.OMPL_ERROR(
"Planner-type is not implemented in allocation function.")
def plan(grid):
#agent and goal are represented by a point(x,y) and radius
global x
global time
x = grid
agent: Agent = grid.agent
goal: Goal = grid.goal
# Construct the robot state space in which we're planning. R2
space = ob.RealVectorStateSpace(2)
# Set the bounds of space to be inside Map
bounds = ob.RealVectorBounds(2)
# projection
pj = ProjectionEvaluator(space)
print('pj=', pj)
pj.setCellSizes(list2vec([1.0, 1.0]))
space.registerDefaultProjection(pj)
# Construct the robot state space in which we're planning.
bounds.setLow(0, 0) #set min x to _ 0
bounds.setHigh(0, grid.size.width) #set max x to _ x width
bounds.setLow(1, 0) #set min y to _ 0
bounds.setHigh(1, grid.size.height) #set max y to _ y height
space.setBounds(bounds)
print("bounds=", bounds.getVolume())
# Construct a space information instance for this state space
si = ob.SpaceInformation(space)
# Set the object used to check which states in the space are valid
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# Set robot's starting state to agent's position (x,y) -> e.g. (0,0)
start = ob.State(space)
start[0] = float(agent.position.x)
start[1] = float(agent.position.y)
print(start[0], start[1])
# Set robot's goal state (x,y) -> e.g. (1.0,0.0)
goal = ob.State(space)
goal[0] = float(grid.goal.position.x)
goal[1] = float(grid.goal.position.y)
# Create a problem instance
pdef = ob.ProblemDefinition(si)
# Set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
#pdef.setOptimizationObjective(allocateObjective(si, objectiveType))
# ******create a planner for the defined space
planner = og.BITstar(si)
# set the problem we are trying to solve for the planner
planner.setProblemDefinition(pdef)
print(planner)
#print('checking projection',planner.getProjectionEvaluator())
print("__________________________________________________________")
# perform setup steps for the planner
planner.setup()
# print the settings for this space
print("space settings\n")
print(si.settings())
print("****************************************************************")
print("problem settings\n")
# print the problem settings
print(pdef)
# attempt to solve the problem within ten second of planning time
solved = planner.solve(time)
# For troubleshooting
if solved:
# get the goal representation from the problem definition (not the same as the goal state)
# and inquire about the found path
path = pdef.getSolutionPath()
print("Found solution:\n%s" % path)
#return trace for _find_path_internal method
print(
"$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$"
)
if path is None:
return None
x = pdef.getSolutionPath().printAsMatrix()
lst = x.split()
lst = [int(round(float(x), 0)) for x in lst]
print(x)
print(lst)
trace = []
for i in range(0, len(lst), 2):
trace.append(Point(lst[i], lst[i + 1]))
print(trace)
return trace
else:
print("No solution found")
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
