ppcpplanner.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Maxim Likhachev
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the University of Pennsylvania nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 #include <iostream>
00030 using namespace std;
00031 
00032 #include "../../sbpl/headers.h"
00033 
00034 
00035 //-----------------------------------------------------------------------------------------------------
00036 
00037 PPCPPlanner::PPCPPlanner(DiscreteSpaceInformation* environment, int sizeofS, int sizeofH)
00038 {
00039 
00040     environment_ = environment;
00041     
00042     
00043 #ifndef ROS
00044     const char* debug = "debug.txt";
00045 #endif
00046     fDeb = SBPL_FOPEN(debug, "w");
00047     if(fDeb == NULL){
00048       SBPL_ERROR("ERROR: could not open planner debug file\n");
00049       throw new SBPL_Exception();
00050     }
00051     
00052     pStateSpace = new PPCPStateSpace_t;
00053 
00054     //TODO - create and initialize the state-space
00055 
00056 }
00057 
00058 PPCPPlanner::~PPCPPlanner()
00059 {
00060   if(pStateSpace != NULL){
00061     //delete the statespace
00062     DeleteStateSpace(pStateSpace);
00063     delete pStateSpace;
00064         pStateSpace = NULL;
00065   }
00066   SBPL_FCLOSE(fDeb);
00067 }
00068 
00069 
00070 //deallocates memory used by StateSpace
00071 void PPCPPlanner::DeleteStateSpace(PPCPStateSpace_t* pStateSpace)
00072 {
00073         //TODO - fill in
00074 
00075 
00076 
00077 }
00078 
00079 //creates (allocates memory) search state space
00080 //does not initialize search statespace
00081 int PPCPPlanner::CreateSearchStateSpace(PPCPStateSpace_t* pStateSpace)
00082 {
00083 
00084         //create a heap
00085         pStateSpace->bReinitializeSearchStateSpace = true;
00086         pStateSpace->currentpolicyconfidence = 0;
00087         pStateSpace->GoalState = NULL;
00088         pStateSpace->StartState = NULL;
00089         pStateSpace->iteration = 0;
00090         pStateSpace->searchiteration = 0;
00091         
00092 
00093         return 1;
00094 }
00095 
00096 
00097 
00098 //--------------------------------------------------------------------------------------------------
00099 
00100 
00101 //-------------------------------------------------------------------------------------------------
00102 //setting start state in S
00103 int PPCPPlanner::set_goal(int goal_stateID)
00104 {
00105         //TODO
00106 
00107 
00108         return 1;
00109 
00110 }
00111 
00112 //setting goal state in S
00113 int PPCPPlanner::set_start(int start_stateID)
00114 {
00115 
00116         //TODO
00117 
00118 
00119         return 1;
00120 }
00121 
00122 
00123 
00124 void PPCPPlanner::costs_changed(StateChangeQuery const & stateChange)
00125 {
00126         SBPL_PRINTF("planner: costs_changed, state-space reset\n");
00127 
00128     pStateSpace->bReinitializeSearchStateSpace = true;
00129 
00130 
00131 }
00132 
00133 void PPCPPlanner::costs_changed()
00134 {
00135         SBPL_PRINTF("planner: costs_changed, state-space reset\n");
00136 
00137     pStateSpace->bReinitializeSearchStateSpace = true;
00138 
00139 }
00140 
00141 
00142 
00143 int PPCPPlanner::force_planning_from_scratch()
00144 {
00145         SBPL_PRINTF("planner: forceplanfromscratch set, state-space reset\n");
00146 
00147     pStateSpace->bReinitializeSearchStateSpace = true;
00148 
00149     return 1;
00150 }
00151 
00152 
00153 int PPCPPlanner::replan(double allocated_time_secs, vector<sbpl_PolicyStatewithBinaryh_t>* SolutionPolicy, float* ExpectedCost, float* ProbofReachGoal)
00154 {
00155         //TODO
00156 
00157 
00158         return 0;
00159 }
00160 
00161 
00162 //-------------------------------------------------------------------------------------------------
00163 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


sbpl
Author(s): Maxim Likhachev/maximl@seas.upenn.edu
autogenerated on Fri Jan 18 2013 13:41:53