[Personalrobots-commit] SF.net SVN: personalrobots: [139] pkg/trunk/gmapping

[<ge...@us...>]

Revision: 139
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=139&view=rev
Author:   gerkey
Date:     2008-04-18 13:31:13 -0700 (Fri, 18 Apr 2008)

Log Message:
-----------
working on extracting map

Modified Paths:
--------------
    pkg/trunk/gmapping/manual.mk
    pkg/trunk/gmapping/playerwrapper/playergfswrapper.cpp

Modified: pkg/trunk/gmapping/manual.mk
===================================================================
--- pkg/trunk/gmapping/manual.mk	2008-04-18 19:46:20 UTC (rev 138)
+++ pkg/trunk/gmapping/manual.mk	2008-04-18 20:31:13 UTC (rev 139)
@@ -1,6 +1,6 @@
 #CPPFLAGS+= -DNDEBUG 
-#CXXFLAGS+= -O3 -Wall -ffast-math
-CXXFLAGS+= -g -O0 -Wall 
+CXXFLAGS+= -O3 -Wall -ffast-math
+#CXXFLAGS+= -g -O0 -Wall 
 PROFILE= false
 
 

Modified: pkg/trunk/gmapping/playerwrapper/playergfswrapper.cpp
===================================================================
--- pkg/trunk/gmapping/playerwrapper/playergfswrapper.cpp	2008-04-18 19:46:20 UTC (rev 138)
+++ pkg/trunk/gmapping/playerwrapper/playergfswrapper.cpp	2008-04-18 20:31:13 UTC (rev 139)
@@ -166,8 +166,8 @@
   }
 
   // start GFS thread
-  //this->gsp->start(this);
-  pthread_create(&this->gui_thread, 0, GUI_thread, this);
+  this->gsp->start(this);
+  //pthread_create(&this->gui_thread, 0, GUI_thread, this);
 
   this->StartThread();
   return(0);
@@ -204,7 +204,52 @@
 
     this->ProcessMessages();
 
-    // TODO: check for new pose / map from GFS thread
+    // check for new pose / map from GFS thread
+    GridSlamProcessorThread::EventDeque events=this->gsp->getEvents();
+    for(GridSlamProcessorThread::EventDeque::const_iterator it=events.begin(); 
+        it!=events.end();
+        it++)
+    {
+      GridSlamProcessorThread::MapEvent* mapEvent = 
+              dynamic_cast<GridSlamProcessorThread::MapEvent*>(*it);
+      if(mapEvent)
+      {
+        int sx, sy;
+        double res;
+        sx = mapEvent->pmap->getMapSizeX();
+        sy = mapEvent->pmap->getMapSizeY();
+        res = mapEvent->pmap->getResolution();
+        printf("got a %d X %d map @ %.3f m / pix\n", sx,sy,res);
+        double v;
+        for(int j=0;j<sy;j++)
+        {
+          for(int i=0;i<sx;i++)
+          {
+            v = mapEvent->pmap->cell(i,j);
+            if (v>=0)
+            {
+              int grayValue=255-(int)(255.*v);
+            }
+          }
+        }
+      }
+      else
+      {
+        GridSlamProcessorThread::DoneEvent* doneEvent = 
+                dynamic_cast<GridSlamProcessorThread::DoneEvent*>(*it);
+        if(doneEvent)
+        {
+          this->gsp->stop();
+          delete doneEvent;
+        } 
+        else
+        {
+          // TODO: handle other events somehow
+          //history.push_back(*it);
+        }
+      }	
+
+    }
   }
 }
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.