robotsimu.hpp

#ifndef DDF_ROBOTSIMU_HPP
#define DDF_ROBOTSIMU_HPP

#include <string>
#include <set>
#include <algorithm>

#include <BayesFilter/bayesFlt.hpp>
#include <BayesFilter/infFlt.hpp>
#include "ddfsimu/accelmotion.hpp"
#include "ddfsimu/measureitem.hpp"
#include "ddf/definitions.hpp"
#include "ddfsimu/accelerometersimu.hpp"
#include "ddfsimu/eventsimu.hpp"
#include "ddfsimu/gpssimu.hpp"

namespace jafar
{
  namespace ddfsimu
  {

    class RobotSimu : public AccelMotion
    {

    public:
      class SortedBufferItem
      {
      public:
        time              m_time;
        SensorSimuBase    * sensor;
        unsigned short    m_rob_id;
        time              m_last_sensed;
  
        bool operator < (const SortedBufferItem & s1) const { return (m_time < s1.m_time); }
      };

      typedef std::multiset<SortedBufferItem> SortedBuffer;  // by default it is sorted chronologically using measured time
      
    private:
      typedef std::vector<SensorSimuBase *>   Sensors;
      typedef std::vector<MeasureItem>  FilterStateStorage;
   
      Sensors               m_sensors;        // contains pointers on sensors
      double                m_min_time_slice; // the overall min time between data
      FilterStateStorage    m_sv_store;       // buffer storing the state vector along time
      FilterStateStorage    m_iv_store;       // buffer storing the information vector along time
      FilterStateStorage    m_svar_store;     // same but for the variance
      FilterStateStorage    m_iv_info_store;
      double                m_sv_min_slice;   // min time between two state vectors (in m_sv_store)
      SortedBuffer          Sbuf;             // chronologically sorted array of measurements
      unsigned short        m_sv_size;        // size of the state vector
      SortedBuffer::iterator  m_meas_it;      // an iterator on the SortedBuffer (reset by FillUpMeasurementSet and modified by GetNextMeas)
      unsigned short        m_nr_meas;
      bool                  m_use_only_vel;
      bool                  m_with_biases;
   
    protected:
      unsigned short        m_rob_id;

    private:
      void InfoInitFilter(Bayesian_filter::Information_scheme &info_scheme, unsigned short sv_size, bool with_biases);
            
    public:
      typedef enum {SORT_STAMP_SENSED, SORT_STAMP_AVAILABLE} SORT_STAMP_TYPE;
   
      RobotSimu(unsigned short sv_size, std::string name, bool use_only_vel = false, bool use_biases = false);
      virtual ~RobotSimu();

      unsigned short GetRobID() const { return m_rob_id; }
      void Filter(bool with_biases);
      void AddSensor(SensorSimuBase * sensor) { m_sensors.push_back(sensor); }
      void RemoveSensorByID(unsigned short id);
      void RemoveAllSensors();
   
      MeasureItem const& GetStateVector(unsigned short idx) const { return m_sv_store[idx]; }
      MeasureItem const& GetInfoVector(unsigned short idx) const { return m_iv_store[idx]; }
      MeasureItem const& GetVarVector(unsigned short idx) const { return m_svar_store[idx]; }
      MeasureItem const& GetInfoMatVector(unsigned short idx) const { return m_iv_info_store[idx]; }

