main.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2008, Maxim Likhachev
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the University of Pennsylvania nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
#include <iostream>
using namespace std;

#include <sbpl/headers.h>
#include <sbpl_dynamic_planner/envInterval.h>
#include <sbpl_dynamic_planner/envDBubble.h>
#include <sbpl_dynamic_planner/envTime.h>
#include <sbpl_dynamic_planner/weightedAStar.h>
#include <sbpl_dynamic_planner/intervalPlanner.h>


void PrintUsage(char *argv[])
{
        printf("USAGE: %s <cfg file>\n", argv[0]);
}


int planxythetalat(int argc, char *argv[])
{

        int bRet = 0;
        double allocated_time_secs = 0.3; //in seconds
        bool bsearchuntilfirstsolution = true;

        //set the perimeter of the robot (it is given with 0,0,0 robot ref. point for which planning is done)
        vector<sbpl_2Dpt_t> perimeterptsV;
        sbpl_2Dpt_t pt_m;
        double halfwidth = 0.01; //0.3;
        double halflength = 0.01; //0.45;
        pt_m.x = -halflength;
        pt_m.y = -halfwidth;
        perimeterptsV.push_back(pt_m);
        pt_m.x = halflength;
        pt_m.y = -halfwidth;
        perimeterptsV.push_back(pt_m);
        pt_m.x = halflength;
        pt_m.y = halfwidth;
        perimeterptsV.push_back(pt_m);
        pt_m.x = -halflength;
        pt_m.y = halfwidth;
        perimeterptsV.push_back(pt_m);
        

        //clear perimeter
        //perimeterptsV.clear();
        //pt_m.x = 0.0;
        //pt_m.y = 0.0;
        //perimeterptsV.push_back(pt_m);

        //Initialize Environment (should be called before initializing anything else)
        EnvironmentNAVXYTHETALAT environment_navxythetalat;

        if(argc >= 3)
        {
                if(!environment_navxythetalat.InitializeEnv(argv[1], perimeterptsV, argv[2]))
                {
                        printf("ERROR: InitializeEnv failed\n");
                        exit(1);
                }
        }
        else
        {
                if(!environment_navxythetalat.InitializeEnv(argv[1], perimeterptsV, NULL))
                {
                        printf("ERROR: InitializeEnv failed\n");
                        exit(1);
                }
        }

        //plan a path
        vector<int> solution_stateIDs_V;
        bool bforwardsearch = true;
        ARAPlanner planner(&environment_navxythetalat, bforwardsearch);
        planner.set_initialsolution_eps(1.0);

        //set search mode
        planner.set_search_mode(bsearchuntilfirstsolution);

  if(planner.set_start(environment_navxythetalat.SetStart(0.0,0.0,0.6)) == 0)
  //if(planner.set_start(environment_navxythetalat.getStartID()) == 0)
  {
      printf("ERROR: failed to set start state\n");
      exit(1);
  }
  if(planner.set_goal(environment_navxythetalat.SetGoal(0.36,0.36,0.6)) == 0)
  //if(planner.set_goal(environment_navxythetalat.getGoalID()) == 0)
  {
      printf("ERROR: failed to set start state\n");
      exit(1);
  }


    printf("start planning...\n");
        bRet = planner.replan(allocated_time_secs, &solution_stateIDs_V);
    printf("done planning\n");
        std::cout << "size of solution=" << solution_stateIDs_V.size() << std::endl;

    environment_navxythetalat.PrintTimeStat(stdout);


    FILE* fSol = fopen("sol.txt", "w");
        vector<EnvNAVXYTHETALAT3Dpt_t> xythetaPath;
        environment_navxythetalat.ConvertStateIDPathintoXYThetaPath(&solution_stateIDs_V, &xythetaPath);
        printf("solution size=%d\n", (int)xythetaPath.size());
        for(unsigned int i = 0; i < xythetaPath.size(); i++) {
                fprintf(fSol, "%.3f %.3f %.3f\n", xythetaPath.at(i).x, xythetaPath.at(i).y, xythetaPath.at(i).theta);
        }
    fclose(fSol);

    environment_navxythetalat.PrintTimeStat(stdout);

        //print a path
        if(bRet)
        {
                //print the solution
                printf("Solution is found\n");
        }
        else
                printf("Solution does not exist\n");

        fflush(NULL);


    return bRet;
}


