forked from damarquezg/rtslam
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add hardwareSensorGpsGenom and enable GPS in demo with option --gps=1.
You need to install pocolibs on the machine where rtslam runs, and the latest git of GPS-genom until version 1.4 on the machine where the GPS is connected (and start h2, the module and posterServ)
- Loading branch information
Cyril Roussillon
committed
Apr 7, 2011
1 parent
52f6e32
commit 4550ead
Showing
7 changed files
with
211 additions
and
9 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,56 @@ | ||
| /** | ||
| * \file hardwareSensorGpsGenom.hpp | ||
| * | ||
| * Header file for getting data from the genom gps module | ||
| * | ||
| * \date 16/03/2011 | ||
| * \author croussil | ||
| * | ||
| * \ingroup rtslam | ||
| */ | ||
|
|
||
| #ifndef HARDWARE_SENSOR_GPSGENOM_HPP_ | ||
| #define HARDWARE_SENSOR_GPSGENOM_HPP_ | ||
|
|
||
| #include <stdlib.h> | ||
| #include <unistd.h> | ||
|
|
||
| #include <jafarConfig.h> | ||
| #include "kernel/jafarMacro.hpp" | ||
| #include "rtslam/hardwareSensorAbstract.hpp" | ||
|
|
||
| #ifdef HAVE_POSTERLIB | ||
| #include "posterLib.h" | ||
| #endif | ||
|
|
||
|
|
||
| namespace jafar { | ||
| namespace rtslam { | ||
| namespace hardware { | ||
|
|
||
|
|
||
| class HardwareSensorGpsGenom: public HardwareSensorProprioAbstract | ||
| { | ||
| private: | ||
| boost::thread *preloadTask_thread; | ||
| void preloadTask(void); | ||
|
|
||
| #ifdef HAVE_POSTERLIB | ||
| POSTER_ID posterId; | ||
| #endif | ||
| jblas::vec reading; | ||
| int mode; | ||
| std::string dump_path; | ||
|
|
||
| public: | ||
| HardwareSensorGpsGenom(kernel::VariableCondition<int> &condition, unsigned bufferSize, const std::string machine, int mode = 0, std::string dump_path = "."); | ||
|
|
||
| virtual int dataSize() { return 3; } | ||
| virtual int varianceSize() { return 3; } | ||
|
|
||
| }; | ||
|
|
||
|
|
||
| }}} | ||
|
|
||
| #endif // HARDWARESENSORABSLOCGPSGENOM_HPP |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,124 @@ | ||
| /** | ||
| * \file hardwareSensorGpsGenom.cpp | ||
| * | ||
| * File for getting data from the genom gps module | ||
| * | ||
| * \date 16/03/2011 | ||
| * \author croussil | ||
| * | ||
| * \ingroup rtslam | ||
| */ | ||
|
|
||
| #include "rtslam/hardwareSensorGpsGenom.hpp" | ||
|
|
||
| #ifdef HAVE_POSTERLIB | ||
| #include "h2timeLib.h" | ||
| #endif | ||
|
|
||
|
|
||
| namespace jafar { | ||
| namespace rtslam { | ||
| namespace hardware { | ||
|
|
||
|
|
||
| void HardwareSensorGpsGenom::preloadTask(void) | ||
| { | ||
| char data[256]; | ||
| #ifdef HAVE_POSTERLIB | ||
| H2TIME h2timestamp; | ||
| #endif | ||
| unsigned long prev_ntick = 0; | ||
| double *date, prev_date = 0.0; | ||
| double *pos; float *var; | ||
|
|
||
| std::fstream f; | ||
| if (mode == 1 || mode == 2) | ||
| { | ||
| std::ostringstream oss; oss << dump_path << "/GPS.log"; | ||
| f.open(oss.str().c_str(), (mode == 1 ? std::ios_base::out : std::ios_base::in)); | ||
| } | ||
|
|
||
| while (true) | ||
| { | ||
| if (mode == 2) | ||
| { | ||
| f >> reading; | ||
| boost::unique_lock<boost::mutex> l(mutex_data); | ||
| if (isFull()) cond_offline_full.notify_all(); | ||
| if (f.eof()) { f.close(); return; } | ||
| while (isFull()) cond_offline_freed.wait(l); | ||
|
|
||
| } else | ||
| { | ||
| #ifdef HAVE_POSTERLIB | ||
| while (true) // wait for new data | ||
| { | ||
| usleep(1000); | ||
| if (posterIoctl(posterId, FIO_GETDATE, &h2timestamp) != ERROR) | ||
| { | ||
| if (h2timestamp.ntick != prev_ntick) | ||
| { | ||
| prev_ntick = h2timestamp.ntick; | ||
| posterRead(posterId, 0, data, 256); | ||
| date = (double*)(data+80+44); | ||
| if (*date != prev_date) | ||
| { | ||
| prev_date = *date; | ||
| break; | ||
| } | ||
| } | ||
| } | ||
| } | ||
| #endif | ||
| pos = (double*)(data+16); | ||
| var = (float*)(data+48); | ||
| reading(0) = *date; | ||
| reading(1) = pos[0]; | ||
| reading(2) = pos[1]; | ||
| reading(3) = pos[2]; | ||
| reading(4) = var[0]; | ||
| reading(5) = var[1]; | ||
| reading(6) = var[2]; | ||
| //std::cout << "GPS poster : " << std::setprecision(15) << reading << std::endl; | ||
|
|
||
| buffer(getWritePos()) = reading; | ||
| buffer(getWritePos())(0) += timestamps_correction; | ||
| incWritePos(); | ||
|
|
||
| if (mode == 1) | ||
| { | ||
| // we put the maximum precision because we want repeatability with the original run | ||
| f << std::setprecision(50) << reading << std::endl; | ||
| } | ||
| prev_date = *date; | ||
| } | ||
| } | ||
| } | ||
|
|
||
|
|
||
| HardwareSensorGpsGenom::HardwareSensorGpsGenom(kernel::VariableCondition<int> &condition, unsigned bufferSize, const std::string machine, int mode, std::string dump_path): | ||
| HardwareSensorProprioAbstract(condition, bufferSize), reading(7), mode(mode), dump_path(dump_path) | ||
| { | ||
| // configure | ||
| #ifdef HAVE_POSTERLIB | ||
| char *backup_poster_path = getenv("POSTER_PATH"); | ||
| setenv("POSTER_PATH", machine.c_str(), 1); | ||
| if (posterFind("GPSInfo", &this->posterId) == ERROR) { | ||
| JFR_ERROR(RtslamException, RtslamException::GENERIC_ERROR, "Poster GPSInfo could not be found"); | ||
| } | ||
| if (backup_poster_path) setenv("POSTER_PATH", backup_poster_path, 1); else unsetenv("POSTER_PATH"); | ||
| #endif | ||
|
|
||
| // start acquire task | ||
| //preloadTask(); | ||
| preloadTask_thread = new boost::thread(boost::bind(&HardwareSensorGpsGenom::preloadTask,this)); | ||
|
|
||
| if (mode == 2) | ||
| { // wait that log has been read before first frame | ||
| boost::unique_lock<boost::mutex> l(mutex_data); | ||
| cond_offline_full.wait(l); | ||
| } | ||
| } | ||
|
|
||
| }}} | ||
|
|