Point2DPlanning.py
1 #!/usr/bin/env python
2 
3 
36 
37 # Author: Ioan Sucan, Mark Moll
38 
39 try:
40  from ompl import util as ou
41  from ompl import base as ob
42  from ompl import geometric as og
43 except:
44  # if the ompl module is not in the PYTHONPATH assume it is installed in a
45  # subdirectory of the parent directory called "py-bindings."
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
51 import sys
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(1.0 / space.getMaximumExtent())
70  # self.ss_.setPlanner(og.RRTConnect(self.ss_.getSpaceInformation()))
71 
72  def plan(self, start_row, start_col, goal_row, goal_col):
73  if not self.ss_:
74  return false
75  start = ob.State(self.ss_.getStateSpace())
76  start()[0] = start_row
77  start()[1] = start_col
78  goal = ob.State(self.ss_.getStateSpace())
79  goal()[0] = goal_row
80  goal()[1] = goal_col
81  self.ss_.setStartAndGoalStates(start, goal)
82  # generate a few solutions; all will be added to the goal
83  for i in range(10):
84  if self.ss_.getPlanner():
85  self.ss_.getPlanner().clear()
86  self.ss_.solve()
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()
92  ps = og.PathSimplifier(self.ss_.getSpaceInformation())
93  ps.simplifyMax(p)
94  ps.smoothBSpline(p)
95  return True
96  else:
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")
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
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.
Definition: State.h:49
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...