00001 #include "continual_planning_executive/plan.h" 00002 #include <math.h> 00003 00004 Plan::Plan() 00005 { 00006 } 00007 00008 Plan::~Plan() 00009 { 00010 } 00011 00012 bool DurativeAction::operator<(const DurativeAction & a) const 00013 { 00014 if(name < a.name) 00015 return true; 00016 else if(name > a.name) 00017 return false; 00018 00019 if(duration < a.duration) 00020 return true; 00021 else if(duration > a.duration) 00022 return false; 00023 00024 if(startTime < a.startTime) 00025 return true; 00026 else if(startTime > a.startTime) 00027 return false; 00028 00029 if(parameters.size() < a.parameters.size()) 00030 return true; 00031 else if(parameters.size() > a.parameters.size()) 00032 return false; 00033 00034 // parameters.size == a.parameters.size 00035 for(unsigned int i = 0; i < parameters.size(); i++) { 00036 if(parameters[i] < a.parameters[i]) 00037 return true; 00038 else if(parameters[i] > a.parameters[i]) 00039 return false; 00040 } 00041 00042 // objects are equal, i.e. NOT a < b 00043 return false; 00044 } 00045 00046 bool DurativeAction::operator==(const DurativeAction & a) const 00047 { 00048 if(*this < a) 00049 return false; 00050 if(a < *this) 00051 return false; 00052 return true; 00053 } 00054 00055 DurativeAction::DurativeAction(const continual_planning_executive::TemporalAction & msg) 00056 { 00057 this->startTime = msg.start_time; 00058 this->duration = msg.duration; 00059 this->name = msg.name; 00060 this->parameters = msg.parameters; 00061 } 00062 00063 void Plan::removeAction(const DurativeAction & a) 00064 { 00065 double earliestTime = HUGE_VAL; 00066 vector<DurativeAction>::iterator it = actions.begin(); 00067 while(it != actions.end()) { 00068 if(*it == a) { 00069 it = actions.erase(it); 00070 } else { 00071 if(it->startTime < earliestTime) 00072 earliestTime = it->startTime; 00073 it++; 00074 } 00075 } 00076 00077 // shift all actions by the earliest time in the plan to make the first action to be at 0.0 again 00078 for(it = actions.begin(); it != actions.end(); it++) { 00079 it->startTime -= earliestTime; 00080 } 00081 } 00082 00083 std::ostream & operator<<(std::ostream & os, const DurativeAction & a) 00084 { 00085 os << a.startTime << ": (" << a.name; 00086 for(vector<string>::const_iterator it = a.parameters.begin(); it != a.parameters.end(); it++) { 00087 os << " " << *it; 00088 } 00089 os << ") [" << a.duration << "]"; 00090 return os; 00091 } 00092 00093 std::ostream & operator<<(std::ostream & os, const Plan & p) 00094 { 00095 for(vector<DurativeAction>::const_iterator it = p.actions.begin(); it != p.actions.end(); it++) { 00096 os << *it << std::endl; 00097 } 00098 return os; 00099 } 00100