Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

CameraStreamBehavior.cc

Go to the documentation of this file.
00001 #include "CameraStreamBehavior.h"
00002 #include "Wireless/Socket.h"
00003 #include "Motion/PostureEngine.h"
00004 #include "Events/EventRouter.h"
00005 #include "Shared/LoadSave.h"
00006 #include "Shared/Config.h"
00007 #include "Shared/WorldState.h"
00008 
00009 using namespace std;
00010 
00011 #if DEBUG
00012 void CameraStreamBehavior::processEvent(const EventBase& e) {
00013   ASSERTRET(e.getGeneratorID()==EventBase::sensorEGID,"unexpected event");
00014 #else
00015 void CameraStreamBehavior::processEvent(const EventBase& /*e*/) {
00016 #endif
00017     sendSensors();
00018 }
00019 
00020 int CameraStreamBehavior::receiveData(char* data, unsigned int len) {
00021   std::string s(data,len);
00022   //cout << "Console Received: " << s << endl;
00023 
00024   static std::string incomplete;
00025 
00026   //pass a line at a time to the controller
00027   while(s.size()>0) {
00028     std::string::size_type endline=s.find('\n');
00029     if(endline==std::string::npos) {
00030       incomplete+=s;
00031       return 0;
00032     }
00033 
00034     //strip a \r\n or a \n
00035     if(endline>0 && s[endline-1]=='\r')
00036       incomplete+=s.substr(0,endline-1);
00037     else
00038       incomplete+=s.substr(0,endline);
00039     
00040     //is now complete
00041     if(incomplete=="refreshSensors") {
00042       sendSensors();
00043     } else if(incomplete=="startSensors") {
00044       if(sensorListeners++ == 0)
00045         erouter->addListener(this,EventBase::sensorEGID);
00046     } else if(incomplete=="stopSensors") {
00047       if(sensorListeners==0)
00048         serr->printf("WARNING: %s sensor listener underflow",getName().c_str());
00049       else if(--sensorListeners == 0)
00050         erouter->removeListener(this,EventBase::sensorEGID);
00051     }
00052     incomplete.erase();
00053     s=s.substr(endline+1);
00054   }
00055   return 0;
00056 }
00057 
00058 void CameraStreamBehavior::sendSensors() {
00059   if ((state->lastSensorUpdateTime - lastProcessedTime) < config->main.worldState_interval) // not enough time has gone by
00060     return;
00061   lastProcessedTime = state->lastSensorUpdateTime;
00062 
00063   ASSERT(LoadSave::stringpad==sizeof(unsigned int)+sizeof(char),"LoadSave::encode(string) format has changed?");
00064   PostureEngine pose;
00065   pose.takeSnapshot();
00066   pose.setWeights(1);
00067   pose.setSaveFormat(true,state);
00068   unsigned int len=pose.getBinSize()+LoadSave::stringpad;
00069   byte* buf=curSocket->getWriteBuffer(len);
00070   if(buf==NULL) {
00071     serr->printf("Unable to serialize sensor data for camera image -- network overflow");
00072     return;
00073   }
00074   unsigned int used;
00075   if((used=pose.saveBuffer((char*)buf+sizeof(unsigned int),len-LoadSave::stringpad))==0) {
00076     cerr << "An error occured during sensor serialization" << endl;
00077     curSocket->write(0);
00078     return;
00079   }
00080   if(used!=len-LoadSave::stringpad-1)
00081     std::cout << "Warning: used==" << used << " len==" << len << std::endl;
00082   //add the LoadSave fields (prepend length, append '\0')
00083   len=LoadSave::encode(used,reinterpret_cast<char*>(buf),LoadSave::getSerializedSize(used));
00084   if(len==0) {
00085     cerr << "An error occured during serialization of buffer length" << endl;
00086     curSocket->write(0);
00087     return;
00088   }
00089   buf[used+sizeof(used)]='\0';
00090   curSocket->write(used+LoadSave::stringpad);
00091   //std::cout << "Sent sensors " << used << std::endl;
00092 }
00093 
00094 
00095 
00096 /*! @file
00097  * @brief Defines CameraStreamBehavior, which is the base class for camera streaming communication classes, handles upstream communication
00098  * @author ejt (Creator)
00099  *
00100  * $Author: ejt $
00101  * $Name: tekkotsu-4_0 $
00102  * $Revision: 1.6 $
00103  * $State: Exp $
00104  * $Date: 2007/11/11 23:57:19 $
00105  */

Tekkotsu v4.0
Generated Thu Nov 22 00:54:51 2007 by Doxygen 1.5.4