ImageInputSystem.cpp
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                         ImageInputSystem.cpp - Copyright klank
00021 
00022 **************************************************************************/
00023 
00024 #include "ImageInputSystem.h"
00025 #include "XMLTag.h"
00026 #include <signal.h>
00027 extern volatile bool g_stopall;
00028 void COPCTRLC(int sig)
00029 {
00030   printf("In SIG INT handler of cop\n");
00031   g_stopall = true;
00032   boost::xtime t;
00033   boost::xtime_get(&t, boost::TIME_UTC);
00034   t.sec += 1;
00035   printf("Waiting 1 seconds before shutting down\n");
00036   boost::thread::sleep(t);
00037   raise(15);
00038 }
00039 
00040 
00041     /*printf("Sleeping\n");*/
00042 void Sleeping(long ms)
00043  {
00044   boost::xtime t;
00045   boost::xtime_get(&t, boost::TIME_UTC);
00046   t.nsec += ms * 1000000;  //TODO Check
00047   boost::thread::sleep(t);
00048  }
00049 
00050 using namespace cop;
00051 
00052 
00053 // Constructors/Destructors
00054 //
00055 
00056 ImageInputSystem::ImageInputSystem (XMLTag* configFile)
00057 {
00058   if(configFile != NULL && configFile->CountChildren() > 0)
00059   {
00060     XMLTag* tag = configFile->GetChild(0);
00061     if(tag == NULL)
00062     {
00063       ROS_WARN("Error Loading Image Acquisition modules\n");
00064       throw "Error in ImageInputSystem::ImageInputSystem";
00065     }
00066     m_cameras = XMLTag::Load(tag, &m_cameras);
00067     for(std::vector<Sensor*>::const_iterator it = m_cameras.begin();
00068       it != m_cameras.end(); it++)
00069      {
00070        if((*it) != NULL && (*it)->RequiresSensorList())
00071        {
00072          (*it)->SetSensorList(m_cameras);
00073        }
00074      }
00075     ROS_INFO("Loaded %ld Cameras\n", m_cameras.size());
00076     m_stConverterNames = configFile->GetProperty(XML_ATTIBUTE_READINGCONVERTER, "");
00077     if(m_stConverterNames.length() > 0)
00078     {
00079       ReadingConverter* reading = ReadingConverter::ReadingConverterFactory(m_stConverterNames);
00080       Reading::s_conv[std::pair<ReadingType_t, ReadingType_t>(reading->TypeIn(), reading->TypeOut())] = reading;
00081     }
00082   }
00083 }
00084 
00085 ImageInputSystem::~ImageInputSystem ( )
00086 {
00087   std::map<std::pair<ReadingType_t, ReadingType_t>, ReadingConverter*>::iterator iter = Reading::s_conv.begin();
00088   for(;iter != Reading::s_conv.end(); )
00089   {
00090     delete (*iter).second;
00091     Reading::s_conv.erase(iter);
00092     iter = Reading::s_conv.begin();
00093   }
00094 }
00095 
00096 
00097 void ImageInputSystem::AddSensor(Sensor* sensor)
00098 {
00099   m_cameras.push_back(sensor);
00100 
00101    for(std::vector<Sensor*>::const_iterator it = m_cameras.begin();
00102        it != m_cameras.end(); it++)
00103    {
00104      if((*it)->RequiresSensorList())
00105      {
00106         (*it)->SetSensorList(m_cameras);
00107      }
00108    }
00109 }
00110 
00111 
00112 // Accessor methods
00113 //
00114 XMLTag* ImageInputSystem::Save()
00115 {
00116         XMLTag* tag = new XMLTag(XML_NODE_IMAGEINPUTSYSTEM);
00117         tag->AddChild(XMLTag::Tag(m_cameras));
00118         return tag;
00119 }
00120 
00121 
00129 std::vector<Sensor*> ImageInputSystem::GetBestSensor (RelPose &pose)
00130 {
00131         size_t nSize = m_cameras.size();
00132   std::vector<Sensor*> sensors_seeing;
00133         for(unsigned int i = 0; i < nSize; i++)
00134         {
00135           if(m_cameras[i] == NULL)
00136       continue;
00137                 if(m_cameras[i]->CanSee(pose))//TODO: choose best
00138                 {
00139                         sensors_seeing.push_back(m_cameras[i]);
00140                 }
00141         }
00142         return sensors_seeing;
00143 }
00144 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


cognitive_perception
Author(s): Ulrich F Klank
autogenerated on Thu May 23 2013 07:38:34