sensorAbstract.hpp

Go to the documentation of this file.

#ifndef SENSORABSTRACT_H_
#define SENSORABSTRACT_H_

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#include <boost/assign.hpp>
#include <boost/smart_ptr.hpp>

#include "kernel/IdFactory.hpp"
#include "jmath/jblas.hpp"
#include "rtslam/rtSlam.hpp"
//include parents
#include "rtslam/parents.hpp"
#include "rtslam/mapAbstract.hpp"
#include "rtslam/mapObject.hpp"
#include "rtslam/robotAbstract.hpp"
#include "rtslam/hardwareSensorAbstract.hpp"

namespace jafar {
  namespace rtslam {

    // Forward declarations of children
    class ObservationAbstract;
    class DataManagerAbstract;

    class SensorAbstract: public MapObject,
                          public ChildOf<RobotAbstract>,
                          public boost::enable_shared_from_this<SensorAbstract>,
                          public kernel::DataLoggable
    {
      private:
        // define the function linkToParentRobot().
        ENABLE_LINK_TO_PARENT(RobotAbstract,Robot,SensorAbstract);
        // define the functions robotPtr() and robot().
        ENABLE_ACCESS_TO_PARENT(RobotAbstract,robot);
    
      protected:
        bool integrate_all;
        bool use_for_init; 
        bool need_init; 
        bool enabled; 
        
        RobotQuantity quantsJacob; 
        RobotQuantity quantsState; 

        static IdFactory sensorIds;

      public:
        
        SensorAbstract(const robot_ptr_t & _robPtr, const filtered_obj_t poseInFilter = UNFILTERED, int extraStateFilterSize = 0,
          std::vector<RobotAbstract::Quantity> robQuant = createQuantPosOriQuat());

        virtual ~SensorAbstract() {
        }

        void setIntegrationPolicy(bool integrate_all) { this->integrate_all = integrate_all; }
        bool getIntegrationPolicy() { return integrate_all; }
        void setUseForInit(bool use_for_init) { this->use_for_init = use_for_init; }
        bool getUseForInit() { return use_for_init; }
        void setNeedInit(bool need_init) { this->need_init = need_init; }
        bool getNeedInit() { return need_init; }

        virtual void move()
        {
          if (poseInFilter)
          {
            jblas::mat QN_q(4,4), XNEW_x(7,7); XNEW_x.clear();
            jblas::sym_mat Q(7,7); Q.clear();

            jblas::vec q = ublas::subrange(pose.x(), 3,7);
            ublasExtra::normalizeJac(q, QN_q);
            ublasExtra::normalize(q);
            ublas::subrange(pose.x(), 3,7) = q;

            ublas::subrange(XNEW_x, 0,3, 0,3) = jblas::identity_mat(3);
            ublas::subrange(XNEW_x, 3,7, 3,7) = QN_q;
            robotPtr()->mapPtr()->filterPtr->predict(robotPtr()->mapPtr()->ia_used_states(), XNEW_x, pose.ia(), Q); // P = F*P*F' + Q
          }
        }

        virtual int queryAvailableRaws(RawInfos &infos) = 0; 
        virtual int queryNextAvailableRaw(RawInfo &info) = 0; 
        virtual double getRawTimestamp(unsigned id) = 0;
        virtual void process(unsigned id, double date_limit = -1.) = 0; 
        virtual void process_fake(unsigned id, bool move) = 0; 
        virtual void discard(unsigned id) = 0; 
        virtual void init(double date) { use_for_init = false; } 
        virtual void start() = 0;
        virtual void stop() = 0;
        virtual bool join(int timed_ms = -1) = 0;
        void enable(bool enabled = true) { this->enabled = enabled; if (!enabled) enableHard(false); }
        bool isEnabled() { return enabled; }
        virtual void enableHard(bool enabled = true) = 0;
        virtual void stopDumping() = 0;
        virtual void showInfos() = 0;

        // to replace (because does not work with gcc 4.4) boost::assign::list_of(RobotAbstract::qPos)(RobotAbstract::qOriQuat)
        static std::vector<RobotAbstract::Quantity> createQuantPosOriQuat()
        {
          std::vector<RobotAbstract::Quantity> quant;
          quant.push_back(RobotAbstract::qPos);
          quant.push_back(RobotAbstract::qOriQuat);
          return quant;
        }

        enum type_enum {
          PINHOLE, PINHOLESTEREO, BARRETO
        } type;
        enum kind_enum {
          PROPRIOCEPTIVE, EXTEROCEPTIVE
        } kind;
        virtual std::string categoryName() const {
          return "SENSOR";
        }

        inline void setId() { id(sensorIds.getId()); }
        
        void setPose(double x, double y, double z, double rollDeg,
            double pitchDeg, double yawDeg);
        void setPoseStd(double x, double y, double z, double rollDeg,
            double pitchDeg, double yawDeg, double xStd, double yStd,
            double zStd, double rollDegStd, double pitchDegStd,
            double yawDegStd);
        void setPoseStd(double x, double y, double z, double rollDeg,
            double pitchDeg, double yawDeg, double posStd, double oriDegStd) {
          setPoseStd(x, y, z, rollDeg, pitchDeg, yawDeg, posStd, posStd,
                     posStd, oriDegStd, oriDegStd, oriDegStd);
        }


        Gaussian pose;

        ind_array ia_globalPose;
        bool poseInFilter;

        size_t missed_data; 

        vec7 globalPose(bool normalize = false);

        void globalPose(jblas::vec7 & senGlobalPose, jblas::mat & SG_rs);

        virtual void writeLogHeader(kernel::DataLogger& log) const;
        virtual void writeLogData(kernel::DataLogger& log) const;
    };
    
    
    class SensorProprioAbstract: public SensorAbstract
    {
      protected:
        RawVec reading;
      public:
        hardware::hardware_sensorprop_ptr_t hardwareSensorPtr;
      public:
        SensorProprioAbstract(const robot_ptr_t & robPtr, const filtered_obj_t poseInFilter = UNFILTERED, int extraStateFilterSize = 0,
          std::vector<RobotAbstract::Quantity> robQuant = createQuantPosOriQuat()):
          SensorAbstract(robPtr, poseInFilter, extraStateFilterSize, robQuant) { kind = PROPRIOCEPTIVE; }

