def plan():
# construct the state space we are planning in
space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# create a control space
cspace = oc.RealVectorControlSpace(space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(-.3)
cbounds.setHigh(.3)
cspace.setBounds(cbounds)
# define a simple setup class
ss = oc.SimpleSetup(cspace)
validityChecker = ob.StateValidityCheckerFn(
partial(isStateValid, ss.getSpaceInformation()))
ss.setStateValidityChecker(validityChecker)
ode = oc.ODE(kinematicCarODE)
odeSolver = oc.ODEBasicSolver(ss.getSpaceInformation(), ode)
propagator = oc.ODESolver.getStatePropagator(odeSolver)
ss.setStatePropagator(propagator)
# create a start state
start = ob.State(space)
start().setX(-0.5)
start().setY(0.0)
start().setYaw(0.0)
# create a goal state
goal = ob.State(space)
goal().setX(0.0)
goal().setY(0.5)
goal().setYaw(0.0)
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# attempt to solve the problem
solved = ss.solve(120.0)
if solved:
# print the path to screen
print("Found solution:\n%s" %
ss.getSolutionPath().asGeometric().printAsMatrix())
def setSpace(self):
self.space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
self.bounds = ob.RealVectorBounds(2)
if not self.costMap:
self.bounds.setLow(-8)
self.bounds.setHigh(20)
else:
ox = self.costMap.getOriginX()
oy = self.costMap.getOriginY()
size_x = self.costMap.getSizeInMetersX()
size_y = self.costMap.getSizeInMetersY()
low = min(ox, oy)
high = max(ox + size_x, oy + size_y)
print("low", low)
print("high", high)
self.bounds.setLow(low)
self.bounds.setHigh(high)
self.space.setBounds(self.bounds)
# create a control space
self.cspace = oc.RealVectorControlSpace(self.space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(0, -1.5)
cbounds.setHigh(0, 1.5)
cbounds.setLow(1, -3)
cbounds.setHigh(1, 3)
self.cspace.setBounds(cbounds)
# define a simple setup class
self.ss = oc.SimpleSetup(self.cspace)
validityChecker = ob.StateValidityCheckerFn(
partial(self.isStateValid, self.ss.getSpaceInformation()))
self.ss.setStateValidityChecker(validityChecker)
ode = oc.ODE(self.kinematicCarODE)
odeSolver = oc.ODEBasicSolver(self.ss.getSpaceInformation(), ode)
propagator = oc.ODESolver.getStatePropagator(odeSolver)
self.ss.setStatePropagator(propagator)