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(), wsJoints(NULL), wsPIDs(NULL)
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->forgetListener(this);
00025 BehaviorBase::DoStop();
00026 }
00027
00028 void WorldStateSerializerBehavior::processEvent(const EventBase& ) {
00029 char *buf=(char*)wsPIDs->getWriteBuffer((NumPIDJoints*3)*sizeof(float)+2*sizeof(unsigned int));
00030 if (buf) {
00031 encode(&buf,state->lastSensorUpdateTime);
00032 encode(&buf,NumPIDJoints);
00033 encode(&buf,state->pids,NumPIDJoints*3);
00034 wsPIDs->write((NumPIDJoints*3)*sizeof(float)+2*sizeof(unsigned int));
00035 }
00036
00037 buf=(char*)wsJoints->getWriteBuffer((NumPIDJoints*2+NumSensors+NumButtons)*sizeof(float)+4*sizeof(unsigned int));
00038 if (buf) {
00039 encode(&buf,state->lastSensorUpdateTime);
00040 encode(&buf,NumPIDJoints);
00041 encode(&buf,&state->outputs[PIDJointOffset], NumPIDJoints);
00042 encode(&buf,NumSensors);
00043 encode(&buf,state->sensors,NumSensors);
00044 encode(&buf,NumButtons);
00045 encode(&buf,state->buttons,NumButtons);
00046 encode(&buf,state->pidduties,NumPIDJoints);
00047 wsJoints->write((NumPIDJoints*2+NumSensors+NumButtons)*sizeof(float)+4*sizeof(unsigned int));
00048 }
00049 }
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060