Orocos Real-Time Toolkit  2.6.0
FunctionGraph.cpp
00001 /***************************************************************************
00002   tag: Peter Soetens  Tue Dec 21 22:43:07 CET 2004  FunctionGraph.cxx
00003 
00004                         FunctionGraph.cxx -  description
00005                            -------------------
00006     begin                : Tue December 21 2004
00007     copyright            : (C) 2004 Peter Soetens
00008     email                : peter.soetens@mech.kuleuven.ac.be
00009 
00010  ***************************************************************************
00011  *   This library is free software; you can redistribute it and/or         *
00012  *   modify it under the terms of the GNU General Public                   *
00013  *   License as published by the Free Software Foundation;                 *
00014  *   version 2 of the License.                                             *
00015  *                                                                         *
00016  *   As a special exception, you may use this file as part of a free       *
00017  *   software library without restriction.  Specifically, if other files   *
00018  *   instantiate templates or use macros or inline functions from this     *
00019  *   file, or you compile this file and link it with other files to        *
00020  *   produce an executable, this file does not by itself cause the         *
00021  *   resulting executable to be covered by the GNU General Public          *
00022  *   License.  This exception does not however invalidate any other        *
00023  *   reasons why the executable file might be covered by the GNU General   *
00024  *   Public License.                                                       *
00025  *                                                                         *
00026  *   This library is distributed in the hope that it will be useful,       *
00027  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00028  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00029  *   Lesser General Public License for more details.                       *
00030  *                                                                         *
00031  *   You should have received a copy of the GNU General Public             *
00032  *   License along with this library; if not, write to the Free Software   *
00033  *   Foundation, Inc., 59 Temple Place,                                    *
00034  *   Suite 330, Boston, MA  02111-1307  USA                                *
00035  *                                                                         *
00036  ***************************************************************************/
00037 
00038 
00039 #include "FunctionGraph.hpp"
00040 #include "GraphCopier.hpp"
00041 #include "../base/AttributeBase.hpp"
00042 #include "ProgramService.hpp"
00043 #include "TaskContext.hpp"
00044 #include "../Service.hpp"
00045 
00046 #include "CommandNOP.hpp"
00047 #include "ConditionFalse.hpp"
00048 #include "ConditionTrue.hpp"
00049 #include <boost/graph/copy.hpp>
00050 #include <utility>
00051 
00052 namespace RTT {
00053     using namespace detail;
00054     using namespace boost;
00055     using namespace std;
00056 
00057 
00058 
00059     FunctionGraph::FunctionGraph(const std::string& _name, bool unload_on_stop)
00060         : myName(_name), retn(0), pausing(false), mstep(false), munload_on_stop(unload_on_stop)
00061     {
00062         // the start vertex of our function graph
00063         startv = add_vertex( program );
00064         put(vertex_exec, program, startv, VertexNode::normal_node );
00065         exitv = add_vertex( program );
00066         put(vertex_exec, program, exitv, VertexNode::normal_node);
00067     }
00068 
00069     FunctionGraph::FunctionGraph( const FunctionGraph& orig )
00070         :  program( orig.getGraph() ), myName( orig.getName() )
00071     {
00072         // The nodes are copied, which causes a clone of their contents.
00073         graph_traits<Graph>::vertex_iterator v1,v2, it;
00074         tie(v1,v2) = vertices(program);
00075         for ( it=v1; it != v2; ++it)
00076             if ( get( vertex_exec, program, *it) == VertexNode::func_start_node )
00077                 break;
00078         startv = *v1;
00079         for ( it=v1; it != v2; ++it)
00080             if ( get( vertex_exec, program, *it) == VertexNode::func_exit_node )
00081                 break;
00082         exitv = *v1;
00083 
00084         // Copy-clone over the TAB pointers.
00085         std::vector<AttributeBase*> argsvect = orig.getArguments();
00086         std::vector<AttributeBase*>::iterator ita = argsvect.begin();
00087         for ( ; ita != argsvect.end(); ++ita)
00088             this->args.push_back( (*ita)->clone() );
00089         if (orig.retn)
00090             retn = orig.retn->clone();
00091         this->finish();
00092     }
00093 
00094     void FunctionGraph::finish()
00095     {
00096         put(vertex_exec, program, startv, VertexNode::func_start_node );
00097         put(vertex_exec, program, exitv, VertexNode::func_exit_node);
00098 
00099         // Because we use listS, we need to re-index the map :-(
00100         // If we do not do this, it can not be copied by the copy_graph
00101         // function.
00102         property_map<Graph, vertex_index_t>::type
00103             index = get(vertex_index, program);
00104 
00105         // initialize the vertex_index property values
00106         // so that it can be copied into other graphs.
00107         graph_traits<Graph>::vertex_iterator vi, vend;
00108         graph_traits<Graph>::vertices_size_type cnt = 0;
00109         for(tie(vi,vend) = vertices(program); vi != vend; ++vi)
00110             put(index, *vi, cnt++);
00111         this->reset();
00112     }
00113 
00114     FunctionGraph::~FunctionGraph()
00115     {
00116         //log(Debug) << "Destroying program '" << getName() << "'" <<endlog();
00117         if ( this->isLoaded() ){
00118             getEngine()->removeFunction(this);
00119         }
00120         std::vector<AttributeBase*>::iterator it = args.begin();
00121         for ( ; it != args.end(); ++it)
00122             delete *it;
00123 
00124     }
00125 
00126     void FunctionGraph::setProgramService(Service::shared_ptr myservice)
00127     {
00128         context = myservice;
00129     }
00130 
00131     void FunctionGraph::setUnloadOnStop(bool unload_on_stop)
00132     {
00133         munload_on_stop = unload_on_stop;
00134     }
00135 
00136     void FunctionGraph::loading()
00137     {
00138         // we need to auto-start, or we would be unloaded right away.
00139         if (munload_on_stop)
00140             this->start();
00141     }
00142 
00143     void FunctionGraph::unloading()
00144     {
00145         // this function is called in a real-time context when execute() returns false.
00146         // for functions that have a context object, these should never return false
00147         // in execute(). See the munload_on_stop flag.
00148         if ( !context )
00149             return; // plain function
00150         // The case for program scripts: they are managed by the ScriptingService, which will
00151         // take care of unloading.
00152         if (context->getParent() ) {
00153             context->getParent()->removeService(context->getName());
00154         }
00155         context.reset();
00156     }
00157 
00158 
00159     bool FunctionGraph::start()
00160     {
00161         if ( !isLoaded() )
00162             return false;
00163         if ( pStatus == Status::stopped ) {
00164             this->reset();
00165         }
00166         if ( pStatus == Status::stopped || pStatus == Status::paused) {
00167             pStatus = Status::running;
00168             return true;
00169         }
00170         return false;
00171     }
00172 
00173     bool FunctionGraph::pause()
00174     {
00175         if ( isLoaded() ) {
00176             if ( pStatus == Status::stopped ) {
00177                 this->reset();
00178             }
00179             pausing = true;
00180             return true;
00181         }
00182         return false;
00183     }
00184 
00185     bool FunctionGraph::step()
00186     {
00187         if ( isLoaded() && (pStatus == Status::paused) && mstep == false) {
00188             mstep = true;
00189             return true;
00190         }
00191         return false;
00192     }
00193 
00194     bool FunctionGraph::stepDone() const
00195     {
00196         return mstep == false;
00197     }
00198 
00199     bool FunctionGraph::execute()
00200     {
00201         if (pausing) {
00202             pStatus = Status::paused;
00203             pausing = false;
00204             return true;
00205         }
00206         switch (pStatus) {
00207         case Status::running:
00208             return this->executeUntil();
00209             break;
00210         case Status::paused:
00211             if (mstep) {
00212                 mstep = false;
00213                 this->executeStep();
00214                 return true;
00215             } else
00216                 return true;
00217             break;
00218         case Status::error:
00219         case Status::unknown:
00220         case Status::stopped:
00221             return !munload_on_stop;
00222             break;
00223         }
00224         return false;
00225     }
00226 
00227 
00228     bool FunctionGraph::executeUntil()
00229     {
00230         graph_traits<Graph>::out_edge_iterator ei, ei_end;
00231         // the map contains _references_ to all vertex_command properties
00232         boost::property_map<Graph, vertex_command_t>::type
00233             cmap = get(vertex_command, program);
00234         boost::property_map<Graph, edge_condition_t>::type
00235             emap = get(edge_condition, program);
00236 
00237         do {
00238             // Check this always on entry of executeUntil :
00239             // initialise current node if needed and reset all its out_edges
00240             // if previous == current, we DO NOT RESET, because we want to check
00241             // if previous command has completed !
00242             if ( previous != current )
00243                 {
00244                     for ( tie(ei, ei_end) = boost::out_edges( current, program ); ei != ei_end; ++ei)
00245                         emap[*ei].reset();
00246                     try {
00247                         cmap[current].startExecution();
00248                     } catch(...) {
00249                         pStatus = Status::error;
00250                         return false;
00251                     }
00252                 }
00253 
00254             // initial conditions :
00255             previous = current;
00256             // execute the current command.
00257             try {
00258                 cmap[current].execute();
00259             } catch(...) {
00260                 pStatus = Status::error;
00261                 return false;
00262             }
00263 
00264             // Branch selecting Logic :
00265             if ( cmap[current].isValid() ) {
00266                 for ( tie(ei, ei_end) = boost::out_edges( current, program ); ei != ei_end; ++ei) {
00267                     try {
00268                         if ( emap[*ei].evaluate() ) {
00269                             current = boost::target(*ei, program);
00270                             // a new node has been found ...
00271                             // so continue
00272                             break; // exit from for loop.
00273                         }
00274                     } catch(...) {
00275                         pStatus = Status::error;
00276                         return false;
00277                     }
00278                 }
00279             }
00280         } while ( previous != current && pStatus == Status::running && !pausing); // keep going if we found a new node
00281 
00282         // check finished state
00283         if (current == exitv) {
00284             this->stop();
00285             return !munload_on_stop;
00286         }
00287         return true; // we need to wait.
00288     }
00289 
00290     bool FunctionGraph::executeStep()
00291     {
00292         graph_traits<Graph>::out_edge_iterator ei, ei_end;
00293         // the map contains _references_ to all vertex_command properties
00294         boost::property_map<Graph, vertex_command_t>::type
00295             cmap = get(vertex_command, program);
00296         boost::property_map<Graph, edge_condition_t>::type
00297             emap = get(edge_condition, program);
00298 
00299         // initialise current node if needed and reset all its out_edges
00300         if ( previous != current )
00301         {
00302             for ( tie(ei, ei_end) = boost::out_edges( current, program ); ei != ei_end; ++ei)
00303                 emap[*ei].reset();
00304             try {
00305                 cmap[current].startExecution();
00306             } catch(...) {
00307                 pStatus = Status::error;
00308                 return false;
00309             }
00310             previous = current;
00311         }
00312 
00313         // execute the current command.
00314         try {
00315             cmap[current].execute();
00316         } catch(...) {
00317             pStatus = Status::error;
00318             return false;
00319         }
00320 
00321         // Branch selecting Logic :
00322         if ( cmap[current].isValid() ) {
00323             for ( tie(ei, ei_end) = boost::out_edges( current, program ); ei != ei_end; ++ei) {
00324                 try {
00325                     if ( emap[*ei].evaluate() ) {
00326                         current = boost::target(*ei, program);
00327                         if (current == exitv)
00328                             this->stop();
00329                         // a new node has been found ...
00330                         // it will be executed in the next step.
00331                         return true;
00332                     }
00333                 } catch(...) {
00334                     pStatus = Status::error;
00335                     return false;
00336                 }
00337             }
00338         }
00339         // check finished state
00340         if (current == exitv)
00341             this->stop();
00342         return true; // no new branch found yet !
00343     }
00344 
00345     void FunctionGraph::reset() {
00346         current = startv;
00347         previous = exitv;
00348         this->stop();
00349     }
00350 
00351     bool FunctionGraph::stop()
00352     {
00353         // stop even works if no pp is present
00354         pStatus = Status::stopped;
00355         return true;
00356     }
00357 
00358     const std::string& FunctionGraph::getName() const
00359     {
00360         return myName;
00361     }
00362 
00363     void FunctionGraph::setName(const std::string& name)
00364     {
00365         myName = name;
00366     }
00367 
00368     std::string FunctionGraph::getText() const
00369     {
00370         return _text;
00371     }
00372 
00373     void FunctionGraph::setText(const std::string& text)
00374     {
00375         _text = text;
00376     }
00377 
00378     int FunctionGraph::getLineNumber() const
00379     {
00380         return get(vertex_command, program)[current].getLineNumber();
00381     }
00382 
00383     FunctionGraph* FunctionGraph::copy( std::map<const DataSourceBase*, DataSourceBase*>& replacementdss ) const
00384     {
00385         typedef boost::property_map<Graph, vertex_index_t>::const_type indexmap_t;
00386         typedef boost::graph_traits<Graph>::vertex_descriptor vd_t;
00387         typedef std::vector<vd_t> o2cvect_t;
00388         typedef boost::iterator_property_map<o2cvect_t::iterator, indexmap_t, vd_t, vd_t&> o2cmap_t;
00389         FunctionGraph* ret = new FunctionGraph( getName(), munload_on_stop );
00390 
00391         // clear out unneccessary vertices ( we will copy new ones below )
00392         remove_vertex( ret->startv, ret->program );
00393         remove_vertex( ret->exitv, ret->program );
00394 
00395         indexmap_t indexmap = get( vertex_index, program );
00396         // here we assume that the indexing of program is set properly...
00397         o2cvect_t o2cvect( num_vertices( program ) );
00398         o2cmap_t o2cmap( o2cvect.begin(), indexmap );
00399 
00400 //         std::cerr << "Start copy of " <<std::endl;
00401 //         this->debugPrintout();
00402 //         std::cerr << "Empty ret: " <<std::endl;
00403 //         ret->debugPrintout();
00404         // The replacementdss map contains mappings from this->datasource to copy->datasource,
00405         // thus we can rebuild a vector<AttributeBase*>, which will be automagically be
00406         // found by copy_graph.
00407         // func args are never instantiated, so that we can keep making copies.
00408         for (unsigned int i=0; i < args.size(); ++i)
00409             ret->addArgument( args[i]->copy( replacementdss, false ) );
00410         if (retn)
00411             ret->setResult( retn->copy(replacementdss, false) );
00412 
00413         boost::copy_graph( program, ret->program,
00414                            boost::vertex_copy( GraphVertexCopier( program, ret->program, replacementdss ) ).
00415                            edge_copy( GraphEdgeCopier( program, ret->program, replacementdss ) ).
00416                            orig_to_copy( o2cmap ) );
00417 
00418         ret->startv = o2cmap[startv];
00419         ret->exitv = o2cmap[exitv];
00420         ret->current = o2cmap[current];
00421         ret->previous = o2cmap[previous];
00422 
00423         // so that ret itself can be copied again :
00424         ret->finish();
00425 
00426 //         std::cerr << "Resulted in :" <<std::endl;
00427 //         ret->debugPrintout();
00428 
00429         return ret;
00430     }
00431 
00432     FunctionGraph* FunctionGraph::clone() const
00433     {
00434         return new FunctionGraph(*this);
00435     }
00436 
00437     void FunctionGraph::debugPrintout() const {
00438 #if 0
00439         graph_traits<Graph>::vertex_iterator v,vend;
00440         tie(v,vend) = vertices(program);
00441         boost::property_map<Graph, vertex_command_t>::const_type
00442             cmap = get(vertex_command, program);
00443         boost::property_map<Graph, vertex_index_t>::const_type
00444             imap = get(vertex_index, program);
00445         std::cerr << "program " << getName() << std::endl;
00446         std::cerr << " number of vertices: " << boost::num_vertices(program) << std::endl;
00447         for ( ; v != vend; ++v )
00448         {
00449             int index = get( imap, *v );
00450             ActionInterface* cmd = get( cmap, *v ).getCommand();
00451             if ( cmd )
00452                 std::cerr << " " << index << " " << typeid( *cmd ).name() << std::endl;
00453             else
00454                 std::cerr << " " << index << " (null)" << std::endl;
00455         }
00456 #endif
00457     }
00458 
00459     void FunctionGraph::clearArguments() {
00460         for (std::vector<AttributeBase*>::iterator it = args.begin(); it != args.end(); ++it)
00461             delete *it;
00462         args.clear();
00463     }
00464 
00465 }
00466