Odometry.h

Go to the documentation of this file.

// -*- C++ -*-
#ifndef ODOMETRY_H
#define ODOMETRY_H

#include <rtm/Manager.h>
#include <rtm/DataFlowComponentBase.h>
#include <rtm/CorbaPort.h>
#include <rtm/DataInPort.h>
#include <rtm/DataOutPort.h>
#include <rtm/idl/BasicDataTypeSkel.h>

//---[add]--------------------------------
#include "intellirobotStub.h"
#include "intellirobotSkel.h"
#include "intellirobot.hh"

//#include <rtm/RingBuffer.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <math.h>
#define LOG 0
#define DEBUG 0
#define DEBUG2 0
#define CALLBACK 0
#define CALLBACK_DEBUG 0

#if CALLBACK            
#include <rtm/RingBuffer.h>
//-CallBackObject--
class Debug
        : public RTC::OnWriteConvert<RTC::TimedDoubleSeq>
{
        //ofstream ofsCallBack;
        public:
                Debug(void) {   
                  //ofsCallBack.open("data/callback.dat");  
                };
                RTC::TimedDoubleSeq operator()(const RTC::TimedDoubleSeq& value)
                {
                        RTC::TimedDoubleSeq ret(value);
#if CALLBACK_DEBUG
                        double dummy_sec = value.tm.sec + 1.0e-9*value.tm.nsec;
                        printf("--[CallBack_TIME(WheelAngle)]: %lf\t[L]=%lf\t[R]=%lf\n",dummy_sec, value.data[0], value.data[1]);
                        //printf("--[CallBack_TIME(LocalizedPosition)]: %lf\tx=%lf\ty=%lf\ttheta=%lf\n",dummy_sec, value.data[0], value.data[1], value.data[2]);
#endif
                        return ret;
                };

};
//--
#endif
using namespace std;
//---[add]--------------------------------

using namespace RTC;
using namespace IIS;

class Odometry
  : public RTC::DataFlowComponentBase
{
 public:
  Odometry(RTC::Manager* manager);
  ~Odometry();

   virtual RTC::ReturnCode_t onInitialize();
  /***
   * (1) update INPORT's data
   * (2) open files for debugging
   * (3) initialize
   *
   * The activated action (Active state entry action)
   * former rtc_active_entry()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * @pre none
   * @post none
   * 
   */
   virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);

  /***
   * (1)close files for debugging
   *
   * The deactivated action (Active state exit action)
   * former rtc_active_exit()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * @pre none
   * @post none
   * 
   */
   virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);

  /***
   * (1)get current Wheel Angle data from MotorControlRTC
   * (2)get current Robot's position data from LocalizationRTC
   * (3)calculate current position by Odometry
   * (4)send current odometry position data to LocalizationRTC
   *
   * The execution action that is invoked periodically
   * former rtc_active_do()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * @pre none
   * @post none
   * 
   */
   virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);


//---[add]--------------------------------
        virtual void getLocalizedPosition();

        virtual bool getWheelAngleData();

        virtual void calcOdometry();
        virtual void setCurrentToOld();

        virtual void outputData();
//---[add]--------------------------------


 protected:
  int m_leftWheelID;
  int m_rightWheelID;
  double m_radiusOfLeftWheel;
  double m_radiusOfRightWheel;
  double m_lengthOfAxle;
  double m_radiusOfBodyArea;
  // </rtc-template>

  // DataInPort declaration
  // <rtc-template block="inport_declare">
  TimedDoubleSeq m_CurrentWheelAngle;
  InPort<TimedDoubleSeq> m_CurrentWheelAngleIn;
  IIS::TimedPose2D m_LocalizedPosition;

  InPort<IIS::TimedPose2D> m_LocalizedPositionIn;
  
  // </rtc-template>


  // DataOutPort declaration
  // <rtc-template block="outport_declare">
  IIS::TimedPose2D m_OdometryPosition;
  OutPort<IIS::TimedPose2D> m_OdometryPositionOut;  

 private:

//---[add]----------------------------------------
        //--static PARAMETER
        const static int Dof = 2;              
        //double Pi;        //!< the ratio of the circumference of a circle to its diameter
        double RotationAngleBorder; 
        double RhoBorder;   

        struct BODY {
                int leftID;    
                int rightID;   
                double length;      
                double wheelRadiusLeft;   
                double wheelRadiusRight;   
                double bodyRadius;   
        }Body;

        struct CURRENT {
                double BodyVel;   
                double RightVel;  
                double LeftVel;   
                double BodyAccel; 
                double BodyOmega; 
                double Rho;      
//              double sumLength;  //!< the sum of distance which robot moved
                double BodyVelOld;   
                double BodyOmegaOld; 
                double RhoOld;       
                double Ang[Dof];     
                double AngVel[Dof];  
                double AngOld[Dof];  
                double AngVelOld[Dof]; 
                double x;    
                double y;    
                double theta;    
                double xOld;    
                double yOld;    
                double thetaOld;   
//              double distanceToGoal;  //! the distance from current position to GOAL
                unsigned long sec;  
                unsigned long nsec;  
                unsigned long secOld;  
                unsigned long nsecOld;  
                double Time;          
                double TimeOld;       
        }Current;

        //--others
        double timeStep;   
        bool InitialPositionFlag; 
        bool InitialAngleFlag; 

#if DEBUG               
        //-- output file pointer for debugging
        ofstream ofs;   
#endif

#if CALLBACK
        //-- callback object
        Debug m_debug;  
#endif

#if DEBUG2
        long counter;
        ofstream ofs2;   
        ofstream ofs3;   
        //--for TimeDebug
        struct timeval TotalTime;  
        struct timeval GetTime;                 
        struct timeval SetTime;                 
#endif

//---[add]----------------------------------------

};


extern "C"
{
  DLL_EXPORT void OdometryInit(RTC::Manager* manager);
};

#endif // ODOMETRY_H