scheduler.cpp
Go to the documentation of this file.
1 // Copyright (C) 2008, Willow Garage, Inc.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of Willow Garage, Inc. nor the names of its
12 // contributors may be used to endorse or promote products derived from
13 // this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 // POSSIBILITY OF SUCH DAMAGE.
27 /*
28  * Author: Wim Meeussen
29  */
30 
32 
33 using namespace std;
34 
35 typedef map<string, list<string> > schedGraph;
36 
37 
38 
39 
40 bool getNextController(string& c, schedGraph& graph)
41 {
42  schedGraph::iterator it, it2;
43  for (it = graph.begin(); it != graph.end(); it++){
44  // found next controller to schedule
45  if (it->second.empty()){
46  c = it->first;
47  // remove this controller form graph
48  graph.erase(it);
49  // remove this controller form before lists
50  for (it2 = graph.begin(); it2 != graph.end(); it2++){
51  list<string>::iterator l=it2->second.begin();
52  while(l!=it2->second.end()){
53  if ((*l) == c){
54  l = it2->second.erase(l);
55  }
56  else l++;
57  }
58  }
59  return true;
60  }
61  }
62  return false;
63 }
64 
65 
66 
67 bool scheduleControllers(const vector<ControllerSpec>& c, vector<size_t>& schedule)
68 {
69  schedGraph graph;
70  schedGraph::iterator it;
71 
72  schedule.resize(c.size());
73 
74  // build graph
75  for (size_t i=0; i<c.size(); i++){
76  graph[c[i].name];
77  for (size_t b=0; b<c[i].c->before_list_.size(); b++)
78  graph[c[i].name].push_back(c[i].c->before_list_[b]);
79  for (size_t a=0; a<c[i].c->after_list_.size(); a++){
80  it = graph.find(c[i].c->after_list_[a]);
81  if (it == graph.end()) return false;
82  it->second.push_back(c[i].name);
83  }
84  }
85 
86  // do the scheduling
87  string name;
88  size_t nr=0;
89  while (!graph.empty()){
90  if (!getNextController(name, graph)) return false;
91 
92  // find controller id
93  for (size_t i=0; i<c.size(); i++)
94  if (c[i].name == name)
95  schedule[nr] = i;
96  nr++;
97  }
98 
99  // show result
100  string schedule_list;
101  for (size_t i=0; i<schedule.size(); i++)
102  schedule_list += c[schedule[i]].name + ", ";
103  ROS_DEBUG("Controller schedule: %s", schedule_list.c_str());
104 
105  return true;
106 }
107 
bool getNextController(string &c, schedGraph &graph)
Definition: scheduler.cpp:40
bool scheduleControllers(const vector< ControllerSpec > &c, vector< size_t > &schedule)
Definition: scheduler.cpp:67
map< string, list< string > > schedGraph
Definition: scheduler.cpp:35
#define ROS_DEBUG(...)


pr2_controller_manager
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Jun 10 2019 14:19:12