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>
43 #include <ompl/config.h>
44 #include <../tests/resources/config.h>
46 #include <boost/filesystem.hpp>
49 namespace ob = ompl::base;
50 namespace og = ompl::geometric;
52 class Plane2DEnvironment
56 Plane2DEnvironment(
const char *ppm_file)
61 ppm_.loadFile(ppm_file);
66 OMPL_ERROR(
"Unable to load %s.\n%s", ppm_file, ex.what());
73 maxWidth_ = ppm_.getWidth() - 1;
74 maxHeight_ = ppm_.getHeight() - 1;
78 ss_->setStateValidityChecker(boost::bind(&Plane2DEnvironment::isStateValid,
this, _1));
80 ss_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->
getMaximumExtent());
85 bool plan(
unsigned int start_row,
unsigned int start_col,
unsigned int goal_row,
unsigned int goal_col)
95 ss_->setStartAndGoalStates(start, goal);
97 for (
int i = 0 ; i < 10 ; ++i)
99 if (ss_->getPlanner())
100 ss_->getPlanner()->clear();
103 const std::size_t ns = ss_->getProblemDefinition()->getSolutionCount();
105 if (ss_->haveSolutionPath())
107 ss_->simplifySolution();
109 ss_->getPathSimplifier()->simplifyMax(p);
110 ss_->getPathSimplifier()->smoothBSpline(p);
117 void recordSolution()
119 if (!ss_ || !ss_->haveSolutionPath())
134 void save(
const char *filename)
138 ppm_.saveFile(filename);
143 bool isStateValid(
const ob::State *state)
const
149 return c.red > 127 && c.green > 127 && c.blue > 127;
159 int main(
int,
char **)
161 std::cout <<
"OMPL version: " << OMPL_VERSION << std::endl;
163 boost::filesystem::path path(TEST_RESOURCES_DIR);
164 Plane2DEnvironment env((path /
"ppm/floor.ppm").
string().c_str());
166 if (env.plan(0, 0, 777, 1265))
168 env.recordSolution();
169 env.save(
"result_demo.ppm");
Load and save .ppm files.
virtual void setup(void)
Perform final setup steps. This function is automatically called by the SpaceInformation. If any default projections are to be registered, this call will set them and call their setup() functions. It is safe to call this function multiple times. At a subsequent call, projections that have been previously user configured are not re-instantiated, but their setup() method is still called.
Definition of a scoped state.
A boost shared pointer wrapper for ompl::base::StateSpace.
virtual double getMaximumExtent(void) const
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
base::State * getState(unsigned int index)
Get the state located at index along the path.
std::size_t getStateCount(void) const
Get the number of states (way-points) that make up this path.
State StateType
Define the type of state allocated by this space.
const T * as(void) const
Cast this instance to a desired type.
Create the set of classes typically needed to solve a geometric problem.
A boost shared pointer wrapper for ompl::geometric::SimpleSetup.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
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.
A state space representing Rn. The distance function is the L2 norm.
Definition of an abstract state.
The exception type for ompl.
void addDimension(double minBound=0.0, double maxBound=0.0)
Increase the dimensionality of the state space by 1. Optionally, bounds can be specified for this add...
Definition of a geometric path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.