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