test_slam_multi.hpp

#ifndef DDF_TEST_SLAM_MULTI_HPP
#define DDF_TEST_SLAM_MULTI_HPP

#include <iostream>
#include <fstream>
#include <BayesFilter/bayesFlt.hpp>
#include <BayesFilter/matSup.hpp>

#include "CANode.hpp"
#include "CSNode.hpp"
#include "upd_const_pose.hpp"
#include "upd_targets.hpp"
#include "robotsimu.hpp"
#include "eventsimu.hpp"
#include "velocitysimu.hpp"
#include "trackersimu.hpp"
#include "gdheclient.hpp"
#include "multirobsimu.hpp"
#include "trackrobot.hpp"
#include "ddf/debugging.hpp"

namespace jafar
{
  namespace ddfsimu
  {
    namespace demo
    {

      using namespace Bayesian_filter_matrix;
      using namespace Bayesian_filter;
      using namespace jafar::ddf;

      typedef SensorNodeIPC<ParametersGeneric>        CLNodeIPC;
      typedef SensorNodePeriodic<ParametersGeneric>   CLNode;

      const double h = 1.0;         // integration step for converting into var vel and var acc
      const double ts_pose = 0.05;  // initial uncertainty about the position +/- ts_pose (two sigma)
      const double ts_pose_targets = 0.15;  // 0.2
      const unsigned short CST_ACC_MODEL_DIM = 3;
      const unsigned short CST_SPD_MODEL_DIM = 3;
      const unsigned short BIASES_SIZE=3;

      const unsigned short PROF_SIZE_R1 = 9;
      double accProfR1[PROF_SIZE_R1][4] = {
        {0.0   ,0.   ,0.,-0.02},
        {10.0  ,0.   ,0.1,-0.02},
        {12.50,0.12  ,0.1,-0.02},
        {13.50,0.17  ,-0.1,0.02},
        {18.00,-0.12,-0.1,0.02},
        {18.50,-0.17,-0.15,0.0},
        {20.   ,-0.07,-0.15,-0.02},
        {25.0  ,-0.01,0.,0.0},
        {35.0  ,0.   ,0.,0.0}};

      const unsigned short PROF_SIZE_R2 = 6;
      double accProfR2[PROF_SIZE_R1][4] = {
        {0.0 ,0.   ,0.   ,0.0},
        {5.0 ,0.06 ,0.09 ,0.0},
        {12.0   ,-0.05,-0.03,0.0},
        {20. ,-0.04,-0.05 ,0.0},
        {25.0   ,0.0  ,0.00 ,0.0},
        {35.0   ,0.   ,0.   ,0.0}};

      const unsigned short NUM_TARGETS = 7;
      double targetPose[NUM_TARGETS][3] = {
        {-0.5,0.5,0.25},
        {5,4.5,1.0},
        {-1,3,0},
        {2,-2,0.5},
        {5.5,-0.5,0},
        {1,7,1.5},
        {1.5,0.75,0}
      };

   
      // Prediction model for the tracker nodes
      class CstPosePredictModelFactory : public PredictModelFactoryBase
      {
        unsigned short m_sv_size;
        VEC            m_q;
   
      public:
        CstPosePredictModelFactory(unsigned short sv_size, VEC const& var):
          m_sv_size(sv_size), m_q(sv_size)
        {   JFR_PRECOND(var.size() == sv_size, "bad sizes");   for(int i = 0; i < sv_size; i++) m_q[i] = var[i]; }
         
        virtual ~CstPosePredictModelFactory() {}
         
        PredictionEngineBase* CreatePredictionModel() { return new Upd_const_pose(m_sv_size,m_q); }
      };

      class TargetsPredictModelFactory : public PredictModelFactoryBase
      {
        unsigned short m_sv_size;
        unsigned short m_num_robots;
        unsigned short m_rob_id;
        VEC            m_q;
   
      public:
        TargetsPredictModelFactory(unsigned short sv_size, unsigned short num_robots, unsigned short rob_id, VEC const& var):
          m_sv_size(sv_size), m_num_robots(num_robots), m_rob_id(rob_id), m_q(var)
        { }
         
        virtual ~TargetsPredictModelFactory() {}
         
        PredictionEngineBase* CreatePredictionModel() { return new Upd_targets(m_sv_size,m_num_robots,m_rob_id,m_q.size(),m_q); }
      };

      //typedef CstAccPredictModelFactory       CANode_PredModFactory;
   
      // Associate a sensor node and a sensor
      template<typename SNODE_CLASS>
      class  SensorNodeCompoundGen
      {
        typedef std::vector<SensorBase* >   SensorArray;
            
      public:
         
        SNODE_CLASS  *snode;     // Pointer on the sensor node associated to sensor
        SensorArray  sensor;     // Pointer on the sensors
        EventSimu    *trigger;   // Pointer on the sensor that triggers the sensor node
        time         T_sync;
        StopWatch    m_sw_check; // Period checker in case of real_time execution
        bool         m_is_tracker;

      
        SensorNodeCompoundGen(): snode(NULL), trigger(NULL), T_sync(time::null()),m_is_tracker(false) {}
        SensorNodeCompoundGen(SensorNodeCompoundGen const& other) {
          snode   = other.snode;    sensor = other.sensor;
          trigger = other.trigger;  T_sync = other.T_sync;
          m_is_tracker = other.m_is_tracker;
        }
        void AddSensor(SensorBase * new_sens) { sensor.push_back(new_sens); }
        bool IsTrackerNode() const { return m_is_tracker; }
        void SetIsTrackerNode() { m_is_tracker = true; }
        unsigned short GetSensorNr() const { return sensor.size(); }
        bool SensorBelongsToNodebyID(unsigned short id) {
          typename SensorArray::iterator it;
  
          for(it = sensor.begin(); it != sensor.end(); it++) {
            if((*it)->GetID() == id) {
              return true;
            }
          }
          if(trigger->GetID() == id) {
            return true;
          }
          return false;
        }
         
        ~SensorNodeCompoundGen() {}
      };

      template<typename SNODE_CLASS>
      class Test_Dump
      {
        typedef std::vector<SensorNodeCompoundGen<SNODE_CLASS> > SNodesArray;
      
        SNodesArray    *m_psens_nodes;
        std::ofstream  m_dump_file;
        unsigned short m_nodes_nr;
        double         *m_snerror;
        unsigned int   m_row;
      
      public:
        Test_Dump(unsigned short nodes_nr, SNodesArray   *nodes_array, std::string fname)
          : m_psens_nodes(nodes_array), m_dump_file(fname.c_str(), std::ios::out|std::ios::trunc), m_nodes_nr(nodes_nr) {
          m_dump_file << std::scientific;
          DumpHeader();
          m_snerror = new double[m_nodes_nr];
          m_row = 0;
          for(int i = 0; i < m_nodes_nr; i++) { m_snerror[i] = 0.0; }
        }
         
        ~Test_Dump() {
          for(unsigned int j = 0; j < m_psens_nodes->size(); j++) JFR_DEBUG( "Rob: " << m_snerror[j]);
          m_dump_file << "\n"; Flush(); m_dump_file.close(); delete [] m_snerror;
        }

        void DumpStateDiag(InfoContainer const& info)
        {
          m_dump_file << info.Getinfo()[AccelMotion::SV_X] << LOGSEP << info.GetInfoMat()(AccelMotion::SV_X,AccelMotion::SV_X) << LOGSEP;
        }

        void DumpStateDiag(InfoContainer const& info, unsigned short robid)
        {
          m_dump_file << info.Getinfo()[AccelMotion::SV_X+6*robid] << LOGSEP << info.GetInfoMat()(AccelMotion::SV_X+6*robid,AccelMotion::SV_X+6*robid) << LOGSEP;
        }

        VEC DumpTrueState(double const& t, RobotSimu const& rob)
        {
          VEC  acc(3), vel(3), pos(3);
            
          rob.ComputeVelocityAndPos(t, vel, pos);
          acc = rob.GetAccelAtTimeT(t);
              
          m_dump_file << t << LOGSEP << pos[0] << LOGSEP << vel[0] << LOGSEP <<  acc[0] << LOGSEP;

          return pos;
        }
  
        void DumpCurrentState(time const& origin, time const& stamp, RobotSimu const& rob, unsigned int POM_node_id)
        {
          int  j = 0;
          VEC  tpose(3);
  
              
          typename SNodesArray::iterator it, it_pom;
          double dtmp;
    
          m_dump_file << std::endl;
          m_dump_file << m_row++ << LOGSEP << rob.GetCurrentSensorID() << LOGSEP << stamp.to_double()  << LOGSEP;
            
          for(it = m_psens_nodes->begin(); it != m_psens_nodes->end(); it++, j++)
            {
              dtmp = ((*it).snode->GetLocalInfoTime() - origin).to_double();
              if(dtmp > rob.GetMaxTime()) break;
              tpose = DumpTrueState(dtmp,rob);
              if(!(*it).IsTrackerNode()){
                DumpStateDiag((*it).snode->GetLocalInfo());
                DumpStateDiag((*it).snode->GetLocalState());
                dtmp = tpose[0] - (*it).snode->GetLocalState().Getinfo()[0];
              }
              else {
                DumpStateDiag((*it).snode->GetLocalInfo(), rob.GetRobID());
                DumpStateDiag((*it).snode->GetLocalState(), rob.GetRobID());
                dtmp = tpose[0] - (*it).snode->GetLocalState().Getinfo()[6*rob.GetRobID()];
              }
                  
              m_snerror[j] += (dtmp * dtmp);
              m_dump_file << m_snerror[j] << LOGSEP;
    
              if((*it).snode->GetID() == POM_node_id) it_pom = it;
            }
    
          DumpAllSv((*it_pom).snode->GetLocalState());
        }
    
        void DumpAllSv(InfoContainer const& info)
        {
          m_dump_file << info.GetSize() << LOGSEP;
          for(unsigned int i = 0; i < info.GetSize(); i++) m_dump_file << info.Getinfo()[i] << LOGSEP;
        }
    
