Get path from ompl::geometric::PathPtr in OMPL

edit

Hi all!

I work with OMPL without MoveIt and I have one simple question. I am not good in boost. I planned path for 6DOF industrial robot. How can I get points of path from ompl::geometric::PathPtr?

namespace ob = ompl::base;
namespace og = ompl::geometric;
namespace oc = ompl::control;  


bool isStateValid(const ob::State *state)
{
float a1 = state->as<ob::RealVectorStateSpace::StateType>()->values[0];
float a2 = state->as<ob::RealVectorStateSpace::StateType>()->values[1];
float a3 = state->as<ob::RealVectorStateSpace::StateType>()->values[2];
float a4 = state->as<ob::RealVectorStateSpace::StateType>()->values[3];
float a5 = state->as<ob::RealVectorStateSpace::StateType>()->values[4];
float a6 = state->as<ob::RealVectorStateSpace::StateType>()->values[5];

cout << "A1 " << a1 << endl;
cout << "A2 " << a2 << endl;
cout << "A3 " << a3 << endl;
cout << "A4 " << a4 << endl;
cout << "A5 " << a5 << endl;
cout << "A6 " << a6 << endl;

if ((abs(a1) > 0.91)||(abs(a2) > 0.91)||(abs(a3) > 0.91)||(abs(a4) > 0.91)||(abs(a5) > 0.91)||(abs(a6) > 0.91))
    return false;
if ((abs(a1) > 0.89)||(abs(a2) > 0.89)||(abs(a3) > 0.89)||(abs(a4) > 0.89)||(abs(a5) > 0.89)||(abs(a6) > 0.89))
    return true;
else
    return false;
}

bool plan_ompl(cut_data & work,rob_ang start_ang,rob_ang finish_ang,vector <rob_ang> & res)
{
// construct the state space we are planning in
ob::StateSpacePtr space(new ob::RealVectorStateSpace(6));

// set the bounds for the R^2 part of SE(2)
ob::RealVectorBounds bounds(6);
bounds.high.push_back(work.kuka.a1_max);
bounds.high.push_back(work.kuka.a2_max);
bounds.high.push_back(work.kuka.a3_max);
bounds.high.push_back(work.kuka.a4_max);
bounds.high.push_back(work.kuka.a5_max);
bounds.high.push_back(work.kuka.a6_max);

bounds.low.push_back(work.kuka.a1_min);
bounds.low.push_back(work.kuka.a2_min);
bounds.low.push_back(work.kuka.a3_min);
bounds.low.push_back(work.kuka.a4_min);
bounds.low.push_back(work.kuka.a5_min);
bounds.low.push_back(work.kuka.a6_min);

space->as<ob::RealVectorStateSpace>()->setBounds(bounds);

// construct an instance of  space information from this state space
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));

// set state validity checking for this space
si->setStateValidityChecker(boost::bind(&isStateValid, _1));

// create a random start state
ob::ScopedState<> start(space);
start.random();
start[0] = start_ang.a1;
start[1] = start_ang.a2;
start[2] = start_ang.a3;
start[3] = start_ang.a4;
start[4] = start_ang.a5;
start[5] = start_ang.a6;

// create a random goal state
ob::ScopedState<> goal(space);
goal.random();
goal[0] = finish_ang.a1;
goal[1] = finish_ang.a2;
goal[2] = finish_ang.a3;
goal[3] = finish_ang.a4;
goal[4] = finish_ang.a5;
goal[5] = finish_ang.a6;

// create a problem instance
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));

// set the start and goal states
pdef->setStartAndGoalStates(start, goal);

// create a planner for the defined space
ob::PlannerPtr planner(new og::RRTConnect(si));

// set the problem we are trying to solve for the planner
planner->setProblemDefinition(pdef);

// perform setup steps for the planner
planner->setup();

// print the settings for this space
si->printSettings(std::cout);

// print the problem settings
pdef->print(std::cout);

// attempt to solve the problem within one second of planning time
ob::PlannerStatus solved = planner- ...