Hello_World.cpp

Go to the documentation of this file.

#include <iostream>

#include "../OMPL_Planner/OMPL_Planner.hpp"

int main(int argc, char **argv)
{
    int planningDimension = 2, solvingTime = 5;
    std::string plannerName, mapFile;
    std::vector<double> start, goal, planning_bounds_x_, planning_bounds_y_, planning_bounds_z_;

    planning_bounds_x_.resize(2);
    planning_bounds_y_.resize(2);
    planning_bounds_z_.resize(2);

    cout<<"Path planning with OMPL version "<<OMPL_VERSION<<"\n";
    cout<<"Choose a dimension for planning 2D or 3D (default :"<<planningDimension<<") : ";
    cin >> planningDimension;

    start.resize(planningDimension);
    goal.resize(planningDimension);

    cout<<"Choose a planner (available planner : CForest, RRTstar) : ";
    cin >> plannerName;

    OMPL_Planner planner(planningDimension);

    planning_bounds_x_[0]=-15;
    planning_bounds_x_[1]=15;
    planning_bounds_y_[0]=-15;
    planning_bounds_y_[1]=15;
    planning_bounds_z_[0]=-15;
    planning_bounds_z_[1]=15;

    if (planningDimension == 3)
        planner.setPlanningBounds(planning_bounds_x_, planning_bounds_y_, planning_bounds_z_);
    else 
        planner.setPlanningBounds(planning_bounds_x_, planning_bounds_y_);

    planner.setPlanningDepth();

    planner.setPlanner(plannerName);

    cout<<"Enter direction to the Octomap file (.bt file usually) : ";
    cin>> mapFile;
    planner.setStateChecker(mapFile,0.5,0.5,0.5);

    cout<<"\n Planner initialization done !\n";

    start[2]=0;
    goal[2]=0;

    cout<<"Choose a start position (expecting "<< planningDimension <<" values) : ";
    if(planningDimension==2)
        cin>> start[0] >> start[1];
    else
        cin>> start[0] >> start[1] >> start[2];
    
    cout<<"Choose a goal position (expecting "<< planningDimension <<" values) : ";
    if(planningDimension==2)
        cin>> goal[0] >> goal[1];
    else
        cin>> goal[0] >> goal[1] >> goal[2];

    planner.setStartGoal(start,goal);

    cout<<"Choose solving time (default :"<<solvingTime<<") : ";
    cin>> solvingTime;

    //cout<<"Searching path from "<<start[0]<<" / "<<start[1]<<" / "<<start[2]<<" to "<<goal[0]<<" / "<<goal[1]<<" / "<<goal[2]<<"\n";

    if(planner.findPath(solvingTime))
    {
        og::PathGeometric path = planner.getPath();
        path.print(cout);
    }
    else
        cout<<"\n No path found\n";

    cout<<"Thanks for using this awsome Hello World !\n";

    return 0;
}