sensorbase.hpp

#ifndef DDF_SENSORBASE_HPP
#define DDF_SENSORBASE_HPP

#include <iostream>
#include <fstream>
#include <deque>
#include <BayesFilter/allFilters.hpp>
#include <BayesFilter/schemeFlt.hpp>
#include "ddf/definitions.hpp"
#include "ddfsimu/measureitem.hpp"


namespace jafar
{
  namespace ddfsimu
  {

    typedef enum {SENSOR_ACCEL, SENSOR_GPS, SENSOR_VEL, SENSOR_TRACK, SENSOR_TRACK_ROBOT, SENSOR_EVENT, SENSOR_VOID} SENSOR_TYPE;
    typedef enum {MODEL_LINRZ_CORRELATED, MODEL_LINRZ_UNCORRELATED, MODEL_ADDATIVE_CORRELATED, MODEL_ADDATIVE_UNCORRELATED, MODEL_VOID} OBS_MODEL_TYPE;
    
    const char SENSOR_DESCRIPTION[][30] =
    {"Accelerometer", "GPS_sensor", "Velocity_sensor", "Tracker_sensor", "Track_robot", "Event_trigger", "void"};

    class SensorBase
    {
      typedef std::deque<MeasureItem> Measurements;   // buffer of stored data most recent data are placed at the end

      static unsigned short   m_static_id;   // Incremented each time SensorBase is instantiated
      unsigned short          m_id;          // a unique id is assigned to each instance of SensorBase
      time                    m_period;      // average time (s) between measurements
      time                    m_proc_time;   // average processing time
      unsigned short          m_size;        // Redundant but facilitate programming !
      unsigned short          m_current;     // Counter for the getnext function
      virtual void Dynamic_Cast() {}         // Dummy virtual function for dynamic_cast

    protected:
      Measurements    m_data;
      SENSOR_TYPE     m_type_name;         // type of sensor
      OBS_MODEL_TYPE  m_obs_model_type;    // used for dynamic casting of SensorBase:
      // in Bayes++ most of the scheme "observe" functions take the same observation models (base class)
      // Uncorrelated_addative_observe_model, Correlated_addative_observe_model
      // Linrz_uncorrelated_observe_model, Linrz_correlated_observe_model
      
    public:

      SensorBase(unsigned short measSize);
      SensorBase(SensorBase const& sensor);
      virtual ~SensorBase();

      SensorBase & operator =(SensorBase const &sensor) {
        JFR_PRECOND(m_size == sensor.m_size, "bad sizes");
        m_period = sensor.m_period;
         
        return *this;
      }

      unsigned short GetID() { return m_id; }
      SENSOR_TYPE  GetTypeName() { return m_type_name; }
      char const* GetName() { return SENSOR_DESCRIPTION[m_type_name]; }
      OBS_MODEL_TYPE GetObservationType() { return m_obs_model_type; }

      void SetPeriod(const time& t) { m_period = t; }
      void SetPeriod(double period) { JFR_PRECOND(period > 0.0, "period <= 0.0"); SetPeriod(time(period)); }
      time const& GetPeriod() const { return m_period; }
      double GetPeriodSec() const { return m_period.to_double(); }
      void SetFrequency(double freq) { JFR_PRECOND (freq > 0.0, "freq <= 0.0"); m_period = time(1.0 / freq); }
      double GetFrequencySec() const { return 1.0 / m_period.to_double(); }
      void SetProcTime(const time& t) { JFR_PRECOND(t < m_period, "proc time >= period"); m_proc_time = t; }
      void SetProcTime(double ptime) { JFR_PRECOND (ptime >= 0.0, "proc time < 0.0"); SetProcTime(time(ptime)); }
      double GetProcTimeSec() const { return m_proc_time.to_double(); }
      time GetProcTime() const { return m_proc_time; }
   
      unsigned short GetMeasureSize() const { return m_size; }
      void AddMeasurement(time sensed, time available, VEC &data);
      void AddMeasurementWithNoise(time sensed, time available, VEC &data);
      void ClearMeasurements() { m_data.clear(); }
      void SortMeasurements_AvailableTime();
      void SortMeasurements_SensedTime();
      unsigned short GetMeasurementsNr() { return (unsigned short) m_data.size();}
      void PrintData();
      double GetMinTimeSlice() const;
      MeasureItem const& GetMeasurement(unsigned short idx) const { JFR_PRECOND(idx < m_data.size(), "index"); return m_data[idx]; }
      time GetSensedTime(unsigned short idx) { JFR_PRECOND(idx < m_data.size(), "index"); return m_data[idx].GetSensedTime(); }
      time GetAvailableTime(unsigned short idx) { JFR_PRECOND(idx < m_data.size(), "index"); return m_data[idx].GetAvailableTime(); }
      double GetNextSensedTimeSec();
      bool EndBuf() const { return m_current == m_data.size(); }
      double GetNextAvailableTimeSec();
      time GetNextAvailableTime();
      time GetNextSensedTime();
      time GetCurrentSensedTime();
      int GetNextMeasurement(MeasureItem &meas);
      MeasureItem const& GetNextMeasurement();
      MeasureItem const& GetCurrentMeasurement() const;
      bool IncCounter() { m_current++; if (m_current < m_data.size()) return true; else return false;}

      VEC GetNextData();
      void ResetGetNext() { m_current = 0; }
      unsigned short GetCurrent() const { return m_current; }
      void DumpMeasurements(const char* fname);

      friend std::ostream& operator << (std::ostream &stream, SensorBase const& pd);

    private:
      void DumpMeasure(unsigned short nb, MeasureItem  const& meas, std::ofstream &stream) const;
      static bool compare_available(MeasureItem const& t1, MeasureItem const &t2) { return t1.compare_available(t2) == -1; }
      static bool compare_sensed(MeasureItem const& t1, MeasureItem const &t2) { return t1.compare_sensed(t2) == -1; }
    };

    std::ostream& operator << (std::ostream &stream, SensorBase const& pd);

  } // namespace ddfsimu
} // namespace jafar
#endif