RS003: CeilingNavigation.cpp Source File

Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include "CeilingNavigation.h"
00011 #include "cv.h"
00012 #include "highgui.h"
00013 
00014 //#define _USE_SHOW_MAP 1 //現状ではmakeのときに宣言
00015 //#define _OFFLINE_IMAGE        1
00016 
00017 #ifdef _USE_SHOW_MAP
00018 #include "Common/sharedmem.h"
00019 robot_status *rstat = NULL;
00020 int count = 130;
00021 int delaycount=0;
00022 #endif
00023 
00024 FILE *fp;
00025 
00026 // Module specification
00027 // <rtc-template block="module_spec">
00028 static const char* ceilingnavigation_spec[] =
00029   {
00030     "implementation_id", "CeilingNavigation",
00031     "type_name",         "CeilingNavigation",
00032     "description",       "CeilingNavigation",
00033     "version",           "1.0.0",
00034     "vendor",            "NAIST and FSI",
00035     "category",          "Navigation",
00036     "activity_type",     "PERIODIC",
00037     "kind",              "DataFlowComponent",
00038     "max_instance",      "1",
00039     "language",          "C++",
00040     "lang_type",         "compile",
00041     // Configuration variables
00042 #ifdef _OFFLINE_IMAGE
00043     "conf.default.NavigationMap", "./CeilingImage2_Small.BMP",
00044     "conf.default.OfflineImage", "./offline_image",
00045     "conf.default.X_POS", "367",
00046     "conf.default.Y_POS", "51",
00047     "conf.default.THETA", "-2.0",
00048 #else
00049     "conf.default.NavigationMap", "./rotRTCcenterCeiling.BMP",
00050     "conf.default.OfflineImage", "",
00051     "conf.default.X_POS", "88",
00052     "conf.default.Y_POS", "76",
00053     "conf.default.THETA", "0",
00054 
00055 #endif
00056     "conf.default.BlockResolution", "1",
00057     "conf.default.BlockSize", "96",
00058     "conf.default.BlockCoefficient", "0.414",//0.3
00059     "conf.default.SearchScope", "2", //この値は関係ないようだ
00060     "conf.default.BlackWhiteValue", "220",
00061     "conf.default.Center_X", "160",
00062     "conf.default.Center_Y", "120",
00063         "conf.default.RealMapHeight", "100.0",
00064         "conf.default.RealMapWidth", "100.0",
00065         "conf.default.VirtualMapHeight", "100.0",
00066         "conf.default.VirtualMapWidth", "100.0",
00067     ""
00068   };
00069 // </rtc-template>
00070 
00071 CeilingNavigation::CeilingNavigation(RTC::Manager* manager)
00072     // <rtc-template block="initializer">
00073   : RTC::DataFlowComponentBase(manager),
00074     m_CameraDataIn("CameraData", m_CameraData),
00075     m_LocalizedPositionIn("LocalizedPosition", m_LocalizedPosition),
00076                 m_CeilingPositionOut("CeilingPosition", m_CeilingPosition),
00077     // </rtc-template>
00078         dummy(0)
00079 {
00080   // Registration: InPort/OutPort/Service
00081   // <rtc-template block="registration">
00082   // Set InPort buffers
00083   registerInPort("CameraData", m_CameraDataIn);
00084   registerInPort("LocalizedPosition", m_LocalizedPositionIn);
00085   
00086   // Set OutPort buffer
00087   registerOutPort("CeilingPosition", m_CeilingPositionOut);
00088   
00089 }
00090 
00091 CeilingNavigation::~CeilingNavigation()
00092 {
00093 }
00094 
00095 
00096 
00097 RTC::ReturnCode_t CeilingNavigation::onInitialize()
00098 {
00099   // <rtc-template block="bind_config">
00100   // Bind variables and configuration variable
00101 #ifdef _OFFLINE_IMAGE
00102   bindParameter("NavigationMap", m_NavigationMap, "./CeilingImage2_Small.BMP");
00103   bindParameter("OfflineImage", m_OfflineImage, "./offline_image");
00104   bindParameter("X_POS", x_pos, "367");
00105   bindParameter("Y_POS", y_pos, "51");
00106   bindParameter("THETA", theta, "-2.0");
00107 #else
00108   bindParameter("NavigationMap", m_NavigationMap, "./rotRTCcenterCeiling.BMP");
00109   bindParameter("OfflineImage", m_OfflineImage, "");
00110   bindParameter("X_POS", x_pos, "88");
00111   bindParameter("Y_POS", y_pos, "76");
00112   bindParameter("THETA", theta, "0");
00113 #endif
00114   bindParameter("BlockResolution", m_BlockResolution, "1");
00115   bindParameter("BlockSize", m_BlockSize, "96");
00116   bindParameter("BlockCoefficient", m_BlockCoefficient, "0.414");//0.3
00117   bindParameter("SearchScope", m_SearchScope, "2");
00118   bindParameter("BlackWhiteValue", m_BlackWhiteValue, "220");
00119   bindParameter("Center_X", m_Center_X, "160");
00120   bindParameter("Center_Y", m_Center_Y, "120");
00121   bindParameter("RealMapHeight",m_RealMapHeight,"100.0");
00122   bindParameter("RealMapWidth",m_RealMapWidth,"100.0");
00123   bindParameter("VirtualMapHeight",m_VirtualMapHeight,"100.0");
00124   bindParameter("VirtualMapWidth",m_VirtualMapWidth,"100.0");
00125 
00126 
00127   // </rtc-template>
00128 #ifdef _USE_SHOW_MAP
00129         rstat = (robot_status *)InitSharedMem(SHM_KEY, sizeof(robot_status));   //共有メモリ関連
00130 #endif
00131 
00132         //各モジュール共通の値．最適な共有メカニズムは何か？
00133         mPerPix = 0.05;
00134         camX = 0.05;
00135         camY = -0.17;//単位 m
00136 
00137         fp = fopen("trajectory.dat", "w");
00138 
00139         return RTC::RTC_OK;
00140 }
00141 
00142 /*
00143 RTC::ReturnCode_t CeilingNavigation::onFinalize()
00144 {
00145   return RTC::RTC_OK;
00146 }
00147 */
00148 
00149 
00150 RTC::ReturnCode_t CeilingNavigation::onStartup(RTC::UniqueId ec_id)
00151 {
00152         // ナビゲーション用マップ画像読み込み
00153         m_CeilingMap.ReadNavigationMap(m_NavigationMap);
00154 
00155         // 初期位置特定
00156 #if 0   // コンフィグに移動
00157         #ifdef _OFFLINE_IMAGE
00158                 m_CeilingMap.SetLocation(376,  51, -2.00);
00159         #else
00160                 m_CeilingMap.SetLocation(x_pos,  y_pos, theta);
00161         #endif
00162 #endif
00163         
00164         return RTC::RTC_OK;
00165 }
00166 
00167 
00168 RTC::ReturnCode_t CeilingNavigation::onShutdown(RTC::UniqueId ec_id)
00169 {
00170         // 動的確保メモリ開放
00171         fclose(fp);
00172         return RTC::RTC_OK;
00173 }
00174 
00175 RTC::ReturnCode_t CeilingNavigation::onActivated(RTC::UniqueId ec_id)
00176 {
00177         ImageData cameraImage;
00178 
00179         // カメラコンポーネントから送信されてきている画像情報を読み込む
00180         if(m_OfflineImage.length() == 0){
00181                 while(!m_CameraDataIn.isEmpty())
00182                         m_CameraDataIn.read();
00183                 // バイナリ配列を適切な状態に変更する
00184                 cameraImage.SetData(m_CameraData);
00185         }
00186 
00187 #ifdef _OFFLINE_IMAGE
00188         else{
00189                 char fname[256];
00190                 // オフラインデータを読み込んで、m_CameraDataに格納する
00191                 memset(fname, 0, sizeof(fname));
00192                 sprintf( fname, "%s/cap_%05d.png", m_OfflineImage.c_str(), count );
00193                 cameraImage.LoadImage( fname, CV_LOAD_IMAGE_GRAYSCALE);
00194         }
00195 #endif
00196 
00197         //ブロックマッチングの準備
00198         m_BlockMat.CreateBlockMap ( m_Center_X, m_Center_Y, cameraImage.GetWidth(),
00199                 cameraImage.GetHeight(), m_BlockSize, m_BlockCoefficient, m_BlockResolution, m_BlackWhiteValue);
00200 
00201         return RTC::RTC_OK;
00202 
00203 }
00204 
00205 /*
00206 RTC::ReturnCode_t CeilingNavigation::onDeactivated(RTC::UniqueId ec_id)
00207 {
00208   return RTC::RTC_OK;
00209 }
00210 */
00211 
00212 RTC::ReturnCode_t CeilingNavigation::onExecute(RTC::UniqueId ec_id)
00213 {
00214         ImageData cameraImage;
00215         // カメラコンポーネントから送信されてきている画像情報を読み込む
00216         if(m_OfflineImage.length() == 0){
00217                 m_CameraDataIn.read();
00218                 // 形式変換
00219                 // バイナリ配列を適切な状態に変更する
00220 
00221                 cameraImage.SetData(m_CameraData);
00222         }
00223 #ifdef _OFFLINE_IMAGE
00224         else{
00225                 char fname[256];
00226 
00227                 // オフラインデータを読み込んで、m_CameraDataに格納する
00228                 memset(fname, 0, sizeof(fname));
00229                 if(count > 1061) count = 130;
00230                 sprintf( fname, "%s/cap_%05d.png", m_OfflineImage.c_str(), count++ );
00231                 cameraImage.LoadImage( fname, CV_LOAD_IMAGE_GRAYSCALE);
00232         }
00233 #endif
00234 
00235         //統合後の位置推定値が得られるときはそれによってx_pos,y_pos,thetaを更新し
00236         //次のマッチング範囲の中心点とする．
00237         if(m_LocalizedPositionIn.isNew())
00238         {
00239         while(!m_LocalizedPositionIn.isEmpty())
00240                 m_LocalizedPositionIn.read();
00241 
00242                 //下のm_CeilingPosition の計算の逆計算により実世界上の位置を画像上の位置に変換
00243                 //コンポーネント間で共通の定数があるので何とかすべし．
00244 
00245                 theta = m_LocalizedPosition.data.heading + M_PI/2.0;
00246                 if (theta > M_PI)               // 180度を越えている場合、マイナスに変更する
00247                         theta = -M_PI * 2 + theta;
00248                 else if (theta < -M_PI) // -180を下回る場合、プラスに変更する
00249                         theta = M_PI * 2 + theta;
00250                 
00251                 x_pos = round( m_LocalizedPosition.data.position.x / mPerPix ) + 69 - m_BlockSize/2;
00252                 y_pos = - (unsigned long)round( m_LocalizedPosition.data.position.y / mPerPix ) + (333.0 - 63.0) - m_BlockSize/2;
00253 
00254         }
00255 
00256         m_CeilingMap.SetLocation(x_pos, y_pos, theta);
00257 
00258         //ブロックマッチングによる位置推定
00259         m_BlockMat.MapTracking(m_CeilingMap, cameraImage);
00260         
00261         theta = m_CeilingMap.GetTheta();
00262         x_pos = m_CeilingMap.GetPosX();
00263         y_pos = m_CeilingMap.GetPosY();
00264 
00265 /*
00266           std::cout <<"[output Data]: "<< m_CeilingOdometryd.data[0]<< ","
00267                     << m_CeilingOdometryd.data[1]<< ","
00268                     <<m_CeilingOdometryd.data[2] << std::endl;*/
00269 
00270 
00271   if(delaycount == 0){          //8ループに一回，出力ポートにデータ送信
00272 
00273 //              m_CeilingOdometry.theta = theta;
00274 //              m_CeilingOdometry.x = x_pos;
00275 //              m_CeilingOdometry.y = y_pos;
00276 
00277           /*std::cout <<"[output Data]: "<< m_CeilingOdometry.x<< ","
00278                     << m_CeilingOdometry.y<< ","
00279                     <<m_CeilingOdometry.theta << std::endl;*/
00280 
00281                 //画像上位置を実世界位置に変換
00282                 //(1ピクセルあたりの実世界長[m])×{(画像上位置[pixel])-(画像上原点位置[m])}
00283     //今はRTC再利用センターの天井地図に合わせてある．
00284     //原点は入り口の丸い電灯の下．入り口に室内を向いて立ったとき右がx，前がy．
00285     //方位角はx＋が0，反時計回りに＋
00286 
00287                 //m_CeilingPosition.x = m_RealMapWidth/m_VirtualMapWidth*(double)x_pos+1.7;
00288                 m_CeilingPosition.data.position.x = mPerPix * ((double)(x_pos + m_BlockSize/2)  - 69.0 );
00289                 m_CeilingPosition.data.position.y = mPerPix * ((double)(y_pos + m_BlockSize/2) - (333.0 - 63.0)) * (-1); //画像と地図ではy軸の向きが逆
00290 
00291                 m_CeilingPosition.data.heading = theta - M_PI/2.0;
00292                 if (m_CeilingPosition.data.heading > M_PI)   //180度を越えている場合マイナスに変更する
00293                         m_CeilingPosition.data.heading = -M_PI * 2 + m_CeilingPosition.data.heading;
00294                 else if (m_CeilingPosition.data.heading < -M_PI)           //-180を下回る場合プラスに変更する
00295                         m_CeilingPosition.data.heading = M_PI * 2 + m_CeilingPosition.data.heading;
00296 
00297                 //ここまでで計算した値はカメラの画像中央の値．これをロボットの原点での値に直す．
00298                 m_CeilingPosition.data.position.x = m_CeilingPosition.data.position.x - camX * cos(m_CeilingPosition.data.heading) + camY * sin(m_CeilingPosition.data.heading);
00299                 m_CeilingPosition.data.position.y = m_CeilingPosition.data.position.y - camX * sin(m_CeilingPosition.data.heading) - camY * cos(m_CeilingPosition.data.heading);
00300 
00301                 std::cout <<"[output Data]: "<< m_CeilingPosition.data.position.x << ","
00302                                   << m_CeilingPosition.data.position.y << ","
00303                                   <<m_CeilingPosition.data.heading << std::endl;
00304         
00305 //              m_CeilingOdometryOut.write();
00306                 m_CeilingPositionOut.write();
00307 
00308                 //GnuPlot表示用(オフライン)に推定値データを出力
00309                 fprintf(fp,"%f %f %f\n",m_CeilingPosition.data.position.x,m_CeilingPosition.data.position.y,m_CeilingPosition.data.heading);
00310                 
00311                 delaycount = 8; //天井カメラによる位置修正を適度な間隔で行うための待ち設定．値はテキトー
00312   }
00313   delaycount--;
00314 
00315         usleep(10000);
00316 
00317   return RTC::RTC_OK;
00318 }
00319 
00320 
00321 
00322 extern "C"
00323 {
00324  
00325   void CeilingNavigationInit(RTC::Manager* manager)
00326   {
00327     RTC::Properties profile(ceilingnavigation_spec);
00328     manager->registerFactory(profile,
00329                              RTC::Create<CeilingNavigation>,
00330                              RTC::Delete<CeilingNavigation>);
00331   }
00332   
00333 };