##############################################
# モンテカルロ位置推定コンポーネントの使い方 #
#                                            #
# Tsuyoshi Suenaga, NAIST                    #
# tsuyo-s@is.naist.jp                        #
#                                            #
# 2010/05/20                                 #
##############################################

1.概要
　本プログラムはLRF距離データ，オドメトリ情報を取得し，地図とのマッチングを行うことで自己位置を推定するRTコンポーネントです．
　外部の地図管理RTCと接続しなかった場合，ローカルに持っている地図を利用し，自己位置推定を行います．
　オドメトリ取得ポートが接続状態である場合，オドメトリ動作モデルにより自己位置を推定します．
　また非接続状態である場合，ロボット位置を中心としてパーティクルを均等にまき，自己位置を推定します．
　GUIではマウスの右クリック＋ドラッグにより描画地図の並進，中クリック＋ドラッグにより描画地図の拡大縮小を行います．

2.依存ライブラリ
　GTK+ toolkit 2.x
　OpenCV 1.0以上

3.コンパイル
　(1)GUIの有無の指定
　　Makefile内のWITHOUT_GUI_MODEをyesにするとウィンドウを一切表示しない．
　　GTK-+2.0も要求しない．WITHOUT_GUI_MODEをnoにするとgtkを利用して地図，
　　現在位置，現在のレーザスキャン，パーティクルなどを表示する．

　(2)コンパイル
　　cd src/
　　make -f Makefile.MonteCarloLocalizationLRF

4.実行
　　./MonteCarloLocalizationLRFComp

5.設定
　5.1 地図サイズの大きなデータ(2MB以上)を扱う場合，
　　rtc.conf 内の下記の値を大きく設定すること．（初期設定 6MB）
　　この値は送信側RTC，受信側RTCの双方で設定する必要がある．
　　corba.args: -ORBgiopMaxMsgSize 6291456

　5.2 コンフィギュレーションセットを設定する
　　※ 変数名が「_」(アンダーバー)で始まるパラメータはActivate前に設定すべき変数である．
　　- _num_particles
　　　位置推定に利用するパーティクル数を指定する．
　　　パーティクルの数を多くすると位置推定精度は向上するが処理速度が低下する．
　　　パーティクルの数が少なすぎる場合は，位置推定に失敗する．

　　- _def_pos_x_m
　　- _def_pos_y_m
　　- _def_pos_theta_deg
　　　初期位置，姿勢を指定する．単位は[m],[deg]．
　　　利用する地図とスタート位置に合わせて指定．
　　　この値はActivate直後の1回のみしか利用されない．

　　- _std_pos_x_m
　　- _std_pos_y_m
　　- _std_pos_theta_deg
　　　初期探索時の位置・姿勢の標準偏差を指定．単位は[m], [deg]．
　　　この値は自己位置初期化時に利用される．

　　- _m_per_pixel
　　　ウィンドウに表示する時の解像度を指定．単位は[m/pixel]．
　　　この値は位置推定には影響を与えない．

　　- _screen_offset_x
　　- _screen_offset_y
　　　ウィンドウに表示する時の中心座標を指定．単位は[pixel]．
　　　この値は位置推定には影響を与えない．

　　- _map_format
　　　地図データのフォーマットを指定．
　　　POINT_MAP, LINE_MAP, OCCUPANCY_GRID_MAPから選択する．
　　　Mapポートが他RTCと接続された場合（サービスポートからデータを取得する場合）には無視される．

　　- _map_filename
　　　地図データファイルへのパスを指定する．
　　　Mapポートが他RTCと接続された場合（サービスポートからデータを取得する場合）には無視される．

　　- _x_coord_front_lrf_info
　　　LRF情報取得ポート(LRFInfo)の座標系の向きを指定．
　　　1: X軸正面, 0: Y軸正面．

　　- _x_coord_front_current_pos_init
　　　現在位置取得ポート(CurrentPosInit)の座標系の向きを指定．
　　　1: X軸正面, 0: Y軸正面．

　　- _x_coord_front_current_pos
　　　現在位置送信ポート(CurrentPos)の座標系の向きを指定．
　　　1: X軸正面, 0: Y軸正面．

　　- _x_coord_front_odometry
　　　オドメトリ取得ポート(Odometry)の座標系の向きを指定．
　　　1: X軸正面, 0: Y軸正面．

