Point2DPlanning.py
1#!/usr/bin/env python
2
3
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
55 def __init__(self, ppm_file):
56 self.ppm_ = ou.PPM()
57 self.ppm_.loadFile(ppm_file)
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")
A state space representing Rn. The distance function is the L2 norm.
Definition of an abstract state.
Definition: State.h:50
This class contains routines that attempt to simplify geometric paths.
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...