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