int planxythetatimelat(int argc, char *argv[])
{

        int bRet = 0;
        double allocated_time_secs = 0.3; //in seconds
        bool bsearchuntilfirstsolution = true;

        //set the perimeter of the robot (it is given with 0,0,0 robot ref. point for which planning is done)
        vector<sbpl_2Dpt_t> perimeterptsV;
        sbpl_2Dpt_t pt_m;
        double halfwidth = 0.01; //0.3;
        double halflength = 0.01; //0.45;
        pt_m.x = -halflength;
        pt_m.y = -halfwidth;
        perimeterptsV.push_back(pt_m);
        pt_m.x = halflength;
        pt_m.y = -halfwidth;
        perimeterptsV.push_back(pt_m);
        pt_m.x = halflength;
        pt_m.y = halfwidth;
        perimeterptsV.push_back(pt_m);
        pt_m.x = -halflength;
        pt_m.y = halfwidth;
        perimeterptsV.push_back(pt_m);
        

        //clear perimeter
        //perimeterptsV.clear();
        //pt_m.x = 0.0;
        //pt_m.y = 0.0;
        //perimeterptsV.push_back(pt_m);

        //Initialize Environment (should be called before initializing anything else)
        EnvironmentNAVXYTHETATIMELAT environment_navxythetatimelat;

        if(argc >= 4){
                if(!environment_navxythetatimelat.InitializeEnv(argv[2], perimeterptsV, argv[4], argv[5])){
                        printf("ERROR: InitializeEnv failed\n");
                        exit(1);
                }
        }
        else{
    printf("Please provide three arguments: environmentFile motionPrimitiveFile dynamicObstacleFile\n");
    exit(1);
        }

        //initialize planner
        vector<int> solution_stateIDs_V;
        bool bforwardsearch = true;
        WeightedAStar planner(&environment_navxythetatimelat, bforwardsearch);

        planner.set_initialsolution_eps(1.0);

        //set search mode
        planner.set_search_mode(bsearchuntilfirstsolution);

  //if(planner.set_start(environment_navxythetatimelat.SetStart(0.11,0.11,0,0)) == 0)
  //if(planner.set_start(environment_navxythetatimelat.SetStart(0.04,0.04,0,0)) == 0)
  if(planner.set_start(environment_navxythetatimelat.getStartID()) == 0)
  {
      printf("ERROR: failed to set start state\n");
      exit(1);
  }
  //if(planner.set_goal(environment_navxythetatimelat.SetGoal(0.2,0.04,0)) == 0)
  //if(planner.set_goal(environment_navxythetatimelat.SetGoal(0.35,0.3,0)) == 0)
  //if(planner.set_goal(environment_navxythetatimelat.SetGoal(0.135,0.135,0)) == 0)
  if(planner.set_goal(environment_navxythetatimelat.getGoalID()) == 0)
  {
      printf("ERROR: failed to set start state\n");
      exit(1);
  }


    printf("start planning...\n");
        bRet = planner.replan(allocated_time_secs, &solution_stateIDs_V);
    printf("done planning\n");
        std::cout << "size of solution=" << solution_stateIDs_V.size() << std::endl;

    environment_navxythetatimelat.PrintTimeStat(stdout);

    FILE* fSol = fopen("sol.txt", "w");
        vector<SBPL_4Dpt_t> xythetatimePath;
        environment_navxythetatimelat.ConvertStateIDPathintoXYThetaPath(&solution_stateIDs_V, &xythetatimePath);
        printf("solution size=%d\n", (int)xythetatimePath.size());
        for(unsigned int i = 0; i < xythetatimePath.size(); i++) {
                fprintf(fSol, "%.3f %.3f %.3f %.3f\n", xythetatimePath.at(i).x, xythetatimePath.at(i).y, xythetatimePath.at(i).theta, xythetatimePath.at(i).t);
        }
    fclose(fSol);

    environment_navxythetatimelat.PrintTimeStat(stdout);

        //print a path
        if(bRet)
        {
                //print the solution
                printf("Solution is found\n");
        }
        else
                printf("Solution does not exist\n");

        fflush(NULL);


    return bRet;
}

