39 from os.path
import abspath, dirname, join
42 from ompl
import util
as ou
43 from ompl
import base
as ob
44 from ompl
import geometric
as og
48 sys.path.insert(0, join(dirname(dirname(abspath(__file__))),
'py-bindings'))
49 from ompl
import util
as ou
50 from ompl
import base
as ob
51 from ompl
import geometric
as og
52 from functools
import partial
55 def __init__(self, ppm_file):
56 self.
ppm_ppm_ = ou.PPM()
57 self.
ppm_ppm_.loadFile(ppm_file)
59 space.addDimension(0.0, self.
ppm_ppm_.getWidth())
60 space.addDimension(0.0, self.
ppm_ppm_.getHeight())
67 partial(Plane2DEnvironment.isStateValid, self)))
69 self.
ss_ss_.getSpaceInformation().setStateValidityCheckingResolution( \
70 1.0 / space.getMaximumExtent())
73 def plan(self, start_row, start_col, goal_row, goal_col):
77 start()[0] = start_row
78 start()[1] = start_col
82 self.
ss_ss_.setStartAndGoalStates(start, goal)
85 if self.
ss_ss_.getPlanner():
86 self.
ss_ss_.getPlanner().clear()
88 ns = self.
ss_ss_.getProblemDefinition().getSolutionCount()
89 print(
"Found %d solutions" % ns)
90 if self.
ss_ss_.haveSolutionPath():
91 self.
ss_ss_.simplifySolution()
92 p = self.
ss_ss_.getSolutionPath()
99 def recordSolution(self):
100 if not self.
ss_ss_
or not self.
ss_ss_.haveSolutionPath():
102 p = self.
ss_ss_.getSolutionPath()
104 for i
in range(p.getStateCount()):
105 w = min(self.
maxWidth_maxWidth_, int(p.getState(i)[0]))
106 h = min(self.
maxHeight_maxHeight_, int(p.getState(i)[1]))
107 c = self.
ppm_ppm_.getPixel(h, w)
112 def save(self, filename):
115 self.
ppm_ppm_.saveFile(filename)
117 def isStateValid(self, state):
118 w = min(int(state[0]), self.
maxWidth_maxWidth_)
119 h = min(int(state[1]), self.
maxHeight_maxHeight_)
121 c = self.
ppm_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")
A state space representing Rn. The distance function is the L2 norm.
Definition of an abstract state.
This class contains routines that attempt to simplify geometric paths.
Create the set of classes typically needed to solve a geometric problem.
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...