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/util/PPM.h>
42 
43 #include <ompl/config.h>
44 #include <../tests/resources/config.h>
45 
46 #include <boost/filesystem.hpp>
47 #include <iostream>
48 
49 namespace ob = ompl::base;
50 namespace og = ompl::geometric;
51 
52 class Plane2DEnvironment
53 {
54 public:
55 
56  Plane2DEnvironment(const char *ppm_file)
57  {
58  bool ok = false;
59  try
60  {
61  ppm_.loadFile(ppm_file);
62  ok = true;
63  }
64  catch(ompl::Exception &ex)
65  {
66  OMPL_ERROR("Unable to load %s.\n%s", ppm_file, ex.what());
67  }
68  if (ok)
69  {
70  auto space(std::make_shared<ob::RealVectorStateSpace>());
71  space->addDimension(0.0, ppm_.getWidth());
72  space->addDimension(0.0, ppm_.getHeight());
73  maxWidth_ = ppm_.getWidth() - 1;
74  maxHeight_ = ppm_.getHeight() - 1;
75  ss_ = std::make_shared<og::SimpleSetup>(space);
76 
77  // set state validity checking for this space
78  ss_->setStateValidityChecker([this](const ob::State *state) { return isStateValid(state); });
79  space->setup();
80  ss_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());
81  // ss_->setPlanner(std::make_shared<og::RRTConnect>(ss_->getSpaceInformation()));
82  }
83  }
84 
85  bool plan(unsigned int start_row, unsigned int start_col, unsigned int goal_row, unsigned int goal_col)
86  {
87  if (!ss_)
88  return false;
89  ob::ScopedState<> start(ss_->getStateSpace());
90  start[0] = start_row;
91  start[1] = start_col;
92  ob::ScopedState<> goal(ss_->getStateSpace());
93  goal[0] = goal_row;
94  goal[1] = goal_col;
95  ss_->setStartAndGoalStates(start, goal);
96  // generate a few solutions; all will be added to the goal;
97  for (int i = 0 ; i < 10 ; ++i)
98  {
99  if (ss_->getPlanner())
100  ss_->getPlanner()->clear();
101  ss_->solve();
102  }
103  const std::size_t ns = ss_->getProblemDefinition()->getSolutionCount();
104  OMPL_INFORM("Found %d solutions", (int)ns);
105  if (ss_->haveSolutionPath())
106  {
107  ss_->simplifySolution();
108  og::PathGeometric &p = ss_->getSolutionPath();
109  ss_->getPathSimplifier()->simplifyMax(p);
110  ss_->getPathSimplifier()->smoothBSpline(p);
111  return true;
112  }
113  else
114  return false;
115  }
116 
117  void recordSolution()
118  {
119  if (!ss_ || !ss_->haveSolutionPath())
120  return;
121  og::PathGeometric &p = ss_->getSolutionPath();
122  p.interpolate();
123  for (std::size_t i = 0 ; i < p.getStateCount() ; ++i)
124  {
125  const int w = std::min(maxWidth_, (int)p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[0]);
126  const int h = std::min(maxHeight_, (int)p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[1]);
127  ompl::PPM::Color &c = ppm_.getPixel(h, w);
128  c.red = 255;
129  c.green = 0;
130  c.blue = 0;
131  }
132  }
133 
134  void save(const char *filename)
135  {
136  if (!ss_)
137  return;
138  ppm_.saveFile(filename);
139  }
140 
141 private:
142 
143  bool isStateValid(const ob::State *state) const
144  {
145  const int w = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[0], maxWidth_);
146  const int h = std::min((int)state->as<ob::RealVectorStateSpace::StateType>()->values[1], maxHeight_);
147 
148  const ompl::PPM::Color &c = ppm_.getPixel(h, w);
149  return c.red > 127 && c.green > 127 && c.blue > 127;
150  }
151 
152  og::SimpleSetupPtr ss_;
153  int maxWidth_;
154  int maxHeight_;
155  ompl::PPM ppm_;
156 
157 };
158 
159 int main(int, char **)
160 {
161  std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
162 
163  boost::filesystem::path path(TEST_RESOURCES_DIR);
164  Plane2DEnvironment env((path / "ppm/floor.ppm").string().c_str());
165 
166  if (env.plan(0, 0, 777, 1265))
167  {
168  env.recordSolution();
169  env.save("result_demo.ppm");
170  }
171 
172  return 0;
173 }
Load and save .ppm files - "portable pixmap format" an image file formats designed to be easily excha...
Definition: PPM.h:46
Definition of a scoped state.
Definition: ScopedState.h:56
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
base::State * getState(unsigned int index)
Get the state located at index along the path.
A shared pointer wrapper for ompl::geometric::SimpleSetup.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void interpolate(unsigned int count)
Insert a number of states in a path so that the path is made up of exactly count states. States are inserted uniformly (more states on longer segments). Changes are performed only if a path has less than count states.
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
The exception type for ompl.
Definition: Exception.h:46
Definition of a geometric path.
Definition: PathGeometric.h:60
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:47
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68