Setting up an environment for X, Y, Theta state space

xytheta.cpp

void planxythetalat(char* envCfgFilename, char* motPrimFilename){
    // set the perimeter of the robot   
    vector<sbpl_2Dpt_t> perimeter;
    createFootprint(perimeter);

    // initialize an environment
    EnvironmentNAVXYTHETALAT env;
    initializeEnv(env, perimeter, envCfgFilename, motPrimFilename);

    // specify a start and goal state
    int start_id, goal_id;
    setEnvStartGoal(env, .11, .11, 0, 35, 47.5, 0, start_id, goal_id);

    // initialize a planner with start and goal state
    SBPLPlanner* planner = NULL;
    double initialEpsilon = 3.0;
    bool bsearchuntilfirstsolution = false;
    initializePlanner(planner, env, start_id, goal_id, initialEpsilon, 
                      bsearchuntilfirstsolution);

    // plan
    vector<int> solution_stateIDs;
    double allocated_time_secs = 10.0; // in seconds
    runPlanner(planner, allocated_time_secs, solution_stateIDs);

    // print stats
    env.PrintTimeStat(stdout);

    // write out solutions
    std::string filename("sol.txt");
    writeSolution(env, solution_stateIDs, filename.c_str());

    delete planner;
}