hardwareSensorAbstract.hpp

Go to the documentation of this file.

#ifndef HARDWARE_SENSOR_HPP_
#define HARDWARE_SENSOR_HPP_

#include <typeinfo>

#include <boost/thread/condition_variable.hpp>

#include <jafarConfig.h>

#ifdef HAVE_POSTERLIB
#include "posterLib.h"
#endif

#include "kernel/threads.hpp"
#include "kernel/dataLog.hpp"

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

#include "rtslam/rawAbstract.hpp"

namespace jafar {
namespace rtslam {

struct RawInfo
{ unsigned id; double timestamp; double arrival;
  RawInfo(unsigned id, double timestamp, double arrival): id(id), timestamp(timestamp), arrival(arrival) {}
  RawInfo() {}
};
struct RawInfos
  { std::vector<RawInfo> available; RawInfo next; double process_time; bool integrate_all; };

struct RawVec
{
  jblas::vec data;
  double arrival;
  RawVec(unsigned n): data(n), arrival(0.) {}
  void resize(unsigned n) { data.resize(n); }
  RawVec() {}
};

// TODO use kernel version when released
class LoggableClose: public kernel::Loggable
{
 private:
  std::fstream *logStream;
  std::ofstream *logoStream;
 public:
  LoggableClose(std::fstream & logStream): logStream(&logStream), logoStream(NULL) {}
  LoggableClose(std::ofstream & logoStream): logStream(NULL), logoStream(&logoStream) {}
  virtual void log() {
    if (logStream && logStream->is_open()) logStream->close();
    if (logoStream && logoStream->is_open()) logoStream->close();
  }
};


namespace hardware {

//namespace ublas = boost::numeric::ublas;

#ifdef HAVE_POSTERLIB
extern STATUS reentrantPosterIoctl(POSTER_ID posterId, int code, void *parg);
extern int reentrantPosterRead(POSTER_ID posterId, int offset, void *buf, int nbytes);
#endif


inline double extractRawTimestamp(raw_ptr_t &raw) { return raw->timestamp; }
inline double extractRawTimestamp(RawVec &raw) { return raw.data(0); }
inline double extractRawArrival(raw_ptr_t &raw) { return raw->arrival; }
inline double extractRawArrival(RawVec &raw) { return raw.arrival; }

enum Mode { mOnline, mOnlineDump, mOffline };

#define narrival_delays 2

template<typename T>
class HardwareSensorAbstract
{
  public:
    typedef ublas::vector<T> VecT;
    typedef ublas::vector_indirect<VecT> VecIndT;
  
  private:
    int write_pos; 
    int read_pos;  
    bool buffer_full; 
    bool read_pos_used;  
    
  protected:
    kernel::VariableCondition<int> *condition; 
    kernel::VariableCondition<int> index; 
    boost::mutex mutex_data; 
    boost::condition_variable cond_offline_full;
    boost::condition_variable cond_offline_freed;
    int data_count; 
    int last_sent_pos; 
    bool no_more_data;
    double timestamps_correction;
    double data_period;
    double arrival_delays[narrival_delays];
    int iarrival_delay;
    double arrival_delay;
    bool started; 

    bool stopping;
    bool enabled; 
    bool initialized_; 
    Mode mode;

    int bufferSize; 
    VecT buffer; 

    int getWritePos(bool locked = false) {
      if (isFull(locked)) JFR_ERROR(RtslamException, RtslamException::GENERIC_ERROR, "HardwareSensor " << typeid(this).name() << ": buffer is full"); // FIXME chose policty when full
      // don't need to lock, because will only be used and modified by writer
      return write_pos;
    }
    void incWritePos(bool locked = false) {
      boost::unique_lock<boost::mutex> l(mutex_data, boost::defer_lock_t()); if (!locked) l.lock();
      ++write_pos;
      if (write_pos >= bufferSize) write_pos = 0;
      if (write_pos == read_pos) buffer_full = true; // full
      ++data_count;
    }
    int getFirstUnreadPos() {
      // don't need to lock, because will only be used and modified by reader
      if (!read_pos_used) return read_pos;
      if (read_pos != bufferSize-1) return read_pos+1; else return 0;
    }
    int getLastUnreadPos(bool locked = false) {
      boost::unique_lock<boost::mutex> l(mutex_data, boost::defer_lock_t()); if (!locked) l.lock();
      return (write_pos == 0 ? bufferSize-1 : write_pos-1);
    }
    void releaseUntil(unsigned id, bool locked = false) {
      boost::unique_lock<boost::mutex> l(mutex_data, boost::defer_lock_t()); if (!locked) l.lock();
      read_pos = id;
      read_pos_used = true;
      if (getFirstUnreadPos() == write_pos) buffer_full = false;
      l.unlock();
      cond_offline_freed.notify_all();
      // cannot be full as id is not released
    }
    void release(unsigned id, bool locked = false) {
      boost::unique_lock<boost::mutex> l(mutex_data, boost::defer_lock_t()); if (!locked) l.lock();
      if (id != (unsigned)(bufferSize-1)) read_pos = id+1; else read_pos = 0;
      if (write_pos == read_pos) buffer_full = false; // empty
      read_pos_used = false;
      l.unlock();
      cond_offline_freed.notify_all();
    }
    bool isFull(bool locked = false)
    {
      boost::unique_lock<boost::mutex> l(mutex_data, boost::defer_lock_t()); if (!locked) l.lock();
      return (read_pos == write_pos && buffer_full);
    }
    bool isEmpty(bool locked = false)
    {
      boost::unique_lock<boost::mutex> l(mutex_data, boost::defer_lock_t()); if (!locked) l.lock();
      return (getFirstUnreadPos() == write_pos && !buffer_full);
    }