6.入出力仕様
　6.1 入力ポート
　　- RangeData: 距離データ列 [mm]
　　-- TimedLong
　　- CurrentPosInit: 初期化用の自己位置 [m, m, rad]
　　-- TimedOdometry（x, y, theta）
　　- Odometry:  オドメトリ [m, m, rad]
　　-- TimedOdometry（x, y, theta）

　6.2 出力ポート
　　- CurrentPos: 推定された現在位置，姿勢 [m, m, rad]
　　-- TimedOdometry（x, y, theta）

　6.3 サービスポート（Consumer）
　　- LRF（LRFInfo.idl）
　　-- getTotalScanPoints(): 計測される総点数 [pionts]
　　-- getResolution()     : 計測解像度 [rad/scan]
　　-- getOffsetFOV()      : 計測範囲のオフセット [rad] 3時方向を0radとする反時計回り．
　　- Map（MapInfo.idl）
　　-- setFullGridMap(...) : 地図全体の登録（未使用）
　　--- mapdata             * 地図データ
　　--- origin_coord_x      * 地図始点x座標 [grid]
　　--- origin_coord_y      * 地図始点y座標 [grid]
　　--- width               * 地図幅（y軸の長さ）   [grid]
　　--- height              * 地図高さ（x軸の長さ） [grid]
　　--- resolution          * 地図解像度 [m/grid]
　　-- getFullGridMap(...) : 地図全体の取得
　　--- mapdata             * 地図データ
　　--- origin_coord_x      * 地図始点x座標 [grid]
　　--- origin_coord_y      * 地図始点y座標 [grid]
　　--- width               * 地図幅（y軸の長さ）   [grid]
　　--- height              * 地図高さ（x軸の長さ） [grid]
　　--- resolution          * 地図解像度 [m/grid]
　　-- getPartialGridMap() : 地図一部の取得（未使用）

　6.4 サービスポート（Provider）
　　- Location（LocationInfo.idl）　（仮実装，排他処理が未実装なためデータが同期しない場合あり）
　　-- setLocation(x, y, theta)    : 自己位置の初期化 [m, m, rad]
　　-- getLocation(&x, &y, &theta) : 自己位置の取得   [m, m, rad]

7.開発環境
　- 保守環境
　-- ubuntu 10.04
　-- OpenRTM 1.0
　-- omniORB 4.1.2-1
　-- G++コンパイラ 4.4.3
　-- GTK+ toolkit 2.20.0
　-- OpenCV 2.0
　- 初期開発（雛形生成時）
　-- ubuntu 8.04
　-- OpenRTM 0.4.2-1
　-- ACE 5.4.7-13
　-- omniORB 4.1.1-2
　-- PatternWeaver 2.3
　-- G++コンパイラ 4.2.4
　-- GTK+ toolkit 2.12
　-- OpenCV 1.0

8.ディレクトリ構成
　- pw_model：Pattern Weaverで作成したモデルデータ群
　- src：Pattern Weaverで生成したひな形に実処理を実装したソースコード

9.ChangeLog
　- 2010.05.20
　　OpenRTM-aist-1.0対応
　　バージョン0.9.0 リリース

　- 2009.11.09
　　バージョン0.8.1 リリース

　- 2009.11.08
　　出力ポートを複数出力に対応するためNullBufferに変更．
　　デフォルトコンフィギュレーションセットの読み込み処理を追加．
　　地図をマウスのドラッグでの並進・拡大縮小に対応．
　　占有グリッドマップをルックアップテーブルとして利用する尤度計算アルゴリズムに変更．
　　オドメトリ動作モデルを導入．
　　バージョン0.8.0 リリース

　- 2009.11.04
　　入出力ポートの座標系変換を判断するコンフィギュレーションセット変数を追加．
　　バージョン0.6.0 リリース

　- 2009.11.01
　　地図管理コンポーネントからの地図データ取得処理を追加．

　- 2009.09.29
　　ProviderポートのLocationサービスを仮実装
　　自己位置の初期化用データポートの追加
　　バージョン0.4.0 リリース

　- 2009.07.06
　　暫定版（バージョン0.2.0）完成

