37#include <ompl/base/spaces/RealVectorStateSpace.h>
38#include <ompl/tools/thunder/Thunder.h>
39#include <ompl/tools/lightning/Lightning.h>
40#include <ompl/util/PPM.h>
42#include <ompl/config.h>
44#include <boost/filesystem.hpp>
51class Plane2DEnvironment
55 Plane2DEnvironment(
const char *ppm_file,
bool useThunder =
true)
65 OMPL_ERROR(
"Unable to load %s.\n%s", ppm_file, ex.what());
69 auto space(std::make_shared<ob::RealVectorStateSpace>());
70 space->addDimension(0.0, ppm_.
getWidth());
71 space->addDimension(0.0, ppm_.
getHeight());
76 expPlanner_ = std::make_shared<ot::Thunder>(space);
77 expPlanner_->setFilePath(
"thunder.db");
81 expPlanner_ = std::make_shared<ot::Lightning>(space);
82 expPlanner_->setFilePath(
"lightning.db");
85 expPlanner_->setStateValidityChecker([
this](
const ob::State *state)
86 {
return isStateValid(state); });
88 expPlanner_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());
89 vss_ = expPlanner_->getSpaceInformation()->allocValidStateSampler();
105 std::cout << std::endl;
106 std::cout <<
"-------------------------------------------------------" << std::endl;
107 std::cout <<
"-------------------------------------------------------" << std::endl;
114 expPlanner_->clear();
117 vss_->sample(start.get());
119 vss_->sample(goal.get());
120 expPlanner_->setStartAndGoalStates(start, goal);
122 bool solved = expPlanner_->solve(10.);
125 expPlanner_->getLastPlanComputationTime());
129 expPlanner_->doPostProcessing();
136 bool isStateValid(
const ob::State *state)
const
138 const int w = std::min((
int)state->
as<ob::RealVectorStateSpace::StateType>()->values[0], maxWidth_);
139 const int h = std::min((
int)state->
as<ob::RealVectorStateSpace::StateType>()->values[1], maxHeight_);
142 return c.red > 127 && c.green > 127 && c.blue > 127;
145 ot::ExperienceSetupPtr expPlanner_;
146 ob::ValidStateSamplerPtr vss_;
152int main(
int argc,
char **)
154 std::cout <<
"OMPL version: " << OMPL_VERSION << std::endl;
156 boost::filesystem::path path(TEST_RESOURCES_DIR);
157 Plane2DEnvironment env((path /
"ppm" /
"floor.ppm").
string().c_str(), argc==1);
159 for (
unsigned int i = 0; i < 100; ++i)
The exception type for ompl.
Load and save .ppm files - "portable pixmap format" an image file formats designed to be easily excha...
void loadFile(const char *filename)
Load a .ppm file. Throw an exception in case of an error.
unsigned int getHeight() const
Get the height of the loaded image.
const Color & getPixel(const int row, const int col) const
Directly access a pixel in the image.
unsigned int getWidth() const
Get the width of the loaded image.
Definition of a scoped state.
Definition of an abstract state.
const T * as() const
Cast this instance to a desired type.
#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.