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
39from os.path import abspath, dirname, join
40import sys
41try:
42    from ompl import util as ou
43    from ompl import base as ob
44    from ompl import geometric as og
45except 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
52from functools import partial
53
54class Plane2DEnvironment:
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
125if __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")
133