hardwareSensorExternalLoc.hpp

Go to the documentation of this file.

#ifndef HARDWARE_SENSOR_EXTERNALLOC_HPP_
#define HARDWARE_SENSOR_EXTERNALLOC_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 {


enum ExtLocType { elCameraBundle = 0, elCameraFullPose, elNExtLocType };
inline size_t ExtLocSizes(ExtLocType t)
{
  static size_t ExtLocSizes[elNExtLocType] = { 67, 98 };
  return ExtLocSizes[t];
}
struct ExtLoc
{
  double date;
  ExtLocType type;
  double meta_x[15]; // things that don't change over time (eg calibration), size max over ExtLocType
  double meta_P[15]; // only variances for this
  double obs_x[14]; // observations, size max over ExtLocType
  double obs_P[56]; // some covariance (upper triangle)
};


class HardwareSensorExternalLoc: public HardwareSensorProprioAbstract
{
  private:
    boost::thread *preloadTask_thread;
    void preloadTask(void);
    void dropline(std::fstream & file) const
    {
      std::string line;
      std::getline(file,line);
    }
    bool readline(std::fstream & file, jblas::vec & v) const
    {
      std::string line;
      std::getline(file,line);
      std::istringstream iss(line);
      unsigned i = 0;
      char tmpc;
      double tmpd;
      bool finished = true;
      for(i = 0; ; ++i)
      {
        iss >> tmpd >> tmpc;
        if (!checkStream(iss, "HardwareSensorExternalLoc")) { finished = false; break; }
        if (iss.eof()) break; else
        {
          if (i >= v.size()) v.resize(i+1);
          v(i) = tmpd;
        }
      }
      if (finished && i != v.size()) v.resize(i);
      if (!finished) readline(file, v);
      return file.eof();
    /*
      char tmpc; for(unsigned i = 0; i < v.size(); ++i) { file >> v(i); file >> tmpc; }
      if (!checkStream(file, "HardwareSensorExternalLoc")) readline(file, v);
      return file.eof();
      */
    }
    void fillReading(jblas::vec const & datavec);
    
#ifdef HAVE_POSTERLIB
    POSTER_ID posterId;
#endif
    ExtLocType data_type;
    int source;
    std::string dump_path;
    double last_timestamp;
    kernel::LoggerTask *loggerTask;
    std::fstream f;

  public:
    HardwareSensorExternalLoc(kernel::VariableCondition<int> *condition, unsigned bufferSize, ExtLocType data_type, int source, Mode mode = mOnline, std::string dump_path = ".", kernel::LoggerTask *loggerTask = NULL);
    
    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; }

    jblas::ind_array instantValues() { return jmath::ublasExtra::ia_set(1,6); }
    jblas::ind_array incrementValues() { return jmath::ublasExtra::ia_set(1,1); }
    void loadACTION(std::string config_file, std::string state_file, std::string detect_file, std::string uncert_filename);

};


}}}

#endif // HARDWARE_SENSOR_GPSGENOM_HPP_