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> 43 #include <../tests/resources/config.h> 45 #include <boost/filesystem.hpp> 52 class Plane2DEnvironment
56 Plane2DEnvironment(
const char *ppm_file,
bool useThunder =
true)
61 ppm_.loadFile(ppm_file);
66 OMPL_ERROR(
"Unable to load %s.\n%s", ppm_file, ex.what());
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;
77 expPlanner_ = std::make_shared<ot::Thunder>(space);
78 expPlanner_->setFilePath(
"thunder.db");
82 expPlanner_ = std::make_shared<ot::Lightning>(space);
83 expPlanner_->setFilePath(
"lightning.db");
86 expPlanner_->setStateValidityChecker([
this](
const ob::State *state)
87 {
return isStateValid(state); });
89 expPlanner_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());
90 vss_ = expPlanner_->getSpaceInformation()->allocValidStateSampler();
106 std::cout << std::endl;
107 std::cout <<
"-------------------------------------------------------" << std::endl;
108 std::cout <<
"-------------------------------------------------------" << std::endl;
115 expPlanner_->clear();
118 vss_->sample(start.get());
120 vss_->sample(goal.get());
121 expPlanner_->setStartAndGoalStates(start, goal);
123 bool solved = expPlanner_->solve(10.);
126 expPlanner_->getLastPlanComputationTime());
130 expPlanner_->doPostProcessing();
137 bool isStateValid(
const ob::State *state)
const 143 return c.red > 127 && c.green > 127 && c.blue > 127;
146 ot::ExperienceSetupPtr expPlanner_;
153 int main(
int argc,
char *argv[])
155 std::cout <<
"OMPL version: " << OMPL_VERSION << std::endl;
157 boost::filesystem::path path(TEST_RESOURCES_DIR);
158 Plane2DEnvironment env((path /
"ppm" /
"floor.ppm").
string().c_str(), argc==1);
160 for (
unsigned int i = 0; i < 100; ++i)
Load and save .ppm files - "portable pixmap format" an image file formats designed to be easily excha...
A shared pointer wrapper for ompl::base::ValidStateSampler.
Definition of a scoped state.
const T * as() const
Cast this instance to a desired type.
ompl::base::State StateType
Define the type of state allocated by this space.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition of an abstract state.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
The exception type for ompl.
This namespace contains code that is specific to planning under geometric constraints.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.