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