sensorManager.hpp

Go to the documentation of this file.

#ifndef SENSOR_MANAGER_HPP_
#define SENSOR_MANAGER_HPP_

#include "rtslam/sensorAbstract.hpp"

#define DEBUG_SENSORMANAGER 0

namespace jafar {
namespace rtslam {

  class SensorManagerAbstract
  {
    protected:
      map_ptr_t mapPtr;
      double start_date;
      bool all_init;
    public:
    
    struct ProcessInfo
    {
      sensor_ptr_t sen;
      unsigned id;
      bool no_more_data; // in offline mode, all of the sensors have no more data, so we stop everything
      double date;
      double arrival;
      double date_next; // date of next data arrival
      ProcessInfo(sensor_ptr_t sen, unsigned id, double date, double arrival, double date_next):
        sen(sen), id(id), no_more_data(false), date(date), arrival(arrival), date_next(date_next) {}
      //ProcessInfo(sensor_ptr_t sen, unsigned id, double date): sen(sen), id(id), no_more_data(false), date(date), date_next(-1.) {}
      ProcessInfo(bool no_more_data, bool date_next): sen(), id(0), no_more_data(no_more_data), date(-1.), arrival(-1.), date_next(date_next) {}
      ProcessInfo(): sen(), id(0), no_more_data(false), date(-1.), arrival(-1.), date_next(-1.) {}
    };

    SensorManagerAbstract(map_ptr_t mapPtr): mapPtr(mapPtr), start_date(0.), all_init(false) {}
    
    virtual void logData(sensor_ptr_t sen, double start_date, double waitedmove_date, double moved_date, double processed_date) {}
    virtual void printStats() {}

    bool allInit() { return all_init; }
    void setStartDate(double start_date) { this->start_date = start_date; }
    virtual ProcessInfo getNextDataToUse_func(double currentTime) = 0;

    ProcessInfo getNextDataToUse(double currentTime)
    {
      ProcessInfo pinfo;
      while (true)
      {
        pinfo = getNextDataToUse_func(currentTime);
        if (pinfo.sen && !pinfo.sen->getUseForInit() && pinfo.date < start_date)
          pinfo.sen->discard(pinfo.id);
        else
          break;
      }
      return pinfo;
    }
    
    int getNextDataToInit(ProcessInfo &result)
    {
      // TODO set an order for init
      RawInfos infos;
      bool hasSensorMissingData = false;
      sensor_ptr_t oldestSen; unsigned oldestId = 0; double oldestTimestamp = 1e20; double oldestArrival = 1e20; double oldestNextArrival = 1e20;
      for (MapAbstract::RobotList::iterator robIter = mapPtr->robotList().begin();
        robIter != mapPtr->robotList().end(); ++robIter)
      {
        for (RobotAbstract::SensorList::iterator senIter = (*robIter)->sensorList().begin();
          senIter != (*robIter)->sensorList().end(); ++senIter)
        {
          if ((*senIter)->getUseForInit())
          {
            (*senIter)->queryAvailableRaws(infos);
            if (infos.available.size() > 0)
            {
              RawInfo &info = infos.available.front();
              if (info.timestamp > start_date) // if we anyway don't have data before the start date, use the first one
              {
                if (info.timestamp < oldestTimestamp) { oldestSen = (*senIter); oldestId = info.id; oldestTimestamp = info.timestamp; }
              } else // else find the last one before the start date
              {
                for(std::vector<RawInfo>::reverse_iterator rawIter = infos.available.rbegin(); rawIter != infos.available.rend(); ++rawIter)
                  if ((*rawIter).timestamp <= start_date)
                  {
                    if ((*rawIter).timestamp < oldestTimestamp)
                    {
                      oldestSen = (*senIter); oldestId = (*rawIter).id;
                      oldestTimestamp = (*rawIter).timestamp; oldestArrival = (*rawIter).arrival;
                      oldestNextArrival = infos.next.arrival;
                    }
                    break;
                  }
              }
            } else
              hasSensorMissingData = true;
          }
        }
      }

      
      if (hasSensorMissingData) return 1; else
      {
        if (!oldestSen) return 0; else
        {
          result = ProcessInfo(oldestSen, oldestId, oldestTimestamp, oldestArrival, oldestNextArrival);
          return 2;
        }
      }
    }

