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