10.利用手順例
　接続例をexample1.png, example2.pngに示す．
　10.1 最小構成（example1.png）
　　（1）コンフィギュレーションにより，初期位置，地図データを設定．
　　（2）LRG RTCを先にActivateする．
　　（3）モンテカルロ位置推定RTCをActivateする．（LRF情報に基づきスキャン範囲を取得）
　　（4）取得した地図データとLRFデータのマッチングにより位置推定が行われ，出力ポートCurrentPosから推定された自己位置が送信される．
　　（5）必要に応じて，入力ポートCurrentPosInitに位置情報を送ることで現在位置を修正できる．
　10.2 地図管理RTCの利用（example2.png）
　　（1）コンフィギュレーションにより，初期位置を設定．
　　（2）LRG RTCと地図管理RTCを先にActivateする．
　　（3）モンテカルロ位置推定RTCをActivateする．（LRF情報に基づきスキャン範囲を取得，地図管理RTCより地図データを取得）
　　（4）取得した地図データとLRFデータのマッチングにより位置推定が行われ，出力ポートCurrentPosから推定された自己位置が送信される．
　　（5）必要に応じて，入力ポートCurrentPosInitに位置情報を送ることで現在位置を修正できる．

11.地図フォーマット
　本プログラムではPOINT_MAP, LINE_MAP, OCCUPANCY_GRID_MAPの3つの地図フォーマットが使用可能である。
　- POINT_MAP
　　2D地図を複数の点で表現した地図であり，テキスト形式の地図ファイルとなる．
　　テキストファイルの1行に，1つの点の座標が記述される．単位はcmである．
　　精度の良い位置推定を行うためには，密な点地図が必要である．
　　
　　=== 地図の例 ===
　　.       .
　　.       .
　　.       . . .
　　.    
　　.    
　　.    
　　. . . . . . .

　　=== 座標の例 ===
　　.(0,60) .(40,60)
　　.(0,50) .(40,50)
　　.(0,40) . . . (40,40) (50,40) (60,40)
　　.(0,30)
　　.(0,20)
　　.(0,10)
　　. . . . . . . (0,0) (10,0) (20,0) (30,0) (40,0) (50,0) (60,0)

　　=== ポイント地図記述（数値の間はTABで区切る） ===
　　0  0
　　10  0
　　20  0
　　30  0
　　40  0
　　50  0
　　60  0
　　0  10
　　0  20
　　0  30
　　0  40
　　40  40
　　50  40
　　60  40
　　0  50
　　40  50
　　0  60
　　40  60


　- LINE_MAP
　　2D地図を複数の直線で表現した地図であり，テキスト形式の地図ファイルとなる．
　　テキストファイルの1行に，1つの直線の始点、終点の座標が記述される．
　　始点x座標, 始点y座標, 終点x座標, 終点y座標の順にTABで区切られる．座標単位はcmである．

　　=== 地図の例 ===
　　+      +
　　|      |
　　|      +----+
　　|    
　　|    
　　|    
　　+-----------+

　　=== 座標の例 ===
　　(0, 300)
　　+      + (200, 300)
　　|      |
　　|      +----+ (350, 200)
　　|  (200, 200)
　　|    
　　|    
　　+-----------+ (350, 0)
　　(0, 0)

　　=== ライン地図記述（数値の間はTABで区切る） ===
　　0  0  350  0
　　0  0  0  300
　　200  200  350  200
　　200  200  200  300


　- OCCUPANCY_GRID_MAP
　　2D地図をグリッドで区切り，各グリッド内の障害物の有無を0〜255の値で表現した地図であり，バイナリPGM画像形式の地図ファイルとなる．
　　"0"が壁（走行不可能領域），"127"が未計測範囲（未知領域），"255"が通路（走行可能領域）である．
　　画像ヘッダのコメント部には，グリッドサイズ（resolution）と原点補正位置（origin）が記述される．
　　グリッドサイズ（resolution）は，m単位で記述され，1グリッドのサイズを規定する．
　　原点補正位置（origin）は，ピクセル単位で原点位置に対するずれ量を指定する．(0, 0)の時は画像左下が原点となる．

　　=== 地図の例 ===
　　+      +
　　|      |
　　|      +----+
　　|    
　　|    
　　|    
　　+-----------+

　　=== 座標の例 ===
　　(0, 3.0)
　　+      + (2.0, 3.0)
　　|      |
　　|      +----+ (3.5, 2.0)
　　|  (2.0, 2.0)
　　|    
　　|    
　　+-----------+ (3.5, 0)
　　(0, 0)

　　=== 占有格子地図記述例 ===
　　P5
　　#resolution 0.5
　　#origin 0 0
　　8 7
　　255
　　以降，画像データ（バイナリ形式で保存すること）
　　0 255 255 255   0 127 127 127
　　0 255 255 255   0 127 127 127
　　0 255 255 255   0   0   0   0
　　0 255 255 255 255 255 255 255
　　0 255 255 255 255 255 255 255
　　0 255 255 255 255 255 255 255
　　0   0   0   0   0   0   0   0