    virtual void stop() {}
  };

  
  class SensorManagerOffline: public SensorManagerAbstract
  {
    protected:
      bool replay;
      bool first;
      std::fstream fb;

      size_t sen_id;
      double raw_date;

      bool wait_data;

    public:
            SensorManagerOffline(map_ptr_t mapPtr, std::string replay_path = ""):
              SensorManagerAbstract(mapPtr), replay(false), first(true), wait_data(false)
      {
        if (replay_path.size() != 0)
        {
          replay = true;
          std::string file_dat = replay_path + "/replay.dat";
          fb.open(file_dat.c_str(), std::ios_base::in | std::ios_base::binary);
          if (fb.fail()) { std::cerr << "SensorManagerOffline: could not read file " << file_dat << ", disabling replay" << std::endl; replay = false; }
        }
      }
      
      virtual ProcessInfo getNextDataToUse_func(double currentTime)
      {
        ProcessInfo result;
        RawInfo info;

        if (replay)
        {
          if (first)
          {
            for(MapAbstract::RobotList::iterator robIter = mapPtr->robotList().begin();
              robIter != mapPtr->robotList().end(); ++robIter)
            {
              size_t size;
              fb.read((char*)&size, sizeof(size_t));
              if (size != (*robIter)->state.size())
                { std::cerr << "SensorManager replay: robot state size is " << (*robIter)->state.size() << " but replay file has size " << size << std::endl; return result; }
              for(size_t i = 0; i < size; ++i) fb.read((char*)&((*robIter)->state.x()(i)), sizeof(double));
              for(size_t i = 0; i < size; ++i) for(size_t j = i; j < size; ++j) fb.read((char*)&((*robIter)->state.P()(i,j)), sizeof(double));
              for(size_t i = 0; i < 3; ++i) fb.read((char*)&((*robIter)->start_pos_export(i)), sizeof(double));
              for(size_t i = 0; i < 3; ++i) fb.read((char*)&((*robIter)->start_pos_abs(i)), sizeof(double));
              fb.read((char*)&((*robIter)->is_start_pos_abs_init), sizeof(bool));
            }
            first = false;
          }

          if (!wait_data)
          {
            fb.read((char*)&sen_id, sizeof(size_t));
            fb.read((char*)&raw_date, sizeof(double));
            if (fb.eof()) { result.no_more_data = true; return result; }
          }
          JFR_VVDEBUG("next data to get is sensor " << sen_id << " at " << std::setprecision(16) << raw_date << std::setprecision(6));

          for (MapAbstract::RobotList::iterator robIter = mapPtr->robotList().begin();
            robIter != mapPtr->robotList().end(); ++robIter)
          {
            for (RobotAbstract::SensorList::iterator senIter = (*robIter)->sensorList().begin();
              senIter != (*robIter)->sensorList().end(); ++senIter)
            {
              if ((*senIter)->id() == sen_id)
              {
                int status;
                while (true)
                {
                  status = (*senIter)->queryNextAvailableRaw(info);
                  if (status != 0 || info.timestamp >= raw_date-1e-9) break;
                  (*senIter)->discard(info.id);
                }
                if (status == -1) wait_data = true;
                if (status == -2)
                {
                  std::cerr << "SensorManagerOffline: missing data from sensor " << sen_id << " at " << std::setprecision(16) << raw_date << std::endl;
                  result.no_more_data = true;
                }
                if (status == 0)
                {
                  wait_data = false;
                  result.id = info.id;
                  result.sen = (*senIter);
                  result.date = info.timestamp;
                }
                return result;
              }
            }
          }
          std::cerr << "SensorManagerOffline: sensor " << sen_id << " not found !" << std::endl;
          return result;
        } else
        {
          if (!all_init)
          {
            ProcessInfo result;
            int res;
            do {
              res = getNextDataToInit(result);
              if (res == 0) { all_init = true; break; } else
              if (res == 2) { return result; } else
              usleep(1000);
            } while (res == 1);
          }

          result.no_more_data = true;
          wait_data = false;
          for (MapAbstract::RobotList::iterator robIter = mapPtr->robotList().begin();
            robIter != mapPtr->robotList().end(); ++robIter)
          {
            for (RobotAbstract::SensorList::iterator senIter = (*robIter)->sensorList().begin();
              senIter != (*robIter)->sensorList().end(); ++senIter)
            {
              if (!(*senIter)->isEnabled()) continue;
              int status = (*senIter)->queryNextAvailableRaw(info);
              if (status == -1) wait_data = true;
              if (status != -2) result.no_more_data = false;
              if (status == 0 && (result.date < 0 || info.timestamp < result.date))
              {
                result.id = info.id;
                result.sen = (*senIter);
                result.date = info.timestamp;
              }
            }
          }

          if (wait_data == true) result.sen.reset();
          return result;
        }
      }
    
