#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/config.h>
#include <iostream>
#include <fstream>
#include <ostream>
namespace ob = ompl::base;
namespace og = ompl::geometric;
bool isStateValid(const ob::State *state)
{
// cast the abstract state type to the type we expect
const auto *se3state = state->as<ob::SE3StateSpace::StateType>();
// extract the first component of the state and cast it to what we expect
const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);
// extract the second component of the state and cast it to what we expect
const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1);
// check validity of state defined by pos & rot
// return a value that is always true but uses the two variables we define, so we avoid compiler warnings
return (const void*)rot != (const void*)pos;
}
void planWithSimpleSetup()
{
// construct the state space we are planning in
auto space(std::make_shared<ob::SE3StateSpace>());
// set the bounds for the R^3 part of SE(3)
ob::RealVectorBounds bounds(3);
bounds.setLow(-1);
bounds.setHigh(1);
space->setBounds(bounds);
// define a simple setup class
og::SimpleSetup ss(space);
// set state validity checking for this space
ss.setStateValidityChecker([](const ob::State *state) { return isStateValid(state); });
// create a random start state
ob::ScopedState<> start(space);
start.random();
// create a random goal state
ob::ScopedState<> goal(space);
goal.random();
// set the start and goal states
ss.setStartAndGoalStates(start, goal);
// this call is optional, but we put it in to get more output information
ss.setup();
ss.print();
// set planner
ob::PlannerPtr planner(new og::RRTConnect(ss.getSpaceInformation()));
ss.setPlanner(planner);
// attempt to solve the problem within one second of planning time
ob::PlannerStatus solved = ss.solve(1.0);
if (solved)
{
std::cout << "Found solution:" << std::endl;
// ------------------------------------------------
// problem encountered: no corresponding file generation after running the programmer
// Reason: Path setting problem, need to reset the path.
std::ofstream ofs0("./plot/path0.dat");
ss.getSolutionPath().printAsMatrix(ofs0);
// print the path to screen
ss.simplifySolution();
ss.getSolutionPath().print(std::cout);
std::ofstream ofs("./plot/path.dat");
ss.getSolutionPath().printAsMatrix(ofs);
// ------------------------------------------------
}
else
std::cout << "No solution found" << std::endl;
}
int main(int /*argc*/, char ** /*argv*/)
{
std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
planWithSimpleSetup();
return 0;
}
遇到的问题:程序不能够输出规划得到的路径文本
原因:可能是路径设置的问题,本人在Qt上新建工作空间运行脚本,默认路径为当前工程路径
解决方案:可以通过更改路径,观察是否可行