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