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

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

Revision: 117
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=117&view=rev
Author:   gerkey
Date:     2008-04-17 12:51:25 -0700 (Thu, 17 Apr 2008)

Log Message:
-----------


Modified Paths:
--------------
    pkg/trunk/gmapping/gui/gsp_thread.cpp
    pkg/trunk/gmapping/gui/gsp_thread.h
    pkg/trunk/gmapping/utils/gvalues.h

Added Paths:
-----------
    pkg/trunk/gmapping/playerwrapper/
    pkg/trunk/gmapping/playerwrapper/Makefile
    pkg/trunk/gmapping/playerwrapper/playergfswrapper.cpp
    pkg/trunk/gmapping/playerwrapper/playergfswrapper.h

Modified: pkg/trunk/gmapping/gui/gsp_thread.cpp
===================================================================

--- pkg/trunk/gmapping/gui/gsp_thread.cpp	2008-04-17 19:49:41 UTC (rev 116)
+++ pkg/trunk/gmapping/gui/gsp_thread.cpp	2008-04-17 19:51:25 UTC (rev 117)
@@ -504,6 +504,12 @@
 	pthread_create(&gfs_thread, 0, (void * (*)(void *))fastslamthread, (void *) this);
 }
 
+void GridSlamProcessorThread::start(PlayerGFSWrapper* pwrapper)
+{
+  m_pwrapper = pwrapper;
+  start();
+}
+
 void GridSlamProcessorThread::stop(){
 	if (! running){
 		cout << "PORCO CAZZO" << endl;

Modified: pkg/trunk/gmapping/gui/gsp_thread.h
===================================================================
--- pkg/trunk/gmapping/gui/gsp_thread.h	2008-04-17 19:49:41 UTC (rev 116)
+++ pkg/trunk/gmapping/gui/gsp_thread.h	2008-04-17 19:51:25 UTC (rev 117)
@@ -36,6 +36,11 @@
 using namespace std;
 using namespace GMapping;
 
+#ifdef PLAYER_SUPPORT
+namespace GMapping {
+class PlayerGFSWrapper;
+}
+#endif
 
 #define MAX_STRING_LENGTH 1024
 
@@ -82,6 +87,10 @@
 		
 		EventDeque getEvents();
 		
+#ifdef PLAYER_SUPPORT
+                PlayerGFSWrapper* m_pwrapper;
+		void start(PlayerGFSWrapper* pwrapper);
+#endif
 		void start();
 		void stop();
 		

Added: pkg/trunk/gmapping/playerwrapper/Makefile
===================================================================
--- pkg/trunk/gmapping/playerwrapper/Makefile	                        (rev 0)
+++ pkg/trunk/gmapping/playerwrapper/Makefile	2008-04-17 19:51:25 UTC (rev 117)
@@ -0,0 +1,16 @@
+OBJS= playergfswrapper.o
+APPS= 
+
+#LIBS+= -L$(CARMEN_HOME)/lib -lnavigator_interface -lsimulator_interface -lrobot_interface -llaser_interface -lparam_interface -lglobal -lipc -lm -lutils -lsensor_range -llog -lscanmatcher -lpthread -lz
+LIBS+= `pkg-config --libs playercore` -lsensor_range
+#-L$(CARMEN_HOME)/lib -lnavigator_interface -lsimulator_interface -lrobot_interface -llaser_interface -lparam_interface -lglobal -lipc -lm -lutils -lsensor_range -llog -lscanmatcher -lpthread -lz
+CPPFLAGS+=-I../sensor `pkg-config --cflags playercore`
+
+-include ../global.mk
+
+ifeq ($(PLAYERSUPPORT), 0)
+OBJS=
+	-include ../build_tools/Makefile.app
+else
+	-include ../build_tools/Makefile.generic-shared-object
+endif

Added: pkg/trunk/gmapping/playerwrapper/playergfswrapper.cpp
===================================================================
--- pkg/trunk/gmapping/playerwrapper/playergfswrapper.cpp	                        (rev 0)
+++ pkg/trunk/gmapping/playerwrapper/playergfswrapper.cpp	2008-04-17 19:51:25 UTC (rev 117)
@@ -0,0 +1,178 @@
+/*****************************************************************
+ *
+ * This file is part of the GMAPPING project
+ *
+ * Player wrapper code (c) 2008 Brian Gerkey
+ *
+ * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * Cyrill Stachniss, and Wolfram Burgard
+ *
+ * This software is licensed under the "Creative Commons 
+ * License (Attribution-NonCommercial-ShareAlike 2.0)" 
+ * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
+ * and Wolfram Burgard.
+ * 
+ * Further information on this license can be found at:
+ * http://creativecommons.org/licenses/by-nc-sa/2.0/
+ * 
+ * GMAPPING is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ *
+ *****************************************************************/
+
+
+#include "playergfswrapper.h"
+
+using namespace GMapping;
+
+PlayerGFSWrapper::PlayerGFSWrapper(ConfigFile* cf, int section)
+    : Driver(cf, section)
+{
+  memset(&this->position_id, 0, sizeof(player_devaddr_t));
+  memset(&this->map_id, 0, sizeof(player_devaddr_t));
+
+  // Do we create a position interface?
+  if(cf->ReadDeviceAddr(&(this->position_id), section, "provides",
+                        PLAYER_POSITION2D_CODE, -1, NULL) == 0)
+  {
+    if(this->AddInterface(this->position_id))
+    {
+      this->SetError(-1);
+      return;
+    }
+  }
+  
+  // Do we create a map interface?
+  if(cf->ReadDeviceAddr(&(this->map_id), section, "provides",
+                        PLAYER_MAP_CODE, -1, NULL) == 0)
+  {
+    if(this->AddInterface(this->map_id))
+    {
+      this->SetError(-1);
+      return;
+    }
+  }
+
+  // Must have a laser device to read from
+  if(cf->ReadDeviceAddr(&(this->laser_id), section, "requires",
+                        PLAYER_LASER_CODE, -1, NULL) != 0)
+  {
+    PLAYER_ERROR("must have a laser device from which to read");
+    this->SetError(-1);
+    return;
+  }
+}
+
+int
+PlayerGFSWrapper::Setup()
+{
+  // Subscribe to the laser device.
+  if(!(this->laser = deviceTable->GetDevice(this->laser_id)))
+  {
+    PLAYER_ERROR("unable to locate suitable laser device");
+    return(-1);
+  }
+  if(this->laser->Subscribe(this->InQueue) != 0)
+  {
+    PLAYER_ERROR("unable to subscribe to laser device");
+    return(-1);
+  }
+
+  pthread_mutex_init(&this->rangeDeque_mutex, NULL);
+
+  // TODO: get laser geometry
+
+  this->gsp = new GridSlamProcessorThread();
+
+  // TODO: create this->laser_rs
+
+  // TODO: compute GFS "sensorMap"
+
+  // start GFS thread
+  this->gsp->start(this);
+
+  this->StartThread();
+  return(0);
+}
+
+int
+PlayerGFSWrapper::Shutdown()
+{
+  this->StopThread();
+
+  this->laser->Unsubscribe(this->InQueue);
+
+  // stop GFS thread
+  this->gsp->stop();
+
+  delete this->gsp;
+
+  pthread_mutex_destroy(&this->rangeDeque_mutex);
+
+  return(0);
+}
+
+void
+PlayerGFSWrapper::Main()
+{
+  for(;;)
+  {
+    this->ProcessMessages();
+
+    // TODO: check for new pose / map from GFS thread
+  }
+}
+
+bool
+PlayerGFSWrapper::getReading(RangeReading& reading)
+{
+  bool ret=false;
+  pthread_mutex_lock(&this->rangeDeque_mutex);
+  if(!this->rangeDeque.empty())
+  {
+    reading=this->rangeDeque.front();
+    this->rangeDeque.pop_front();
+    ret=true;
+  }
+  pthread_mutex_unlock(&this->rangeDeque_mutex);
+  return(ret);
+}
+
+int 
+PlayerGFSWrapper::ProcessMessage(QueuePointer & resp_queue,
+                                 player_msghdr * hdr,
+                                 void * data)
+{
+  // TODO: handle:
+  //   map requests
+  //
+  //   laser scanpose messages (convert and cache them for consumption by
+  //   GFS thread)
+  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
+                           PLAYER_LASER_DATA_SCANPOSE,
+                           this->laser_id))
+  {
+    this->ProcessLaser(hdr, (player_laser_data_scanpose_t*)data);
+    return(0);
+  }
+  else
+    return(-1);
+}
+
+void
+PlayerGFSWrapper::ProcessLaser(player_msghdr_t* hdr,
+                               player_laser_data_scanpose_t* data)
+{
+  RangeReading reading(this->laser_rs,hdr->timestamp);
+  for(unsigned int i=0;i<data->scan.ranges_count;i++)
+    reading[i] = data->scan.ranges[i];
+  reading.setPose(OrientedPoint(data->pose.px,
+                                data->pose.py,
+                                data->pose.pa));
+
+  pthread_mutex_lock(&this->rangeDeque_mutex);
+  this->rangeDeque.push_back(reading);
+  pthread_mutex_unlock(&this->rangeDeque_mutex);
+}

