00001 #include "WorldStateSerializerBehavior.h"
00002 #include "Shared/WorldState.h"
00003 #include "Wireless/Wireless.h"
00004 #include "Shared/Config.h"
00005 #include "Behaviors/Controller.h"
00006 #include "Events/EventRouter.h"
00007 #include "Shared/LoadSave.h"
00008
00009
00010 inline size_t strlen(const std::string& st) { return st.size(); }
00011
00012 WorldStateSerializerBehavior::WorldStateSerializerBehavior()
00013 : BehaviorBase("WorldStateSerializerBehavior"), wsJoints(NULL), wsPIDs(NULL), lastProcessedTime(0)
00014 {
00015 }
00016
00017 void WorldStateSerializerBehavior::DoStart() {
00018 BehaviorBase::DoStart();
00019
00020 const unsigned int transmitPIDSize=(NumPIDJoints*3)*LoadSave::getSerializedSize<float>()+2*LoadSave::getSerializedSize<unsigned int>();
00021 wsPIDs=wireless->socket(Socket::SOCK_STREAM, 1024, transmitPIDSize*4);
00022 wireless->setDaemon(wsPIDs);
00023 wireless->listen(wsPIDs, config->main.wspids_port);
00024
00025 const size_t robotNameLen=strlen(RobotName)+1;
00026 const size_t transmitJointsSize=(NumOutputs+NumSensors+NumButtons+NumPIDJoints)*LoadSave::getSerializedSize<float>()+6*LoadSave::getSerializedSize<unsigned int>()+robotNameLen;
00027 wsJoints=wireless->socket(Socket::SOCK_STREAM, 1024, transmitJointsSize*4);
00028 wireless->setDaemon(wsJoints);
00029 wireless->listen(wsJoints, config->main.wsjoints_port);
00030
00031 Controller::loadGUI(getGUIType(),getGUIType(),getPort());
00032
00033 erouter->addListener(this,EventBase::sensorEGID);
00034 }
00035
00036 void WorldStateSerializerBehavior::DoStop() {
00037 erouter->removeListener(this);
00038
00039 Controller::closeGUI(getGUIType());
00040
00041 wireless->setDaemon(wsJoints,false);
00042 wireless->close(wsJoints);
00043 wireless->setDaemon(wsPIDs,false);
00044 wireless->close(wsPIDs);
00045
00046 BehaviorBase::DoStop();
00047 }
00048
00049 void WorldStateSerializerBehavior::processEvent(const EventBase& e) {
00050 const size_t transmitPIDSize=(NumPIDJoints*3)*LoadSave::getSerializedSize<float>()+2*LoadSave::getSerializedSize<unsigned int>();
00051 if ((e.getTimeStamp() - lastProcessedTime) < config->main.worldState_interval)
00052 return;
00053 lastProcessedTime = e.getTimeStamp();
00054 char *buf=(char*)wsPIDs->getWriteBuffer(transmitPIDSize);
00055 if(buf==NULL) {
00056 if(wireless->isConnected(wsPIDs->sock))
00057 std::cout << "WorldStateSerializer dropped pid at " << e.getTimeStamp() << std::endl;
00058 } else {
00059 #if LOADSAVE_SWAPBYTES
00060
00061 unsigned int remain=transmitPIDSize;
00062 LoadSave::encodeInc(state->lastSensorUpdateTime,buf,remain);
00063 LoadSave::encodeInc(NumPIDJoints,buf,remain);
00064 for(unsigned int i=0; i<NumPIDJoints; ++i)
00065 for(unsigned int j=0; j<3; ++j)
00066 LoadSave::encodeInc(state->pids[i][j],buf,remain);
00067 ASSERT(remain==0,"buffer remains");
00068 wsPIDs->write(transmitPIDSize-remain);
00069 #else
00070
00071 copy(&buf,state->lastSensorUpdateTime);
00072 copy(&buf,NumPIDJoints);
00073 copy(&buf,state->pids,NumPIDJoints*3);
00074 wsPIDs->write(transmitPIDSize);
00075 #endif
00076 }
00077
00078 const size_t robotNameLen=strlen(RobotName)+1;
00079 const size_t transmitJointsSize=(NumOutputs+NumSensors+NumButtons+NumPIDJoints)*LoadSave::getSerializedSize<float>()+6*LoadSave::getSerializedSize<unsigned int>()+robotNameLen;
00080
00081 buf=(char*)wsJoints->getWriteBuffer(transmitJointsSize);
00082 if(buf==NULL) {
00083 if(wireless->isConnected(wsJoints->sock))
00084 std::cout << "WorldStateSerializer dropped state at " << e.getTimeStamp() << std::endl;
00085 } else {
00086 #if LOADSAVE_SWAPBYTES
00087
00088 unsigned int remain=transmitJointsSize;
00089 memcpy(buf,RobotName,robotNameLen);
00090 buf+=robotNameLen; remain-=robotNameLen;
00091 LoadSave::encodeInc(state->lastSensorUpdateTime,buf,remain);
00092 LoadSave::encodeInc(state->frameNumber,buf,remain);
00093 LoadSave::encodeInc(NumOutputs,buf,remain);
00094 for(unsigned int i=0; i<NumOutputs; ++i)
00095 LoadSave::encodeInc(state->outputs[i],buf,remain);
00096 LoadSave::encodeInc(NumSensors,buf,remain);
00097 for(unsigned int i=0; i<NumSensors; ++i)
00098 LoadSave::encodeInc(state->sensors[i],buf,remain);
00099 LoadSave::encodeInc(NumButtons,buf,remain);
00100 for(unsigned int i=0; i<NumButtons; ++i)
00101 LoadSave::encodeInc(state->buttons[i],buf,remain);
00102 LoadSave::encodeInc(NumPIDJoints,buf,remain);
00103 for(unsigned int i=0; i<NumPIDJoints; ++i)
00104 LoadSave::encodeInc(state->pidduties[i],buf,remain);
00105 ASSERT(remain==0,"buffer remains");
00106 wsJoints->write(transmitJointsSize-remain);
00107 #else
00108
00109 copy(&buf,RobotName,robotNameLen);
00110 copy(&buf,state->lastSensorUpdateTime);
00111 copy(&buf,state->frameNumber);
00112 copy(&buf,NumOutputs);
00113 copy(&buf,state->outputs,NumOutputs);
00114 copy(&buf,NumSensors);
00115 copy(&buf,state->sensors,NumSensors);
00116 copy(&buf,NumButtons);
00117 copy(&buf,state->buttons,NumButtons);
00118 copy(&buf,NumPIDJoints);
00119 copy(&buf,state->pidduties,NumPIDJoints);
00120 wsJoints->write(transmitJointsSize);
00121 #endif
00122 }
00123 }
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135