Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

WorldStateSerializerBehavior.cc

Go to the documentation of this file.
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 //! a little trick to get RobotName length under aperios, which has to use a string class instead of char*
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(); // do this first
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); // enough for 4 packets queued up
00022   wireless->setDaemon(wsPIDs);
00023   wireless->listen(wsPIDs, config->main.wspids_port);
00024 
00025   const size_t robotNameLen=strlen(RobotName)+1; //note the +1 to include '\0' at the end
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); // enough for 4 packets queued up
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(); // do this last
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) // not enough time has gone by
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     // if we need to swap bytes, need to use this slightly slower method
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     // this is the original version, doesn't handle byte swapping, but faster
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; //note the +1 to include '\0' at the end
00079   const size_t transmitJointsSize=(NumOutputs+NumSensors+NumButtons+NumPIDJoints)*LoadSave::getSerializedSize<float>()+6*LoadSave::getSerializedSize<unsigned int>()+robotNameLen;
00080   //std::cout << "transmitting at " << e.getTimeStamp() << " with " << (wsJoints->getAvailableSendSize() / float(transmitJointsSize)) << std::endl;
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     // if we need to swap bytes, need to use this slightly slower method
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     // this is the original version, doesn't handle byte swapping, but faster
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 /*! @file
00127 * @brief Implements WorldStateSerializer, which copies WorldState into a buffer for transmission over the network
00128 * @author alokl (Creator)
00129 *
00130 * $Author: ejt $
00131 * $Name: tekkotsu-4_0 $
00132 * $Revision: 1.12 $
00133 * $State: Exp $
00134 * $Date: 2007/06/26 04:27:44 $
00135 */

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