Point2DPlanning.py
1 #!/usr/bin/env python
2 
3 ######################################################################
4 # Software License Agreement (BSD License)
5 #
6 # Copyright (c) 2010, Rice University
7 # All rights reserved.
8 #
9 # Redistribution and use in source and binary forms, with or without
10 # modification, are permitted provided that the following conditions
11 # are met:
12 #
13 # * Redistributions of source code must retain the above copyright
14 # notice, this list of conditions and the following disclaimer.
15 # * Redistributions in binary form must reproduce the above
16 # copyright notice, this list of conditions and the following
17 # disclaimer in the documentation and/or other materials provided
18 # with the distribution.
19 # * Neither the name of the Rice University nor the names of its
20 # contributors may be used to endorse or promote products derived
21 # from this software without specific prior written permission.
22 #
23 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES
30 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 # POSSIBILITY OF SUCH DAMAGE.
35 ######################################################################
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  from os.path import abspath, dirname, join
47  import sys
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(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")
boost::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a boost function can be spec...
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:65
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:50