hardwareSensorMti.hpp

Go to the documentation of this file.

#ifndef HARDWARE_SENSOR_MTI_HPP_
#define HARDWARE_SENSOR_MTI_HPP_

#include "jafarConfig.h"

#ifdef HAVE_MTI
#include <MTI-clients/MTI.h>
#endif

#include <boost/thread.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>

#include "jmath/jblas.hpp"
#include "jmath/indirectArray.hpp"

#include "rtslam/hardwareSensorAbstract.hpp"



namespace jafar {
namespace rtslam {
namespace hardware {

  class HardwareSensorMti: public HardwareSensorProprioAbstract
  {
    private:
#ifdef HAVE_MTI
      MTI *mti;
#endif
      
      std::string dump_path;
      double realFreq;
      double last_timestamp;
      kernel::LoggerTask *loggerTask;

      boost::thread *preloadTask_thread;
      void preloadTask(void);

      unsigned mti_count;
      double init_ts1, init_ts2;
    
    public:
      
      HardwareSensorMti(kernel::VariableCondition<int> *condition, std::string device, double trigger_mode,
        double trigger_freq, double trigger_shutter, int bufferSize_, Mode mode = mOnline, std::string dump_path = ".", kernel::LoggerTask *loggerTask = NULL);
      ~HardwareSensorMti();
      virtual void start();
      virtual void stop();
      virtual bool join(int timed_ms = -1);
      virtual double getLastTimestamp() { boost::unique_lock<boost::mutex> l(mutex_data); return last_timestamp; }
      virtual void showInfos()
      {
        boost::unique_lock<boost::mutex> l(mutex_data);
        double first = last_timestamp-mti_count*10e-3;
        std::cout << "MTI: delay between " << (first-init_ts2)*1000 << " and " << (first-init_ts1)*1000 << " milliseconds." << std::endl;
      }
      void setSyncConfig(double timestamps_correction = 0.0/*, bool tightly_synchronized = false, double tight_offset*/);
      
      jblas::ind_array instantValues() { return jmath::ublasExtra::ia_set(1,10); }
      jblas::ind_array incrementValues() { return jmath::ublasExtra::ia_set(1,1); }

      double getFreq() { return realFreq; } // trigger freq
  };

}}}

#endif