    void update_arrival_delay(double delay)
    {
      // find min of current and past narrival_delays
      arrival_delay = delay;
      for(int i = 0; i < narrival_delays; ++i) if (arrival_delays[i] < arrival_delay) arrival_delay = arrival_delays[i];
      // write current
      ++iarrival_delay; if (iarrival_delay >= narrival_delays) iarrival_delay = 0;
      arrival_delays[iarrival_delay] = delay;
    }

  public:
    HardwareSensorAbstract(kernel::VariableCondition<int> *condition, Mode mode, unsigned bufferSize):
      write_pos(0), read_pos(0), buffer_full(false), read_pos_used(false),
      condition(condition), index(-1),
      data_count(0), no_more_data(false), timestamps_correction(0.0), iarrival_delay(0), started(false), stopping(false), enabled(true), initialized_(true), mode(mode),
      bufferSize(bufferSize), buffer(bufferSize)
    { for(int i = 0; i < narrival_delays; ++i) arrival_delays[i] = 0.0; }
    virtual void start() = 0; 
    virtual void stop() = 0; 
    virtual bool join(int timed_ms = -1) = 0; 
    bool stopped() { return no_more_data || stopping; }
    bool initialized() { return initialized_; }

    void setSyncConfig(double timestamps_correction = 0.0)
      { this->timestamps_correction = timestamps_correction; }
    virtual void getTimingInfos(double &data_period, double &arrival_delay, bool locked = false)
    {
      boost::unique_lock<boost::mutex> l(mutex_data, boost::defer_lock_t()); if (!locked) l.lock();
      data_period = this->data_period; arrival_delay = this->arrival_delay;
    }
    virtual void setTimingInfos(double data_period, double arrival_delay)
      { this->data_period = data_period; this->arrival_delay = arrival_delay; for(int i = 0; i < narrival_delays; ++i) arrival_delays[i] = arrival_delay; }
    
    
    virtual double getLastTimestamp() = 0;
    virtual void showInfos() {}
    
    virtual VecIndT getRaws(double t1, double t2, bool release = true); 
    virtual int getUnreadRawInfos(RawInfos &infos); 
    virtual int getNextRawInfo(RawInfo &info); 
    virtual void getRaw(unsigned id, T& raw); 
    virtual void observeRaw(unsigned id, T& raw); 
    virtual double getRawTimestamp(unsigned id);
    virtual int getLastUnreadRaw(T& raw); 
    virtual void getLastProcessedRaw(T& raw) { raw = buffer(last_sent_pos); } 
    virtual void release() { release(read_pos); }
    virtual void enable(bool enabled = true) { this->enabled = enabled; }
    void stopDumping() { if (this->mode == mOnlineDump) this->mode = mOnline; }

    friend class rtslam::SensorProprioAbstract;
    friend class rtslam::SensorExteroAbstract;
};


bool checkStream(std::istream &f, std::string const & owner);


class LoggableProprio: public kernel::Loggable
{
 private:
  std::fstream &fl;
  std::fstream &fb;
  jblas::vec data;

 public:
  LoggableProprio(std::fstream &fl, std::fstream &fb, jblas::vec & data):
    fl(fl), fb(fb), data(data) {}
  virtual void log();
};


class ProprioLog
{
  protected:
    std::fstream fl;
    std::fstream fb;
    bool binary; // binary format available
    bool old; // old log format
    int size; // if !binary, 0 means old text format, -1 means unknown
    kernel::LoggerTask *loggerTask;
    std::string name;

