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