void RRTstar_planning(){
ROS_INFO("RRT star planning");
ob::StateSpacePtr space(new ob::RealVectorStateSpace(2));
space->as<ob::RealVectorStateSpace>()->setBounds(minX(local_map->info), maxX(local_map->info));
space->setLongestValidSegmentFraction(0.01/(maxX(local_map->info)-minX(local_map->info)));
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
si->setup();
ob::ScopedState<> start(space);
start->as<ob::RealVectorStateSpace::StateType>()->values[0] = startState[0];
start->as<ob::RealVectorStateSpace::StateType>()->values[1] = startState[1];
ob::ScopedState<> goal(space);
goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = goalState[0];
goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = goalState[1];
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
pdef->setStartAndGoalStates(start, goal);
pdef->setOptimizationObjective(ob::OptimizationObjectivePtr(new ClearanceObjective(si)));
og::RRTstar *plan_pt=new og::RRTstar(si);
plan_pt->setGoalBias(0.05);
plan_pt->setRange(1.0);
ob::PlannerPtr optimizingPlanner(plan_pt);
optimizingPlanner->setProblemDefinition(pdef);
optimizingPlanner->setup();
ob::PlannerStatus solved;
int it=0;
while(solved!=ompl::base::PlannerStatus::StatusType::EXACT_SOLUTION && it<100){
it++;
solved = optimizingPlanner->solve(1.0);
}
if(solved==ompl::base::PlannerStatus::StatusType::EXACT_SOLUTION){
std::vector< ob::State * > sol = boost::static_pointer_cast<og::PathGeometric>(pdef->getSolutionPath())->getStates();
current_path_raw.resize(sol.size());
std::transform(sol.begin(), sol.end(), current_path_raw.begin(), &obstateState);
/*std::cout << "Path length: " << pdef->getSolutionPath()->length() << std::endl;
std::cout << "Path cost: " << pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()) << std::endl;
std::cout << "Path :" << std::endl;
for(std::deque<State<2>>::iterator it=current_path_raw.begin(),end=current_path_raw.end();it!=end;++it){
std::cout << (*it) << " -- ";
}
std::cout << std::endl;*/
planned=true;
}else{
ROS_INFO("Planning failed");
planned=false;
}
}
void plan(int argc, char** argv)
{
// Construct the robot state space in which we're planning. We're
// planning in [0,1]x[0,1], a subset of R^2.
ob::StateSpacePtr space(new ob::RealVectorStateSpace(2));
// Set the bounds of space to be in [0,1].
space->as<ob::RealVectorStateSpace>()->setBounds(0.0, 1.0);
// Construct a space information instance for this state space
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
// Set the object used to check which states in the space are valid
si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
si->setup();
// Set our robot's starting state to be the bottom-left corner of
// the environment, or (0,0).
ob::ScopedState<> start(space);
start->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
start->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.0;
// Set our robot's goal state to be the top-right corner of the
// environment, or (1,1).
ob::ScopedState<> goal(space);
goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = 1.0;
goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = 1.0;
// Create a problem instance
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
// Set the start and goal states
pdef->setStartAndGoalStates(start, goal);
// Since we want to find an optimal plan, we need to define what
// is optimal with an OptimizationObjective structure. Un-comment
// exactly one of the following 6 lines to see some examples of
// optimization objectives.
pdef->setOptimizationObjective(getPathLengthObjective(si));
// pdef->setOptimizationObjective(getThresholdPathLengthObj(si));
// pdef->setOptimizationObjective(getClearanceObjective(si));
// pdef->setOptimizationObjective(getBalancedObjective1(si));
// pdef->setOptimizationObjective(getBalancedObjective2(si));
// pdef->setOptimizationObjective(getPathLengthObjWithCostToGo(si));
// Construct our optimal planner using the RRTstarAR algorithm.
ob::PlannerPtr optimizingPlanner(new og::RRTstarAR(si));
// Set the problem instance for our planner to solve
optimizingPlanner->setProblemDefinition(pdef);
optimizingPlanner->setup();
// attempt to solve the planning problem within one second of
// planning time
ob::PlannerStatus solved = optimizingPlanner->solve(1.0);
if (solved)
{
// Output the length of the path found
std::cout
<< "Found solution of path length "
<< pdef->getSolutionPath()->length()
<< " with an optimization objective value of "
<< pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()) << std::endl;
// If a filename was specified, output the path as a matrix to
// that file for visualization
if (argc > 1)
{
std::ofstream outFile(argv[1]);
boost::static_pointer_cast<og::PathGeometric>(pdef->getSolutionPath())->
printAsMatrix(outFile);
outFile.close();
}
}
else
std::cout << "No solution found." << std::endl;
}
