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

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

Revision: 134
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=134&view=rev
Author:   gerkey
Date:     2008-04-17 19:26:32 -0700 (Thu, 17 Apr 2008)

Log Message:
-----------
added pthread condition variable logics to sync up mapper and driver thread

Modified Paths:
--------------
    pkg/trunk/gmapping/build_tools/Makefile.generic-shared-object
    pkg/trunk/gmapping/gui/gsp_thread.cpp
    pkg/trunk/gmapping/playerwrapper/examples/readlog.cfg
    pkg/trunk/gmapping/playerwrapper/playergfswrapper.cpp
    pkg/trunk/gmapping/playerwrapper/playergfswrapper.h

Modified: pkg/trunk/gmapping/build_tools/Makefile.generic-shared-object
===================================================================
--- pkg/trunk/gmapping/build_tools/Makefile.generic-shared-object	2008-04-18 02:26:07 UTC (rev 133)
+++ pkg/trunk/gmapping/build_tools/Makefile.generic-shared-object	2008-04-18 02:26:32 UTC (rev 134)
@@ -33,6 +33,7 @@
 	@$(MESSAGE) "Creating library lib$(LIBNAME).so"
 ifeq ($(MACOSX),1)
 	@$(PRETTY) "$(CXX) $(LDFLAGS) -dynamiclib $(OBJS) $(COBJS) -L$(LIBDIR) $(LIBS) -install_name $@ -o $@"
+	ln -sf $@ $(LIBDIR)/lib$(LIBNAME).so
 endif
 ifeq ($(LINUX),1)
 	@$(PRETTY) "$(CXX) $(LDFLAGS) -fPIC -shared $(OBJS) $(COBJS) -L $(LIBDIR) $(LIBS) -o $@"

Modified: pkg/trunk/gmapping/gui/gsp_thread.cpp
===================================================================
--- pkg/trunk/gmapping/gui/gsp_thread.cpp	2008-04-18 02:26:07 UTC (rev 133)
+++ pkg/trunk/gmapping/gui/gsp_thread.cpp	2008-04-18 02:26:32 UTC (rev 134)
@@ -430,13 +430,22 @@
   #ifdef PLAYER_SUPPORT
 	if (gpt->onLine){
 		while (1){
-                  RangeReading* nr;
-                  while((nr = gpt->m_pwrapper->getReading()))
-                  {
-                    gpt->processScan(*nr);
-                    delete nr;
+                  // getReading() will block until data is available
+                  RangeReading* nr = gpt->m_pwrapper->getReading();
+
+                  gpt->processScan(*nr);
+                  const OdometryReading* o=dynamic_cast<const OdometryReading*>(nr);
+                  if (o && gpt->running){
+                    gpt->processTruePos(*o);
+                    TruePosEvent* truepos=new TruePosEvent;
+                    truepos->pose=o->getPose();
                   }
-                  usleep(1000);
+
+                  // Should we delete this reading, or does the mapper
+                  // still need it?
+                  //delete nr;
+
+                  // Give ourselves a chance to be canceled.
                   pthread_testcancel();
                 }
         }

Modified: pkg/trunk/gmapping/playerwrapper/examples/readlog.cfg
===================================================================
--- pkg/trunk/gmapping/playerwrapper/examples/readlog.cfg	2008-04-18 02:26:07 UTC (rev 133)
+++ pkg/trunk/gmapping/playerwrapper/examples/readlog.cfg	2008-04-18 02:26:32 UTC (rev 134)
@@ -13,12 +13,14 @@
  name "laserposeinterpolator"
  requires ["laser:0" "position2d:0"]
  provides ["laser:1"]
+ update_thresh [0.5 10.0]
+ send_all_scans 0
 )
 
 driver
 (
  name "playergfswrapper"
- plugin "libplayerwrapper"
+ plugin "../../lib/libplayerwrapper"
  requires [ "laser:1"]
  provides ["position2d:1"]
  alwayson 1

Modified: pkg/trunk/gmapping/playerwrapper/playergfswrapper.cpp
===================================================================
--- pkg/trunk/gmapping/playerwrapper/playergfswrapper.cpp	2008-04-18 02:26:07 UTC (rev 133)
+++ pkg/trunk/gmapping/playerwrapper/playergfswrapper.cpp	2008-04-18 02:26:32 UTC (rev 134)
@@ -150,6 +150,8 @@
   }
 
   pthread_mutex_init(&this->rangeDeque_mutex, NULL);