      virtual void stop() { if (replay) fb.close(); }
  };



  class SensorManagerOnline: public SensorManagerAbstract
  {
    protected:
      struct SenInfo
      {
        sensor_ptr_t sen;
        RawInfos infos;
        int res;
        size_t first_ok;
        size_t num_ok;
      };
      bool dump;
      bool first;
      std::fstream fl;
      std::fstream fb;
      kernel::LoggerTask *loggerTask;

      class LoggableReplayInitialState: public kernel::Loggable
      {
       private:
        std::fstream &fl;
        std::fstream &fb;
        jblas::vec stateX;
        jblas::sym_mat stateP;
        jblas::vec start_pos_export;
        jblas::vec start_pos_abs;
        bool is_start_pos_abs_init;

       public:
        LoggableReplayInitialState(std::fstream &fl, std::fstream &fb, jblas::vec stateX, jblas::sym_mat stateP, jblas::vec start_pos_export, jblas::vec start_pos_abs, bool is_start_pos_abs_init):
          fl(fl), fb(fb), stateX(stateX), stateP(stateP), start_pos_export(start_pos_export), start_pos_abs(start_pos_abs), is_start_pos_abs_init(is_start_pos_abs_init) {}
        virtual void log()
        {
          fl << stateX << std::endl;
          fl << stateP << std::endl;
          fl << start_pos_export << std::endl;
          fl << start_pos_abs << std::endl;
          fl << is_start_pos_abs_init << std::endl;

          size_t size = stateX.size();
          fb.write((char*)&size, sizeof(size_t));
          for(size_t i = 0; i < size; ++i) fb.write((char*)&(stateX(i)), sizeof(double));
          for(size_t i = 0; i < size; ++i) for(size_t j = i; j < size; ++j) fb.write((char*)&(stateP(i,j)), sizeof(double));
          for(size_t i = 0; i < 3; ++i) fb.write((char*)&(start_pos_export(i)), sizeof(double));
          for(size_t i = 0; i < 3; ++i) fb.write((char*)&(start_pos_abs(i)), sizeof(double));
          fb.write((char*)&is_start_pos_abs_init, sizeof(bool));
        }
      };

      class LoggableReplay: public kernel::Loggable
      {
       private:
        std::fstream &fl;
        std::fstream &fb;
        size_t sen_id;
        std::string sen_name;
        size_t raw_id;
        double raw_date;

       public:
        LoggableReplay(std::fstream &fl, std::fstream &fb, size_t sen_id, std::string sen_name, size_t raw_id, double raw_date):
          fl(fl), fb(fb), sen_id(sen_id), sen_name(sen_name), raw_id(raw_id), raw_date(raw_date) {}
        virtual void log()
        {
          fl << sen_id << "\t" << sen_name << "\t" << raw_id << "\t" << std::setprecision(19) << raw_date << std::endl;

          fb.write((char*)&sen_id, sizeof(size_t));
          fb.write((char*)&raw_date, sizeof(double));
        }
      };

      class LoggableChronogram: public kernel::Loggable
      {
       protected:
        std::fstream &fc;
        enum { tSenProcessed, tWaitedNoData, tWaitedData, tZappedDataAccident, tZappedDataPurpose } type;
        sensor_ptr_t sen;
        unsigned id;
        double raw_date, arrival_date, start_date, waitedmove_date, moved_date, processed_date;