  public:
    ProprioLog(): binary(true), old(false), size(-1), loggerTask(NULL) {}
    void openWrite(std::string name, std::string dump_path, int format, int size, kernel::LoggerTask *loggerTask);
    void write(jblas::vec &v) { loggerTask->push(new LoggableProprio(fl, fb, v)); }
    void openRead(std::string name, std::string dump_path, int &format, int &size);
    bool read(jblas::vec &v);
    void close();
};





class HardwareSensorProprioAbstract: public HardwareSensorAbstract<RawVec>
{
  public:
    enum Quantity { qPos, qOriQuat, qOriEuler, qVel, qAbsVel, qNormVel, qAngVel, qAbsAngVel, qAcc, qAbsAcc, qBundleobs, qMag, qNQuantity };
    static const int QuantityDataSizes[qNQuantity];
    static const int QuantityObsSizes[qNQuantity];
    enum CovType { ctNone, ctVar, ctFull };
  private:
    int quantities[qNQuantity];
    size_t data_size;
    size_t obs_size;
    CovType cov_type;
  protected:
    RawVec reading;
    ProprioLog log;
    void addQuantity(Quantity quantity) { quantities[quantity] = data_size+1; data_size += QuantityDataSizes[quantity]; obs_size += QuantityObsSizes[quantity]; }
    void addDataQuantities(int n) { data_size += n; }
    void clearQuantities() { for(int i = 0; i < qNQuantity; ++i) quantities[i] = -1; data_size = obs_size = 0; }
  public:
    HardwareSensorProprioAbstract(kernel::VariableCondition<int> *condition, Mode mode, unsigned bufferSize, CovType covType):
      HardwareSensorAbstract<RawVec>(condition, mode, bufferSize), cov_type(covType) { clearQuantities(); }
    size_t dataSize() { return data_size; } 
    size_t obsSize() { return obs_size; } 
    size_t readingSize() { switch (cov_type) { case ctNone: return 1+data_size; case ctVar: return 1+data_size*2; case ctFull: return 1+data_size*(data_size+3)/2; default: return 0; } } 
    size_t getQuantity(Quantity quantity) { return quantities[quantity]; } 
    CovType covType() { return cov_type; } 
    jblas::sym_mat extractDataCovBlock(size_t pos, size_t size)
    {
      switch (cov_type) {
        case ctNone: { jblas::sym_mat res(size,size); res.clear(); return res; }
        case ctVar : { jblas::sym_mat res(size,size); for(size_t i = 0; i < size; ++i) res(i,i) = reading.data(data_size+pos+i); return res; }
        case ctFull: { return jmath::ublasExtra::createSymMat(reading.data, data_size+1, data_size, pos, size); }
        default: return jblas::sym_mat(0,0);
      }
    }

