WorldStateSerializerBehavior.ccGo 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 "Events/EventRouter.h"
00006
00007 WorldStateSerializerBehavior::WorldStateSerializerBehavior()
00008 : BehaviorBase("WorldStateSerializerBehavior"), wsJoints(NULL), wsPIDs(NULL), lastProcessedTime(0)
00009 {
00010 wsJoints=wireless->socket(SocketNS::SOCK_STREAM, 1024, 2048);
00011 wireless->setDaemon(wsJoints);
00012 wireless->listen(wsJoints, config->main.wsjoints_port);
00013 wsPIDs=wireless->socket(SocketNS::SOCK_STREAM, 1024, 2048);
00014 wireless->setDaemon(wsPIDs);
00015 wireless->listen(wsPIDs, config->main.wspids_port);
00016 }
00017
00018 void WorldStateSerializerBehavior::DoStart() {
00019 BehaviorBase::DoStart();
00020 erouter->addListener(this,EventBase::sensorEGID);
00021 }
00022
00023 void WorldStateSerializerBehavior::DoStop() {
00024 erouter->removeListener(this);
00025 BehaviorBase::DoStop();
00026 }
00027
00028 void WorldStateSerializerBehavior::processEvent(const EventBase& e) {
00029 if ((e.getTimeStamp() - lastProcessedTime) < config->main.worldState_interval)
00030 return;
00031 lastProcessedTime = e.getTimeStamp();
00032 char *buf=(char*)wsPIDs->getWriteBuffer((NumPIDJoints*3)*sizeof(float)+2*sizeof(unsigned int));
00033 if (buf) {
00034 encode(&buf,state->lastSensorUpdateTime);
00035 encode(&buf,NumPIDJoints);
00036 encode(&buf,state->pids,NumPIDJoints*3);
00037 wsPIDs->write((NumPIDJoints*3)*sizeof(float)+2*sizeof(unsigned int));
00038 }
00039
00040 buf=(char*)wsJoints->getWriteBuffer((NumPIDJoints*2+NumSensors+NumButtons)*sizeof(float)+4*sizeof(unsigned int));
00041 if (buf) {
00042 encode(&buf,state->lastSensorUpdateTime);
00043 encode(&buf,NumPIDJoints);
00044 encode(&buf,&state->outputs[PIDJointOffset], NumPIDJoints);
00045 encode(&buf,NumSensors);
00046 encode(&buf,state->sensors,NumSensors);
00047 encode(&buf,NumButtons);
00048 encode(&buf,state->buttons,NumButtons);
00049 encode(&buf,state->pidduties,NumPIDJoints);
00050 wsJoints->write((NumPIDJoints*2+NumSensors+NumButtons)*sizeof(float)+4*sizeof(unsigned int));
00051 }
00052 }
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
|