amcl.yaml とgazebo で設定した初期座標値がずれる件

gazsim.bash で実行したときに，amclthreadが

Applying Pose 自分の入力した座標
Setting Pose carologistics のデフォルト値
Applying Pose carologistics のデフォルト値

の順で上書きされ、シミュレータ上のrobotinoの座標がおかしくなります．

fawkes-robotino/fawkes/src/plugins/amcl/amcl_thread.cpp を読んでみました．

「Applying Pose 自分の入力した座標」に関しては，
init() にて，

　init_pose_へ，設定ファイル（amcl.yaml）のinit_pose_{x, y, a}を代入．
　pf_init_pose_mean へ，init_pose_ を代入．
　initial_pose_hyp_ へ pf_init_pose_mean を代入．
　apply_initial_pose() を呼び出し，座標値としてinitial_pose_hyp_ を使う．

つまり，設定ファイルのinit_pose_? で，座標値を設定しています．

次の，Setting Pose caro デフォルト値 ですが，
これは，set_initial_pose() で出力しているメッセージです．

この関数は，bb_interface_message_received() で呼び出しています．
ここを見てみると，LocalizationInterface の SetInitialPoseMessage を
受け取ったときに，その中のデータに基づいて，set_initial_pose() を
呼び出して，座標値を設定しているっぽいです．

つまり，amcl としての初期化だけでなく，
BlackBoard 経由で，初期値情報を受け取ったら，
そちらで上書きするというプログラムになっています．

さて，LocalizationInterface は，誰が扱っているかというと，
起動した後に，ffplugin -l webview でwebview を読み込み，
8081 ポートで確認します．
（gazsim.bash では，２つのfawkes を動かしますので，
　ログを見てポート番号を探します）

Showing LocalizationInterface::AMCL

Type:	LocalizationInterface
ID:	AMCL
Writer:	AmclThread
Readers:	AmclROSThread, WebviewThread (2)

ということなので，AcmlROSThread が犯人ですので，acml/ros_thread.cpp を開き，
LocalizationInterface である loc_if_ を追いかけます．

initial_pose_received() の関数で，書き込んでいるようなので，
その呼び出し元を見てみると，init ()にて，

  initial_pose_sub_ =
    rosnode->subscribe("initialpose", 2,
                       &AmclROSThread::initial_pose_received, this);

と，rosnode から受け取っているっぽいです．

で，ros のどこでinitialpose を出しているのか探してみましたが，
全然見つかりませんでした．

gazebo-rcll/worlds/test.world の座標を使っているようでもありません．

gazsim.bash のターミナルを見ていて，ようやく答えがわかりました．

gazsim.bash の中に，

# publish initial poses
sleep 15s
echo "publish initial poses"
$initial_pose_script_location -d

こんな作業が書かれています．

initial_pose_script_location の中身は，

initial_pose_script_location=$script_path/gazsim-publish-initial-pose.bash

です．

というわけで，gazsim-publish-initial-pose.bash を見てみると，

$set_pose -r localhost:1921 -t 2.0 --  3.5  -0.5 0.0  0.0 0.0 0.7 0.7

見覚えのある数字が出てきました．

ここで，各ロボットの初期値を，アナウンスすることで，
再設定しているわけです．