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& ) {
00016 #endif
00017 sendSensors();
00018 }
00019
00020 int CameraStreamBehavior::receiveData(char* data, unsigned int len) {
00021 std::string s(data,len);
00022
00023
00024 static std::string incomplete;
00025
00026
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
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
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)
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
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
00092 }
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105