        void DumpCustom(VEC const& data) {
          for(unsigned int i = 0; i < data.size(); i++) m_dump_file << data[i] << LOGSEP;
        }
    
        void Flush() { m_dump_file << std::flush; }
            
      private:
        void DumpHeader()
        {
          m_dump_file << "# log file for " << m_nodes_nr << " nodes" << std::endl;
          m_dump_file << "# 1=row number | 2=sensor id | 3=time stamp(available). Then a blocks for each sensor node\n";
          m_dump_file << "# o0=local filter time o1,2,3=true state at time o0 (px,vx,ax) o4,o5=info vec px, info px o6,o7=state px,var px o8=sum of squared error | then the size of the state vector of POM and its content | finally custom user data\n";
        }
      };
    
        // Associate a robot with an array of sensor nodes/sensors
      template<typename SNODE_CLASS>
      class RobotSimuNodesGen : public RobotSimu
      {
        typedef std::vector<SensorNodeCompoundGen<SNODE_CLASS> > SNodesArray;
        typedef std::vector<VEC>                                 TrajRob;

      public:
        SNodesArray           m_nodes;
        ParametersGeneric     m_params;
        SNODE_CLASS           *m_POM_node;         // the prefered node to get state from
        SNODE_CLASS           *m_Tracker_node;
        TrajRob               m_traj;
        unsigned short        m_traj_id;           // for GDHE
        unsigned short        m_true_traj_id;
        std::vector<unsigned char>     m_color;    // color of traj
        Test_Dump<SNODE_CLASS>   *m_pDump;
        InfoContainer         m_CurInfo;           // temp variables
        bool                  m_just_perceive;     // for gdhe
        bool                  m_first;
        TrackerSimuUncorr     *m_tracker_sensor;
        unsigned short        m_gdhe_persist;
            
      public:
        RobotSimuNodesGen(unsigned short sv_size, std::string name, unsigned short rob_id, bool use_only_vel, bool use_biases = false)
          : RobotSimu(sv_size, name, use_only_vel, use_biases),
            m_POM_node(NULL),
            m_Tracker_node(NULL),
            m_traj_id(0),
            m_true_traj_id(0),
            m_color(3),
            m_pDump(NULL),
            m_CurInfo(sv_size),
            m_just_perceive(false),
            m_first(true),
            m_tracker_sensor(NULL),
            m_gdhe_persist(10) {m_rob_id = rob_id;}
    
        ~RobotSimuNodesGen() {
          typename SNodesArray::iterator  it;
          for(it = m_nodes.begin(); it != m_nodes.end(); it++)
            if ((*it).snode != NULL) delete (*it).snode;
          if (m_pDump != NULL) delete m_pDump;
        }
    
        int GetTrackerID() { return static_cast<SensorBase*>(m_tracker_sensor)->GetID(); }
        void SetTrackerPtr(TrackerSimuUncorr *tracker_sensor) {
          m_tracker_sensor = tracker_sensor;
        }
    
        void AddTrajPoint(VEC const& point) {
          if(m_traj.empty() || !vec_compare(point,m_traj.back())) m_traj.push_back(point);
        }
    
        void CreateDumpFile(std::string const& fname)
        {
          JFR_PRECOND (m_pDump == NULL, "not NULL");
          m_pDump = new Test_Dump<SNODE_CLASS>(m_nodes.size(), &m_nodes, fname);
        }
    
        void DumpCurrentState(time const& rel_time)
        {
          m_pDump->DumpCurrentState(this->GetMinTime(), rel_time, *this, this->m_POM_node->GetID());
        }
    
        void DumpCustom(VEC const& data) {
          m_pDump->DumpCustom(data);
        }
    
        void ClearTraj()
        {
          m_traj.clear();
          m_traj_id = 0;
          m_true_traj_id = 0;
        }
    
        void AddTrajPoint(double x, double y, double z) {
          VEC point(3);
          point[0] = x; point[1] = y; point[2] = z;
          AddTrajPoint(point);
        }
    
        void SetColor(unsigned char r, unsigned char g, unsigned char b) {
          m_color[0] = r;  m_color[1] = g;   m_color[2] = b;
        }
      };
    
      class Target
      {
        VEC            m_position;    // True position of the target
        unsigned short m_gdhe_id;     // id of the ellipsoid
        std::vector<unsigned short>   m_gdhe_seg_ids;
        unsigned short m_num_robots;
          
      public:
        Target(VEC const& pose, unsigned short num_rob): m_position(pose), m_gdhe_id(0)/*,m_gdhe_id_segment(0)*/
        { for(int i = 0; i < num_rob; i++)m_gdhe_seg_ids.push_back(0);  }
        void SetGDHEID(unsigned short id) {m_gdhe_id = id; }
        void SetGDHEIDSegment(unsigned short id, unsigned short rob_num) {m_gdhe_seg_ids[rob_num] = id; }
        unsigned short GetGDHEID() const { return m_gdhe_id; }
        unsigned short GetGDHEIDSegment(unsigned short rob_num) {return m_gdhe_seg_ids[rob_num]; }
        VEC const& GetPose() const { return m_position; }
      };
    
      template<typename SNODE_CLASS>
      class test_SimuSensorNode
        {
        typedef std::vector<RobotSimuNodesGen<SNODE_CLASS> * >   RobotsArray;
        typedef std::vector<Target * >                           TargetsArray;     // useless make array of x,y,z and m_gdhe_id
        typedef SensorNodeCompoundGen<SNODE_CLASS>               SensorNodeCompound;
        typedef std::vector<SensorNodeCompoundGen<SNODE_CLASS> > SNodesArray;
        typedef std::vector<unsigned short>                      SVsizeArray;
          
      private:
        unsigned short m_rob_num;
        unsigned short m_num_targets;
        unsigned short m_targets_sv_size;
        unsigned short m_targets_q_size;
        unsigned short m_obj_to_track;
        bool           m_real_time;
        bool           m_use_biases;
        bool           m_track_other_robot;
        RobotsArray    m_rob_array;
        TargetsArray   m_targets;
        GdheClient     m_gdhecl;
        MultiRobSimu   m_multi_rob;
  
      public:
          
        test_SimuSensorNode(unsigned short num_robots, bool real_time, bool track_other_robot)
          :   m_rob_num(num_robots),
              m_num_targets(NUM_TARGETS),
              m_targets_sv_size((m_rob_num*2+m_num_targets)*3),
              m_targets_q_size((m_rob_num+m_num_targets)*3),
              m_real_time(real_time),
              m_use_biases(true),
              m_track_other_robot(track_other_robot),
              m_gdhecl((char *)"localhost")
        {
          JFR_PRECOND(num_robots <= 2, "more robot not supported yet");
          m_obj_to_track = m_num_targets + (m_track_other_robot ? (1):(0));
          CreateGDHEWorld();
          InitTargets(targetPose, m_num_targets);
          CreateGDHETargets();
        }
          
        virtual ~test_SimuSensorNode() {
          typename RobotsArray::iterator  it;
          typename TargetsArray::iterator itt;
          
          for(it = m_rob_array.begin(); it != m_rob_array.end(); it++)
            delete *it;
        
          for(itt = m_targets.begin(); itt != m_targets.end(); itt++)
            delete *itt;
          
          ClearGDHEWorld();
        }
  
        void test_Simu()
        {
          if(m_rob_num == 1)
          {
            run_Simu(false);
            run_Simu(true);
          }
          else
          {
            if(!m_real_time)
            {      
              run_Simu_Multi(true);
            }
            else
              JFR_DEBUG("Not yet implemented");
          }
        }

        // *** Print functions ***
        void PrintNodes(RobotSimuNodesGen<SNODE_CLASS> & rob)
        {
          std::ostringstream stream;

          typename SNodesArray::iterator it;
          for(it = rob.m_nodes.begin(); it != rob.m_nodes.end(); it++){
            stream << "\n** Node id: " << (*it).snode->GetID() << "\n";
            stream << "Assoc. sensor ids: ";
            for(int i = 0; i < (*it).GetSensorNr(); i++) stream << (*it).sensor[i]->GetID() << " ";
            if(!m_real_time) stream << "\nAssoc. trigger id: " << (*it).trigger->GetID() << "\n";
            stream << "Next sync: " << (*it).snode->GetNextHorizon() << "\nT_sync: " << (*it).T_sync << "\n----\n";
          }

          JFR_DEBUG(stream.str());
        }
        
        void PrintRobots()
        {
          typename RobotsArray::iterator  it;
          for(it = m_rob_array.begin(); it != m_rob_array.end(); it++) {
            (*it)->PrintRobot();
            PrintNodes(*(*it));
          }
        }
    
        unsigned short GetNumRobots() { return m_rob_num; }
  
      private:
  
        void PrepareSimu(bool delayed, bool multi)
        {
          for(int i = 0; i < GetNumRobots(); i++) InitRobotSensors(i, delayed);
          PrintRobots();
    
          ClearAllMeasurements();
          SimulatePropPerception(delayed, multi);
  
          CreateGDHERobots();
          SetGDHERobotsPos();
          DrawGDHETrueTraj();
        }
    
        void ClearSimu()
        {
          DestroyAllSensorNodes();
          ClearRobotsAccelProfile();
          RemoveGDHETraj();
          RemoveGDHETargetsSegment();
          DestroyRobots();
        }
    
        void run_Simu_Multi(bool delayed)
        {
          typename RobotsArray::iterator  it;
          
          PrepareSimu(delayed, true);
          m_multi_rob.ResetCounter();
          m_multi_rob.ClearBuffer();
            
          for(it = m_rob_array.begin(); it != m_rob_array.end(); it++)
              m_multi_rob.AddRobotMeasurements(*(*it));
    
          if(delayed)
            m_multi_rob.DumpAllMeasures("sim_multi_delayed.dat");
          else
            m_multi_rob.DumpAllMeasures("sim_multi.dat");
    
          // complete TrackerSimu if needed
          if(m_track_other_robot) {
            m_rob_array[0]->m_tracker_sensor->AddRobotToTracker(m_rob_array[1]);
            m_rob_array[1]->m_tracker_sensor->AddRobotToTracker(m_rob_array[0]);
          }
            
          m_rob_array[0]->m_nodes[2].snode->RequestConnection(m_rob_array[1]->m_nodes[2].snode->GetID());
    
          FilterMultiPerceptionSimulation(delayed);
    
          ClearSimu();
        }
          
