LocalizeCenter.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include "LocalizeCenter.h"
00011 
00012 // Module specification
00013 // <rtc-template block="module_spec">
00014 static const char* localizecenter_spec[] =
00015   {
00016     "implementation_id", "LocalizeCenter",
00017     "type_name",         "LocalizeCenter",
00018     "description",       "${rtcParam.description}",
00019     "version",           "1.0.0",
00020     "vendor",            "AIST",
00021     "category",          "example",
00022     "activity_type",     "PERIODIC",
00023     "kind",              "DataFlowComponent",
00024     "max_instance",      "1",
00025     "language",          "C++",
00026     "lang_type",         "compile",
00027     "conf.default.startX", "3.24",
00028     "conf.default.startY", "7.81",
00029     "conf.default.startTheta", "-1.57",
00030                 "conf.default.cycle", "20",
00031     ""
00032   };
00033 // </rtc-template>
00034 
00039 LocalizeCenter::LocalizeCenter(RTC::Manager* manager)
00040     // <rtc-template block="initializer">
00041   : RTC::DataFlowComponentBase(manager),
00042     m_odmIn("dp_odmIn", m_odm),
00043     m_ceilIn("dp_ceilIn", m_ceil),
00044     //m_in2In("dp_in2", m_in2),
00045     m_dp_out0Out("dp_out0", m_dp_out0)
00046 
00047     // </rtc-template>
00048 {
00049 }
00050 
00054 LocalizeCenter::~LocalizeCenter()
00055 {
00056 }
00057 
00058 
00059 
00060 RTC::ReturnCode_t LocalizeCenter::onInitialize()
00061 {
00062   bindParameter("startX", m_start_x, "3.29");
00063   bindParameter("startY", m_start_y, "7.70");
00064   bindParameter("startTheta", m_start_theta, "-1.571");
00065         bindParameter("cycle", m_cycle, "20");  
00066         
00067         start_x_prev = m_start_x;
00068         start_y_prev = m_start_y;
00069         start_theta_prev = m_start_theta;
00070         
00071         pos_init_cnt = 0;
00072 
00073   // Registration: InPort/OutPort/Service
00074   // <rtc-template block="registration">
00075   // Set InPort buffers
00076   addInPort("dp_odmIn", m_odmIn);
00077   addInPort("dp_ceilIn", m_ceilIn);
00078   //addInPort("dp_in2", m_in2In);
00079   
00080   // Set OutPort buffer
00081   addOutPort("dp_out0", m_dp_out0Out);
00082   
00083         //初期値設定
00084         m_odm.data.position.x = m_ceil.data.position.x = m_avPos.x = m_start_x;
00085         m_odm.data.position.y = m_ceil.data.position.y = m_avPos.y = m_start_y;
00086         m_odm.data.heading = m_ceil.data.heading = m_avPos.theta = m_start_theta;
00087 
00088         //重みの設定
00089         m_odmWgt = 19.0;
00090         m_ceilWgt = 1.0;
00091         m_totalWgt = m_odmWgt + m_ceilWgt;
00092         counter = 0;
00093   return RTC::RTC_OK;
00094 }
00095 
00096 
00097 RTC::ReturnCode_t LocalizeCenter::onActivated(RTC::UniqueId ec_id)
00098 {
00099         //スタート位置に関するコンフィギュレーションパラメータが変更されていたらそれを反映させる
00100         if( ! ( start_x_prev == m_start_x && start_y_prev == m_start_y && start_theta_prev == m_start_theta ) )
00101         {
00102                 pos_init_cnt = 0;
00103                 start_x_prev = m_start_x;
00104                 start_y_prev = m_start_y;
00105                 start_theta_prev = m_start_theta;
00106                 
00107                 m_odm.data.position.x = m_ceil.data.position.x = m_avPos.x = m_start_x;
00108                 m_odm.data.position.y = m_ceil.data.position.y = m_avPos.y = m_start_y;
00109                 m_odm.data.heading = m_ceil.data.heading = m_avPos.theta = m_start_theta;
00110 
00111         }
00112         
00113         m_dp_out0.data.position.x = m_avPos.x;
00114         m_dp_out0.data.position.y = m_avPos.y;
00115         m_dp_out0.data.heading = m_avPos.theta;
00116 
00117         //最初に一回出力ポートから送信
00118         m_dp_out0Out.write();
00119 
00120   return RTC::RTC_OK;
00121 }
00122 
00123 
00124 RTC::ReturnCode_t LocalizeCenter::onExecute(RTC::UniqueId ec_id)
00125 {
00126         int odmFlag=0, ceilFlag=0;
00127 
00128         //onActivateで位置を変えるときの処理.はじめに数回位置推定モジュールに
00129         //変更後の位置情報を流し込む.
00130         if(pos_init_cnt < 20){
00131                 m_dp_out0Out.write();
00132                 pos_init_cnt ++;
00133         }
00134         else{
00135                 if(m_odmIn.isNew()){
00136                         while(!m_odmIn.isEmpty())//H.T 20100822 リングバッファの最新の値を読む
00137                         {
00138                                 m_odmIn.read();
00139                                 odmFlag = 1;
00140                         }
00141                 }
00142                 if(m_ceilIn.isNew()){
00143                         while(!m_ceilIn.isEmpty())//H.T 20100822 リングバッファの最新の値を読む
00144                         {
00145                                 m_ceilIn.read();
00146                                 ceilFlag = 1;
00147                         }
00148 
00149                         m_avPos.x = ( m_odmWgt * m_odm.data.position.x + m_ceilWgt * m_ceil.data.position.x ) / m_totalWgt;
00150                         m_avPos.y = ( m_odmWgt * m_odm.data.position.y + m_ceilWgt * m_ceil.data.position.y ) / m_totalWgt;
00151 
00152                         //if( m_odm.theta * m_ceil.theta < 0 && abs( m_ceil.theta ) > 3.0 * M_PI / 4.0)
00153                         if( m_odm.data.heading * m_ceil.data.heading < 0 && abs( m_ceil.data.heading ) > 3.0 * M_PI / 4.0)
00154                         { //π側と-π側で打ち消してしまいそうな時
00155                                 if( m_odm.data.heading > m_ceil.data.heading)
00156                                         m_ceil.data.heading = m_ceil.data.heading + 2 * M_PI;
00157                                 else
00158                                         m_odm.data.heading = m_odm.data.heading + 2 * M_PI;
00159                         }
00160                 
00161                                 m_avPos.theta = ( m_odmWgt * m_odm.data.heading + m_ceilWgt * m_ceil.data.heading ) / m_totalWgt;
00162                         
00163 
00164                         if (m_avPos.theta > M_PI)               // 180度を越えている場合、マイナスに変更する
00165                                 m_avPos.theta = -M_PI * 2 + m_avPos.theta;
00166                         else if (m_avPos.theta < -M_PI) // -180を下回る場合、プラスに変更する
00167                                 m_avPos.theta = M_PI * 2 + m_avPos.theta;
00168 
00169                         m_dp_out0.data.position.x = m_avPos.x;
00170                         m_dp_out0.data.position.y = m_avPos.y;
00171                         m_dp_out0.data.heading = m_avPos.theta;
00172 
00173                         m_dp_out0Out.write();
00174                         printf("%d %d : X=%f Y=%f Th=%f\n",odmFlag,ceilFlag,m_avPos.x,m_avPos.y,m_avPos.theta);
00175                         counter=0;
00176                 }
00177 
00178                 else{//天井ナビゲーションからのデータが来ていないとき
00179                         ceilFlag = 0;
00180                         m_avPos.x = m_odm.data.position.x;
00181                         m_avPos.y = m_odm.data.position.y;
00182                         m_avPos.theta = m_odm.data.heading;
00183                         
00184 
00185                         if (m_avPos.theta > M_PI)               // 180度を越えている場合、マイナスに変更する
00186                                 m_avPos.theta = -M_PI * 2 + m_avPos.theta;
00187                         else if (m_avPos.theta < -M_PI) // -180を下回る場合、プラスに変更する
00188                                 m_avPos.theta = M_PI * 2 + m_avPos.theta;
00189 
00190                         m_dp_out0.data.position.x = m_avPos.x;
00191                         m_dp_out0.data.position.y = m_avPos.y;
00192                         m_dp_out0.data.heading = m_avPos.theta;
00193                         counter++;
00194                 
00195                         if(counter%m_cycle ==0){
00196                                 counter = 0;
00197                                 m_dp_out0Out.write();
00198                                 printf("%d %d : X=%f Y=%f Th=%f\n",odmFlag,ceilFlag,m_avPos.x,m_avPos.y,m_avPos.theta);
00199                         }
00200                 }
00201         }
00202 
00203 
00204   return RTC::RTC_OK;
00205 }
00206 
00207 extern "C"
00208 {
00209  
00210   void LocalizeCenterInit(RTC::Manager* manager)
00211   {
00212     coil::Properties profile(localizecenter_spec);
00213     manager->registerFactory(profile,
00214                              RTC::Create<LocalizeCenter>,
00215                              RTC::Delete<LocalizeCenter>);
00216   }
00217   
00218 };
00219 
00220 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


RS003
Author(s):
autogenerated on Thu Jun 27 2013 14:58:49