RetinaI.C

00001 #include "Robots/RobotBrain/RetinaI.H"
00002 
00003 
00004 // ######################################################################
00005 RetinaI::RetinaI(int id, OptionManager& mgr,
00006     const std::string& descrName, const std::string& tagName) :
00007   RobotBrainComponent(mgr, descrName, tagName),
00008   itsIfs(new InputFrameSeries(mgr)),
00009   itsRunning(true)
00010 {
00011   addSubComponent(itsIfs);
00012 }
00013 
00014 // ######################################################################
00015 RetinaI::~RetinaI()
00016 {
00017 }
00018 
00019 void RetinaI::registerTopics()
00020 {
00021   LINFO("Registering Retina Message");
00022   this->registerPublisher("RetinaMessageTopic");
00023   this->registerSubscription("CameraConfigTopic");
00024 }
00025 
00026 void RetinaI::evolve()
00027 {
00028   if(itsRunning)
00029   {
00030     Image<PixRGB<byte> > img;
00031     itsIfs->updateNext(); img = itsIfs->readRGB();
00032     if(img.initialized())
00033     {
00034       LINFO("Sending Frame");
00035       RobotSimEvents::RetinaMessagePtr msg = new RobotSimEvents::RetinaMessage;
00036       msg->img = Image2Ice(img);
00037       msg->cameraID = itsIceIdentity.getVal();
00038       this->publish("RetinaMessageTopic", msg);
00039     }
00040     else
00041     {
00042       LERROR("Image Not Initialized!");
00043     }
00044   }
00045   printf("Evolve\n");
00046 
00047 }
00048 
00049 // ######################################################################
00050 void RetinaI::updateMessage(const RobotSimEvents::EventMessagePtr& eMsg,
00051     const Ice::Current&)
00052 {
00053   //Get a retina message
00054   if(eMsg->ice_isA("::RobotSimEvents::CameraConfigMessage"))
00055   {
00056     RobotSimEvents::CameraConfigMessagePtr ccMsg = RobotSimEvents::CameraConfigMessagePtr::dynamicCast(eMsg);
00057 
00058     LINFO("Message is a camera config message: cameraID=%s, active:%d", ccMsg->cameraID.c_str(), ccMsg->active);
00059 
00060     if(ccMsg->cameraID == itsIceIdentity.getVal())
00061       itsRunning = ccMsg->active;
00062 
00063   }
00064 }
00065 
00066