        void run_Simu(bool delayed)
        {
          typename RobotsArray::iterator  it;
          
          PrepareSimu(delayed, false);
            
          // Main loop
          if (m_real_time) {
            for(it = m_rob_array.begin(); it != m_rob_array.end(); it++)
              FilterPropPerceptionRealTime(*(*it));
          }
          else {
            for(it = m_rob_array.begin(); it != m_rob_array.end(); it++){
              (*it)->PrintRobot();
              PrintNodes(*(*it));
              FilterPropPerceptionSimulation(*(*it),delayed);
            }
          }
            
          ClearSimu();
        }
        
        // *** Initialization functions ***
        
        // pm_pose is the initial uncertainty of the position expressed with two sigma (same for all the dimensions)
        void InitState(bool use_biases, bool use_vel_model, double ts_position, VEC const& x0, VEC const& v0, VEC const& a0, double h, VEC & y_init, MSYM & Y_init)
        {
          unsigned short sv_x, sv_vx, sv_ax = 0;
          unsigned short sv_y, sv_vy, sv_ay = 0;
          unsigned short sv_z, sv_vz, sv_az = 0;
    
          sv_x = AccelMotion::SV_X;  sv_vx = AccelMotion::SV_VX;
          if(use_vel_model) {
            sv_y = 2; sv_vy = 3;
            sv_z = 4; sv_vz = 5;
          }
          else {
            sv_ax = AccelMotion::SV_AX;   sv_ay = AccelMotion::SV_AY;   sv_az = AccelMotion::SV_AZ;
            sv_y  = AccelMotion::SV_Y;    sv_vy = AccelMotion::SV_VY;
            sv_z  = AccelMotion::SV_Z;    sv_vz = AccelMotion::SV_VZ;
          }
          
          VEC  x_init(y_init.size());
          MSYM X_init(y_init.size(),y_init.size());
        
          y_init.clear();  Y_init.clear();   x_init.clear(); X_init.clear();
          
          x_init[sv_x]  = x0[AXIS_X];  x_init[sv_vx] = v0[AXIS_X];
          x_init[sv_y]  = x0[AXIS_Y];  x_init[sv_vy] = v0[AXIS_Y];
          x_init[sv_z]  = x0[AXIS_Z];  x_init[sv_vz] = v0[AXIS_Z];
          
          X_init(sv_x,sv_x)   = TWOSIGMA_TO_VAR(ts_position);
          X_init(sv_vx,sv_vx) = VARPOS_TO_VARVEL(TWOSIGMA_TO_VAR(ts_position), h);
    
          X_init(sv_y,sv_y)   = TWOSIGMA_TO_VAR(ts_position);
          X_init(sv_vy,sv_vy) = VARPOS_TO_VARVEL(TWOSIGMA_TO_VAR(ts_position), h);
          
          X_init(sv_z,sv_z)   = TWOSIGMA_TO_VAR(ts_position);
          X_init(sv_vz,sv_vz) = VARPOS_TO_VARVEL(TWOSIGMA_TO_VAR(ts_position), h);
          
    
          if(!use_vel_model) {
            X_init(sv_ax,sv_ax) = VARPOS_TO_VARACC(TWOSIGMA_TO_VAR(ts_position), h);
            X_init(sv_ay,sv_ay) = VARPOS_TO_VARACC(TWOSIGMA_TO_VAR(ts_position), h);
            X_init(sv_az,sv_az) = VARPOS_TO_VARACC(TWOSIGMA_TO_VAR(ts_position), h);
            x_init[sv_ax] = a0[AXIS_X];   x_init[sv_ay] = a0[AXIS_Y];   x_init[sv_az] = a0[AXIS_Z];
    
            if (use_biases)
              {
              X_init(AccelMotion::SV_BAX,AccelMotion::SV_BAX) = TWOSIGMA_TO_VAR(0.08);
              X_init(AccelMotion::SV_BAY,AccelMotion::SV_BAY) = TWOSIGMA_TO_VAR(0.08);
              X_init(AccelMotion::SV_BAZ,AccelMotion::SV_BAZ) = TWOSIGMA_TO_VAR(0.08);
              }
          }
    
          
          UdUinversePD (Y_init, X_init);
          Float rcond = UdUinversePD (Y_init, X_init);
          Numerical_rcond  rclimit;
          rclimit.check_PD(rcond, "Initial X not PD");
          noalias(y_init) = prod(Y_init,x_init);
        }
    
          
        void InitRobotSensors(unsigned short robnr, bool delayed)
        {
          std::ostringstream     rob_name;
          CANode_CommFactory*    pCommFact;
          SNODE_CLASS*           pNode;
          EventSimu*             pEventSimu;
          RobotSimuNodesGen<SNODE_CLASS>*       rob;// = m_rob_array[robnr];
          TargetsPredictModelFactory*  pPredModeFact_targets;
          VEC                    var_tar(3*m_obj_to_track), var_sv_targets(m_targets_q_size), y_targ_init(m_targets_sv_size);
          MSYM                   Y_targ_init(m_targets_sv_size,m_targets_sv_size);
          int                    i,j;
    
          var_sv_targets.clear();
    
          rob_name << "R" << robnr;
            
          switch (robnr)
            {
            case 0 :
              {
              // Create R0
              // The robot has 4 sensors: 1 accelerometer, 1 velocimeter, 1 GPS and a 1 tracker
              // (exteroceptive sensor for tracking targets in the scene).
              // The accelerometer and the velocimeter feed the same sensor node 0
              // The GPS and the tracker have their own node (respectively 1 and 2)
              // This architecture has been chosen because:
              // 1) we separate sensors without delay (computational time neglectable) and the sensors with delay generally with a much longer period
              //    this allows to limit the size of the history buffer and thus, makes the insertion of asequent data with less computation time
              // 2) node 0 and 1 share the same state vector whereas 2 estimates another state vector
              // A constant acceleration prediction model has been chosen for node 0 and 1 because the acceleration is available whereas
              // a simpler model (constant speed has been chosen for node 2): the update rate is slow and the acceleration cannot be estimated
              // properly (using the position only)
              // The state vector of node 0 and 1 constains position, velocity, acceleration for each axis (dimension 9)
              // and the state vector of the node 2 has a dimension of 6+num_targets*3 (position, velocity for each axis and position of each target)
              // the prediction model of the target is static (constant position)
              // The node chosen as POM is node 0 because it updates the state more frequently.
  
              // Add a new robot to the list
              if(m_use_biases)
                rob = new RobotSimuNodesGen<SNODE_CLASS>(3*CST_ACC_MODEL_DIM+BIASES_SIZE, rob_name.str(), robnr, false);
              else
                rob = new RobotSimuNodesGen<SNODE_CLASS>(3*CST_ACC_MODEL_DIM, rob_name.str(), robnr, false);
                      
              m_rob_array.push_back(rob);
    
              // Setup initial conditions
              const unsigned short  num_sensors = 4;
              const unsigned short  num_nodes = 3;
              enum {NODE_POM, NODE_GPS, NODE_TRACK} ;
              CANode_PredModFactory*   pPredModeFact_accel;
              SensorNodeCompound       compound[num_nodes];
              VEC                      v3(CST_ACC_MODEL_DIM), y_init(rob->GetSvSize());
              VEC                      x_init(CST_ACC_MODEL_DIM), v_init(CST_ACC_MODEL_DIM);
              VEC                      v6(6);
              MSYM                     Y_init(rob->GetSvSize(),rob->GetSvSize());
              time                     t_orig;
                  
              x_init[0] = 0.0;  x_init[1] = 0.0; x_init[2] = 4.0;
              v_init[0] = 0.03; v_init[1] = 0.05;  v_init[2] = 0.15;
    
              //v_init[0] = 0.0; v_init[1] = 0.0; v_init[2] = 0.0;
    
              rob->FillProfile(PROF_SIZE_R1,accProfR1);
              rob->SetInitialConditions(v_init, x_init);
              t_orig = rob->GetMinTime();
                  
              // Create accelerometer
              AcceleroSimuUncorr *accelero = new AcceleroSimuUncorr(rob->GetSvSize(), CST_ACC_MODEL_DIM, m_use_biases);
              v3[0] = TWOSIGMA_TO_VAR(0.04);  v3[1] = v3[0];      v3[2] = v3[0];
              accelero->SetSeed(0);          accelero->SetSensorSimVariance(v3);
              v3[0] = TWOSIGMA_TO_VAR(0.041); v3[1] = v3[0];      v3[2] = v3[0];
              accelero->SetSensorModelVar(v3);
              accelero->SetPeriod(time::from_ms(16));
              if(m_use_biases) {
                VEC v(3);
                v.clear(); v[0]= 0.04; v[1] = 0.01;
    
                v.clear();
                accelero->SetBiases(v);
              }
              //accelero->PrintObservationModel();
    
              // Create GPS
              GPSSimuUncorr *gps = new GPSSimuUncorr(rob->GetSvSize(), CST_ACC_MODEL_DIM);
              v3[0] = TWOSIGMA_TO_VAR(0.05); v3[1] = v3[0];    v3[2] = v3[0];
              gps->SetSeed(1);                  gps->SetSensorSimVariance(v3);
              v3[0] = TWOSIGMA_TO_VAR(0.051);   v3[1] = v3[0];    v3[2] = v3[0];
              gps->SetSensorModelVar(v3);
              gps->SetPeriod(time::from_ms(250));
              if(delayed) gps->SetProcTime(time::from_ms(100));   // 100
              else gps->SetProcTime(time::null());
    
              // Create velocimeter
              VelocitySimuUncorr *vel = new VelocitySimuUncorr(rob->GetSvSize(), CST_ACC_MODEL_DIM);
              v3[0] = TWOSIGMA_TO_VAR(0.03); v3[1] = v3[0];    v3[2] = v3[0];
              vel->SetSeed(2);                  vel->SetSensorSimVariance(v3);
              v3[0] = TWOSIGMA_TO_VAR(0.031);   v3[1] = v3[0];    v3[2] = v3[0];
              vel->SetSensorModelVar(v3);
              vel->SetPeriod(time::from_ms(80));
    
              // Create tracker
              TrackerSimuUncorr  *tracker = new TrackerSimuUncorr(m_targets_sv_size,0, m_num_targets, rob->UseOnlyVel(),m_track_other_robot ? 1:0);
              for(i = 0; i < m_obj_to_track*3; i++) var_tar[i] = TWOSIGMA_TO_VAR(0.30);
              tracker->SetSeed(3);        tracker->SetSensorSimVariance(var_tar);
              for(i = 0; i < m_obj_to_track*3; i++) var_tar[i] = TWOSIGMA_TO_VAR(0.31);
              
              tracker->SetSensorModelVar(var_tar);
              tracker->SetPeriod(time::from_ms(1000));
              tracker->SetTimeOffset(time::from_ms(1250));
              if(delayed) tracker->SetProcTime(time::from_ms(700));
              else tracker->SetProcTime(time::null());
              InitSensorTargets(tracker);
              rob->SetTrackerPtr(tracker);
    
              // Add a trigger for feeding the tracker node with POM data
             /* pEventSimu = new EventSimu(0);
              pEventSimu->SetSeed(18);
              pEventSimu->SetPeriod(time::from_ms(60000));
              pEventSimu->SetUpdateTrackerWithPOM(true);*/
    
              // Add sensors to the robot
              rob->AddSensor(accelero);   compound[NODE_POM].AddSensor(accelero);
              rob->AddSensor(vel);        compound[NODE_POM].AddSensor(vel);
              rob->AddSensor(gps);        compound[NODE_GPS].AddSensor(gps);
              rob->AddSensor(tracker);    compound[NODE_TRACK].AddSensor(tracker);
    
              time now = time::from_ms(400);
                  
              //pEventSimu->SetTimeOffset(time::from_ms(400));
              //rob->AddSensor(pEventSimu);compound[NODE_TRACK].AddSensor(pEventSimu);
    
              for(i = 0; i < num_nodes; i++)
                {
    
                  if(i != NODE_TRACK){
                    compound[i].T_sync = time::from_ms(125);
    
                    if(i == NODE_POM)
                      for(j = 0; j < CST_ACC_MODEL_DIM; j++) v3[j] = TWOSIGMA_TO_VAR(0.2);  // 0.2
                    else
                      for(j = 0; j < CST_ACC_MODEL_DIM; j++) v3[j] = TWOSIGMA_TO_VAR(0.2); /* 2e-2prediction more uncertain than NODE_POM
                                                                          because not direct measure of acceleration and longer period */
                        
                    pCommFact      = new CANode_CommFactory(rob->GetSvSize());
                    if(m_use_biases) {
                            
                    for(j = 0; j < CST_ACC_MODEL_DIM; j++) v6[j] = v3[j];
                    if(i == NODE_POM)
                      for(j = 3; j < 2*CST_ACC_MODEL_DIM; j++) v6[j] = TWOSIGMA_TO_VAR(0.01); // for the biases 1e-1
                    else
                      for(j = 3; j < 2*CST_ACC_MODEL_DIM; j++) v6[j] = TWOSIGMA_TO_VAR(0.01);   //
                            
                    pPredModeFact_accel   = new CANode_PredModFactory(CST_ACC_MODEL_DIM, rob->GetSvSize(), 2*CST_ACC_MODEL_DIM, v6, m_use_biases);
                    pNode = new  SNODE_CLASS(rob->GetSvSize(), compound[i].T_sync, pPredModeFact_accel, pCommFact, delayed);
                    //pNode->PrintPredModel();
                    }
                    else {
                      pPredModeFact_accel   = new CANode_PredModFactory(CST_ACC_MODEL_DIM, rob->GetSvSize(), CST_ACC_MODEL_DIM, v3, m_use_biases);
                      pNode = new  SNODE_CLASS(rob->GetSvSize(), compound[i].T_sync, pPredModeFact_accel, pCommFact, delayed);
                    }
                    InitState(m_use_biases, false, ts_pose, rob->GetInitialPosition(), rob->GetInitialVelocity(), rob->GetInitialAccel(), h, y_init, Y_init);
                    pNode->SetInitWithInfo(y_init, Y_init);
                  }
                  else {
                    compound[i].T_sync = time::from_ms(500);
                    compound[i].SetIsTrackerNode();
                        
                    // uncertainty for the robot's pose (11)
                    if(delayed) {
                      for(j = 0; j < 3; j++) var_sv_targets[j] = TWOSIGMA_TO_VAR(0.8);   // 0.8
                      for(j = 3; j < 3*m_rob_num; j++) var_sv_targets[j] = TWOSIGMA_TO_VAR(0.8); // 0.8 ?
                    }
                    else {
                      for(j = 0; j < 3; j++) var_sv_targets[j] = TWOSIGMA_TO_VAR(0.5); // 11, 6
                      for(j = 3; j < 3*m_rob_num; j++) var_sv_targets[j] = TWOSIGMA_TO_VAR(0.5);
                    }
                        
                    // 0.01 -> 2 sigma = 5 cm for a period of 1s (this is the uncertainty for the state prediction of the target)
                    for(j = 3*m_rob_num; j < m_targets_q_size; j++) var_sv_targets[j] = TWOSIGMA_TO_VAR(0.0001); // 0.0001
    
                    pCommFact      = new CANode_CommFactory(m_targets_sv_size);
                    pPredModeFact_targets   = new TargetsPredictModelFactory(m_targets_sv_size, m_rob_num, robnr, var_sv_targets);
                    pNode = new SNODE_CLASS(m_targets_sv_size, compound[i].T_sync, pPredModeFact_targets, pCommFact, delayed);
                    InitTargetState(ts_pose_targets, ts_pose, rob->GetRobID(), rob->GetInitialPosition(), rob->GetInitialVelocity(), y_targ_init, Y_targ_init);
                    pNode->SetInitWithInfo(y_targ_init, Y_targ_init);
                  }
    
                  pNode->SetEpsilon(time::from_us(1));
                  pNode->SetHistoryLength(time(2.0));
                  pNode->SetNow(t_orig);
                  compound[i].snode = pNode;
    
                  if (!m_real_time)
                  {
                    pEventSimu = new EventSimu(pNode->GetID());
                    if(i == NODE_TRACK) pEventSimu->SetSyncTracker(true);
                    
                    pEventSimu->SetTimeOffset(now);
                    pNode->SetNextHorizon(now);
                        
                    pEventSimu->SetSeed(num_sensors+i);
                    pEventSimu->SetPeriod(compound[i].T_sync);
                        
                    compound[i].trigger = pEventSimu;
                    rob->AddSensor(pEventSimu);
                    pNode->SetLogChanFilters(true);
                  }
                  else pNode->InitNode();
                      
                  rob->m_nodes.push_back(compound[i]);
                      
                  now = now + time::from_ms(compound[i].T_sync.ms()/num_nodes);
                }
    
              // Add same time stamping jitter for all sensors
              rob->InitStampJitter(time::from_ms(1));
    
              // Connect nodes together
              rob->m_nodes[NODE_POM].snode->RequestConnection(rob->m_nodes[NODE_GPS].snode->GetID());
              rob->m_POM_node = rob->m_nodes[NODE_POM].snode;
              rob->m_Tracker_node = rob->m_nodes[NODE_TRACK].snode;
    
              // Set robot's color
              rob->SetColor(255,0,0);
              }
              break;
                  
            case 1 :
              {
              // Create R1
              // The robot has 3 sensors: 1 velocimeter (sensor node 0), 1 GPS (sensor node 1) and a 1 tracker (sensor node 2)
              // A constant velocity prediction model has been chosen for all nodes (the acceleration is not measured)
              // The state vector of node 0 and 1 constains position, velocity for each axis (dimension 6)
              // Node 2 has the same caracteristics as node 2 of R0
              // The node chosen as POM is node 0 because it updates the state more frequently
    
              // Create a new robot and add to the list
              rob = new RobotSimuNodesGen<SNODE_CLASS>(2*CST_SPD_MODEL_DIM, rob_name.str(), robnr, true);
              m_rob_array.push_back(rob);
    
              // Setup initial condition and acceleration profile
              const unsigned short  num_sensors = 4;
              enum {NODE_POM, NODE_GPS, NODE_TRACK, NODE_TRACK_ROB};
              CSNode_PredModFactory*   pPredModeFact;
              SensorNodeCompound    compound[num_sensors];
              VEC                      v3(CST_SPD_MODEL_DIM), y_init(rob->GetSvSize());
              VEC                      x_init(CST_SPD_MODEL_DIM), v_init(CST_SPD_MODEL_DIM);
              MSYM                     Y_init(rob->GetSvSize(),rob->GetSvSize());
              time                     t_orig;
                  
              x_init[0] = 3.0;   x_init[1] = -1.0;    x_init[2] = 0.0;
              v_init[0] = 0.05;  v_init[1] = -0.02;   v_init[2] = 0.0;
    
              rob->FillProfile(PROF_SIZE_R2,accProfR2);
              rob->SetInitialConditions(v_init, x_init);
              t_orig = rob->GetMinTime();
    
    
              // Create GPS
              GPSSimuUncorr *gps = new GPSSimuUncorr(rob->GetSvSize(), CST_SPD_MODEL_DIM, true);
              v3[0] = TWOSIGMA_TO_VAR(0.06); v3[1] = v3[0];    v3[2] = v3[0];
              gps->SetSeed(4);                  gps->SetSensorSimVariance(v3);
              v3[0] = TWOSIGMA_TO_VAR(0.061);   v3[1] = v3[0];    v3[2] = v3[0];
              gps->SetSensorModelVar(v3);
              gps->SetPeriod(time::from_ms(200));
              if(delayed) gps->SetProcTime(time::from_ms(120));
              else gps->SetProcTime(time::null());
    
              // Create velocimeter
              VelocitySimuUncorr *vel = new VelocitySimuUncorr(rob->GetSvSize(), CST_SPD_MODEL_DIM, true);
              v3[0] = TWOSIGMA_TO_VAR(0.02); v3[1] = v3[0];    v3[2] = v3[0];
              vel->SetSeed(5);                  vel->SetSensorSimVariance(v3);
              v3[0] = TWOSIGMA_TO_VAR(0.021);   v3[1] = v3[0];    v3[2] = v3[0];
              vel->SetSensorModelVar(v3);
              vel->SetPeriod(time::from_ms(20));
    
              // Create tracker
              TrackerSimuUncorr  *tracker = new TrackerSimuUncorr(m_targets_sv_size,robnr, m_num_targets, rob->UseOnlyVel(),m_track_other_robot ? 1:0);
              for(i = 0; i < m_obj_to_track*3; i++) var_tar[i] = TWOSIGMA_TO_VAR(0.25);
              tracker->SetSeed(6);        tracker->SetSensorSimVariance(var_tar);
              for(i = 0; i < m_obj_to_track*3; i++) var_tar[i] = TWOSIGMA_TO_VAR(0.26);
              tracker->SetSensorModelVar(var_tar);
              tracker->SetPeriod(time::from_ms(1000));
              tracker->SetTimeOffset(time::from_ms(1000));
              if(delayed) tracker->SetProcTime(time::from_ms(900));
              else tracker->SetProcTime(time::null());
              InitSensorTargets(tracker);
              tracker->PrintTargets();
              rob->SetTrackerPtr(tracker);
    
              rob->AddSensor(gps);     compound[NODE_GPS].AddSensor(gps);
              rob->AddSensor(vel);     compound[NODE_POM].AddSensor(vel);
              rob->AddSensor(tracker); compound[NODE_TRACK].AddSensor(tracker);
    
              // Add triggers
              time now = time::from_ms(450);
    
             /* pEventSimu = new EventSimu(0);
              pEventSimu->SetSeed(19);
              pEventSimu->SetPeriod(time::from_ms(60000));
              pEventSimu->SetUpdateTrackerWithPOM(true);
    
              pEventSimu->SetTimeOffset(time::from_ms(400));
              //rob->AddSensor(pEventSimu);compound[NODE_TRACK].AddSensor(pEventSimu);*/
                  
    
              // Configure sensor nodes
              for(i = 0; i < num_sensors; i++)
                {
                  if(i == NODE_TRACK_ROB) break;
                  if (i != NODE_TRACK) {
                    compound[i].T_sync = time::from_ms(125);
    
                    if(i == NODE_POM)
                    for(j = 0; j < CST_ACC_MODEL_DIM; j++) v3[j] = TWOSIGMA_TO_VAR(0.7); // 0.7
                    else
                    for(j = 0; j < CST_ACC_MODEL_DIM; j++) v3[j] = TWOSIGMA_TO_VAR(0.7); // 0.7
    
                    pCommFact      = new CANode_CommFactory(rob->GetSvSize());
                    pPredModeFact  = new CSNode_PredModFactory(CST_SPD_MODEL_DIM, rob->GetSvSize(), CST_SPD_MODEL_DIM, v3);
                    pNode       = new SNODE_CLASS(rob->GetSvSize(), compound[i].T_sync, pPredModeFact, pCommFact, delayed);
                    InitState(false, true, ts_pose, rob->GetInitialPosition(), rob->GetInitialVelocity(), rob->GetInitialAccel(), h, y_init, Y_init);
                    pNode->SetInitWithInfo(y_init, Y_init);
                  }
                  else {
                    compound[i].T_sync = time::from_ms(500);
                    compound[i].SetIsTrackerNode();
    
                    if(delayed){
                    for(j = 3; j < 3*m_rob_num; j++) var_sv_targets[j] = TWOSIGMA_TO_VAR(0.23); // 0.23
                    for(j = 0; j < 3; j++) var_sv_targets[j] = TWOSIGMA_TO_VAR(0.23);  // 0.23
                    }
                    else {
                    for(j = 3; j < 3*m_rob_num; j++) var_sv_targets[j] = TWOSIGMA_TO_VAR(3); // 5e3
                    for(j = 0; j < 3; j++) var_sv_targets[j] = TWOSIGMA_TO_VAR(3);
                    }
                            
                    for(j = 3*m_rob_num; j < m_targets_q_size; j++) var_sv_targets[j] = TWOSIGMA_TO_VAR(0.0001); // 0.0001
                        
                    pCommFact      = new CANode_CommFactory(m_targets_sv_size);
                    pPredModeFact_targets   = new TargetsPredictModelFactory(m_targets_sv_size, m_rob_num,1, var_sv_targets);
                    pNode = new SNODE_CLASS(m_targets_sv_size, compound[i].T_sync, pPredModeFact_targets, pCommFact, delayed);
                    //pNode->PrintPredModel();
                    InitTargetState(ts_pose_targets, ts_pose, rob->GetRobID(), rob->GetInitialPosition(), rob->GetInitialVelocity(), y_targ_init, Y_targ_init);
                    pNode->SetInitWithInfo(y_targ_init, Y_targ_init);
                  }
    
                  pNode->SetEpsilon(time::from_us(1));
                  pNode->SetHistoryLength(time(1.5));
                  pNode->SetNow(t_orig);
                  compound[i].snode = pNode;
                      
                  if (!m_real_time) {
                    pEventSimu = new EventSimu(pNode->GetID());
                    if(i == NODE_TRACK) pEventSimu->SetSyncTracker(true);
                    pEventSimu->SetSeed(num_sensors+i);
                    pEventSimu->SetPeriod(compound[i].T_sync);
                    if(i == NODE_TRACK){
                    pEventSimu->SetSyncTracker(true);
                    //pEventSimu->SetTimeOffset(time::from_ms(2050));
                    //pNode->SetNextHorizon(time::from_ms(2050));
                    }
                    else
                          {
                            pEventSimu->SetTimeOffset(now);
                            pNode->SetNextHorizon(now);
                          }
                        
                        
                    compound[i].trigger = pEventSimu;
                    rob->AddSensor(pEventSimu);
                    pNode->SetLogChanFilters(true);
                  }
                  else pNode->InitNode();
                      
                  
                  //pNode->SetLastSyncTime(now);
                  now = now + time::from_ms(compound[i].T_sync.ms()/num_sensors);
                  rob->m_nodes.push_back(compound[i]);
                }
    
              // Same jitter for all sensors
              rob->InitStampJitter(time::from_ms(1));
    
              // Connect nodes together (accelerometer in the middle)
              rob->m_nodes[NODE_POM].snode->RequestConnection(rob->m_nodes[NODE_GPS].snode->GetID());
              rob->m_POM_node = rob->m_nodes[NODE_POM].snode;
              rob->m_Tracker_node = rob->m_nodes[NODE_TRACK].snode;
    
              // Setcolor
              rob->SetColor(0,255,0);
              }
              break;
    
            default: JFR_DEBUG( "Internal error!\n"); exit(EXIT_FAILURE);
            }
    
        }
    
