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>
51 class Plane2DEnvironment
55 Plane2DEnvironment(
const char *ppm_file,
bool useThunder =
true)
60 ppm_.loadFile(ppm_file);
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());
72 maxWidth_ = ppm_.getWidth() - 1;
73 maxHeight_ = ppm_.getHeight() - 1;
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
142 return c.red > 127 && c.green > 127 && c.blue > 127;
145 ot::ExperienceSetupPtr expPlanner_;
146 ob::ValidStateSamplerPtr vss_;
152 int 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...
Definition of a scoped state.
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.
#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.