Loading...
Searching...
No Matches
Point2DPlanning.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Ioan Sucan */
36
37#include <ompl/base/spaces/RealVectorStateSpace.h>
38#include <ompl/geometric/SimpleSetup.h>
39#include <ompl/geometric/planners/rrt/RRTstar.h>
40#include <ompl/geometric/planners/rrt/RRTConnect.h>
41#include <ompl/geometric/planners/prm/PRMstar.h>
42
43#include <ompl/util/PPM.h>
44#include <ompl/base/samplers/DeterministicStateSampler.h>
45#include <ompl/base/samplers/deterministic/HaltonSequence.h>
46#include <ompl/config.h>
47
48#include <boost/filesystem.hpp>
49#include <iostream>
50
51namespace ob = ompl::base;
52namespace og = ompl::geometric;
53
54class Plane2DEnvironment
55{
56public:
57 Plane2DEnvironment(const char *ppm_file, bool use_deterministic_sampling = false)
58 {
59 bool ok = false;
60 useDeterministicSampling_ = use_deterministic_sampling;
61 try
62 {
63 ppm_.loadFile(ppm_file);
64 ok = true;
65 }
66 catch (ompl::Exception &ex)
67 {
68 OMPL_ERROR("Unable to load %s.\n%s", ppm_file, ex.what());
69 }
70 if (ok)
71 {
72 auto space(std::make_shared<ob::RealVectorStateSpace>());
73 space->addDimension(0.0, ppm_.getWidth());
74 space->addDimension(0.0, ppm_.getHeight());
75 maxWidth_ = ppm_.getWidth() - 1;
76 maxHeight_ = ppm_.getHeight() - 1;
77 ss_ = std::make_shared<og::SimpleSetup>(space);
78
79 // set state validity checking for this space
80 ss_->setStateValidityChecker([this](const ob::State *state) { return isStateValid(state); });
81 space->setup();
82 ss_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());
83
84 // set the deterministic sampler
85 // 2D space, no need to specify bases specifically
86 if (useDeterministicSampling_)
87 {
88 // PRMstar can use the deterministic sampling
89 ss_->setPlanner(std::make_shared<og::PRMstar>(ss_->getSpaceInformation()));
90 space->setStateSamplerAllocator(std::bind(&Plane2DEnvironment::allocateHaltonStateSamplerRealVector,
91 this, std::placeholders::_1, 2,
92 std::vector<unsigned int>{2, 3}));
93 }
94 }
95 }
96
97 bool plan(unsigned int start_row, unsigned int start_col, unsigned int goal_row, unsigned int goal_col)
98 {
99 if (!ss_)
100 return false;
101 ob::ScopedState<> start(ss_->getStateSpace());
102 start[0] = start_row;
103 start[1] = start_col;
104 ob::ScopedState<> goal(ss_->getStateSpace());
105 goal[0] = goal_row;
106 goal[1] = goal_col;
107 ss_->setStartAndGoalStates(start, goal);
108 // generate a few solutions; all will be added to the goal;
109 for (int i = 0; i < 10; ++i)
110 {
111 if (ss_->getPlanner())
112 ss_->getPlanner()->clear();
113 ss_->solve();
114 }
115 const std::size_t ns = ss_->getProblemDefinition()->getSolutionCount();
116 OMPL_INFORM("Found %d solutions", (int)ns);
117 if (ss_->haveSolutionPath())
118 {
119 if (!useDeterministicSampling_)
120 ss_->simplifySolution();
121
122 og::PathGeometric &p = ss_->getSolutionPath();
123 if (!useDeterministicSampling_)
124 {
125 ss_->getPathSimplifier()->simplifyMax(p);
126 ss_->getPathSimplifier()->smoothBSpline(p);
127 }
128
129 return true;
130 }
131
132 return false;
133 }
134
135 void recordSolution()
136 {
137 if (!ss_ || !ss_->haveSolutionPath())
138 return;
139 og::PathGeometric &p = ss_->getSolutionPath();
140 p.interpolate();
141 for (std::size_t i = 0; i < p.getStateCount(); ++i)
142 {
143 const int w = std::min(maxWidth_, (int)p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[0]);
144 const int h =
145 std::min(maxHeight_, (int)p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[1]);
146 ompl::PPM::Color &c = ppm_.getPixel(h, w);
147 c.red = 255;
148 c.green = 0;
149 c.blue = 0;
150 }
151 }
152
153 void save(const char *filename)
154 {
155 if (!ss_)
156 return;
157 ppm_.saveFile(filename);
158 }
159
160private:
161 bool isStateValid(const ob::State *state) const
162 {
163 const int w = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[0], maxWidth_);
164 const int h = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[1], maxHeight_);
165
166 const ompl::PPM::Color &c = ppm_.getPixel(h, w);
167 return c.red > 127 && c.green > 127 && c.blue > 127;
168 }
169
170 ob::StateSamplerPtr allocateHaltonStateSamplerRealVector(const ompl::base::StateSpace *space, unsigned int dim,
171 std::vector<unsigned int> bases = {})
172 {
173 // specify which deterministic sequence to use, here: HaltonSequence
174 // optionally we can specify the bases used for generation (otherwise first dim prime numbers are used)
175 if (bases.size() != 0)
176 return std::make_shared<ompl::base::RealVectorDeterministicStateSampler>(
177 space, std::make_shared<ompl::base::HaltonSequence>(bases.size(), bases));
178 else
179 return std::make_shared<ompl::base::RealVectorDeterministicStateSampler>(
180 space, std::make_shared<ompl::base::HaltonSequence>(dim));
181 }
182
183 og::SimpleSetupPtr ss_;
184 int maxWidth_;
185 int maxHeight_;
186 ompl::PPM ppm_;
187 bool useDeterministicSampling_;
188};
189
190int main(int /*argc*/, char ** /*argv*/)
191{
192 std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
193
194 boost::filesystem::path path(TEST_RESOURCES_DIR);
195 bool useDeterministicSampling = true;
196 Plane2DEnvironment env((path / "ppm/floor.ppm").string().c_str(), useDeterministicSampling);
197
198 if (env.plan(0, 0, 777, 1265))
199 {
200 env.recordSolution();
201 env.save("result_demo.ppm");
202 }
203
204 return 0;
205}
The exception type for ompl.
Definition: Exception.h:47
Load and save .ppm files - "portable pixmap format" an image file formats designed to be easily excha...
Definition: PPM.h:47
Definition of a scoped state.
Definition: ScopedState.h:57
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:71
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
Definition of a geometric path.
Definition: PathGeometric.h:66
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
void interpolate(unsigned int count)
Insert a number of states in a path so that the path is made up of exactly count states....
base::State * getState(unsigned int index)
Get the state located at index along the path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:48