40 from ompl
import util
as ou
41 from ompl
import base
as ob
42 from ompl
import geometric
as og
46 sys.path.insert(0, join(dirname(dirname(abspath(__file__))),
'py-bindings'))
47 from ompl
import util
as ou
48 from ompl
import base
as ob
49 from ompl
import geometric
as og
50 from os.path
import abspath, dirname, join
52 from functools
import partial
55 def __init__(self, ppm_file):
57 self.
ppm_.loadFile(ppm_file)
59 space.addDimension(0.0, self.
ppm_.getWidth())
60 space.addDimension(0.0, self.
ppm_.getHeight())
67 partial(Plane2DEnvironment.isStateValid, self)))
69 self.
ss_.getSpaceInformation().setStateValidityCheckingResolution(1.0 / space.getMaximumExtent())
72 def plan(self, start_row, start_col, goal_row, goal_col):
76 start()[0] = start_row
77 start()[1] = start_col
81 self.
ss_.setStartAndGoalStates(start, goal)
84 if self.
ss_.getPlanner():
85 self.
ss_.getPlanner().clear()
87 ns = self.
ss_.getProblemDefinition().getSolutionCount()
88 print(
"Found %d solutions" % ns)
89 if self.
ss_.haveSolutionPath():
90 self.
ss_.simplifySolution()
91 p = self.
ss_.getSolutionPath()
99 def recordSolution(self):
100 if not self.
ss_ or not self.
ss_.haveSolutionPath():
102 p = self.
ss_.getSolutionPath()
104 for i
in range(p.getStateCount()):
105 w = min(self.
maxWidth_, int(p.getState(i)[0]))
106 h = min(self.
maxHeight_, int(p.getState(i)[1]))
107 c = self.
ppm_.getPixel(h, w)
112 def save(self, filename):
115 self.
ppm_.saveFile(filename)
117 def isStateValid(self, state):
121 c = self.
ppm_.getPixel(h, w)
122 return c.red > 127
and c.green > 127
and c.blue > 127
125 if __name__ ==
"__main__":
126 fname = join(join(join(join(dirname(dirname(abspath(__file__))),
127 'tests'),
'resources'),
'ppm'),
'floor.ppm')
130 if env.plan(0, 0, 777, 1265):
132 env.save(
"result_demo.ppm")
Create the set of classes typically needed to solve a geometric problem.
This class contains routines that attempt to simplify geometric paths.
A state space representing Rn. The distance function is the L2 norm.
Definition of an abstract state.
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...