        void InitSensorTargets(TrackerSimuUncorr * tracker)
        {
          for(int i = 0; i < tracker->GetNumTargets(); i++){
            tracker->AddTarget(m_targets[i]->GetPose());
          }
        }
    
        void InitTargetState(double ts_position, double ts_pose_robot, unsigned short rob_num, VEC const& p0, VEC const& v0, VEC & y_init, MSYM & Y_init)
        {
          int i,j,k;
          VEC  x_init(y_init.size()),v3(3);
          MSYM X_init(y_init.size(),y_init.size());
          
          y_init.clear();  Y_init.clear();   x_init.clear(); X_init.clear();
    
            // Init state vector
          k = rob_num * 6;
          for (i = 0; i < 3; i++) {
            x_init[k++] = p0[i];
            x_init[k++] = v0[i];
          }
    
          k = m_rob_num * 6;
          for(i = 0; i < m_num_targets; i++) {
            v3 = m_targets[i]->GetPose();
            for(j = 0; j < 3; j++) x_init[k++] = v3[j];
          }
    
            // Init cov matrix
          for(i = 3*m_rob_num; i < m_targets_sv_size; i++) X_init(i,i) = TWOSIGMA_TO_VAR(ts_position);
            
          for(i = 0; i < 6*m_rob_num; i += 2) {
            X_init(i,i)     = TWOSIGMA_TO_VAR(ts_pose_robot);
            X_init(i+1,i+1) = VARPOS_TO_VARVEL(TWOSIGMA_TO_VAR(ts_pose_robot), 1.0);
          }
    
          
          UdUinversePD (Y_init, X_init);
          Float rcond = UdUinversePD (Y_init, X_init);
          Numerical_rcond  rclimit;
          rclimit.check_PD(rcond, "Initial X not PD");
          noalias(y_init) = prod(Y_init,x_init);
        }
    
