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 
51 namespace ob = ompl::base;
52 namespace og = ompl::geometric;
53 
54 class Plane2DEnvironment
55 {
56 public:
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 
160 private:
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 
190 int 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 }
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:135
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:80
Definition of an abstract state.
Definition: State.h:114
This namespace contains sampling based planning routines shared by both planning under geometric cons...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
Definition of a geometric path.
Definition: PathGeometric.h:98
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
Load and save .ppm files - "portable pixmap format" an image file formats designed to be easily excha...
Definition: PPM.h:79
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Definition of a scoped state.
Definition: ScopedState.h:121
void interpolate(unsigned int count)
Insert a number of states in a path so that the path is made up of exactly count states....
The exception type for ompl.
Definition: Exception.h:79
base::State * getState(unsigned int index)
Get the state located at index along the path.