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

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:36 2016 by Doxygen 1.6.3