        void InitTargets(double tpose[][3] , unsigned short num_targets)
        {
          Target     *target;
          VEC p0(3);
          int i,j;
            
          p0.clear();
            
          for(i = 0; i < num_targets; i++)
            {
              for(j=0;j<3;j++) p0[j] = tpose[i][j];
              target = new Target(p0, m_rob_num);
              m_targets.push_back(target);
            }
        }
    
        void SetLocalInfoTime(RobotSimuNodesGen<SNODE_CLASS> & rob, time const& t)
        {
          typename SNodesArray::iterator it;
          for(it = rob.m_nodes.begin(); it != rob.m_nodes.end(); it++)
            (*it).snode->SetLocalInfoTime(t);
        }
    
        void StartNodesRealTime(RobotSimuNodesGen<SNODE_CLASS> & rob, time const& now)
        {
          typename SNodesArray::iterator it;
    
          for(it = rob.m_nodes.begin(); it != rob.m_nodes.end(); it++)
            {
              (*it).snode->SetLocalInfoTime(now);
              (*it).snode->SetNow(now);
              (*it).snode->Start(now+(*it).snode->GetNextHorizon());
            }
        }
    
        void ResetTimers(RobotSimuNodesGen<SNODE_CLASS> & rob)
        {
          typename SNodesArray::iterator it;
    
          for(it = rob.m_nodes.begin(); it != rob.m_nodes.end(); it++)
            (*it).m_sw_check.Restart();
        }
    
