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