scheduler.cpp
Go to the documentation of this file.
00001 
00002 // Copyright (C) 2008, Willow Garage, Inc.
00003 //
00004 // Redistribution and use in source and binary forms, with or without
00005 // modification, are permitted provided that the following conditions are met:
00006 //   * Redistributions of source code must retain the above copyright notice,
00007 //     this list of conditions and the following disclaimer.
00008 //   * Redistributions in binary form must reproduce the above copyright
00009 //     notice, this list of conditions and the following disclaimer in the
00010 //     documentation and/or other materials provided with the distribution.
00011 //   * Neither the name of Willow Garage, Inc. nor the names of its
00012 //     contributors may be used to endorse or promote products derived from
00013 //     this software without specific prior written permission.
00014 //
00015 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025 // POSSIBILITY OF SUCH DAMAGE.
00027 /*
00028  * Author: Wim Meeussen
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     // found next controller to schedule
00045     if (it->second.empty()){
00046       c = it->first;
00047       // remove this controller form graph 
00048       graph.erase(it);
00049       // remove this controller form before lists
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   // build graph
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   // do the scheduling
00087   string name;
00088   size_t nr=0;
00089   while (!graph.empty()){
00090     if (!getNextController(name, graph)) return false;
00091 
00092     // find controller id
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   // show result
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 


pr2_controller_manager
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Dec 2 2013 13:13:13