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 では,2つの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

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

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

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です

*