00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include "NeovisionII/SimulationViewerService.H"
00039 #include "Util/sformat.H"
00040 #include "Image/DrawOps.H"
00041 #include "Raster/Raster.H"
00042 #include "Media/FrameSeries.H"
00043 #include "Transport/FrameInfo.H"
00044 #include "Raster/GenericFrame.H"
00045 #include "GUI/ImageDisplayStream.H"
00046 #include "GUI/XWinManaged.H"
00047 #include "GUI/DebugWin.H"
00048 #include "Util/MathFunctions.H"
00049
00050
00051 using namespace std;
00052 using namespace ImageIceMod;
00053
00054 SimulationViewerI::SimulationViewerI(ModelManager& mgr,
00055 nub::ref<OutputFrameSeries> ofs,
00056 const std::string& descrName,
00057 const std::string& tagName) :
00058 ModelComponent(mgr, descrName, tagName),
00059 itsOfs(ofs)
00060 {
00061
00062 IceStorm::TopicPrx topicPrx;
00063 itsTopicsSubscriptions.push_back(TopicInfo("RetinaTopic", topicPrx));
00064 itsTopicsSubscriptions.push_back(TopicInfo("VisualCortexTopic", topicPrx));
00065 itsTopicsSubscriptions.push_back(TopicInfo("SaliencyMapTopic", topicPrx));
00066 itsTopicsSubscriptions.push_back(TopicInfo("VisualTrackerTopic", topicPrx));
00067 itsTopicsSubscriptions.push_back(TopicInfo("SegmenterTopic", topicPrx));
00068 itsTopicsSubscriptions.push_back(TopicInfo("InferotemporalCortexTopic", topicPrx));
00069 itsTopicsSubscriptions.push_back(TopicInfo("PrefrontalCortexTopic", topicPrx));
00070 itsTopicsSubscriptions.push_back(TopicInfo("HippocampusTopic", topicPrx));
00071
00072 itsDiceImg.reset(6);
00073 for(int i=0; i<6; i++)
00074 {
00075 char filename[255];
00076 sprintf(filename, "./etc/dice/120px-Dice-%i.png", i+1);
00077 itsDiceImg[i] = Raster::ReadRGB(filename);
00078 }
00079
00080 }
00081
00082 SimulationViewerI::~SimulationViewerI()
00083 {
00084 unsubscribeSimEvents();
00085 }
00086
00087
00088 void SimulationViewerI::initSimEvents(Ice::CommunicatorPtr icPtr, Ice::ObjectPrx objectPrx)
00089 {
00090
00091 Ice::ObjectPrx obj = icPtr->stringToProxy("SimEvents/TopicManager:tcp -p 11111");
00092 IceStorm::TopicManagerPrx topicManager =
00093 IceStorm::TopicManagerPrx::checkedCast(obj);
00094
00095
00096 IceStorm::TopicPrx topic;
00097 try {
00098 topic = topicManager->retrieve("GUITopic");
00099 } catch (const IceStorm::NoSuchTopic&) {
00100 topic = topicManager->create("GUITopic");
00101 }
00102
00103 Ice::ObjectPrx pub = topic->getPublisher()->ice_oneway();
00104 itsEventsPub = SimEvents::EventsPrx::uncheckedCast(pub);
00105
00106
00107 itsObjectPrx = objectPrx;
00108
00109 for(uint i=0; i<itsTopicsSubscriptions.size(); i++)
00110 {
00111 try {
00112 IceStorm::QoS qos;
00113 itsTopicsSubscriptions[i].topicPrx =
00114 topicManager->retrieve(itsTopicsSubscriptions[i].name.c_str());
00115 itsTopicsSubscriptions[i].topicPrx->subscribeAndGetPublisher(qos, itsObjectPrx);
00116 } catch (const IceStorm::NoSuchTopic&) {
00117 LINFO("Error! No %s topic found!", itsTopicsSubscriptions[i].name.c_str());
00118 } catch (const Ice::Exception& e) {
00119 cerr << e << endl;
00120 }
00121
00122 }
00123 }
00124
00125 void SimulationViewerI::unsubscribeSimEvents()
00126 {
00127
00128 for(uint i=0; i<itsTopicsSubscriptions.size(); i++)
00129 {
00130 itsTopicsSubscriptions[i].topicPrx->unsubscribe(itsObjectPrx);
00131 }
00132 }
00133
00134
00135 void SimulationViewerI::evolve(const SimEvents::EventMessagePtr& eMsg,
00136 const Ice::Current&)
00137 {
00138 if(eMsg->ice_isA("::SimEvents::RetinaMessage")){
00139 SimEvents::RetinaMessagePtr msg = SimEvents::RetinaMessagePtr::dynamicCast(eMsg);
00140 itsRetinaImg = Ice2Image<PixRGB<byte> > (msg->img);
00141 itsMarkupImg = itsRetinaImg;
00142 } else if (eMsg->ice_isA("::SimEvents::VisualCortexMessage")){
00143 SimEvents::VisualCortexMessagePtr msg = SimEvents::VisualCortexMessagePtr::dynamicCast(eMsg);
00144 itsVCX = Ice2Image<byte>(msg->vco);
00145 } else if (eMsg->ice_isA("::SimEvents::SaliencyMapMessage")){
00146 SimEvents::SaliencyMapMessagePtr msg = SimEvents::SaliencyMapMessagePtr::dynamicCast(eMsg);
00147 itsSMap = Ice2Image<byte>(msg->smap);
00148
00149 if (itsMarkupImg.initialized())
00150 {
00151 for(uint i=0; i<msg->nMostSalientLoc.size(); i++)
00152 {
00153 SimEvents::LocInfo locInfo = msg->nMostSalientLoc[i];
00154
00155 if (i == 0)
00156 {
00157 drawRectSquareCorners(itsMarkupImg,
00158 Rectangle(Point2D<int>(locInfo.fullresMaxpos.i, locInfo.fullresMaxpos.j), Dims(3,3)),
00159 PixRGB<byte>(255, 0, 0), 6);
00160 } else {
00161 drawRectSquareCorners(itsMarkupImg,
00162 Rectangle(Point2D<int>(locInfo.fullresMaxpos.i, locInfo.fullresMaxpos.j), Dims(3,3)),
00163 PixRGB<byte>(150, 0, 0), 3);
00164 }
00165 }
00166 }
00167 } else if (eMsg->ice_isA("::SimEvents::VisualTrackerMessage")){
00168 SimEvents::VisualTrackerMessagePtr msg = SimEvents::VisualTrackerMessagePtr::dynamicCast(eMsg);
00169
00170 if (itsMarkupImg.initialized())
00171 {
00172 for(uint i=0; i<msg->trackLocs.size(); i++)
00173 {
00174 SimEvents::TrackInfo trackInfo = msg->trackLocs[i];
00175 drawRectSquareCorners(itsMarkupImg,
00176 Rectangle(Point2D<int>(trackInfo.pos.i, trackInfo.pos.j), Dims(3,3)),
00177 PixRGB<byte>(0, 255, 0), 3);
00178 }
00179 }
00180 } else if (eMsg->ice_isA("::SimEvents::SegmenterMessage")){
00181 SimEvents::SegmenterMessagePtr msg = SimEvents::SegmenterMessagePtr::dynamicCast(eMsg);
00182
00183 if (itsMarkupImg.initialized())
00184 {
00185 for(uint i=0; i<msg->segLocs.size(); i++)
00186 {
00187 SimEvents::SegInfo segInfo = msg->segLocs[i];
00188 Rectangle segRect(Point2D<int>(segInfo.rect.tl.i, segInfo.rect.tl.j),
00189 Dims(segInfo.rect.br.i - segInfo.rect.tl.i,
00190 segInfo.rect.br.j - segInfo.rect.tl.j));
00191 if (segRect.isValid() && itsMarkupImg.rectangleOk(segRect))
00192 {
00193 drawRect(itsMarkupImg, segRect, PixRGB<byte>(255, 255, 0), 1);
00194 }
00195 itsSegImg = Ice2Image<PixRGB<byte> >(segInfo.img);
00196 }
00197 }
00198 } else if (eMsg->ice_isA("::SimEvents::InfrotemporalCortexMessage")){
00199 SimEvents::InfrotemporalCortexMessagePtr msg = SimEvents::InfrotemporalCortexMessagePtr::dynamicCast(eMsg);
00200
00201
00202 if (itsMarkupImg.initialized())
00203 {
00204 const std::string txt =
00205 sformat("%s:%0.2f", msg->objectName.c_str(), msg->confidence);
00206
00207 writeText(itsMarkupImg, Point2D<int>(msg->objLoc.i, msg->objLoc.j),
00208 txt.c_str(),
00209 PixRGB<byte>(255), PixRGB<byte>(0));
00210 }
00211 } else if (eMsg->ice_isA("::SimEvents::GUIInputMessage")){
00212 SimEvents::GUIInputMessagePtr msg = SimEvents::GUIInputMessagePtr::dynamicCast(eMsg);
00213 itsCurrentMsg = msg->msg;
00214 } else if(eMsg->ice_isA("::SimEvents::HippocampusMessage")){
00215 SimEvents::HippocampusMessagePtr hMsg = SimEvents::HippocampusMessagePtr::dynamicCast(eMsg);
00216
00217
00218 itsMutex.lock();
00219 itsObjectsState.clear();
00220 for(uint i=0; i<hMsg->objectsState.size(); i++)
00221 {
00222 SimEvents::ObjectState objectState = hMsg->objectsState[i];
00223
00224
00225 ObjectState objState;
00226 objState.objName = objectState.objectName;
00227 objState.objMsg = objectState.objectMsg;
00228 objState.loc.i = objectState.loc.i;
00229 objState.loc.j = objectState.loc.j;
00230 objState.pos.x = objectState.pos.x/2;
00231 objState.pos.y = objectState.pos.y/2;
00232 objState.pos.z = objectState.pos.z;
00233 objState.rotation = objectState.rotation;
00234 objState.size = objectState.size;
00235 itsObjectsState.push_back(objState);
00236 }
00237 itsMutex.unlock();
00238 }
00239
00240
00241 }
00242
00243 int SimulationViewerI::getKey(nub::ref<OutputFrameSeries> &ofs)
00244 {
00245 const nub::soft_ref<ImageDisplayStream> ids =
00246 ofs->findFrameDestType<ImageDisplayStream>();
00247
00248 const rutz::shared_ptr<XWinManaged> uiwin =
00249 ids.is_valid()
00250 ? ids->getWindow("GameInterface")
00251 : rutz::shared_ptr<XWinManaged>();
00252
00253 if (uiwin.is_valid())
00254 return uiwin->getLastKeyPress();
00255 else
00256 return -1;
00257 }
00258
00259 Point2D<int> SimulationViewerI::getMouseClick(nub::ref<OutputFrameSeries> &ofs, const char* wname)
00260 {
00261 const nub::soft_ref<ImageDisplayStream> ids =
00262 ofs->findFrameDestType<ImageDisplayStream>();
00263
00264 const rutz::shared_ptr<XWinManaged> uiwin =
00265 ids.is_valid()
00266 ? ids->getWindow(wname)
00267 : rutz::shared_ptr<XWinManaged>();
00268
00269 if (uiwin.is_valid())
00270 return uiwin->getLastMouseClick();
00271 else
00272 return Point2D<int>(-1,-1);
00273 }
00274
00275
00276 void SimulationViewerI::run()
00277 {
00278 initRandomNumbers();
00279 int rollDice=0;
00280
00281 Image<PixRGB<byte> > gameInterface(640,240,ZEROS);
00282 gameInterface.clear(PixRGB<byte>(255,255,255));
00283 inplacePaste(gameInterface, itsDiceImg[5], Point2D<int>(320/2-(itsDiceImg[5].getWidth()/2),0));
00284
00285 while(1)
00286 {
00287 Layout<PixRGB<byte> > outDisp;
00288
00289 if (itsMarkupImg.initialized())
00290 itsOfs->writeRGB(itsMarkupImg, "Markup", FrameInfo("Markup", SRC_POS));
00291
00292 Point2D<int> clickLoc = getMouseClick(itsOfs, "Markup");
00293 if (clickLoc.isValid())
00294 {
00295 LINFO("Click at %i %i", clickLoc.i, clickLoc.j);
00296 SimEvents::GUIOutputMessagePtr goMsg = new SimEvents::GUIOutputMessage;
00297 goMsg->key = -1;
00298 goMsg->loc.i = clickLoc.i;
00299 goMsg->loc.j = clickLoc.j;
00300 goMsg->dice = -1;
00301 itsEventsPub->evolve(goMsg);
00302 }
00303
00304
00305
00306
00307
00308 if (itsRetinaImg.initialized())
00309 outDisp = itsMarkupImg;
00310
00311 if (itsVCX.initialized() && itsRetinaImg.initialized())
00312 outDisp = vcat(outDisp, toRGB(rescale(itsVCX, itsRetinaImg.getDims())));
00313
00314 if (itsSMap.initialized() &&
00315 itsRetinaImg.initialized() &&
00316 itsRetinaImg.getWidth() > 0 &&
00317 itsRetinaImg.getHeight() > 0 &&
00318 itsSMap.getWidth() > 0 &&
00319 itsSMap.getHeight() > 0 )
00320 outDisp = vcat(outDisp, toRGB(rescale(itsSMap, itsRetinaImg.getDims())));
00321
00322
00323
00324
00325 itsMutex.lock();
00326 std::vector<ObjectState> tmpObjects = itsObjectsState;
00327 itsMutex.unlock();
00328
00329 Image<PixRGB<byte> > stmMap(512,512, ZEROS);
00330 for(uint i=0; i<tmpObjects.size(); i++)
00331 {
00332 ObjectState objState = tmpObjects[i];
00333
00334 Point2D<int> loc( (stmMap.getWidth()/2) + objState.pos.x, stmMap.getHeight() - objState.pos.y);
00335 drawCircle(stmMap, loc,
00336 (int)objState.size, PixRGB<byte>(0,255,0),2);
00337 writeText(stmMap, loc, objState.objName.c_str(),
00338 PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0));
00339 drawLine(stmMap, loc,
00340 objState.rotation, objState.size*3, PixRGB<byte>(255,0,0));
00341
00342 if (objState.objName == "soldier")
00343 {
00344 int length = stmMap.getWidth()-loc.i;
00345 drawRect(stmMap, Rectangle(Point2D<int>(loc.i,loc.j-30), Dims(length, 60)), PixRGB<byte>(0,255,0));
00346 }
00347
00348 writeText(stmMap, Point2D<int>(0,0), objState.objMsg.c_str(),
00349 PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0));
00350
00351 }
00352
00353 outDisp = hcat(outDisp, stmMap);
00354
00355
00356 if (outDisp.initialized())
00357 {
00358 itsOfs->writeRgbLayout(outDisp, "Output", FrameInfo("Output", SRC_POS));
00359 itsOfs->updateNext();
00360 }
00361
00362
00363 if (rollDice)
00364 {
00365 int num = randomUpToIncluding(5);
00366 inplacePaste(gameInterface, itsDiceImg[num], Point2D<int>(320/2-(itsDiceImg[num].getWidth()/2),0));
00367 rollDice--;
00368 if (rollDice == 0)
00369 LINFO("Final Number %i", num+1);
00370 }
00371
00372 if (itsCurrentMsg.size())
00373 {
00374 itsCurrentMsg += " ";
00375 writeText(gameInterface, Point2D<int>(0, 200), itsCurrentMsg.c_str(),
00376 PixRGB<byte>(0,255,0), PixRGB<byte>(255,255,255));
00377 itsCurrentMsg.clear();
00378 }
00379
00380 itsOfs->writeRGB(gameInterface, "GameInterface", FrameInfo("GameInterface", SRC_POS));
00381
00382 int key = getKey(itsOfs);
00383 if (key != -1)
00384 {
00385 switch(key)
00386 {
00387 case 27:
00388 rollDice = 10;
00389 break;
00390 default:
00391 {
00392 SimEvents::GUIOutputMessagePtr goMsg = new SimEvents::GUIOutputMessage;
00393 goMsg->key = key;
00394 goMsg->loc.i = -1;
00395 goMsg->loc.j = -1;
00396 goMsg->dice = -1;
00397 itsEventsPub->evolve(goMsg);
00398 }
00399 break;
00400 }
00401 }
00402
00403 usleep(10000);
00404 }
00405 }
00406
00407
00408
00409 class SimulationViewerService : public Ice::Service {
00410 protected:
00411 virtual bool start(int, char* argv[]);
00412 virtual bool stop()
00413 {
00414 if (itsMgr)
00415 delete itsMgr;
00416 return true;
00417 }
00418 private:
00419 Ice::ObjectAdapterPtr itsAdapter;
00420 ModelManager *itsMgr;
00421 };
00422
00423 bool SimulationViewerService::start(int argc, char* argv[])
00424 {
00425
00426 itsMgr = new ModelManager("SimulationViewerService");
00427
00428 nub::ref<OutputFrameSeries> ofs(new OutputFrameSeries(*itsMgr));
00429 itsMgr->addSubComponent(ofs);
00430
00431 nub::ref<SimulationViewerI> simulationViewer(new SimulationViewerI(*itsMgr, ofs));
00432 itsMgr->addSubComponent(simulationViewer);
00433
00434 itsMgr->parseCommandLine((const int)argc, (const char**)argv, "", 0, 0);
00435
00436 char adapterStr[255];
00437 sprintf(adapterStr, "default -p %i", BrainObjects::SimulationViewerPort);
00438 itsAdapter = communicator()->createObjectAdapterWithEndpoints(
00439 "SimulationViewerAdapter", adapterStr);
00440
00441 Ice::ObjectPtr object = simulationViewer.get();
00442 Ice::ObjectPrx objectPrx = itsAdapter->add(object, communicator()->stringToIdentity("SimulationViewer"))->ice_oneway();
00443
00444 simulationViewer->initSimEvents(communicator(), objectPrx);
00445 itsAdapter->activate();
00446
00447 itsMgr->start();
00448
00449
00450 IceUtil::ThreadPtr svThread = simulationViewer.get();
00451 svThread->start();
00452 return true;
00453 }
00454
00455
00456
00457 int main(int argc, char** argv) {
00458
00459 SimulationViewerService svc;
00460 return svc.main(argc, argv);
00461 }
00462
1.6.3