KalmanFilter.h
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #ifndef NULL_COMPONENT_H
00011 #define NULL_COMPONENT_H
00012 
00013 #include <rtm/idl/BasicDataType.hh>
00014 #include <rtm/idl/ExtendedDataTypes.hh>
00015 #include <rtm/Manager.h>
00016 #include <rtm/DataFlowComponentBase.h>
00017 #include <rtm/CorbaPort.h>
00018 #include <rtm/DataInPort.h>
00019 #include <rtm/DataOutPort.h>
00020 #include <rtm/idl/BasicDataTypeSkel.h>
00021 #include <rtm/idl/ExtendedDataTypesSkel.h>
00022 #include <hrpModel/Body.h>
00023 
00024 #include "RPYKalmanFilter.h"
00025 #include "EKFilter.h"
00026 
00027 // Service implementation headers
00028 // <rtc-template block="service_impl_h">
00029 #include "KalmanFilterService_impl.h"
00030 
00031 // </rtc-template>
00032 
00033 // Service Consumer stub headers
00034 // <rtc-template block="consumer_stub_h">
00035 
00036 // </rtc-template>
00037 
00038 using namespace RTC;
00039 
00043 class KalmanFilter
00044   : public RTC::DataFlowComponentBase
00045 {
00046 public:
00051   KalmanFilter(RTC::Manager* manager);
00055   virtual ~KalmanFilter();
00056 
00057   // The initialize action (on CREATED->ALIVE transition)
00058   // formaer rtc_init_entry()
00059   virtual RTC::ReturnCode_t onInitialize();
00060 
00061   // The finalize action (on ALIVE->END transition)
00062   // formaer rtc_exiting_entry()
00063   // virtual RTC::ReturnCode_t onFinalize();
00064 
00065   // The startup action when ExecutionContext startup
00066   // former rtc_starting_entry()
00067   // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);
00068 
00069   // The shutdown action when ExecutionContext stop
00070   // former rtc_stopping_entry()
00071   // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);
00072 
00073   // The activated action (Active state entry action)
00074   // former rtc_active_entry()
00075   virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
00076 
00077   // The deactivated action (Active state exit action)
00078   // former rtc_active_exit()
00079   virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
00080 
00081   // The execution action that is invoked periodically
00082   // former rtc_active_do()
00083   virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
00084 
00085   // The aborting action when main logic error occurred.
00086   // former rtc_aborting_entry()
00087   // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);
00088 
00089   // The error action in ERROR state
00090   // former rtc_error_do()
00091   // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);
00092 
00093   // The reset action that is invoked resetting
00094   // This is same but different the former rtc_init_entry()
00095   // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
00096 
00097   // The state update action that is invoked after onExecute() action
00098   // no corresponding operation exists in OpenRTm-aist-0.2.0
00099   // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);
00100 
00101   // The action that is invoked when execution context's rate is changed
00102   // no corresponding operation exists in OpenRTm-aist-0.2.0
00103   // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
00104   bool setKalmanFilterParam(const OpenHRP::KalmanFilterService::KalmanFilterParam& i_param);
00105   bool getKalmanFilterParam(OpenHRP::KalmanFilterService::KalmanFilterParam& i_param);
00106   bool resetKalmanFilterState();
00107 
00108 protected:
00109   // Configuration variable declaration
00110   // <rtc-template block="config_declare">
00111   
00112   // </rtc-template>
00113 
00114   TimedAngularVelocity3D m_rate;
00115   TimedAcceleration3D m_acc;
00116   TimedAcceleration3D m_accRef;
00117   TimedOrientation3D m_rpy;
00118   TimedOrientation3D m_rpyRaw;
00119   TimedOrientation3D m_rpy_prev;
00120   TimedOrientation3D m_rpyRaw_prev;
00121 
00122   // DataInPort declaration
00123   // <rtc-template block="inport_declare">
00124   InPort<TimedAngularVelocity3D> m_rateIn;
00125   InPort<TimedAcceleration3D> m_accIn;
00126   InPort<TimedAcceleration3D> m_accRefIn;
00127   InPort<TimedAngularVelocity3D> m_rpyIn; // for dummy usage
00128   
00129   // </rtc-template>
00130 
00131   // DataOutPort declaration
00132   // <rtc-template block="outport_declare">
00133   OutPort<TimedOrientation3D> m_rpyOut;
00134   OutPort<TimedOrientation3D> m_rpyRawOut;
00135   RTC::TimedDoubleSeq m_qCurrent;
00136   RTC::InPort<RTC::TimedDoubleSeq> m_qCurrentIn;
00137   RTC::TimedOrientation3D m_baseRpyCurrent;
00138   RTC::OutPort<RTC::TimedOrientation3D> m_baseRpyCurrentOut;
00139   
00140   // </rtc-template>
00141 
00142   // CORBA Port declaration
00143   // <rtc-template block="corbaport_declare">
00144   
00145   // </rtc-template>
00146 
00147   // Service declaration
00148   // <rtc-template block="service_declare">
00149   RTC::CorbaPort m_KalmanFilterServicePort;
00150   
00151   // </rtc-template>
00152 
00153   // Consumer declaration
00154   // <rtc-template block="consumer_declare">
00155   KalmanFilterService_impl m_service0;
00156   
00157   // </rtc-template>
00158 
00159 private:
00160   double m_dt;
00161   RPYKalmanFilter rpy_kf;
00162   EKFilter ekf_filter;
00163   hrp::BodyPtr m_robot;
00164   hrp::Matrix33 m_sensorR, sensorR_offset;
00165   hrp::Vector3 acc_offset;
00166   unsigned int m_debugLevel;
00167   int dummy, loop;
00168   OpenHRP::KalmanFilterService::KFAlgorithm kf_algorithm;
00169 };
00170 
00171 
00172 extern "C"
00173 {
00174   void KalmanFilterInit(RTC::Manager* manager);
00175 };
00176 
00177 #endif // NULL_COMPONENT_H


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:18