Added: pkg/trunk/gmapping/playerwrapper/playergfswrapper.h
===================================================================
--- pkg/trunk/gmapping/playerwrapper/playergfswrapper.h	                        (rev 0)
+++ pkg/trunk/gmapping/playerwrapper/playergfswrapper.h	2008-04-17 19:51:25 UTC (rev 117)
@@ -0,0 +1,151 @@
+/*****************************************************************
+ *
+ * This file is part of the GMAPPING project
+ *
+ * Player wrapper code (c) 2008 Brian Gerkey
+ *
+ * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * Cyrill Stachniss, and Wolfram Burgard
+ *
+ * This software is licensed under the "Creative Commons 
+ * License (Attribution-NonCommercial-ShareAlike 2.0)" 
+ * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
+ * and Wolfram Burgard.
+ * 
+ * Further information on this license can be found at:
+ * http://creativecommons.org/licenses/by-nc-sa/2.0/
+ * 
+ * GMAPPING is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ *
+ *****************************************************************/
+
+
+#ifndef PLAYERWRAPPER_H
+#define PLAYERWRAPPER_H
+
+//#include <iostream>
+#include <deque>
+#include <pthread.h>
+//#include <semaphore.h>
+
+//#include <carmen/carmen.h>
+//#include <carmen/global.h>
+//#include <log/carmenconfiguration.h>
+
+#include <libplayercore/playercore.h>
+
+#include <gui/gsp_thread.h>
+
+//#include <sensor/sensor_base/sensor.h>
+//#include <log/sensorstream.h>
+//#include <log/sensorlog.h>
+#include <sensor/sensor_range/rangesensor.h>
+#include <sensor/sensor_range/rangereading.h>
+
+namespace GMapping {
+
+class PlayerGFSWrapper : public Driver
+{
+  private:
+    virtual void Main();
+    int SetupLaser();
+
+    // Devices we provide
+    player_devaddr_t position_id;
+    player_devaddr_t map_id;
+
+    // Devices to which we subscribe
+    player_devaddr_t laser_id;
+    Device* laser;
+    RangeSensor* laser_rs;
+
+    // The GFS object
+    GridSlamProcessorThread* gsp;
+
+    // Queue of range readings, with mutex
+    std::deque<RangeReading> rangeDeque;
+    pthread_mutex_t rangeDeque_mutex;
+
+    void ProcessLaser(player_msghdr_t* hdr,
+                      player_laser_data_scanpose_t* data);
+
+  public:
+    PlayerGFSWrapper(ConfigFile* cf, int section);
+    virtual int Setup();
+    virtual int Shutdown();
+    virtual int ProcessMessage(QueuePointer & resp_queue,
+                               player_msghdr * hdr,
+                               void * data);
+
+    bool getReading(RangeReading& reading);
+
+    /*
+    void initializeIPC(const char* name);
+    bool start(const char* name);
+    bool isRunning();
+    void lock();
+    void unlock();
+    int registerLocalizationMessages();
+
+    int queueLength();
+    OrientedPoint getTruePos();
+    bool getReading(RangeReading& reading);
+    void addReading(RangeReading& reading);
+    const SensorMap& sensorMap();
+    bool sensorMapComputed();
+    bool isStopped();
+
+    // conversion function  
+    carmen_robot_laser_message reading2carmen(const RangeReading& reading);
+    RangeReading carmen2reading(const carmen_robot_laser_message& msg);
+    carmen_point_t point2carmen (const OrientedPoint& p);
+    OrientedPoint carmen2point (const carmen_point_t& p);
+
+
+    // carmen interaction
+    void robot_frontlaser_handler(carmen_robot_laser_message* frontlaser);
+    void robot_rearlaser_handler(carmen_robot_laser_message* frontlaser);
+    void simulator_truepos_handler(carmen_simulator_truepos_message* truepos);
+    //babsi:
+    void navigator_go_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) ;
+    void navigator_stop_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) ;
+
+    //babsi:
+    void publish_globalpos(carmen_localize_summary_p summary);
+    void publish_particles(carmen_localize_particle_filter_p filter, 
+                                  carmen_localize_summary_p summary);
+
+    void shutdown_module(int sig);
+
+  private:
+    std::deque<RangeReading> m_rangeDeque;
+    sem_t m_dequeSem;
+    pthread_mutex_t m_mutex, m_lock;  
+    pthread_t m_readingThread;
+    void * m_reading_function(void*);
+    bool m_threadRunning;
+    SensorMap m_sensorMap;
+    RangeSensor* m_frontLaser, *m_rearLaser;
+    OrientedPoint m_truepos;
+    bool stopped;
+    */
+};
+
+} //end namespace
+
+
+
+#endif
+/*
+int main (int argc, char** argv) {
+
+	CarmenWrapper::init_carmen(argc, argv);
+	while (true) {
+		IPC_listenWait(100);
+	}    
+	return 1;
+}
+*/

Modified: pkg/trunk/gmapping/utils/gvalues.h
===================================================================
--- pkg/trunk/gmapping/utils/gvalues.h	2008-04-17 19:49:41 UTC (rev 116)
+++ pkg/trunk/gmapping/utils/gvalues.h	2008-04-17 19:51:25 UTC (rev 117)
@@ -8,7 +8,7 @@
 	#include <limits.h>
 	#include <math.h>
 	#define MAXDOUBLE 1e1000
-	#define isnan(x) (x==FP_NAN)
+	//#define isnan(x) (x==FP_NAN)
 #endif
 #ifdef _WIN32
   #include <limits>


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




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

personalrobots-commit