          // *** Simulation functions ***
        void SimulatePropPerception(RobotSimuNodesGen<SNODE_CLASS> *rob, bool delayed, bool multi)
        {
          std::ostringstream      rob_name, sensor_name;
          rob->SimulatePropPerception();
          std::string smulti = multi ? "multi_":"";
      
    
          if(delayed) {
            rob->FillUpMeasurementSet(RobotSimu::SORT_STAMP_AVAILABLE);
            rob_name << "sim_all_delayed_" << smulti << rob->GetName() << ".dat";
            rob->DumpAllMeasures((char *) rob_name.str().c_str());
            for(int i = 0; i < rob->GetNumSensors(); i++) {
              sensor_name.str("");
              sensor_name << rob->GetSensorName(i) << "_delayed_" <<  smulti << rob->GetName() << ".dat";
              rob->DumpSensor(sensor_name.str().c_str(),i);
            }
          }
          else {
            rob->FillUpMeasurementSet(RobotSimu::SORT_STAMP_SENSED);
            rob_name << "sim_all_not_delayed_" <<  smulti << rob->GetName() << ".dat";
            rob->DumpAllMeasures((char *) rob_name.str().c_str());
            for(int i = 0; i < rob->GetNumSensors(); i++) {
              sensor_name.str("");
              sensor_name << rob->GetSensorName(i) << "_not_delayed_" << smulti << rob->GetName() << ".dat";
              rob->DumpSensor(sensor_name.str().c_str(),i);
            }
          }

          rob_name.str("");
          rob_name << "true_state_" << smulti << rob->GetName() << ".dat";
          rob->DumpTruth(rob_name.str().c_str());
        }
    
        void SimulatePropPerception(bool delayed, bool multi)
        {
          typename RobotsArray::iterator  it;
          for(it = m_rob_array.begin(); it != m_rob_array.end(); it++) SimulatePropPerception(*it, delayed, multi);
        }
    
        void FilterMultiPerceptionSimulation(bool delayed)
        {
          unsigned short     PERSIST = 20;
          unsigned int       i,m;
          time               rel_time, node_time;
          InfoContainer      iI_tracker(m_targets_sv_size);
          InfoContainer      iI_POM(m_targets_sv_size);
          SensorNodeCompound compound, compound_other;
          ParametersGeneric  params;
          VEC                targets_sv(m_num_targets * 3);    // true x, estim x, var x
          VEC                targets_var(m_num_targets * 3);
          VEC                pose_robot(3);
          RobotSimuNodesGen<SNODE_CLASS>  *rob;
    
          targets_var.clear();
          targets_sv.clear();
            
            // Create dump files
          for(i = 0; i < m_rob_num; i++)
          {
            rob = m_rob_array[i];
            rob->CreateDumpFile(delayed ? ("sim_multi_"+rob->GetName()+"_delayed.log"):("sim_multi_"+rob->GetName()+".log"));
            rob->ResetSensorsGetNext();
          }
    
          m_multi_rob.ResetCounter();
          StopWatch sw;
            
          for (m = 0; m < m_multi_rob.GetNrMeas(); m++)
          {
            rob = GetRobByID(m_multi_rob.GetCurRobID());
            rel_time = m_multi_rob.GetCurRelTime();

            if(m_multi_rob.GetCurSensorType() != SENSOR_EVENT)    // Normal information fusion
            {
              compound = GetCompoundBySensorID(*rob, m_multi_rob.GetCurSensorID());
              node_time = compound.snode->GetLocalInfoTime();
              params.h = rel_time - node_time;

              if(m_multi_rob.GetCurSensorType() != SENSOR_TRACK)
              {
                m_multi_rob.GetCurInfo(rob->m_CurInfo);
                compound.snode->FeedWithSensorData(rob->m_CurInfo, params);
              }
              else
              {
                m_multi_rob.GetCurInfo(iI_tracker);

                if (rob->m_first) {
                  compound.snode->SetLocalInfoTime(iI_tracker.GetTime());
                  //compound.snode->SetLocalInfoTime(rob->m_CurInfo.GetTime());
                  params.h = time::null();
                  rob->m_first = false;
                }
                //else
                {

                  // FIXME use covariance intersect instead
                  rob->m_CurInfo = rob->m_POM_node->GetLocalInfo(); // in case of delayed data then recall history of POM !!!
                  FillInfoTargets(rob->GetRobID(), rob->UseOnlyVel(), rob->m_CurInfo, iI_POM);
                  iI_POM.SetTime(rob->m_CurInfo.GetTime());


                  if(iI_POM.GetTime() > iI_tracker.GetTime()){
                    compound.snode->FeedWithSensorData(iI_tracker, params);
                    params.h = iI_POM.GetTime() - iI_tracker.GetTime();
                    compound.snode->FeedWithSensorData(iI_POM, params);
                  }
                  else {
                    params.h = node_time - iI_POM.GetTime();
                    compound.snode->FeedWithSensorData(iI_POM, params);
                    params.h = iI_tracker.GetTime() - iI_POM.GetTime();
                    compound.snode->FeedWithSensorData(iI_tracker, params);
                  }
                }
                pose_robot = GetPose(rob->m_POM_node->GetLocalState().m_info, rob->UseOnlyVel());
                rob->m_gdhe_persist = PERSIST;
              }
            }
            else // Node sync
            {
              compound = GetCompoundBySNodeID(*rob, m_multi_rob.GetCurTriggerNodeID());
              compound.snode->UpdateWithChannelFilters(rel_time);
              if(m_multi_rob.IsCurTriggerSyncTracker())
              {
                if(rob->GetRobID() == 0)
                  m_rob_array[1]->m_Tracker_node->DbgWaitInfoFromNode(compound.snode->GetID());
                else
                  m_rob_array[0]->m_Tracker_node->DbgWaitInfoFromNode(compound.snode->GetID());
              }
              else DbgWaitOtherNodes(*rob, compound.snode->GetID());

              compound.snode->SetNextHorizon(rel_time + compound.snode->GetSyncPeriod());
            }

            if(rob->GetTrackerID() != -1) FillTargetSV(*rob, rob->GetTrackerID(), targets_sv, targets_var);

            if(m % 30 == 0) {
              SetGDHERobotsPos();
              DrawGDHERobotsTraj();
              m_gdhecl.Redraw();
            }

            if(rob->GetTrackerID() != -1)
            {
              if(rob->m_gdhe_persist == PERSIST) {
                SetGDHERobotsPos();
                DrawGDHERobotsTraj();
                DrawGDHEPerception(pose_robot, rob->GetRobID());
                ChangeGDHEAllEllipsoids(targets_var);
                m_gdhecl.Redraw();
              }

              if(rob->m_gdhe_persist == 0) {
                SetGDHERobotsPos();
                DrawGDHERobotsTraj();
                RemoveGDHETargetsSegment(rob->GetRobID());
                m_gdhecl.Redraw();
              }

              rob->m_gdhe_persist--;
            }

            rob->DumpCurrentState(rel_time);
            rob->DumpCustom(targets_sv);

            m_multi_rob.IncCounter();
            
          } // end for
    
          JFR_DEBUG( "Simulation effective time: " << sw.ElapsedSec() << "\n");
        }
    
