OMPL学习（一）

 

#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上新建工作空间运行脚本，默认路径为当前工程路径

解决方案：可以通过更改路径，观察是否可行

To be continued ...