Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [117] pkg/trunk/gmapping
|
From: <ge...@us...> - 2008-04-17 19:51:25
|
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.
|
