diff --git a/CMakeLists.txt b/CMakeLists.txt index 2d450ae..9c1103a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,7 +13,7 @@ build_jafar_module(rtslam OPTIONAL_MODULES qdisplay gdhe dseg REQUIRED_EXTLIBS boost_thread boost_system boost_filesystem boost_regex - OPTIONAL_EXTLIBS qt4 viam MTI + OPTIONAL_EXTLIBS qt4 viam MTI posterLib # CXXFLAGS -g -ggdb -O0 -Wall # CXXFLAGS -O2 -g -DJFR_NDEBUG -DNDEBUG -DBOOST_UBLAS_NDEBUG -Wall -pthread diff --git a/demo_suite/demo_slam.cpp b/demo_suite/demo_slam.cpp index a608ef9..8adc51b 100644 --- a/demo_suite/demo_slam.cpp +++ b/demo_suite/demo_slam.cpp @@ -135,6 +135,7 @@ #include "rtslam/hardwareSensorCameraFirewire.hpp" #include "rtslam/hardwareEstimatorMti.hpp" +#include "rtslam/hardwareSensorGpsGenom.hpp" #include "rtslam/display_qt.hpp" #include "rtslam/display_gdhe.hpp" @@ -181,7 +182,7 @@ time_t rseed; * program parameters * ###########################################################################*/ -enum { iDispQt = 0, iDispGdhe, iRenderAll, iReplay, iDump, iRandSeed, iPause, iLog, iVerbose, iMap, iRobot, iTrigger, iSimu, nIntOpts }; +enum { iDispQt = 0, iDispGdhe, iRenderAll, iReplay, iDump, iRandSeed, iPause, iLog, iVerbose, iMap, iRobot, iTrigger, iGps, iSimu, nIntOpts }; int intOpts[nIntOpts] = {0}; const int nFirstIntOpt = 0, nLastIntOpt = nIntOpts-1; @@ -215,6 +216,7 @@ struct option long_options[] = { // double options {"freq", 2, 0, 0}, // should be in config file {"shutter", 2, 0, 0}, // should be in config file + {"gps", 2, 0, 0}, // string options {"data-path", 1, 0, 0}, {"config-setup", 1, 0, 0}, @@ -362,8 +364,7 @@ class ConfigEstimation: public kernel::KeyValueFileSaveLoad * ###########################################################################*/ -void demo_slam01_main(world_ptr_t *world) { - +void demo_slam01_main(world_ptr_t *world) { try { vec intrinsic, distortion; int img_width, img_height; if (intOpts[iSimu] != 0) @@ -788,6 +789,16 @@ void demo_slam01_main(world_ptr_t *world) { senPtr11->setHardwareSensor(hardSen11); } #endif + + if (intOpts[iGps]) + { + absloc_ptr_t senPtr13(new SensorAbsloc(robPtr1, MapObject::UNFILTERED)); + senPtr13->setId(); + hardware::hardware_sensorprop_ptr_t hardGps( + new hardware::HardwareSensorGpsGenom(rawdata_condition, 100, "mana-base", mode, strOpts[sDataPath])); + hardGps->setSyncConfig(0.0/*configSetup.GPS_TIMESTAMP_CORRECTION*/); + senPtr13->setHardwareSensor(hardGps); + } } //--- force a first display with empty slam to ensure that all windows are loaded @@ -1072,6 +1083,8 @@ std::cout << "average_robot_innovation " << average_robot_innovation << std::end (*world)->slam_blocked(true); // std::cout << "\nFINISHED ! Press a key to terminate." << std::endl; // getchar(); + +} catch (kernel::Exception &e) { std::cout << e.what(); } } // demo_slam01_main @@ -1080,7 +1093,7 @@ std::cout << "average_robot_innovation " << average_robot_innovation << std::end * Display function * ###########################################################################*/ -void demo_slam01_display(world_ptr_t *world) { +void demo_slam01_display(world_ptr_t *world) { try { // static unsigned prev_t = 0; kernel::Timer timer(display_period*1000); while(true) @@ -1197,6 +1210,8 @@ void demo_slam01_display(world_ptr_t *world) { if (intOpts[iDispQt]) break; else timer.wait(); } + +} catch (kernel::Exception &e) { std::cout << e.what(); } } @@ -1314,6 +1329,7 @@ void demo_slam01() { * --simu 0/1 * --freq camera frequency in double Hz (with trigger==0/1) * --shutter shutter time in double seconds (0=auto); for trigger modes 0,2,3 the value is relative between 0 and 1 + * --gps whether use or not a gps * * You can use the following examples and only change values: * online test (old mode=0): diff --git a/include/rtslam/hardwareSensorAbstract.hpp b/include/rtslam/hardwareSensorAbstract.hpp index 1dceffc..618ae87 100644 --- a/include/rtslam/hardwareSensorAbstract.hpp +++ b/include/rtslam/hardwareSensorAbstract.hpp @@ -68,6 +68,7 @@ class HardwareSensorAbstract boost::condition_variable cond_offline_freed; int image_count; /// image count since last image read bool no_more_data; + double timestamps_correction; int bufferSize; /// size of the ring buffer VecT buffer; /// the ring buffer @@ -88,6 +89,7 @@ class HardwareSensorAbstract int getLastUnreadPos() { return (write_pos == 0 ? bufferSize-1 : write_pos-1); } + bool isFull() { return (read_pos == write_pos); } public: /** Constructor @@ -95,8 +97,11 @@ class HardwareSensorAbstract */ HardwareSensorAbstract(kernel::VariableCondition &condition, unsigned bufferSize): write_pos(0), read_pos(bufferSize-1), condition(condition), index(0), - image_count(0), no_more_data(false), bufferSize(bufferSize), buffer(bufferSize) + image_count(0), no_more_data(false), timestamps_correction(0.0), + bufferSize(bufferSize), buffer(bufferSize) {} + void setSyncConfig(double timestamps_correction = 0.0) + { this->timestamps_correction = timestamps_correction; } VecIndT getRaws(double t1, double t2); /// will also release the raws before the first one RawInfos getUnreadRawInfos(); /// get timing informations about unread raws diff --git a/include/rtslam/hardwareSensorGpsGenom.hpp b/include/rtslam/hardwareSensorGpsGenom.hpp new file mode 100644 index 0000000..243cd6c --- /dev/null +++ b/include/rtslam/hardwareSensorGpsGenom.hpp @@ -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 +#include + +#include +#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 &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 diff --git a/include/rtslam/rtslamException.hpp b/include/rtslam/rtslamException.hpp index 3b7646a..1b77ded 100755 --- a/include/rtslam/rtslamException.hpp +++ b/include/rtslam/rtslamException.hpp @@ -28,7 +28,7 @@ namespace jafar { UNKNOWN_TYPES_FOR_FACTORY, BUFFER_OVERFLOW, SIMU_ERROR, - ERROR + GENERIC_ERROR // MY_ERROR /**< my error */ }; diff --git a/include/rtslam/sensorAbsloc.hpp b/include/rtslam/sensorAbsloc.hpp index eaac603..b6ed8ad 100644 --- a/include/rtslam/sensorAbsloc.hpp +++ b/include/rtslam/sensorAbsloc.hpp @@ -57,7 +57,7 @@ namespace jafar { ublas::subrange(INN_rs, 0,3, 0,3) = -jblas::identity_mat(3); break; default: - JFR_ERROR(RtslamException, RtslamException::ERROR, + JFR_ERROR(RtslamException, RtslamException::GENERIC_ERROR, "SensorAbsloc reading size " << innovation->size()+1 << " not supported."); } if (hardwareSensorPtr->varianceSize() == hardwareSensorPtr->dataSize()) @@ -91,7 +91,8 @@ namespace jafar { } break; default: - JFR_ERROR(RtslamException, RtslamException::ERROR, "SensorAbsloc reading size " << reading.size() << " not supported."); + JFR_ERROR(RtslamException, RtslamException::GENERIC_ERROR, + "SensorAbsloc reading size " << reading.size() << " not supported."); } map_ptr_t mapPtr = robotPtr()->mapPtr(); diff --git a/src/hardwareSensorGpsGenom.cpp b/src/hardwareSensorGpsGenom.cpp new file mode 100644 index 0000000..b898341 --- /dev/null +++ b/src/hardwareSensorGpsGenom.cpp @@ -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 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 &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 l(mutex_data); + cond_offline_full.wait(l); + } + } + +}}} +