       public:
        LoggableChronogram(std::fstream &fc, sensor_ptr_t sen, unsigned id, double raw_date, double arrival_date, double start_date, double waitedmove_date, double moved_date, double processed_date):
          fc(fc), sen(sen), id(id), raw_date(raw_date), arrival_date(arrival_date), start_date(start_date), waitedmove_date(waitedmove_date), moved_date(moved_date), processed_date(processed_date)
        { type = tSenProcessed; }
        LoggableChronogram(std::fstream &fc, sensor_ptr_t sen, unsigned id, double raw_date, double arrival_date):
          fc(fc), sen(sen), id(id), raw_date(raw_date), arrival_date(arrival_date)
        { type = tSenProcessed; }
        void complete(double start_date, double waitedmove_date, double moved_date, double processed_date)
        {
          this->waitedmove_date = waitedmove_date; this->moved_date = moved_date;
          this->processed_date = processed_date; this->start_date = start_date;
          JFR_ASSERT(type == tSenProcessed, "");
        }
        LoggableChronogram(std::fstream &fc, double waitbegin_date, double waitend_date):
          fc(fc), raw_date(waitbegin_date), arrival_date(waitend_date)
        { type = tWaitedNoData; }
        LoggableChronogram(std::fstream &fc, sensor_ptr_t sen, double waitbegin_date, double waitend_date):
          fc(fc), sen(sen), raw_date(waitbegin_date), arrival_date(waitend_date)
        { type = tWaitedData; }

        virtual void log()
        {
          fc << std::setprecision(16) << sen->name() << "\t" << sen->id() << "\t" << id << "\t"
             << raw_date << "\t" << arrival_date << "\t" << start_date << "\t"
             << waitedmove_date << "\t" << moved_date << "\t" << processed_date << std::endl;
        }
        friend class SensorManagerOnline;
      };

      LoggableChronogram *loggableChronogram;
      std::fstream fc;


      // stats
      struct SenStats
      {
        double total_time, min_time, max_time;
        int nIntegrated, nMissed;
        SenStats(): total_time(0.), min_time(1e3), max_time(0.), nIntegrated(0), nMissed(0) {}
      };
      std::map<int,SenStats> senStats;

      struct GlobalStats
      {
        double start_date, stop_date;
        double wait_estimator, wait_sensor;
        double move;
        GlobalStats(): start_date(-1.), stop_date(-1.), wait_estimator(0.), wait_sensor(0.), move(0.) {}
      } globalStats;

      void printStats()
      {
        std::cout << "SensorManager Stats ------" << std::endl;
        globalStats.start_date = globalStats.stop_date - globalStats.start_date;
        std::cout << "Work time: " << globalStats.start_date << "s (100%)" << std::endl;
        std::cout << "Wait sensor time: " << globalStats.wait_sensor << "s (" << globalStats.wait_sensor/globalStats.start_date*100 << "%)" << std::endl;
        std::cout << "Wait estimator time: " << globalStats.wait_estimator << "s (" << globalStats.wait_estimator/globalStats.start_date*100 << "%)" << std::endl;
        std::cout << "Move time: " << globalStats.move << "s (" << globalStats.move/globalStats.start_date*100 << "%)" << std::endl;
        for (std::map<int,SenStats>::iterator it = senStats.begin(); it != senStats.end(); ++it)
        {
          std::cout << "Sensor " << it->first << " process time: " << it->second.total_time << "s (" << it->second.total_time/globalStats.start_date*100 << "%, min/avg/max "
           << it->second.min_time*1000 << "/" << (it->second.total_time/it->second.nIntegrated)*1000 << "/" << it->second.max_time*1000 << " ms) ; integration rate: "
           << it->second.nIntegrated/(double)(it->second.nIntegrated+it->second.nMissed)*100 << "% (" << it->second.nIntegrated
           << "/" << it->second.nIntegrated+it->second.nMissed << ")" << std::endl;
        }
      }

    public:
      SensorManagerOnline(map_ptr_t mapPtr, std::string dump_path = "", kernel::LoggerTask *loggerTask = NULL):
        SensorManagerAbstract(mapPtr), loggerTask(loggerTask)
      {
        first = true;
        dump = false;
        if (dump_path.size() != 0)
        {
          dump = true;
          std::string file_log = dump_path + "/replay.log";
          std::string file_dat = dump_path + "/replay.dat";
          fl.open(file_log.c_str(), std::ios_base::out);
          fb.open(file_dat.c_str(), std::ios_base::out | std::ios_base::binary);
          std::string file_chrono = dump_path + "/chronogram.log";
          fc.open(file_chrono.c_str(), std::ios_base::out);
        }

      }

