Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00027
00028
00029
00030
00031 #include "pr2_controller_manager/scheduler.h"
00032
00033 using namespace std;
00034
00035 typedef map<string, list<string> > schedGraph;
00036
00037
00038
00039
00040 bool getNextController(string& c, schedGraph& graph)
00041 {
00042 schedGraph::iterator it, it2;
00043 for (it = graph.begin(); it != graph.end(); it++){
00044
00045 if (it->second.empty()){
00046 c = it->first;
00047
00048 graph.erase(it);
00049
00050 for (it2 = graph.begin(); it2 != graph.end(); it2++){
00051 list<string>::iterator l=it2->second.begin();
00052 while(l!=it2->second.end()){
00053 if ((*l) == c){
00054 l = it2->second.erase(l);
00055 }
00056 else l++;
00057 }
00058 }
00059 return true;
00060 }
00061 }
00062 return false;
00063 }
00064
00065
00066
00067 bool scheduleControllers(const vector<ControllerSpec>& c, vector<size_t>& schedule)
00068 {
00069 schedGraph graph;
00070 schedGraph::iterator it;
00071
00072 schedule.resize(c.size());
00073
00074
00075 for (size_t i=0; i<c.size(); i++){
00076 graph[c[i].name];
00077 for (size_t b=0; b<c[i].c->before_list_.size(); b++)
00078 graph[c[i].name].push_back(c[i].c->before_list_[b]);
00079 for (size_t a=0; a<c[i].c->after_list_.size(); a++){
00080 it = graph.find(c[i].c->after_list_[a]);
00081 if (it == graph.end()) return false;
00082 it->second.push_back(c[i].name);
00083 }
00084 }
00085
00086
00087 string name;
00088 size_t nr=0;
00089 while (!graph.empty()){
00090 if (!getNextController(name, graph)) return false;
00091
00092
00093 for (size_t i=0; i<c.size(); i++)
00094 if (c[i].name == name)
00095 schedule[nr] = i;
00096 nr++;
00097 }
00098
00099
00100 string schedule_list;
00101 for (size_t i=0; i<schedule.size(); i++)
00102 schedule_list += c[schedule[i]].name + ", ";
00103 ROS_DEBUG("Controller schedule: %s", schedule_list.c_str());
00104
00105 return true;
00106 }
00107