int planxythetatimebubblelat(int argc, char *argv[])
{

        int bRet = 0;
        double allocated_time_secs = 1200.0; //in seconds
        bool bsearchuntilfirstsolution = true;

        //set the perimeter of the robot (it is given with 0,0,0 robot ref. point for which planning is done)
        vector<sbpl_2Dpt_t> perimeterptsV;
        sbpl_2Dpt_t pt_m;
        double halfwidth = 0.01;//0.335; //0.3;
        double halflength = 0.01;//0.335; //0.45;
        pt_m.x = -halflength;
        pt_m.y = -halfwidth;
        perimeterptsV.push_back(pt_m);
        pt_m.x = halflength;
        pt_m.y = -halfwidth;
        perimeterptsV.push_back(pt_m);
        pt_m.x = halflength;
        pt_m.y = halfwidth;
        perimeterptsV.push_back(pt_m);
        pt_m.x = -halflength;
        pt_m.y = halfwidth;
        perimeterptsV.push_back(pt_m);
        

        //clear perimeter
        //perimeterptsV.clear();
        //pt_m.x = 0.0;
        //pt_m.y = 0.0;
        //perimeterptsV.push_back(pt_m);

        //Initialize Environment (should be called before initializing anything else)
        EnvDBubbleLat environment_navxythetatimebubblelat;

        /*if(argc >= 4){
                if(!environment_navxythetatimebubblelat.InitializeEnv(argv[2], perimeterptsV, argv[4], argv[5])){
                        printf("ERROR: InitializeEnv failed\n");
                        exit(1);
                }
        }*/
        if(argc >= 4){
                if(!environment_navxythetatimebubblelat.InitializeEnv(argv[1], perimeterptsV, argv[2], argv[3])){
                        printf("ERROR: InitializeEnv failed\n");
                        exit(1);
                }
        }
        else{
    printf("Please provide three arguments: environmentFile motionPrimitiveFile dynamicObstacleFile\n");
    exit(1);
        }

        //initialize planner
        vector<int> solution_stateIDs_V;
        bool bforwardsearch = true;
        WeightedAStar planner(&environment_navxythetatimebubblelat, bforwardsearch);

        planner.set_initialsolution_eps(1.0);

        //set search mode
        planner.set_search_mode(bsearchuntilfirstsolution);

  //if(planner.set_start(environment_navxythetatimelat.SetStart(0.11,0.11,0,0)) == 0)
  //if(planner.set_start(environment_navxythetatimelat.SetStart(0.04,0.04,0,0)) == 0)
  if(planner.set_start(environment_navxythetatimebubblelat.getStartID()) == 0)
  {
      printf("ERROR: failed to set start state\n");
      exit(1);
  }
  //if(planner.set_goal(environment_navxythetatimelat.SetGoal(0.2,0.04,0)) == 0)
  //if(planner.set_goal(environment_navxythetatimelat.SetGoal(0.35,0.3,0)) == 0)
  //if(planner.set_goal(environment_navxythetatimelat.SetGoal(0.135,0.135,0)) == 0)
  if(planner.set_goal(environment_navxythetatimebubblelat.getGoalID()) == 0)
  {
      printf("ERROR: failed to set start state\n");
      exit(1);
  }


    printf("start planning...\n");
        bRet = planner.replan(allocated_time_secs, &solution_stateIDs_V);
    printf("done planning\n");
        std::cout << "size of solution=" << solution_stateIDs_V.size() << std::endl;

    environment_navxythetatimebubblelat.PrintTimeStat(stdout);

    FILE* fSol = fopen("sol.txt", "w");
        vector<SBPL_4Dpt_t> xythetatimePath;
        environment_navxythetatimebubblelat.ConvertStateIDPathintoXYThetaPath(&solution_stateIDs_V, &xythetatimePath);
        printf("solution size=%d\n", (int)xythetatimePath.size());
        for(unsigned int i = 0; i < xythetatimePath.size(); i++) {
                fprintf(fSol, "%.3f %.3f %.3f %.3f\n", xythetatimePath.at(i).x, xythetatimePath.at(i).y, xythetatimePath.at(i).theta, xythetatimePath.at(i).t);
        }
    fclose(fSol);

    environment_navxythetatimebubblelat.PrintTimeStat(stdout);

        //print a path
        if(bRet)
        {
                //print the solution
                printf("Solution is found\n");
        }
        else
                printf("Solution does not exist\n");

        fflush(NULL);

    //environment_navxythetatimebubblelat.dumpStatesToFile();

    return bRet;
}