      unsigned short GetSvSize() { return m_sv_size; }
      bool HasBias() { return m_with_biases; }
      bool UseOnlyVel() { return m_use_only_vel; }
      unsigned short GetStatesNr() { JFR_PRECOND(m_sv_store.size() == m_iv_store.size(), "bad sizes"); return m_sv_store.size(); }
      void SimulatePropPerception();
      void SimulateExtPerception(RobotSimu &other_rob);
      void SimulateAll(RobotSimu &other_rob);
      void FillUpMeasurementSet(SORT_STAMP_TYPE  stmp_type);
      void ClearUpMeasurementSet() { Sbuf.clear(); m_nr_meas = 0; }
      void ClearAllSensorsMeasurements();
      void InitStampJitter(time const& jitter);
      short GetCurrentSensorID() const;
      SensorSimuBase * GetCurrentSensorPointer() const { return m_meas_it->sensor; }
      SortedBuffer::iterator   GetCurrentIterator() const { return m_meas_it; }
      SENSOR_TYPE GetCurrentSensorType() const;
      time GetCurrentLastSensed() const;
      int GetCurrentTriggerNodeID();
      bool   IsCurrentTriggerUpdatePOM();
      time GetCurrentRelTime();
      time const& GetCurrentSensorPeriod() const;
      bool GetCurrentInfo(InfoContainer &meas);
      MeasureItem const& GetCurrentMeasure() const;
      bool GetNextInfo(InfoContainer &meas);
      void ResetCounter() { m_meas_it = Sbuf.begin();  ResetSensorsGetNext(); }
      bool IncCounter();
      bool EndBuf() { return m_meas_it == Sbuf.end(); }
      void DumpAllMeasures(const char * fname);
      void DumpTruth(const char* fname);
      void DumpSensor(const char* fname, unsigned short rank);
      void DumpSensorByID(const char* fname, unsigned short id);
      unsigned short GetNrMeas() { return m_nr_meas; }
      unsigned short GetSensorID(unsigned short rank_in_array) { return m_sensors[rank_in_array]->GetID(); }
      std::string GetSensorName(unsigned short rank_in_array) { return m_sensors[rank_in_array]->GetName(); }
      unsigned short GetNumSensors() { return m_sensors.size(); }
   
      void ListAllSensors();

      void ListSensorsOfType(SENSOR_TYPE type)
      {
        std::ostringstream stream;

        for (Sensors::iterator it = m_sensors.begin(); it != m_sensors.end(); ++it)
        {
          if ((*it)->GetTypeName() == type)
            stream <<" " << *(*it) << " Time offset: " << (*it)->GetTimeOffset().to_double() << \
                      " Simu variance: " << (*it)->GetSensorSimVariance()<< " Seed: " << (*it)->GetSeed() << "\n";
        }
        JFR_DEBUG(stream.str());
      }

      // Return a pointer on the sensor of type T, given the rank in the array
      // (example: GetSensorOfType<GPSSensor>(2) -> return the third GPS of NULL if not available)
      template<typename T>
      T* GetSensorOfType(unsigned short rank)
      {
        unsigned short idx = 0;
        T* sensor;
        Sensors::iterator it = m_sensors.begin();
      
        while (it != m_sensors.end())
        {      
          sensor = dynamic_cast<T*> (*it);
          if (sensor != NULL) {
              if (idx == rank) return sensor;
              else idx++;
          }
          it++;
        }
        JFR_DEBUG("GetSensorOfType -> Sensor not found!\n");
        exit(EXIT_FAILURE);
        return NULL;   // make the compiler happy
      }

      unsigned short GetNumSensorsOfType(SENSOR_TYPE   type)
      {
        unsigned short nb = 0;
        for(unsigned short i = 0; i < GetNumSensors(); i++) if (isSensorOfType(m_sensors[i], type)) nb++;
        return nb;
      }

      int GetSensorID(SENSOR_TYPE type, unsigned short rank)
      {
        int num = GetNumSensorsOfType(type);
        unsigned short item = 0;
      
        if (num == 0) return -1;
        for(unsigned int i = 0; i < GetNumSensors(); i++)
          if (isSensorOfType(m_sensors[i], type) && (item++ == rank)) return m_sensors[i]->GetID();
      
        return -1;
      }

      void PrintRobot();
      void SearchMinTimeSlice();
      double GetMinTimeSlice() const {
        JFR_PRECOND(m_min_time_slice != std::numeric_limits<double>::max(), "== max double");
        return m_min_time_slice;
      }
      
      unsigned short GetMinSamplesNr() const;
      void PerceiveWithPropSensor(SensorSimuBase &sensor);
   
      SensorBase * GetSensorByID(unsigned short id);

      void ResetSensorsGetNext() {
        for (int j = 0; j < GetNumSensors(); j++) m_sensors[j]->ResetGetNext();
      }

    private:
      void DumpMeasure(unsigned short nb, MeasureItem  const& meas, std::ofstream & stream) const;

      bool isSensorOfType(SensorSimuBase *sensor, SENSOR_TYPE type) { if (sensor->GetTypeName() == type) return true; else return false; }
      SensorSimuBase * GetSensorPointer(unsigned short rank)
      {
        JFR_PRECOND(rank < m_sensors.size(), "bad sizes");
        return m_sensors[rank];
      }

    };

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