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>
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>
48 #include <boost/filesystem.hpp>
54 class Plane2DEnvironment
57 Plane2DEnvironment(
const char *ppm_file,
bool use_deterministic_sampling =
false)
60 useDeterministicSampling_ = use_deterministic_sampling;
63 ppm_.loadFile(ppm_file);
68 OMPL_ERROR(
"Unable to load %s.\n%s", ppm_file, ex.what());
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);
80 ss_->setStateValidityChecker([
this](
const ob::State *state) {
return isStateValid(state); });
82 ss_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());
86 if (useDeterministicSampling_)
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}));
97 bool plan(
unsigned int start_row,
unsigned int start_col,
unsigned int goal_row,
unsigned int goal_col)
102 start[0] = start_row;
103 start[1] = start_col;
107 ss_->setStartAndGoalStates(start, goal);
109 for (
int i = 0; i < 10; ++i)
111 if (ss_->getPlanner())
112 ss_->getPlanner()->clear();
115 const std::size_t ns = ss_->getProblemDefinition()->getSolutionCount();
117 if (ss_->haveSolutionPath())
119 if (!useDeterministicSampling_)
120 ss_->simplifySolution();
123 if (!useDeterministicSampling_)
125 ss_->getPathSimplifier()->simplifyMax(p);
126 ss_->getPathSimplifier()->smoothBSpline(p);
135 void recordSolution()
137 if (!ss_ || !ss_->haveSolutionPath())
153 void save(
const char *filename)
157 ppm_.saveFile(filename);
161 bool isStateValid(
const ob::State *state)
const
167 return c.red > 127 && c.green > 127 && c.blue > 127;
170 ob::StateSamplerPtr allocateHaltonStateSamplerRealVector(
const ompl::base::StateSpace *space,
unsigned int dim,
171 std::vector<unsigned int> bases = {})
175 if (bases.size() != 0)
176 return std::make_shared<ompl::base::RealVectorDeterministicStateSampler>(
177 space, std::make_shared<ompl::base::HaltonSequence>(bases.size(), bases));
179 return std::make_shared<ompl::base::RealVectorDeterministicStateSampler>(
180 space, std::make_shared<ompl::base::HaltonSequence>(dim));
183 og::SimpleSetupPtr ss_;
187 bool useDeterministicSampling_;
190 int main(
int ,
char ** )
192 std::cout <<
"OMPL version: " << OMPL_VERSION << std::endl;
194 boost::filesystem::path path(TEST_RESOURCES_DIR);
195 bool useDeterministicSampling =
true;
196 Plane2DEnvironment env((path /
"ppm/floor.ppm").
string().c_str(), useDeterministicSampling);
198 if (env.plan(0, 0, 777, 1265))
200 env.recordSolution();
201 env.save(
"result_demo.ppm");
The exception type for ompl.
Load and save .ppm files - "portable pixmap format" an image file formats designed to be easily excha...
Definition of a scoped state.
Representation of a space in which planning can be performed. Topology specific sampling,...
ompl::base::State StateType
Define the type of state allocated by this space.
Definition of an abstract state.
const T * as() const
Cast this instance to a desired type.
Definition of a geometric path.
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.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
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.