00001 #include "SegCamBehavior.h"
00002 #include "Wireless/Wireless.h"
00003 #include "Events/EventRouter.h"
00004 #include "Shared/Config.h"
00005 #include "Events/FilterBankEvent.h"
00006 #include "Behaviors/Controller.h"
00007 #include "Shared/ProjectInterface.h"
00008 #include "Vision/SegmentedColorGenerator.h"
00009 #include "Vision/RLEGenerator.h"
00010
00011 SegCamBehavior::SegCamBehavior()
00012 : BehaviorBase(), visRLE(NULL), packet(NULL), cur(NULL), avail(0)
00013 {
00014 }
00015
00016 void
00017 SegCamBehavior::DoStart() {
00018 BehaviorBase::DoStart();
00019 visRLE=wireless->socket(SocketNS::SOCK_STREAM, 1024, WIRELESS_BUFFER_SIZE);
00020 wireless->setDaemon(visRLE,true);
00021 wireless->listen(visRLE,config->vision.rle_port);
00022
00023
00024
00025
00026
00027
00028 erouter->addListener(this,EventBase::visSegmentEGID,ProjectInterface::visSegmentSID);
00029 erouter->addListener(this,EventBase::visRLEEGID,ProjectInterface::visRLESID);
00030 }
00031
00032 void
00033 SegCamBehavior::DoStop() {
00034 erouter->forgetListener(this);
00035
00036
00037
00038
00039 wireless->setDaemon(visRLE,false);
00040 wireless->close(visRLE->sock);
00041 BehaviorBase::DoStop();
00042 }
00043
00044 void
00045 SegCamBehavior::processEvent(const EventBase& e) {
00046 if(!wireless->isConnected(visRLE->sock))
00047 return;
00048 const FilterBankEvent* fbke=dynamic_cast<const FilterBankEvent*>(&e);
00049 ASSERTRET(fbke!=NULL,"unexpected event");
00050 if(config->vision.rlecam_compression==Config::vision_config::COMPRESS_NONE && e.getGeneratorID()==EventBase::visSegmentEGID) {
00051 bool succ=writeSeg(*fbke);
00052 ASSERTRET(succ,"serialization failed");
00053 }
00054 if(config->vision.rlecam_compression==Config::vision_config::COMPRESS_RLE && e.getGeneratorID()==EventBase::visRLEEGID) {
00055 bool succ=writeRLE(*fbke);
00056 ASSERTRET(succ,"serialization failed");
00057 }
00058 }
00059
00060 bool
00061 SegCamBehavior::openPacket(FilterBankGenerator& fbkgen, unsigned int time, unsigned int layer) {
00062 if(packet!=NULL)
00063 return false;
00064
00065 avail=WIRELESS_BUFFER_SIZE-1;
00066 ASSERT(cur==NULL,"cur non-NULL");
00067 cur=NULL;
00068 char * buf=packet=(char*)visRLE->getWriteBuffer(avail);
00069 ASSERTRETVAL(packet!=NULL,"could not get buffer",false);
00070
00071 unsigned int used;
00072 used=LoadSave::encode("TekkotsuImage",buf,avail);
00073 ASSERTRETVAL(used!=0,"ran out of space",false);
00074 avail-=used; buf+=used;
00075 used=LoadSave::encode(Config::vision_config::ENCODE_SINGLE_CHANNEL,buf,avail);
00076 ASSERTRETVAL(used!=0,"ran out of space",false);
00077 avail-=used; buf+=used;
00078 used=LoadSave::encode(Config::vision_config::COMPRESS_RLE,buf,avail);
00079 ASSERTRETVAL(used!=0,"ran out of space",false);
00080 avail-=used; buf+=used;
00081
00082 used=LoadSave::encode(fbkgen.getWidth(layer),buf,avail);
00083 ASSERTRETVAL(used!=0,"ran out of space",false);
00084 avail-=used; buf+=used;
00085 used=LoadSave::encode(fbkgen.getHeight(layer),buf,avail);
00086 ASSERTRETVAL(used!=0,"ran out of space",false);
00087 avail-=used; buf+=used;
00088 used=LoadSave::encode(time,buf,avail);
00089 ASSERTRETVAL(used!=0,"ran out of space",false);
00090 avail-=used; buf+=used;
00091 used=LoadSave::encode(fbkgen.getFrameNumber(),buf,avail);
00092 ASSERTRETVAL(used!=0,"ran out of space",false);
00093 avail-=used; buf+=used;
00094
00095 cur=buf;
00096 return true;
00097 }
00098
00099 bool
00100 SegCamBehavior::writeRLE(const FilterBankEvent& e) {
00101 FilterBankGenerator& fbkgen=*e.getSource();
00102
00103 unsigned int layer=fbkgen.getNumLayers()-1-config->vision.rlecam_skip;
00104 unsigned int used=0;
00105 openPacket(fbkgen,e.getTimeStamp(),layer);
00106 ASSERTRETVAL(cur!=NULL,"header failed",false);
00107
00108 RLEGenerator * rle = dynamic_cast<RLEGenerator*>(&fbkgen);
00109 ASSERTRETVAL(rle!=NULL,"fbkgen isn't an RLEGenerator",false);
00110
00111 rle->selectSaveImage(layer,config->vision.rlecam_channel);
00112 used=rle->SaveBuffer(cur,avail);
00113 ASSERTRETVAL(used!=0,"save rle image failed",false);
00114 avail-=used; cur+=used;
00115
00116
00117 const SegmentedColorGenerator * seg = dynamic_cast<const SegmentedColorGenerator*>(rle->getSourceGenerator());
00118 ASSERTRETVAL(seg!=NULL,"RLE's source is not a SegmentedColorGenerator - how do i know what the colors are?",false);
00119 if(0==(used=seg->encodeColors(cur,avail))) return false;
00120 avail-=used; cur+=used;
00121
00122 closePacket();
00123
00124 return true;
00125 }
00126
00127 bool
00128 SegCamBehavior::writeSeg(const FilterBankEvent& e) {
00129 FilterBankGenerator& fbkgen=*e.getSource();
00130
00131 unsigned int layer=fbkgen.getNumLayers()-1-config->vision.rlecam_skip;
00132 unsigned int used=0;
00133 openPacket(fbkgen,e.getTimeStamp(),layer);
00134 ASSERTRETVAL(cur!=NULL,"header failed",false);
00135
00136 fbkgen.selectSaveImage(layer,config->vision.rlecam_channel);
00137 used=fbkgen.SaveBuffer(cur,avail);
00138 ASSERTRETVAL(used!=0,"save seg image failed",false);
00139 avail-=used; cur+=used;
00140
00141 closePacket();
00142
00143 return true;
00144 }
00145
00146 void
00147 SegCamBehavior::closePacket() {
00148 if(packet==NULL)
00149 return;
00150 visRLE->write(cur-packet);
00151 packet=cur=NULL;
00152 avail=0;
00153 }
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166