        void FilterPropPerceptionSimulation(RobotSimuNodesGen<SNODE_CLASS> & rob, bool delayed)
        {
          int m;
          Test_Dump<SNODE_CLASS>    dump(rob.m_nodes.size(), &rob.m_nodes, (delayed ? "sim_"+rob.GetName()+"_delayed.log":"sim_"+rob.GetName()+".log"));
          time                      rel_time, node_time, max_time;
          InfoContainer             iI(rob.GetSvSize());
          InfoContainer             iI_tracker(m_targets_sv_size);
          InfoContainer             iI_POM(m_targets_sv_size);
          ParametersGeneric         params;
          SensorNodeCompound        compound;
          time                      now = rob.GetMinTime(), save;
          VEC                       targets_sv(m_num_targets * 3); // true x, estim x, var x
          VEC                       targets_var(m_num_targets * 3);  // variances vx_t0 vy_to vz_to etc...
          VEC                       pose_robot(3);
          bool                      just_perceived = false, first = true;
          int                       tracker_id = -1; // id of the sensor tracker -1 if no tracker
    
          tracker_id = rob.GetSensorID(SENSOR_TRACK,0);
  
          //std::cout << "toto\n";
    
          pose_robot.clear();
          rob.ResetCounter();
          max_time = time(rob.GetMaxTime());
          StopWatch sw;
    
          for (m = 0; m < rob.GetNrMeas(); m++)
            {
              rel_time = rob.GetCurrentRelTime();
    
              if(rob.GetCurrentSensorType() != SENSOR_EVENT)  // Normal information fusion
              {
                if(rob.GetCurrentSensorType() != SENSOR_TRACK) rob.GetCurrentInfo(iI);
                else {
                  rob.GetCurrentInfo(iI_tracker);
                }
                compound = GetCompoundBySensorID(rob, rob.GetCurrentSensorID());
                /*if(rob.GetCurrentSensorID() == 0 || rob.GetCurrentSensorID() == 2)
                      {
                      JFR_DEBUG( "S id " << rob.GetCurrentSensorID() << " node " << compound.snode->GetID() << "\n");
                  }*/
                node_time = compound.snode->GetLocalInfoTime();
                params.h = rel_time - node_time;
                if(rob.GetCurrentSensorType() != SENSOR_TRACK) compound.snode->FeedWithSensorData(iI, params);
                else {
                  // Extract info from pom (considered as a sensor)
                  // !!! delayed data not ready
                      
                  if (first) {
                    compound.snode->SetLocalInfoTime(iI_tracker.GetTime());
                    params.h = time::null();//time(1,0);
                    first = false;
                  }
                      
                  iI = rob.m_POM_node->GetLocalInfo(); // in case of delayed data then recall history of POM !!!
                      
                  FillInfoTargets(rob.GetRobID(), rob.UseOnlyVel(), iI, iI_POM);
                  //PrettyPrintMatrix(iI.m_InfoMat, "iI");
                  /*PrettyPrintMatrix(iI_POM.m_InfoMat, "iI_POM");
                        PrettyPrintVector(iI_POM.m_info, "info POM");
                        PrettyPrintMatrix(iI_multi.m_InfoMat, "iI_multi");
                        PrettyPrintVector(iI_multi.m_info, "iI_multi info");*/
                  //PrettyPrintVector(iI.m_info, "info");
                      
                  // Add to the tracker info
                  //JFR_DEBUG( "Time pom: " << iI.GetTime() << " tracker: " << iI_multi.GetTime() << "\n");
                      
                  //iI_POM.SetTime(iI_tracker.GetTime());
                  //iI_multi =  iI_multi + iI_POM;
                  /* PrettyPrintMatrix(iI_multi.m_InfoMat, "sum");
                    PrettyPrintVector(iI_multi.m_info, "sum info");*/
    
                  // Feed the node with POM and iI_track
                  //exit(-1);
                      
                  iI_POM.SetTime(iI.GetTime());
    
                  if(iI_POM.GetTime() > iI_tracker.GetTime()){
                    compound.snode->FeedWithSensorData(iI_tracker, params);
                    params.h = iI_POM.GetTime() - iI_tracker.GetTime();
                    compound.snode->FeedWithSensorData(iI_POM, params);
                  }
                  else {
                    compound.snode->FeedWithSensorData(iI_POM, params);
                    params.h = iI_tracker.GetTime() - iI_POM.GetTime();
                    compound.snode->FeedWithSensorData(iI_tracker, params);
                  }
    
                  pose_robot = GetPose(rob.m_POM_node->GetLocalState().m_info, rob.UseOnlyVel());
                      
                  just_perceived = true;
                }
              }
              else {   // Event triggers sensor node sync
              if(rob.IsCurrentTriggerUpdatePOM())
                {
                  JFR_DEBUG( "Update tracker with POM caused by trigger nr: " << rob.GetCurrentSensorID() << "\n");
                  compound = GetCompoundBySensorID(rob, rob.GetCurrentSensorID());
                  node_time = compound.snode->GetLocalInfoTime();
                  params.h = rel_time - node_time;
                  JFR_DEBUG( "h: " << params.h << "\n");
                      
                  iI = rob.m_POM_node->GetLocalInfo(); // in case of delayed data then recall history of POM !!!
                  FillInfoTargets(rob.GetRobID(), rob.UseOnlyVel(), iI, iI_POM);
                  iI_POM.SetTime(iI.GetTime());
                  compound.snode->FeedWithSensorData(iI_POM, params);
                }
              else {
                compound = GetCompoundBySNodeID(rob, rob.GetCurrentTriggerNodeID());
                if(compound.T_sync > time(35,0)) { JFR_DEBUG( "Sync tracker\n"); }
                else {
                  compound.snode->UpdateWithChannelFilters(rel_time);
                  DbgWaitOtherNodes(rob, compound.snode->GetID());
                  compound.snode->SetNextHorizon(rel_time + compound.snode->GetSyncPeriod());
                }
              }
              }
    
              if(tracker_id != -1) FillTargetSV(rob, tracker_id, targets_sv, targets_var);
                
              if(m % 30 == 0)
              {
                SetGDHERobotsPos();
                DrawGDHERobotsTraj();
    
                if(tracker_id != -1) {
                  ChangeGDHEAllEllipsoids(targets_var);
                  if (just_perceived) {
                    DrawGDHEPerception(pose_robot, rob.GetRobID());
                    just_perceived = false;
                  }
                  else RemoveGDHETargetsSegment();
                }
                  
                m_gdhecl.Redraw();
              }
    
              dump.DumpCurrentState(now, rel_time, rob, rob.m_POM_node->GetID());
              dump.DumpCustom(targets_sv);
              rob.IncCounter();
            }
            std::ostringstream stream;

            
            stream << "Simulation effective time: " << sw.ElapsedSec() << "\n";
            stream << "Simulation time: " << rob.GetTimeSpan() << "\n";
            stream <<"Total measurements: " << rob.GetNrMeas() << "\n";
            JFR_DEBUG(stream.str());
          
        }
    
        void FilterPropPerceptionRealTime(RobotSimuNodesGen<SNODE_CLASS> & rob)
        {
          int m;
          Test_Dump<SNODE_CLASS>    dump(rob.m_nodes.size(), &rob.m_nodes, "rt_"+rob.GetName()+"_delayed.log");
          InfoContainer                iI(rob.GetSvSize());
          ParametersGeneric            params;
          double                    refresh = 0.0, elapsed_sec;
          time                      now, rel_time, stamp_time, node_time, elapsed;
    
          rel_time = time(3.5);
          JFR_DEBUG( "Wait connection ...");
          rel_time.sleep_period();
          JFR_DEBUG( "Done!\n");
            
            
          now = time::current();
          JFR_DEBUG( "Time origin: " << now << "\n");
      
          rob.ResetCounter();
          ResetTimers(rob);
            
          StartNodesRealTime(rob, now);
          JFR_DEBUG( "Nodes are running... \n");
    
          StopWatch sw;
            
          for (m = 0; m < rob.GetNrMeas() ; m++)
            {
              rel_time = rob.GetCurrentRelTime();    // sorted chronologically by sensed or available
              stamp_time = now + rel_time;
              stamp_time.sleep_until();
    
              if(rob.GetCurrentSensorType() == SENSOR_TRACK) {
              rob.IncCounter();
              continue;
              }
    
              SensorNodeCompound   &compound = GetCompoundBySensorID(rob, rob.GetCurrentSensorID());
              elapsed = compound.m_sw_check.ElapsedAndReset();
              node_time = compound.snode->GetLocalInfoTime();
    
              if (rel_time != time::null() && elapsed > rob.GetCurrentSensorPeriod() + rob.GetCurrentSensorPeriod())
              {
                JFR_DEBUG( "Big delay! elapsed: " << elapsed << "\n");
                //break;
              }
                
              rob.GetCurrentInfo(iI);          // comes with the relative sensed time stamp
              iI.SetTime(now + iI.GetTime());
    
              if(node_time > stamp_time) params.h = time::null();
              else params.h = stamp_time - node_time;
              compound.snode->FeedWithSensorData(iI, params);
                
                // We are running in real time -> don't dump everything
              if(m % 10 == 0) {
              dump.DumpCurrentState(now, rel_time, rob, rob.m_POM_node->GetID());
              }
                
              if((elapsed_sec = sw.Elapsed().to_double()) > refresh) {
              refresh = elapsed_sec;
              //compound.snode->ResizeHistory();
              /*SetGDHERobotsPos();
                m_gdhecl.Redraw();
                DrawGDHERobotsTraj();*/
              }
              rob.IncCounter();
            }

            std::ostringstream stream;
            stream << "Simulation time: " << rob.GetTimeSpan() << "\n";
            stream << "Real time: " << sw.Elapsed() << "\n";
            JFR_DEBUG(stream.str());
            KillNodes(rob);
        }
    
          // *** GDHE related functions ***
        void CreateGDHEWorld() {
          m_gdhecl.CleanAll();
          m_gdhecl.SetBkGroundColor(180,180,180);
          m_gdhecl.CreateGrid(-3., -3., 8., 8., 1.0, 255,160,160,"GRID0");
          m_gdhecl.CreateFrame(1.5,0,200,0,"FRAME0");
          m_gdhecl.Redraw();
        }
    
        void ClearGDHEWorld() {
            /*m_gdhecl.DeleteObject("GRID0");
            m_gdhecl.DeleteObject("FRAME0");*/
          m_gdhecl.CleanAll();
        }
    
        void CreateGDHERobots()
        {
          typename RobotsArray::iterator  it;
          char str[255];
            
          for(it = m_rob_array.begin(); it != m_rob_array.end(); it++){
            (*it)->GetName(str);
            m_gdhecl.CreateSphere(0.2, (*it)->m_color[0], (*it)->m_color[1], (*it)->m_color[2], str);
          }
        }
    
        void SetGDHERobotsPos()
        {
          typename RobotsArray::iterator  it;
          char str[255];
          VEC  pos(3);
            
          for(it = m_rob_array.begin(); it != m_rob_array.end(); it++){
            (*it)->GetName(str);
            InfoContainer const& state = (*it)->m_POM_node->GetLocalState();
            pos = GetPose(state.m_info, (*it)->UseOnlyVel());
            m_gdhecl.SetPos(str, pos[0], pos[1], pos[2]);
            (*it)->AddTrajPoint(pos[0], pos[1], pos[2]);
          }
        }
    
        void DrawGDHERobotsTraj()
        {
          typename RobotsArray::iterator  it;
            
          for(it = m_rob_array.begin(); it != m_rob_array.end(); it++){
            if((*it)->m_traj_id != 0) m_gdhecl.DeleteObject((*it)->m_traj_id);
            (*it)->m_traj_id = m_gdhecl.CreatePolyline((*it)->m_traj,(*it)->m_traj.size(),(*it)->m_color[0],(*it)->m_color[1],(*it)->m_color[2]);
          }
        }
    
        void DrawGDHESegment(unsigned short id, unsigned short rob_id, VEC const& origin, VEC const& destination)
        {
          unsigned int gd_id = m_targets[id]->GetGDHEIDSegment(rob_id);
          if(gd_id != 0) m_gdhecl.DeleteObject(gd_id);
          std::vector<VEC> poly;
          poly.push_back(origin);
          poly.push_back(destination);
          m_targets[id]->SetGDHEIDSegment(m_gdhecl.CreatePolyline(poly,2,0,255,255), rob_id);
        }
    
