Tekkotsu Homepage

Demos

Overview

Downloads

Dev. Resources

Reference

Credits

WorldStateSerializerBehavior.cc Go to the documentation of this file. #include "WorldStateSerializerBehavior.h" #include "Shared/WorldState.h" #include "Wireless/Wireless.h" #include "Shared/Config.h" #include "Behaviors/Controller.h" #include "Events/EventRouter.h" #include "Shared/LoadSave.h" WorldStateSerializerBehavior::WorldStateSerializerBehavior() : BehaviorBase("WorldStateSerializerBehavior"), wsJoints(NULL), wsPIDs(NULL), lastProcessedTime(0) { } void WorldStateSerializerBehavior::DoStart() { BehaviorBase::DoStart(); // do this first const unsigned int transmitPIDSize=(NumPIDJoints*3)*LoadSave::getSerializedSize<float>()+2*LoadSave::getSerializedSize<unsigned int>(); wsPIDs=wireless->socket(SocketNS::SOCK_STREAM, 1024, transmitPIDSize*4); // enough for 4 packets queued up wireless->setDaemon(wsPIDs); wireless->listen(wsPIDs, config->main.wspids_port); const size_t robotNameLen=strlen(RobotName)+1; //note the +1 to include '\0' at the end const size_t transmitJointsSize=(NumOutputs+NumSensors+NumButtons+NumPIDJoints)*LoadSave::getSerializedSize<float>()+6*LoadSave::getSerializedSize<unsigned int>()+robotNameLen; wsJoints=wireless->socket(SocketNS::SOCK_STREAM, 1024, transmitJointsSize*4); // enough for 4 packets queued up wireless->setDaemon(wsJoints); wireless->listen(wsJoints, config->main.wsjoints_port); Controller::loadGUI(getGUIType(),getGUIType(),getPort()); erouter->addListener(this,EventBase::sensorEGID); } void WorldStateSerializerBehavior::DoStop() { erouter->removeListener(this); Controller::closeGUI(getGUIType()); wireless->setDaemon(wsJoints,false); wireless->close(wsJoints); wireless->setDaemon(wsPIDs,false); wireless->close(wsPIDs); BehaviorBase::DoStop(); // do this last } void WorldStateSerializerBehavior::processEvent(const EventBase& e) { const size_t transmitPIDSize=(NumPIDJoints*3)*LoadSave::getSerializedSize<float>()+2*LoadSave::getSerializedSize<unsigned int>(); if ((e.getTimeStamp() - lastProcessedTime) < config->main.worldState_interval) // not enough time has gone by return; lastProcessedTime = e.getTimeStamp(); char *buf=(char*)wsPIDs->getWriteBuffer(transmitPIDSize); if(buf==NULL) { if(wireless->isConnected(wsPIDs->sock)) std::cout << "WorldStateSerializer dropped pid at " << e.getTimeStamp() << std::endl; } else { #if LOADSAVE_SWAPBYTES // if we need to swap bytes, need to use this slightly slower method unsigned int remain=transmitPIDSize; LoadSave::encodeInc(state->lastSensorUpdateTime,buf,remain); LoadSave::encodeInc(NumPIDJoints,buf,remain); for(unsigned int i=0; i<NumPIDJoints; ++i) for(unsigned int j=0; j<3; ++j) LoadSave::encodeInc(state->pids[i][j],buf,remain); ASSERT(remain==0,"buffer remains"); wsPIDs->write(transmitPIDSize-remain); #else // this is the original version, doesn't handle byte swapping, but faster copy(&buf,state->lastSensorUpdateTime); copy(&buf,NumPIDJoints); copy(&buf,state->pids,NumPIDJoints*3); wsPIDs->write(transmitPIDSize); #endif } const size_t robotNameLen=strlen(RobotName)+1; //note the +1 to include '\0' at the end const size_t transmitJointsSize=(NumOutputs+NumSensors+NumButtons+NumPIDJoints)*LoadSave::getSerializedSize<float>()+6*LoadSave::getSerializedSize<unsigned int>()+robotNameLen; //std::cout << "transmitting at " << e.getTimeStamp() << " with " << (wsJoints->getAvailableSendSize() / float(transmitJointsSize)) << std::endl; buf=(char*)wsJoints->getWriteBuffer(transmitJointsSize); if(buf==NULL) { if(wireless->isConnected(wsJoints->sock)) std::cout << "WorldStateSerializer dropped state at " << e.getTimeStamp() << std::endl; } else { #if LOADSAVE_SWAPBYTES // if we need to swap bytes, need to use this slightly slower method unsigned int remain=transmitJointsSize; memcpy(buf,RobotName,robotNameLen); buf+=robotNameLen; remain-=robotNameLen; LoadSave::encodeInc(state->lastSensorUpdateTime,buf,remain); LoadSave::encodeInc(state->frameNumber,buf,remain); LoadSave::encodeInc(NumOutputs,buf,remain); for(unsigned int i=0; i<NumOutputs; ++i) LoadSave::encodeInc(state->outputs[i],buf,remain); LoadSave::encodeInc(NumSensors,buf,remain); for(unsigned int i=0; i<NumSensors; ++i) LoadSave::encodeInc(state->sensors[i],buf,remain); LoadSave::encodeInc(NumButtons,buf,remain); for(unsigned int i=0; i<NumButtons; ++i) LoadSave::encodeInc(state->buttons[i],buf,remain); LoadSave::encodeInc(NumPIDJoints,buf,remain); for(unsigned int i=0; i<NumPIDJoints; ++i) LoadSave::encodeInc(state->pidduties[i],buf,remain); ASSERT(remain==0,"buffer remains"); wsJoints->write(transmitJointsSize-remain); #else // this is the original version, doesn't handle byte swapping, but faster copy(&buf,RobotName,robotNameLen); copy(&buf,state->lastSensorUpdateTime); copy(&buf,state->frameNumber); copy(&buf,NumOutputs); copy(&buf,state->outputs,NumOutputs); copy(&buf,NumSensors); copy(&buf,state->sensors,NumSensors); copy(&buf,NumButtons); copy(&buf,state->buttons,NumButtons); copy(&buf,NumPIDJoints); copy(&buf,state->pidduties,NumPIDJoints); wsJoints->write(transmitJointsSize); #endif } } /*! @file * @brief Implements WorldStateSerializer, which copies WorldState into a buffer for transmission over the network * @author alokl (Creator) * * $Author: ejt $ * $Name: tekkotsu-3_0 $ * $Revision: 1.10 $ * $State: Exp $ * $Date: 2006/09/19 22:51:38 $ */

Tekkotsu v3.0

Generated Wed Oct 4 00:03:47 2006 by Doxygen 1.4.7