ExecControl.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, LABUST, UNIZG-FER
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the LABUST nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  *  Author: Dula Nad
00035  *  Created: 30.10.2013.
00036  *********************************************************************/
00037 #include <labust/control/ExecControl.hpp>
00038 
00039 #include <boost/graph/graphviz.hpp>
00040 #include <boost/graph/dijkstra_shortest_paths.hpp>
00041 
00042 #include <std_msgs/String.h>
00043 
00044 #include <fstream>
00045 #include <iostream>
00046 
00047 using namespace labust::control;
00048 
00049 ExecControl::ExecControl():
00050                 tnum(0),
00051                 pnum(6),
00052                 marking(pnum)
00053 {
00054         this->onInit();
00055 }
00056 
00057 void ExecControl::onInit()
00058 {
00059         ros::NodeHandle nh,ph("~");
00060         depGraphPub = nh.advertise<std_msgs::String>("dependency_graph",1);
00061         pnGraphPub = nh.advertise<std_msgs::String>("petri_net_graph",1);
00062 
00063         registerController = nh.advertiseService("register_controller",
00064                         &ExecControl::onRegisterController, this);
00065 
00066         activateController = nh.subscribe<std_msgs::String>("activate_cnt",1,
00067                         &ExecControl::onActivateController, this);
00068 
00069         //Import main vertex names
00070         std::string dofs[]={"X","Y","Z","K","M","N"};
00071         //Add DOFs to the name list
00072         for (int i=X; i<=N;++i)
00073         {
00074                 boost::add_vertex(VertexProperty(dofs[i],VertexProperty::p),graph);
00075                 marking(i) = 1;
00076                 pname[i] = dofs[i];
00077         }
00078 }
00079 
00080 bool ExecControl::onRegisterController(
00081                 navcon_msgs::RegisterController::Request& req,
00082                 navcon_msgs::RegisterController::Response& resp)
00083 {
00084         //Check if controller with same name exists.
00085         if (controllers.find(req.name) != controllers.end())
00086         {
00087                 ROS_ERROR("The %s controller is already registered.", req.name.c_str());
00088                 resp.accepted = false;
00089                 return true;
00090         }
00091 
00092         //Check if other dependencies are satisfied
00093         for (int i=0; i<req.used_cnt.size(); ++i)
00094         {
00095                 if (controllers.find(req.used_cnt[i]) == controllers.end())
00096                 {
00097                         ROS_ERROR("The controller %s is missing dependency %s.",
00098                                         req.name.c_str(),
00099                                         req.used_cnt[i].c_str());
00100                         resp.unmet_cnt.push_back(req.used_cnt[i]);
00101                 }
00102         }
00103 
00104         if (resp.unmet_cnt.size())
00105         {
00106                 resp.accepted = false;
00107                 return true;
00108         }
00109 
00110         //Add the controller
00111         ROS_INFO("Adding controller %s.",req.name.c_str());
00112         resp.accepted = true;
00113         controllers[req.name].info = req;
00114         depGraph.addToGraph(req);
00115         pnGraph.addToGraph(req);
00116         pnCon.addToGraph(req);
00117         pnCon.reachability();
00118         //addToMatrix(req.name);
00119         names.push_back(req.name);
00120 
00121         std_msgs::String out;
00122         std::fstream dep_file("dep_graph.dot",std::ios::out);
00123         std::fstream pn_file("pn_graph.dot",std::ios::out);
00124         std::fstream r_file("r_graph.dot",std::ios::out);
00125         std::string temp;
00126         depGraph.getDotDesc(temp);
00127         dep_file<<temp;
00128         out.data = temp;
00129         depGraphPub.publish(out);
00130         pnGraph.getDotDesc(temp);
00131         pn_file<<temp;
00132         out.data = temp;
00133         pnGraphPub.publish(out);
00134         pnCon.getDotDesc(temp);
00135         r_file<<temp;
00136 
00137         return true;
00138 }
00139 
00140 void ExecControl::addToMatrix(const std::string& name)
00141 {
00142         //Build from scratch
00143         using namespace boost;
00144         ControllerInfo& newcon = controllers[name];
00145         newcon.place_num = pnum++;
00146         pname[newcon.place_num] = name;
00147         marking.conservativeResize(pnum);
00148         marking(newcon.place_num) = 0;
00149         newcon.en_t_num = tnum++;
00150         newcon.dis_t_num = tnum++;
00151         std::cout<<"Resize matrix 2."<<std::endl;
00152         Dm.conservativeResize(pnum, tnum);
00153         Dp.conservativeResize(pnum, tnum);
00154         Dm.row(newcon.place_num) = Eigen::VectorXi::Zero(tnum);
00155         Dp.row(newcon.place_num) = Eigen::VectorXi::Zero(tnum);
00156         Dm.col(newcon.en_t_num)= Eigen::VectorXi::Zero(pnum);
00157         Dp.col(newcon.en_t_num)= Eigen::VectorXi::Zero(pnum);
00158         Dm.col(newcon.dis_t_num)= Eigen::VectorXi::Zero(pnum);
00159         Dp.col(newcon.dis_t_num)= Eigen::VectorXi::Zero(pnum);
00160 
00161         //Add local connection
00162         Dm(newcon.place_num, newcon.dis_t_num) = 1;
00163         Dp(newcon.place_num, newcon.en_t_num) = 1;
00164         //Add basic dependencies.
00165         for (int i=0; i<newcon.info.used_dofs.size(); ++i)
00166         {
00167                 if (newcon.info.used_dofs[i])
00168                 {
00169                         Dm(i, newcon.en_t_num) = 1;
00170                         Dp(i, newcon.dis_t_num) = 1;
00171                 }
00172         }
00173 
00174         //Add basic dependencies.
00175         for (int i=0; i<newcon.info.used_cnt.size(); ++i)
00176         {
00177                 const std::string& dep = newcon.info.used_cnt[i];
00178                 ControllerMap::iterator it = controllers.end();
00179                 if ((it = controllers.find(dep)) != controllers.end())
00180                 {
00181                         //ROS_INFO("Add edge %d -> %d.",it->second.graph_idx,newcon.graph_idx);
00182                         //boost::add_edge(newcon.graph_idx, it->second.graph_idx, graph);
00183                         Dm(it->second.place_num, newcon.en_t_num) = 1;
00184                         Dp(it->second.place_num, newcon.dis_t_num) = 1;
00185                 }
00186         }
00187 
00188         std::cout<<"Current marking:"<<marking<<std::endl;
00189         std::cout<<"Current Dm matrix:"<<Dm<<std::endl;
00190         std::cout<<"Current Dp matrix:"<<Dp<<std::endl;
00191         std::cout<<"Current I matrix:"<<Dp-Dm<<std::endl;
00192 
00193         reachability();
00194 }
00195 
00196 void ExecControl::onActivateController(const std_msgs::String::ConstPtr& name)
00197 {
00198         if (controllers.find(name->data) != controllers.end())
00199         {
00200                 //firing_seq.clear();
00201                 //this->get_firing2(name->data);
00202                 //pnCon.get_firing(name->data);
00203                 pnCon.get_firing_r(name->data);
00204         }
00205         else
00206         {
00207                 //A testing example, we can try setting a PN place directly.
00208                 std::string dofs[]={"X","Y","Z","K","M","N"};
00209                 //Add DOFs to the name list
00210                 for (int i=X; i<=N;++i)
00211                 {
00212                         if (name->data == dofs[i])
00213                         {
00214                                 pnCon.get_firing_r(name->data);
00215                                 break;
00216                         }
00217                 }
00218         }
00219 }
00220 
00221 bool ExecControl::firing_rec2(int des_place, std::vector<int>& skip_transitions, std::vector<int>& visited_places)
00222 {
00223         visited_places.push_back(des_place);
00224 
00225         //Find all possible transition activators for this place
00226         Eigen::VectorXi transitions = Dp.row(des_place);
00227         std::vector<int> activators;
00228         for (int i=0; i<transitions.size(); ++i)
00229         {
00230                 if (transitions(i))
00231                 {
00232                         bool skipped = false;
00233                         //Filter skipped transitions
00234                         for (int j=0; j< skip_transitions.size(); ++j)
00235                         {
00236                                 if ((skipped = (skip_transitions[j] == i))) break;
00237                         }
00238 
00239                         if (!skipped) activators.push_back(i);
00240                 }
00241         }
00242 
00243         std::cout<<"Place "<<pname[des_place]<<" depends on transitions:";
00244         for (int i=0; i<activators.size(); ++i)
00245         {
00246                 std::cout<<activators[i]<<", ";
00247         }
00248         std::cout<<std::endl;
00249 
00250         //If no activators this is a dead-end.
00251         if (activators.empty())
00252         {
00253                 std::cout<<"Dead-end."<<std::endl;
00254                 return false;
00255         }
00256 
00257         //For each activator find places
00258         bool add_seq = false;
00259         for (int i=0; i<activators.size(); ++i)
00260         {
00261                 Eigen::VectorXi t_en = Dm.col(activators[i]);
00262                 std::vector<int> places;
00263                 for (int j=0; j<t_en.size(); ++j)
00264                 {
00265                         if (t_en(j))
00266                         {
00267 
00268                                 bool skipped = false;
00269                                 //Filter skipped transitions
00270                                 for (int k=0; k< visited_places.size(); ++k)
00271                                 {
00272                                         if ((skipped = (visited_places[k] == j))) break;
00273                                 }
00274 
00275                                 if (!skipped) places.push_back(j);
00276                         }
00277                 }
00278 
00279                 std::cout<<"Transition "<<activators[i]<<" depends on places:";
00280                 for (int j=0; j<places.size(); ++j)
00281                 {
00282                         std::cout<<pname[places[j]]<<", ";
00283                 }
00284                 std::cout<<std::endl;
00285 
00286                 bool add_cur_seq=true;
00287                 for (int j=0; j<places.size(); ++j)
00288                 {
00289                         //If place is active add
00290                         if (!marking(places[j]))
00291                         {
00292                                 if (!firing_rec2(places[j], skip_transitions, visited_places))
00293                                 {
00294                                         add_cur_seq = false;
00295                                         break;
00296                                 }
00297                         }
00298                 }
00299 
00300                 if (add_cur_seq && !places.empty())
00301                 {
00302                         bool add = true;
00303                         for (int j=0; j<firing_seq.size(); ++j)
00304                         {
00305                                 if (firing_seq[j] == activators[i])
00306                                 {
00307                                         add = false;
00308                                         break;
00309                                 }
00310                         }
00311                         if (add)
00312                         {
00313                                 std::cout<<"Adding to firing sequence:"<<activators[i]<<std::endl;
00314                                 firing_seq.push_back(activators[i]);
00315                                 add_seq = true;
00316                                 if (activators[i]%2 == 0)
00317                                 {
00318                                         skip_transitions.push_back(activators[i]+1);
00319                                 }
00320                                 else
00321                                 {
00322                                         skip_transitions.push_back(activators[i]-1);
00323                                 }
00324                         }
00325                 }
00326         }
00327         return add_seq;
00328 }
00329 
00330 bool ExecControl::firing_rec(int des_place, std::vector<int>& skip_transitions, std::vector<int>& visited_places){}
00331 
00332 void ExecControl::get_firing(const std::string& name)
00333 {}
00334 
00335 void ExecControl::get_firing2(const std::string& name)
00336 {
00337         std::vector<int> skip_transitions;
00338         std::vector<int> visited_places;
00339 
00340         //Desired place to activate
00341         int des_place = controllers[name].place_num;
00342         firing_rec2(des_place, skip_transitions, visited_places);
00343 
00344         std::cout<<"The firing sequence is:";
00345         for (int i=0; i<firing_seq.size(); ++i)
00346         {
00347                 std::cout<<firing_seq[i]<<" ,";
00348         }
00349         std::cout<<std::endl;
00350 
00351         //Do the firing
00352         //Add testing "simulation" of the firing before applying.
00353         //Add enable/disable service calls for these.
00354         for(int i=0; i<firing_seq.size(); ++i)
00355         {
00356                 Eigen::VectorXi fire = Eigen::VectorXi::Zero(Dm.cols());
00357                 fire(firing_seq[i]) = 1;
00358                 marking = marking + (Dp-Dm)*fire;
00359                 std::cout<<"Transition "<<firing_seq[i]<<" fired, new marking:"<<marking<<std::endl;
00360                 if (marking(des_place)) break;
00361         }
00362 }
00363 
00364 
00365 void ExecControl::reachability()
00366 {
00367         rgraph.clear();
00368         Eigen::VectorXi trans = marking.transpose()*Dm;
00369 
00370         std::cout<<"Transition matrix:"<<trans<<std::endl;
00371 
00372         int last_m = boost::add_vertex(rgraph);
00373         rgraph[last_m].marking = marking;
00374 
00375         Eigen::VectorXi fire = Eigen::VectorXi::Zero(trans.size());
00376         int tnum = -1;
00377         for (int i=0; i<trans.size(); ++i)
00378         {
00379                 if (trans(i)>0)
00380                 {
00381                         std::cout<<"Transition "<<i<<" can fire."<<std::endl;
00382                         fire(i) = 1;
00383                         tnum = i;
00384                         break;
00385                 }
00386         }
00387 
00388         //marking = marking + (Dp-Dm)*fire;
00389 
00390         int new_m = boost::add_vertex(rgraph);
00391         rgraph[new_m].marking = marking;
00392         REdgeProperty prop;
00393         prop.t = tnum;
00394         prop.weight = 1;
00395         //Enable edge
00396         boost::add_edge(last_m, new_m, prop, rgraph);
00397         prop.t+=1;
00398         //Disable edge added so we can skip it in next iteration.
00399         boost::add_edge(new_m, last_m, prop, rgraph);
00400 
00401 
00402         std::cout<<"Transition fired, new marking:"<<marking<<std::endl;
00403 }
00404 
00405 int main(int argc, char* argv[])
00406 {
00407         ros::init(argc,argv,"exec_control");
00408         labust::control::ExecControl exec;
00409         ros::spin();
00410         return 0;
00411 }


labust_execution
Author(s): Gyula Nagy
autogenerated on Fri Feb 7 2014 11:36:24