+  pthread_cond_init(&this->rangeDeque_cond, NULL);
+  pthread_mutex_init(&this->rangeDeque_cond_mutex, NULL);
 
   // TODO: get laser geometry
 
@@ -182,6 +184,8 @@
   delete this->gsp;
 
   pthread_mutex_destroy(&this->rangeDeque_mutex);
+  pthread_mutex_destroy(&this->rangeDeque_cond_mutex);
+  pthread_cond_destroy(&this->rangeDeque_cond);
 
   return(0);
 }
@@ -204,16 +208,31 @@
 GMapping::RangeReading*
 PlayerGFSWrapper::getReading()
 {
-  GMapping::RangeReading* ret = NULL;
+  // Do we need to wait?
+  bool needtowait;
   pthread_mutex_lock(&this->rangeDeque_mutex);
-  if(!this->rangeDeque.empty())
+  needtowait = this->rangeDeque.empty();
+  pthread_mutex_unlock(&this->rangeDeque_mutex);
+
+  if(needtowait)
   {
-    ret=this->rangeDeque.front();
-    this->rangeDeque.pop_front();
+    pthread_cleanup_push((void(*)(void*))pthread_mutex_unlock,
+                         (void*)&this->rangeDeque_cond_mutex);
+    pthread_mutex_lock(&this->rangeDeque_cond_mutex);
+    pthread_cond_wait(&this->rangeDeque_cond,&this->rangeDeque_cond_mutex);
+    pthread_mutex_unlock(&this->rangeDeque_cond_mutex);
+    pthread_cleanup_pop(0);
   }
+
+  // Now there must be data
+  pthread_mutex_lock(&this->rangeDeque_mutex);
+  assert(!this->rangeDeque.empty());
+
+  // remove the reading
+  GMapping::RangeReading* ret=this->rangeDeque.front();
+  this->rangeDeque.pop_front();
+
   pthread_mutex_unlock(&this->rangeDeque_mutex);
-  if(ret)
-    puts("returned a reading");
   return(ret);
 }
 
@@ -268,6 +287,12 @@
 
   pthread_mutex_lock(&this->rangeDeque_mutex);
   this->rangeDeque.push_back(reading);
+
+  // Signal that data is available on the queue
+  pthread_mutex_lock(&this->rangeDeque_cond_mutex);
+  pthread_cond_broadcast(&this->rangeDeque_cond);
+  pthread_mutex_unlock(&this->rangeDeque_cond_mutex);
+
   printf("queue size: %d\n", this->rangeDeque.size());
   pthread_mutex_unlock(&this->rangeDeque_mutex);
 }

Modified: pkg/trunk/gmapping/playerwrapper/playergfswrapper.h
===================================================================
--- pkg/trunk/gmapping/playerwrapper/playergfswrapper.h	2008-04-18 02:26:07 UTC (rev 133)
+++ pkg/trunk/gmapping/playerwrapper/playergfswrapper.h	2008-04-18 02:26:32 UTC (rev 134)
@@ -57,6 +57,8 @@
     // Queue of range readings, with mutex
     std::deque<RangeReading*> rangeDeque;
     pthread_mutex_t rangeDeque_mutex;
+    pthread_cond_t rangeDeque_cond;
+    pthread_mutex_t rangeDeque_cond_mutex;
 
     void ProcessLaser(player_msghdr_t* hdr,
                       player_laser_data_scanpose_t* data);


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