Point2DPlanning.py
1 #!/usr/bin/env python
2 
3 
36 
37 # Author: Ioan Sucan, Mark Moll
38 
39 from os.path import abspath, dirname, join
40 import sys
41 try:
42  from ompl import util as ou
43  from ompl import base as ob
44  from ompl import geometric as og
45 except ImportError:
46  # if the ompl module is not in the PYTHONPATH assume it is installed in a
47  # subdirectory of the parent directory called "py-bindings."
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
53 
55  def __init__(self, ppm_file):
56  self.ppm_ = ou.PPM()
57  self.ppm_.loadFile(ppm_file)
58  space = ob.RealVectorStateSpace()
59  space.addDimension(0.0, self.ppm_.getWidth())
60  space.addDimension(0.0, self.ppm_.getHeight())
61  self.maxWidth_ = self.ppm_.getWidth() - 1
62  self.maxHeight_ = self.ppm_.getHeight() - 1
63  self.ss_ = og.SimpleSetup(space)
64 
65  # set state validity checking for this space
66  self.ss_.setStateValidityChecker(ob.StateValidityCheckerFn(
67  partial(Plane2DEnvironment.isStateValid, self)))
68  space.setup()
69  self.ss_.getSpaceInformation().setStateValidityCheckingResolution( \
70  1.0 / space.getMaximumExtent())
71  # self.ss_.setPlanner(og.RRTConnect(self.ss_.getSpaceInformation()))
72 
73  def plan(self, start_row, start_col, goal_row, goal_col):
74  if not self.ss_:
75  return False
76  start = ob.State(self.ss_.getStateSpace())
77  start()[0] = start_row
78  start()[1] = start_col
79  goal = ob.State(self.ss_.getStateSpace())
80  goal()[0] = goal_row
81  goal()[1] = goal_col
82  self.ss_.setStartAndGoalStates(start, goal)
83  # generate a few solutions; all will be added to the goal
84  for _ in range(10):
85  if self.ss_.getPlanner():
86  self.ss_.getPlanner().clear()
87  self.ss_.solve()
88  ns = self.ss_.getProblemDefinition().getSolutionCount()
89  print("Found %d solutions" % ns)
90  if self.ss_.haveSolutionPath():
91  self.ss_.simplifySolution()
92  p = self.ss_.getSolutionPath()
93  ps = og.PathSimplifier(self.ss_.getSpaceInformation())
94  ps.simplifyMax(p)
95  ps.smoothBSpline(p)
96  return True
97  return False
98 
99  def recordSolution(self):
100  if not self.ss_ or not self.ss_.haveSolutionPath():
101  return
102  p = self.ss_.getSolutionPath()
103  p.interpolate()
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)
108  c.red = 255
109  c.green = 0
110  c.blue = 0
111 
112  def save(self, filename):
113  if not self.ss_:
114  return
115  self.ppm_.saveFile(filename)
116 
117  def isStateValid(self, state):
118  w = min(int(state[0]), self.maxWidth_)
119  h = min(int(state[1]), self.maxHeight_)
120 
121  c = self.ppm_.getPixel(h, w)
122  return c.red > 127 and c.green > 127 and c.blue > 127
123 
124 
125 if __name__ == "__main__":
126  fname = join(join(join(join(dirname(dirname(abspath(__file__))), \
127  'tests'), 'resources'), 'ppm'), 'floor.ppm')
128  env = Plane2DEnvironment(fname)
129 
130  if env.plan(0, 0, 777, 1265):
131  env.recordSolution()
132  env.save("result_demo.ppm")
Definition of an abstract state.
Definition: State.h:113
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:126
This class contains routines that attempt to simplify geometric paths.
A state space representing Rn. The distance function is the L2 norm.
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...