int planIntervallat(int argc, char *argv[], double eps)
{

        int bRet = 0;
        double allocated_time_secs = 1200.0; //in seconds
        bool bsearchuntilfirstsolution = true;

        //set the perimeter of the robot (it is given with 0,0,0 robot ref. point for which planning is done)
        vector<sbpl_2Dpt_t> perimeterptsV;
        sbpl_2Dpt_t pt_m;
        double halfwidth = 0.01; //0.335; 
        double halflength = 0.01; //0.335; 
        pt_m.x = -halflength;
        pt_m.y = -halfwidth;
        perimeterptsV.push_back(pt_m);
        pt_m.x = halflength;
        pt_m.y = -halfwidth;
        perimeterptsV.push_back(pt_m);
        pt_m.x = halflength;
        pt_m.y = halfwidth;
        perimeterptsV.push_back(pt_m);
        pt_m.x = -halflength;
        pt_m.y = halfwidth;
        perimeterptsV.push_back(pt_m);
        

        //clear perimeter
        //perimeterptsV.clear();
        //pt_m.x = 0.0;
        //pt_m.y = 0.0;
        //perimeterptsV.push_back(pt_m);

        //Initialize Environment (should be called before initializing anything else)
        EnvIntervalLat environment_navIntervallat;

        /*if(argc >= 4){
                if(!environment_navxythetatimebubblelat.InitializeEnv(argv[2], perimeterptsV, argv[4], argv[5])){
                        printf("ERROR: InitializeEnv failed\n");
                        exit(1);
                }
        }*/
        if(argc >= 4){
                if(!environment_navIntervallat.InitializeEnv(argv[1], perimeterptsV, argv[2], argv[3])){
                        printf("ERROR: InitializeEnv failed\n");
                        exit(1);
                }
        }
        else{
    printf("Please provide three arguments: environmentFile motionPrimitiveFile dynamicObstacleFile\n");
    exit(1);
        }

        //initialize planner
        vector<int> solution_stateIDs_V;
        bool bforwardsearch = true;
        IntervalPlanner planner(&environment_navIntervallat);

        planner.set_initialsolution_eps(eps);

        //set search mode
        planner.set_search_mode(bsearchuntilfirstsolution);

  //if(planner.set_start(environment_navxythetatimelat.SetStart(0.11,0.11,0,0)) == 0)
  //if(planner.set_start(environment_navxythetatimelat.SetStart(0.04,0.04,0,0)) == 0)
  if(planner.set_start(environment_navIntervallat.getStartID()) == 0)
  {
      printf("ERROR: failed to set start state\n");
      exit(1);
  }
  //if(planner.set_goal(environment_navxythetatimelat.SetGoal(0.2,0.04,0)) == 0)
  //if(planner.set_goal(environment_navxythetatimelat.SetGoal(0.35,0.3,0)) == 0)
  //if(planner.set_goal(environment_navxythetatimelat.SetGoal(0.135,0.135,0)) == 0)
  if(planner.set_goal(environment_navIntervallat.getGoalID()) == 0)
  {
      printf("ERROR: failed to set start state\n");
      exit(1);
  }


    printf("start planning...\n");
        bRet = planner.replan(allocated_time_secs, &solution_stateIDs_V);
    printf("done planning\n");
        std::cout << "size of solution=" << solution_stateIDs_V.size() << std::endl;

    environment_navIntervallat.PrintTimeStat(stdout);

    FILE* fSol = fopen("sol.txt", "w");
        vector<SBPL_4Dpt_t> xythetatimePath;
        environment_navIntervallat.ConvertStateIDPathintoXYThetaPath(&solution_stateIDs_V, &xythetatimePath);
        printf("solution size=%d\n", (int)xythetatimePath.size());
        for(unsigned int i = 0; i < xythetatimePath.size(); i++) {
                fprintf(fSol, "%.3f %.3f %.3f %.3f\n", xythetatimePath.at(i).x, xythetatimePath.at(i).y, xythetatimePath.at(i).theta, xythetatimePath.at(i).t);
        }
    fclose(fSol);

    environment_navIntervallat.PrintTimeStat(stdout);

        //print a path
        if(bRet)
        {
                //print the solution
                printf("Solution is found\n");
        }
        else
                printf("Solution does not exist\n");

        fflush(NULL);

    environment_navIntervallat.dumpStatesToFile();

    return bRet;
}


int main(int argc, char *argv[])
{

        if(argc < 2){
                PrintUsage(argv);
                exit(1);
        }

  //xytheta planning
  //planxythetalat(argc, argv);

  //xythetatime planning
  //planxythetatimelat(argc, argv);

  //xythetatimebubble planning
  //planxythetatimebubblelat(argc, argv);

  //interval planning
  //planIntervallat(argc, argv,1.2);
  planIntervallat(argc, argv,1.0);

        return 0;
}

---

sbpl_dynamic_planner
Author(s): Michael Phillips, Maxim Likhachev
autogenerated on Fri Jan 11 09:41:03 2013