        void DrawGDHEPerception(VEC const& pos_rob, unsigned short rob_id)
        {
          for(int i = 0; i < m_num_targets; i++) {
            DrawGDHESegment(i,rob_id,pos_rob, m_targets[i]->GetPose());
          }
        }
    
        void DrawGDHETrueTraj()
        {
          typename RobotsArray::iterator  it;
          const int NPTS = 100;
          float traj[NPTS][3];
            
          for(it = m_rob_array.begin(); it != m_rob_array.end(); it++){
            if((*it)->m_true_traj_id != 0) m_gdhecl.DeleteObject((*it)->m_true_traj_id);
            (*it)->FillGdheTrajectory(NPTS, traj);
            (*it)->m_true_traj_id = m_gdhecl.CreatePolyline(traj,NPTS,255,255,255);
          }
        }
    
        void CreateGDHETargets()
        {
          typename TargetsArray::iterator it;
          unsigned short id;
            
          for(it = m_targets.begin(); it != m_targets.end(); it++){
            id = m_gdhecl.CreateEllipsoid(ts_pose_targets, ts_pose_targets, ts_pose_targets, 0, 255, 255);
            (*it)->SetGDHEID(id);
            VEC const& p0 = (*it)->GetPose();
            m_gdhecl.SetPos((*it)->GetGDHEID(), p0[0], p0[1], p0[2]);
          }
        }
    
        void ChangeGDHEEllipsoid(unsigned short target_nr, double vx, double vy, double vz) {
          m_gdhecl.DeleteObject(m_targets[target_nr]->GetGDHEID());
          m_targets[target_nr]->SetGDHEID(m_gdhecl.CreateEllipsoid(vx, vy, vz, 0, 255, 255));
          VEC const& p0 = m_targets[target_nr]->GetPose();
          m_gdhecl.SetPos(m_targets[target_nr]->GetGDHEID(), p0[0], p0[1], p0[2]);
        }
  
        void ChangeGDHEAllEllipsoids(VEC const& targets_var) {
          for(int i = 0, j = 0; i < m_num_targets; i++, j+=3)
            ChangeGDHEEllipsoid(i, 3.0 * sqrt(targets_var[j]), 3.0 * sqrt(targets_var[j+1]), 3.0 * sqrt( targets_var[j+2]));
        }
    
        void RemoveGDHETraj()
        {
          typename RobotsArray::iterator  it;
            
          for(it = m_rob_array.begin(); it != m_rob_array.end(); it++){
            if((*it)->m_traj_id != 0) {
              m_gdhecl.DeleteObject((*it)->m_traj_id);
              m_gdhecl.DeleteObject((*it)->m_true_traj_id);
            }
            (*it)->ClearTraj();
          }
        }
    
        void RemoveGDHETargetsSegment(unsigned short rob_num)
        {
          for(int i = 0; i < m_num_targets; i++) {
            if (m_targets[i]->GetGDHEIDSegment(rob_num) != 0) {
              m_gdhecl.DeleteObject(m_targets[i]->GetGDHEIDSegment(rob_num));
              m_targets[i]->SetGDHEIDSegment(0,rob_num);
            }
          }
        }
    
        void RemoveGDHETargetsSegment()
        {
          for(int i = 0; i < m_rob_num; i++) {
            RemoveGDHETargetsSegment(i);
          }
        }
    
        void RemoveGDHETargets()
        {
          typename TargetsArray::iterator it;
            
          for(it = m_targets.begin(); it != m_targets.end(); it++)
            m_gdhecl.DeleteObject((*it)->GetGDHEID());
        }
    
          // *** Misc functions ***
        VEC GetPose(VEC const& sv, bool use_vel_model)
        {
          VEC  p(3);
    
          p[0] = sv[AccelMotion::SV_X];
          if(use_vel_model){
            p[1] = sv[2];
            p[2] = sv[4];
          }
          else {
            p[1] = sv[AccelMotion::SV_Y];
            p[2] = sv[AccelMotion::SV_Z];
          }
          return p;
        }
    
        VEC GetVel(VEC const& sv, bool use_vel_model)
        {
          VEC  p(3);
    
          p[0] = sv[AccelMotion::SV_VX];
          if(use_vel_model){
            p[1] = sv[3];
            p[2] = sv[5];
          }
          else {
            p[1] = sv[AccelMotion::SV_VY];
            p[2] = sv[AccelMotion::SV_VZ];
          }
          return p;
        }
    
        SensorNodeCompound & GetCompoundBySensorID(RobotSimuNodesGen<SNODE_CLASS> & rob, unsigned short sensor_id)
        {
          typename SNodesArray::iterator it;
          it = rob.m_nodes.begin();
          while(it != rob.m_nodes.end() && !(*it).SensorBelongsToNodebyID(sensor_id)) it++;
    
          JFR_PRECOND (it != rob.m_nodes.end(), "end of buffer");
          return ((*it));
        }
    
        SensorNodeCompound const& GetCompoundBySNodeID(RobotSimuNodesGen<SNODE_CLASS> & rob, unsigned short node_id)
        {
          typename SNodesArray::iterator it;
          it = rob.m_nodes.begin();
          while(it != rob.m_nodes.end() && (*it).snode->GetID() != node_id) it++;
    
          JFR_PRECOND (it != rob.m_nodes.end(), "end of buffer");
          return ((*it));
        }
    
        RobotSimuNodesGen<SNODE_CLASS> * GetRobByID(unsigned short robid)
        {
          for(int i = 0; i < m_rob_num; i++) if(m_rob_array[i]->GetRobID() == robid) return m_rob_array[i];
          JFR_POSTCOND(0, "robot not found");
          return NULL;
        }
    
        void DbgWaitOtherNodes(RobotSimuNodesGen<SNODE_CLASS> & rob, unsigned short node_sending)
        {
          typename SNodesArray::iterator it;
          for (it = rob.m_nodes.begin(); it != rob.m_nodes.end(); it++)
            if((*it).snode->GetID() != node_sending) (*it).snode->DbgWaitInfoFromNode(node_sending);
        }
    
        void FillTargetSV(RobotSimuNodesGen<SNODE_CLASS> & rob, unsigned short sensor_id, VEC & targets_sv, VEC & targets_var) {
            // Get node containing target
          SensorNodeCompound        compound;
          unsigned short sv_size,offset,target_id;
    
          compound = GetCompoundBySensorID(rob, sensor_id);
          sv_size = compound.snode->GetSvSize();
          offset = sv_size-m_num_targets*3;
          InfoContainer const& iCont = compound.snode->GetLocalState();
          for(unsigned int i = 0, j = 0; i < targets_sv.size(); i = i+3,j = j + 3){
            target_id = i/3;
            targets_sv[i]     = m_targets[target_id]->GetPose()[0];
            targets_sv[i+1]   = iCont.m_info[offset+j];
            targets_sv[i+2]   = iCont.m_InfoMat(offset+j,offset+j);
            targets_var[i]    = iCont.m_InfoMat(offset+j,offset+j);
            targets_var[i+1]  = iCont.m_InfoMat(offset+j+1,offset+j+1);
            targets_var[i+2]  = iCont.m_InfoMat(offset+j+2,offset+j+2);
          }
        }
          
        void FillInfoTargets(unsigned short rob_id, bool use_only_vel, InfoContainer const& iI, InfoContainer & iI_POM)
        {
          unsigned short pad,i,j,offset;
    
          if(use_only_vel) pad = 2;
          else pad = 3;
    
          offset = 6*rob_id;
    
          iI_POM.ClearAll();
            
            // Pic x vx,y vy,z vz cov mat
          for(i = 0; i < 3; i++)
            for(j = i; j < 3; j++)
              {
              iI_POM.m_InfoMat(offset+2*i,offset+2*j) = iI.m_InfoMat(i*pad,j*pad);
              iI_POM.m_InfoMat(offset+2*i,offset+2*j+1) = iI.m_InfoMat(i*pad,j*pad+1);
              iI_POM.m_InfoMat(offset+2*i+1,offset+2*j+1) = iI.m_InfoMat(i*pad+1,j*pad+1);
              }
    
            // Copy x vx, y vy, z vz
          for(i = 0,j = 0; i < 3; i++, j = j+2) {
            iI_POM.m_info(offset+j) = iI.m_info(i*pad);
            iI_POM.m_info(offset+j+1) = iI.m_info(i*pad+1);
          }
        }
    
    
          // *** Destroy functions ***
        void DestroySensorNodes(RobotSimuNodesGen<SNODE_CLASS> *rob)
        {
          typename SNodesArray::iterator  it;
          for(it = rob->m_nodes.begin(); it != rob->m_nodes.end(); it++) {
            delete (*it).snode;
            (*it).snode = NULL;
          }
    
          rob->m_nodes.clear();
          rob->RemoveAllSensors();
        }
    
        void DestroyAllSensorNodes()
        {
          typename RobotsArray::iterator  it;
          for(it = m_rob_array.begin(); it != m_rob_array.end(); it++) {
            DestroySensorNodes((*it));
          }
        }
    
        void DestroyRobots()
        {
          typename RobotsArray::iterator  it;
          for(it = m_rob_array.begin(); it != m_rob_array.end(); it++) {
            delete(*it);
          }
          m_rob_array.clear();
        }
    
        void KillNodes(RobotSimuNodesGen<SNODE_CLASS> & rob)
        {
          typename SNodesArray::iterator it;
          for(it = rob.m_nodes.begin(); it != rob.m_nodes.end(); it++)
            (dynamic_cast<CANode*>((*it).snode))->KillNode();
        }
    
        void ClearRobotsAccelProfile()
        {
          typename RobotsArray::iterator  it;
          for(it = m_rob_array.begin(); it != m_rob_array.end(); it++) (*it)->ClearProfile();
        }
    
        void ClearAllMeasurements()
        {
          typename RobotsArray::iterator  it;
          for(it = m_rob_array.begin(); it != m_rob_array.end(); it++) {
            (*it)->ClearUpMeasurementSet();
            (*it)->ClearAllSensorsMeasurements();
          }
        }
      };


    } // namespace test_nodes

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