      inline void do_dump(ProcessInfo const &info, int missed)
      {
        if (dump)
        {
          loggerTask->push(new LoggableReplay(fl, fb, info.sen->id(), info.sen->name(), info.id, info.date));
          loggableChronogram = new LoggableChronogram(fc, info.sen, info.id, info.date, info.arrival);
        }
        globalStats.wait_sensor += kernel::Clock::getTime()-globalStats.stop_date;
        senStats[info.sen->id()].nMissed += missed;
      }

      virtual ProcessInfo getNextDataToUse_func(double currentTime)
      {
        // TODO when we don't receive data from a sensor for some time we should put it in some state
        // stop waiting for these data, and if we receive them again then we start waiting again
        // (eg GPS, or external loc...)

        if (first) { globalStats.start_date = globalStats.stop_date = start_date; }


        if (dump && first)
        {
          for(MapAbstract::RobotList::iterator robIter = mapPtr->robotList().begin(); robIter != mapPtr->robotList().end(); ++robIter)
            loggerTask->push(new LoggableReplayInitialState(fl, fb, (*robIter)->state.x(), (*robIter)->state.P(), (*robIter)->start_pos_export, (*robIter)->start_pos_abs, (*robIter)->is_start_pos_abs_init));
          first = false;
        }

        // ensure all sensors are init
        if (!all_init)
        {
          ProcessInfo result;
          int res;
          do {
            res = getNextDataToInit(result);
            if (res == 0) { all_init = true; break; } else
            if (res == 2) { do_dump(result, 0); return result; } else
            usleep(1000);
          } while (res == 1);
        }

        // extract information
        std::vector<SenInfo> infos;
        for(MapAbstract::RobotList::iterator robIter = mapPtr->robotList().begin();
          robIter != mapPtr->robotList().end(); ++robIter)
        {
          for(RobotAbstract::SensorList::iterator senIter = (*robIter)->sensorList().begin();
            senIter != (*robIter)->sensorList().end(); ++senIter)
          {
            if (!(*senIter)->isEnabled()) continue;
            SenInfo info;
            info.sen = *senIter;
            info.res = info.sen->queryAvailableRaws(info.infos);
            infos.push_back(info);
          }
        }

        // find largest possible date of data than can be integrated now
        // it is the min of the nexts dates over all the sensors, except if it is late
        const double delay_margin = 2.2;
        double tnow = kernel::Clock::getTime();
        double max_date = tnow + 3600;
        double max_arrival = -1;
        for(std::vector<SenInfo>::iterator it = infos.begin(); it != infos.end(); ++it)
        {
          double next_arrival = it->infos.next.timestamp + (it->infos.next.arrival-it->infos.next.timestamp)*delay_margin; // add some margin to arrival
          double next_timestamp = it->infos.next.timestamp - 0.002;
          if (next_arrival > tnow && next_timestamp < max_date) { max_date = next_timestamp; max_arrival = next_arrival; }
        }

        // set first_ok and num_ok (data between currentTime of the filter and max_date)
        for(std::vector<SenInfo>::iterator it = infos.begin(); it != infos.end(); ++it)
        {
          SenInfo &info = *it;
          int i = 0;
          for(; i < (int)info.infos.available.size(); ++i)
            if (info.infos.available[i].timestamp >= currentTime) break;
          info.first_ok = i;
          for(; i < (int)info.infos.available.size(); ++i)
            if (info.infos.available[i].timestamp > max_date) break;
          info.num_ok = i-info.first_ok;
        }

        // check if we are late
        // ie if all sensors have at least 2 data waiting (except those that don't provide data anymore)
        size_t min_data = 1000;
        for(std::vector<SenInfo>::iterator it = infos.begin(); it != infos.end(); ++it)
          if ((it->num_ok < min_data) && (it->infos.next.timestamp + (it->infos.next.arrival-it->infos.next.timestamp)*delay_margin > tnow))
            min_data = it->num_ok;
        bool late = (min_data > 1);

        #if DEBUG_SENSORMANAGER
        std::cout << std::setprecision(16) << tnow << ": min_date " << currentTime << " max_date " << max_date << ", min_data " << min_data << std::endl;
        #endif

        // find the best candidate to integrate now
        // it is the oldest among oldest "all" and newest "latest" that are
        // available and older than max_date
        bool no_more_data;
        std::vector<SenInfo>::iterator best_candidate;
        RawInfo *best_candidate_info;
        double oldest_date;

        for(int t = 0; t < 2; t++)
        {
          no_more_data = true;
          best_candidate = infos.end();
          best_candidate_info = NULL;
          oldest_date = tnow + 3600;
          for(std::vector<SenInfo>::iterator it = infos.begin(); it != infos.end(); ++it)
          {
            #if DEBUG_SENSORMANAGER
            std::cout << "\tsen " << it->sen->id() << "(" << it->res << "," << it->first_ok << "," << it->num_ok << "," << it->infos.available.size() << ")";
            #endif
            if (it->res != -2) no_more_data = false;
            if (it->num_ok > 0)
            {
              RawInfo &info = it->infos.available[it->first_ok];
              if (info.timestamp < oldest_date) { best_candidate = it; best_candidate_info = &info; oldest_date = info.timestamp; }
              #if DEBUG_SENSORMANAGER
              std::cout << std::setprecision(16) << " " << info.timestamp;
              #endif
            }
            #if DEBUG_SENSORMANAGER
            if (it->infos.available.size() > 0)
              std::cout << " (first " << std::setprecision(16) << it->infos.available.front().timestamp << " last " << it->infos.available.back().timestamp << ")";
            std::cout << std::setprecision(16) << " (next " << it->infos.next.timestamp << std::setprecision(6) << " delay " << (it->infos.next.arrival-it->infos.next.timestamp) << ")" << std::endl;
            #endif
          }

          // if late remove data
          if (t == 0 && late && !best_candidate->infos.integrate_all && (best_candidate->sen->missed_data+1 <= min_data-1))
          {
            int n_remove = std::min(best_candidate->num_ok-1, min_data-1 - best_candidate->sen->missed_data);
            #if DEBUG_SENSORMANAGER
            std::cout << "LATE! remove " << n_remove << " data from sen " << best_candidate->sen->id() << "(prev missed " << best_candidate->sen->missed_data << ") and start again" << std::endl;
            #endif
            best_candidate->first_ok += n_remove;
            best_candidate->sen->missed_data += n_remove;
          } else
          break;
        }

        // return result
        if (best_candidate_info)
        {
          ProcessInfo result(best_candidate->sen, best_candidate_info->id, best_candidate_info->timestamp, best_candidate_info->arrival, best_candidate->infos.next.arrival);
          result.sen->missed_data = 0;
          do_dump(result, best_candidate->first_ok);
          #if DEBUG_SENSORMANAGER
          std::cout << "\t" << best_candidate->sen->id() << " <- select"  << std::endl;
          #endif

          return result;
        }
        else
        {
          #if DEBUG_SENSORMANAGER
          std::cout << "\tnone <- select" << std::endl;
          #endif
          return ProcessInfo(no_more_data, max_arrival); // wait
        }
      }