        void setHardwareSensor(hardware::hardware_sensorprop_ptr_t hardwareSensorPtr_)
          { hardwareSensorPtr = hardwareSensorPtr_; }
        virtual void start() { hardwareSensorPtr->start(); }
        virtual void stop() { hardwareSensorPtr->stop(); }
        virtual bool join(int timed_ms = -1) { return hardwareSensorPtr->join(timed_ms); }
        virtual void enableHard(bool enabled = true) { hardwareSensorPtr->enable(enabled); if (enabled) enable(true); }
        virtual void stopDumping() { hardwareSensorPtr->stopDumping(); }
        virtual void showInfos() { hardwareSensorPtr->showInfos(); }


        virtual int queryAvailableRaws(RawInfos &infos)
          { int res = hardwareSensorPtr->getUnreadRawInfos(infos); infos.integrate_all = integrate_all; return res; }
        virtual int queryNextAvailableRaw(RawInfo &info)
          { return hardwareSensorPtr->getNextRawInfo(info); }
        virtual double getRawTimestamp(unsigned id) { return hardwareSensorPtr->getRawTimestamp(id); } 
        //process(id) will do the filtering, so it is specific to each hardware
        void process_fake(unsigned id, bool move) { hardwareSensorPtr->getRaw(id, reading); if (move) robotPtr()->move_fake(reading.data(0)); use_for_init = false; }
        void discard(unsigned id) { hardwareSensorPtr->getRaw(id, reading); }
    };
    
    class SensorProprioPredictorAbstract: public SensorProprioAbstract
    {
      public:
        SensorProprioPredictorAbstract(const robot_ptr_t & robPtr, const filtered_obj_t poseInFilter = UNFILTERED, int extraStateFilterSize = 0):
          SensorProprioAbstract(robPtr, poseInFilter, extraStateFilterSize) {}
        virtual void predict(double t) = 0;
    
    };
    
    class SensorExteroAbstract: public SensorAbstract,
                                public ParentOf<DataManagerAbstract>
    {
      protected:
        // define the type DataManagerList, and the function dataManagerList().
        ENABLE_ACCESS_TO_CHILDREN(DataManagerAbstract,DataManager,dataManager);
      
        raw_ptr_t rawPtr;
      public:
        hardware::hardware_sensorext_ptr_t hardwareSensorPtr;
      
      public:
        
        SensorExteroAbstract(const robot_ptr_t & robPtr, const filtered_obj_t poseInFilter = UNFILTERED, int extraStateFilterSize = 0):
          SensorAbstract(robPtr, poseInFilter, extraStateFilterSize)
        {
          kind = EXTEROCEPTIVE; 
          rawCounter = 0;
        }

        unsigned rawCounter;

        void setHardwareSensor(hardware::hardware_sensorext_ptr_t hardwareSensorPtr_)
          { hardwareSensorPtr = hardwareSensorPtr_; }
        virtual void start() { hardwareSensorPtr->start(); }
        virtual void stop() { hardwareSensorPtr->stop(); }
        virtual bool join(int timed_ms = -1) { return hardwareSensorPtr->join(timed_ms); }
        virtual void enableHard(bool enabled = true) { hardwareSensorPtr->enable(enabled); if (enabled) enable(true); }
        virtual void stopDumping() { hardwareSensorPtr->stopDumping(); }
        virtual void showInfos() { hardwareSensorPtr->showInfos(); }

//        virtual int acquireRaw() = 0;
//        virtual raw_ptr_t getRaw() = 0;

        virtual raw_ptr_t getLastProcessedRaw() { raw_ptr_t raw; hardwareSensorPtr->getLastProcessedRaw(raw); return raw; }
        virtual int queryAvailableRaws(RawInfos &infos)
          { int res = hardwareSensorPtr->getUnreadRawInfos(infos); infos.integrate_all = integrate_all; return res; }
        virtual int queryNextAvailableRaw(RawInfo &info)
          { return hardwareSensorPtr->getNextRawInfo(info); }
        virtual double getRawTimestamp(unsigned id) { return hardwareSensorPtr->getRawTimestamp(id); } 
        void process(unsigned id, double date_limit = -1.);
        void process_fake(unsigned id, bool move) { hardwareSensorPtr->getRaw(id, rawPtr); if (move) robotPtr()->move_fake(rawPtr->timestamp); rawCounter++; }
        void discard(unsigned id) { hardwareSensorPtr->getRaw(id, rawPtr); }
    };

  }
}

#endif /* SENSORABSTRACT_H_ */