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
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
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
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
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
00100
00101
00102 property_map<Graph, vertex_index_t>::type
00103 index = get(vertex_index, program);
00104
00105
00106
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
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
00139 if (munload_on_stop)
00140 this->start();
00141 }
00142
00143 void FunctionGraph::unloading()
00144 {
00145
00146
00147
00148 if ( !context )
00149 return;
00150
00151
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
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
00239
00240
00241
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
00255 previous = current;
00256
00257 try {
00258 cmap[current].execute();
00259 } catch(...) {
00260 pStatus = Status::error;
00261 return false;
00262 }
00263
00264
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
00271
00272 break;
00273 }
00274 } catch(...) {
00275 pStatus = Status::error;
00276 return false;
00277 }
00278 }
00279 }
00280 } while ( previous != current && pStatus == Status::running && !pausing);
00281
00282
00283 if (current == exitv) {
00284 this->stop();
00285 return !munload_on_stop;
00286 }
00287 return true;
00288 }
00289
00290 bool FunctionGraph::executeStep()
00291 {
00292 graph_traits<Graph>::out_edge_iterator ei, ei_end;
00293
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
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
00314 try {
00315 cmap[current].execute();
00316 } catch(...) {
00317 pStatus = Status::error;
00318 return false;
00319 }
00320
00321
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
00330
00331 return true;
00332 }
00333 } catch(...) {
00334 pStatus = Status::error;
00335 return false;
00336 }
00337 }
00338 }
00339
00340 if (current == exitv)
00341 this->stop();
00342 return true;
00343 }
00344
00345 void FunctionGraph::reset() {
00346 current = startv;
00347 previous = exitv;
00348 this->stop();
00349 }
00350
00351 bool FunctionGraph::stop()
00352 {
00353
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
00392 remove_vertex( ret->startv, ret->program );
00393 remove_vertex( ret->exitv, ret->program );
00394
00395 indexmap_t indexmap = get( vertex_index, program );
00396
00397 o2cvect_t o2cvect( num_vertices( program ) );
00398 o2cmap_t o2cmap( o2cvect.begin(), indexmap );
00399
00400
00401
00402
00403
00404
00405
00406
00407
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
00424 ret->finish();
00425
00426
00427
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