      virtual void stop()
      {
        if (dump)
        {
          loggerTask->push(new LoggableClose(fl));
          loggerTask->push(new LoggableClose(fb));
          loggerTask->push(new LoggableClose(fc));
          std::cout << "Waiting that SensorManager logs are closed by logging task..."; std::cout.flush();
          while (fb.is_open() || fl.is_open() || fc.is_open()) usleep(100000);
          std::cout << "OK." << std::endl;
        }
      }

      virtual void logData(sensor_ptr_t sen, double start_date, double waitedmove_date, double moved_date, double processed_date)
      {
        if (dump)
        {
          JFR_ASSERT(sen->id() == loggableChronogram->sen->id(), "Sensor inconsistency in SensorManager::logData");
          loggableChronogram->complete(start_date, waitedmove_date, moved_date, processed_date);
          loggerTask->push(loggableChronogram);
          loggableChronogram = NULL;
        }
        globalStats.move += moved_date-waitedmove_date;
        globalStats.wait_estimator += waitedmove_date-start_date;
        globalStats.stop_date = processed_date;
        SenStats &s = senStats[sen->id()];
        double process_time = processed_date-moved_date;
        s.total_time += process_time;
        if (process_time < s.min_time) s.min_time = process_time;
        if (process_time > s.max_time) s.max_time = process_time;
        s.nIntegrated++;
      }

  };

  
}}

#endif // SENSORMANAGER_HPP