cop.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2009 by Ulrich Friedrich Klank <klank@in.tum.de>
00003  *
00004  * This program is free software; you can redistribute it and/or modify
00005  * it under the terms of the GNU General Public License as published by
00006  * the Free Software Foundation; either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018 
00019 /************************************************************************
00020                         cop.h - Copyright klank
00021 
00022 
00023 **************************************************************************/
00024 
00025 
00026 #ifndef COP_H
00027 #define COP_H
00028 
00029 #include "VisFinder.h"
00030 #include "VisLearner.h"
00031 #include "ImageInputSystem.h"
00032 
00033 
00034 #ifdef _DEBUG
00035 #define DEBUG(A) A
00036 #else
00037 #define DEBUG(A)  ;
00038 #endif
00039 
00040 
00041 #ifdef LOGFILE
00042 
00043 #define LOGFILE_ADDED , *s_logFile
00044 
00045 #else /*LOGFILE*/
00046 
00047 #define LOGFILE_ADDED
00048 
00049 #endif /*LOGFILE*/
00050 #ifndef NO_MAIN_PRG
00051 volatile bool g_stopall = false;
00052 #endif
00053 
00054 #ifdef BOOST_THREAD
00055 #define BOOST(A) A
00056 #include <signal.h>
00057 #else
00058 #define BOOST(A)
00059 #endif /*BOOST_THREAD*/
00060 
00061 /* LO Service*/
00062 #ifdef USE_YARP_COMM
00063 
00064 #include "YarpComm.h"
00065 
00066 #ifdef NO_LO_SERVICE_AVAILABLE
00067 #else 
00068 #include "YarpRpcComm.h"
00069 #endif 
00071 #else 
00073 #include "ROSComm.h"
00074 
00075 #ifdef NO_LO_SERVICE_AVAILABLE
00076 #else 
00077 #include "ROSjloComm.h"
00078 #include <ros/ros.h>
00079 #endif 
00081 #endif 
00084 void COPCTRLC(int);
00085 
00097 namespace cop
00098 {
00099   /********************************************************************
00100   *   class cop_world                                                       */
00116   class cop_world
00117   {
00118   public:
00119     ImageInputSystem* s_inputSystem;
00120     SignatureDB*      s_sigDb;
00121     AttentionManager* s_attManager;
00122     VisFinder*        s_visFinder;
00123     VisLearner*       s_visLearner;
00124     Statistics*       s_stats;
00125   #ifdef LOGFILE
00126       LogFile*            s_logFile;
00127   #endif /*LOGFILE*/
00128   #ifndef NO_LO_SERVICE_AVAILABLE
00129     ros::NodeHandle* s_node;
00130   #endif
00131   protected:
00132 
00133     virtual int Setup()
00134     {
00135       return 0;
00136     }
00137       virtual int Shutdown()
00138     {
00139       return 0;
00140     }
00141   public:
00142   /********************************************************************
00143   *   cop_world                                                       */
00156     cop_world(XMLTag* config, std::string nodename = "cop") :
00157       s_inputSystem(NULL),
00158       s_sigDb(NULL),
00159       s_attManager(NULL),
00160       s_visFinder(NULL),
00161       s_visLearner(NULL),
00162       s_stats(NULL)
00163   #ifdef LOGFILE
00164           ,s_logFile(NULL)
00165   #endif /*LOGFILE*/
00166   #ifndef NO_LO_SERVICE_AVAILABLE
00167       ,s_node(NULL)
00168   #endif  /*NO_LO_SERVICE_AVAILABLE*/
00169     {
00170        /*Install signal handler for Ctrl-C*/
00171        BOOST(signal(SIGINT, COPCTRLC));
00172   #ifdef NO_LO_SERVICE_AVAILABLE
00173   /*  Local position control, normally not necessary*/
00174         RelPoseFactory::LoadList(config->GetChild(XML_NODE_RELPOSELIST));
00175   #else
00176   #ifdef USE_YARP_COMM
00177         /*Setup the yarp interface for lo service*/
00178         std::string loPortName = STD_LO_RPC_PORT_INTERNAL;
00179         if(config->GetChild(XML_NODE_RELPOSELIST) != NULL)
00180         {
00181           std::string temp = config->GetChild(XML_NODE_RELPOSELIST)->GetProperty(XML_PROPERTY_LO_RPC_PORT);
00182           if(temp.length() > 0)
00183             loPortName = temp;
00184         }
00185         RelPoseFactory::s_loService = new YarpRpcComm(loPortName);
00186   #else
00187         /*OR Setup the ros interface for lo service*/
00188         s_node = new ros::NodeHandle(nodename);
00189         std::string loPortName = STD_LO_RPC_PORT_INTERNAL;
00190         if(config->GetChild(XML_NODE_RELPOSELIST) != NULL)
00191         {
00192           std::string temp = config->GetChild(XML_NODE_RELPOSELIST)->GetProperty(XML_PROPERTY_LO_RPC_PORT);
00193           if(temp.length() > 0)
00194             loPortName = temp;
00195         }
00196         try
00197         {
00198         RelPoseFactory::s_loService = new ROSjloComm(loPortName);
00199   #endif
00200   #endif
00201 
00202 #ifdef LOGFILE
00203         XMLTag* logfilename = config->GetChild(XML_NODE_LOGFILE);
00204         s_logFile = new  LogFile(logfilename != NULL ? logfilename->GetCDataST() : "");
00205         DEBUG(printf("Log file opened\n"));
00206 #endif /*LOGFILE*/
00207         s_inputSystem = new ImageInputSystem( config->GetChild(XML_NODE_IMAGEINPUTSYSTEM));
00208         DEBUG(printf("Camera System loaded\n"));
00209         s_sigDb = new SignatureDB (config->GetChild(XML_NODE_SIGNATUREDB));
00210         DEBUG(printf("Signature DB loaded\n"));
00211         s_attManager = new AttentionManager(config->GetChild(XML_NODE_ATTENTIONMANAGER), *s_sigDb, *s_inputSystem LOGFILE_ADDED);
00212         DEBUG(printf("Attention Management loaded\n"));
00213         s_visLearner = new VisLearner(config->GetChild(XML_NODE_VISLEARNER), *s_sigDb, *s_inputSystem LOGFILE_ADDED,  true);
00214         DEBUG(printf("Visial Learner Loaded\n"));
00215         XMLTag* visfind = config->GetChild(XML_NODE_VISFINDER);
00216         if(visfind == NULL)
00217         {
00218           throw "Outdated xml file";
00219         }
00221         s_visFinder = new VisFinder(visfind, s_inputSystem, s_sigDb, s_attManager, s_visLearner LOGFILE_ADDED);
00222         DEBUG(printf("Algorithm Selection loaded\n"));
00223         }
00224         catch(const char* text)
00225         {
00226           printf("Error loading cop: %s\n", text);
00227         }
00228     }
00229    /********************************************************************
00230     *   destructor ~cop_world                                                       */
00234     ~cop_world()
00235     {
00236       delete s_inputSystem;
00237       delete s_visLearner;
00238       delete s_attManager;
00239       delete s_visFinder;
00240       delete s_sigDb;
00241       delete s_stats;
00242   #ifndef NO_LO_SERVICE_AVAILABLE
00243   #ifndef USE_YARP_COMM
00244       delete s_node;
00245   #endif
00246   #endif
00247       RelPoseFactory::DisposeList();
00248     }
00249 
00250    /********************************************************************
00251     *   SaveCop                                                      */
00256     void SaveCop(std::string filename)
00257     {
00258       XMLTag* config = new XMLTag("config");
00259 #ifdef NO_LO_SERVICE_AVAILABLE
00260       printf("Save relpose list\n");
00261       config->AddChild(RelPoseFactory::SaveList());
00262 #endif /*NO_LO_SERVICE_AVAILABLE*/
00263       printf("Save vis finder\n");
00264       config->AddChild(s_visFinder->Save());
00265       printf("Save vis learner\n");
00266       config->AddChild(s_visLearner->Save());
00267       printf("Save vis input system\n");
00268       config->AddChild(s_inputSystem->Save());
00269       printf("Save vis db\n");
00270       config->AddChild(s_sigDb->Save());
00271       printf("Save vis att man\n");
00272       config->AddChild(s_attManager->Save());
00273       printf("write it\n");
00274       config->WriteToFile(filename);
00275       printf("Written\n");
00276     }
00277 
00278   #ifdef USE_YARP_COMM
00279 
00286       void StartListeningYarpPort(char* yarpbottle)
00287       {
00288         printf("Entering StartListeningYarpPort\n");
00289         yarp::os::Network yarp_network;
00290         yarp_network.init();
00291         YarpPortManager port_manager;
00292         try
00293         {
00294           yarp::os::BufferedPort<yarp::os::Bottle>* port = port_manager.OpenCommOnYarpPort(yarpbottle);
00295           port_manager.Listen(*port, g_stopall, s_visFinder, s_sigDb);
00296         }
00297         catch(char const* text)
00298         {
00299           printf("StartListeningYarpPort: Port %s could not openend (Error: %s).\n", yarpbottle, text);
00300         }
00301         catch(...)
00302         {
00303           printf("StartListeningYarpPort: Unknown Exception\n");
00304         }
00305         printf("Leaving Function\n");
00306       }
00307   #else
00308 
00315       void StartNodeSubscription(char* topic)
00316       {
00317         printf("Entering StartNodeSubscription\n");
00318 
00319         ROSTopicManager port_manager(s_visFinder, s_sigDb);
00320         try
00321         {
00322           /*TODO*/
00323           port_manager.Listen(topic, g_stopall, s_node);
00324           g_stopall = true;
00325         }
00326         catch(char const* text)
00327         {
00328           printf("StartListening Failed: Service %s could not be advertised. Error: %s\n", topic, text);
00329         }
00330         catch(...)
00331         {
00332           printf("StartListeningYarpPort: Unknown Exception\n");
00333         }
00334         printf("Leaving Function\n");
00335       }
00336   #endif /*USE_YARP_COMM*/
00337 
00347       bool AddElement(XMLTag* tag)
00348       {
00349           try
00350           {
00351               Sensor* cam = Sensor::SensorFactory(tag);
00352               if(cam != NULL)
00353               {
00354                   s_inputSystem->AddSensor(cam);
00355                   return true;
00356               }
00357               Elem* elem = Elem::ElemFactory(tag);
00358               if(elem != NULL)
00359               {
00360                   switch(elem->GetType())
00361                   {
00362                   case SIGNATURE:
00363                       s_sigDb->AddSignature((Signature*)elem);
00364                       return true;
00365                   case CLASS:
00366                       ((Class*)elem)->m_ID = s_sigDb->AddClass(((Class*)elem)->GetName(), ((Class*)elem)->m_ID);
00367                       return true;
00368                   default:
00369                       break;
00370                   }
00371               }
00372               Algorithm<std::vector<RelPose*> >* alg = LocateAlgorithm::AlgFactory(tag);
00373               if(alg != NULL)
00374               {
00375                   s_visFinder->AddAlgorithm(alg);
00376                   return true;
00377               }
00378               /* TODO: Refine algorithm*/
00379           }
00380           catch(char const*
00381   #ifdef _DEBUG
00382               temp
00383   #endif /*_DEBUG*/
00384               )
00385           {
00386   #ifdef _DEBUG
00387               printf("%s\n", temp);
00388   #endif /*_DEBUG*/
00389               return false;
00390           }
00391           return false;
00392       }
00393   };
00394 }
00395 #endif /*COP_H*/


cognitive_perception
Author(s): Ulrich F Klank
autogenerated on Mon Oct 6 2014 10:48:45