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;
}