    void initData() {
      int size = readingSize();
      for(int i = 0; i < bufferSize; ++i) { buffer[i].resize(size); buffer(i).data(0) = -99.; }
      reading.resize(size);
    }
    virtual jblas::ind_array instantValues() = 0;
    virtual jblas::ind_array incrementValues() = 0;
};

class HardwareSensorExteroAbstract: public HardwareSensorAbstract<raw_ptr_t>
{
  public:
    HardwareSensorExteroAbstract(kernel::VariableCondition<int> *condition, Mode mode, unsigned bufferSize):
      HardwareSensorAbstract<raw_ptr_t>(condition, mode, bufferSize) {}
  
  
};

typedef boost::shared_ptr<hardware::HardwareSensorExteroAbstract> hardware_sensorext_ptr_t;
typedef boost::shared_ptr<hardware::HardwareSensorProprioAbstract> hardware_sensorprop_ptr_t;


// Template implementations


template<typename T>
typename HardwareSensorAbstract<T>::VecIndT HardwareSensorAbstract<T>::getRaws(double t1, double t2, bool release)
{
  JFR_ASSERT(t1 <= t2, "");
  boost::unique_lock<boost::mutex> l(mutex_data);
  int i1, i2;
  int i, j;

  // find first by dichotomy
  int i_left = write_pos, i_right = write_pos + bufferSize-1;
  while(i_left != i_right)
  {
    j = (i_left+i_right)/2;
    i = j % bufferSize;
    if (extractRawTimestamp(buffer(i)) >= t1) i_right = j; else i_left = j+1;
  }
  i = i_left % bufferSize;
  i1 = (i-1 + bufferSize) % bufferSize; // get the one before for interpolation
  if (t1 <= -0.1) i1 = i; // or not if we explicitly asked for the first one
  bool no_larger = (extractRawTimestamp(buffer(i)) < t1);
  bool no_smaller = (i == write_pos);
  if (no_larger && extractRawTimestamp(buffer(i1)) < 0.0)  // no data at all
    return ublas::project(buffer, jmath::ublasExtra::ia_set(ublas::range(0,0)));
  if (no_smaller && t1 > 0) JFR_ERROR(RtslamException, RtslamException::BUFFER_OVERFLOW, "HardwareSensor " << typeid(this).name() << ": missing data (increase buffer size !)");

  // find last by dichotomy
  if (no_larger)
    i2 = i1;
  else
  {
    i_right = write_pos + bufferSize-1;
    while(i_left != i_right)
    {
      j = (i_left+i_right)/2;
      i = j % bufferSize;
      if (extractRawTimestamp(buffer(i)) >= t2) i_right = j; else i_left = j+1;
    }
    i = i_left % bufferSize;
    i2 = i; // this is already the one after for interpolation, or the last one if there is none after
  }

  // return mat_indirect
  if (release) read_pos = i1;
  l.unlock();
  if (release) cond_offline_freed.notify_all();

  if (i1 <= i2)
  {
    return ublas::project(buffer,
      jmath::ublasExtra::ia_set(ublas::range(i1,i2+1)));
  } else
  {
    return ublas::project(buffer,
      jmath::ublasExtra::ia_concat(jmath::ublasExtra::ia_set(ublas::range(i1,buffer.size())),
                                   jmath::ublasExtra::ia_set(ublas::range(0,i2+1))));
  }
}


template<typename T>
int HardwareSensorAbstract<T>::getUnreadRawInfos(RawInfos &infos)
{
  infos.available.clear();
  if (!isEmpty())
  {
    int first_stop, second_stop;
    int first = getFirstUnreadPos(), last = getLastUnreadPos();
    JFR_DEBUG("getUnreadRawInfos: first " << first << " last " << last);

    if (first <= last)
    {
      first_stop = last;
      second_stop = -1;
    } else
    {
      first_stop = bufferSize-1;
      second_stop = last;
    }
    
    for(int pos = first; pos <= first_stop; ++pos)
      infos.available.push_back(RawInfo(pos,extractRawTimestamp(buffer(pos)),extractRawArrival(buffer(pos))));
    for(int pos = 0; pos <= second_stop; ++pos)
      infos.available.push_back(RawInfo(pos,extractRawTimestamp(buffer(pos)),extractRawArrival(buffer(pos))));
  }
  
  double data_period, arrival_delay;
  getTimingInfos(data_period, arrival_delay); // use the accessor for mutex protection
  double next_date = getLastTimestamp() + data_period;
  infos.next = RawInfo(0,next_date,next_date+arrival_delay);
  infos.process_time = 0.;
  
  if (infos.available.size() == 0)
  {
    if (no_more_data) return -2; else return -1;
  }
  return 0;
}

template<typename T>
int HardwareSensorAbstract<T>::getNextRawInfo(RawInfo &info)
{
  if (!isEmpty())
  {
    int first = getFirstUnreadPos();
    info = RawInfo(first,extractRawTimestamp(buffer(first)),0.0);
    return 0;
  } else
  {
    if (no_more_data) return -2; else return -1;
  }
}

template<typename T>
double HardwareSensorAbstract<T>::getRawTimestamp(unsigned id)
{
  return extractRawTimestamp(buffer[id]);
}

template<typename T>
void HardwareSensorAbstract<T>::observeRaw(unsigned id, T& raw)
{
  raw = buffer[id];
}

template<typename T>
void HardwareSensorAbstract<T>::getRaw(unsigned id, T& raw)
{
  releaseUntil(id);
  raw = buffer[id];
  last_sent_pos = id;
  index.applyAndNotify(boost::lambda::_1++);
}

template<typename T>
int HardwareSensorAbstract<T>::getLastUnreadRaw(T& raw)
{
  boost::unique_lock<boost::mutex> l(mutex_data);
  int missed_count = data_count-1;
  if (data_count > 0)
  {
    unsigned id = getLastUnreadPos(true);
    releaseUntil(id, true);
    raw = buffer[id];
    last_sent_pos = id;
    boost::unique_lock<boost::mutex> l(mutex_data);
    data_count = 0;
    l.unlock();
    index.applyAndNotify(boost::lambda::_1++);
  }
  if (no_more_data && missed_count == -1) return -2; else return missed_count;
}



}}}

#endif