robot.hpp

/* $Id$ */

#ifndef SIMU_ROBOT_HPP
#define SIMU_ROBOT_HPP

#include <list>

#include "kernel/dataLog.hpp"

#include "jmath/jblas.hpp"

#include "geom/t3dEuler.hpp"

#include "slam/odo3dPredictModel.hpp"
#include "slam/feature.hpp"
#include "slam/segmentFeature.hpp"

namespace jafar {
  namespace slam {
    class SegmentObservationsSelector;
  }
  namespace simu {

    class Environment;
    class Point;
    class SensorPoint;
    class SensorSegmentImage;
    class SensorRobot;

    class Robot : public jafar::kernel::DataLoggable {
    
    public:

      jafar::slam::OdoNoiseModel const& odoNoiseModel;
      jblas::sym_mat gpsNoise;

      typedef std::list<jafar::slam::Observation*> SequenceObs;

      jafar::geom::T3DEuler pose;

      jafar::geom::T3DEuler odoPose;

      jafar::geom::T3DEuler robotToSensor;

      jblas::vec u;
      
      unsigned int robotId;
      
      Environment* environment;

      Robot(jafar::slam::OdoNoiseModel const& odoNoiseModel_, SensorPoint* sensor_, 
      bool doAddNoise = false, unsigned int randomSeed = 1,
      double noiseFactor = 1.0, unsigned int _robotId = 0);

      Robot(jafar::slam::OdoNoiseModel const& odoNoiseModel_, SensorSegmentImage* sensor_,
      bool doAddNoise = false, unsigned int randomSeed = 1,
      double noiseFactor = 1.0, unsigned int _robotId = 0);

      Robot(jafar::slam::OdoNoiseModel const& odoNoiseModel_, SensorPoint* sensor_, SensorSegmentImage* segmentSensor_,
        bool doAddNoise = false, unsigned int randomSeed = 1,
        double noiseFactor = 1.0, unsigned int _robotId = 0);

      void setOdometryBias(jblas::vec const& bias_);

      void setPose(const jblas::vec& pose_);

      jblas::vec const& getRobotToSensor() const;
      void setRobotToSensor(const jblas::vec& robotToSensor_);

      void moveOdo(double v, double w, double dt);

      void fillObservations(SequenceObs& seqObs);
      
      void getGpsPose( jafar::geom::T3DEuler& pose );

      unsigned int const getRobotId(); 
      
      void setSensorRobot( SensorRobot* _sensorRobot );
      SensorRobot* getSensorRobot( );

      void setSegmentObservationsSelector( jafar::slam::SegmentObservationsSelector* _observationsSelector );
      jafar::slam::SegmentObservationsSelector* getSegmentObservationsSelector();
    protected:

      bool p_doAddNoise;

      jafar::jmath::NormalDistribution noise;

      jblas::vec bias;

      SensorPoint* sensorPt;

      SensorSegmentImage* sensorSeg;
      
      SensorRobot* sensorRobot;
      
      jafar::slam::SegmentObservationsSelector* m_obsevationsSelector;

      void writeLogHeader(kernel::DataLogger& log) const;
      void writeLogData(kernel::DataLogger& log) const;

      friend std::ostream& operator <<(std::ostream& s, const Robot& r_);
    private:
      void initGpsNoise();
    };

    std::ostream& operator <<(std::ostream& s, const Robot& r_);

  }
}

#endif // SIMU_ROBOT_HPP