寫在前面：本文為原創(chuàng)残黑，如需轉(zhuǎn)載請(qǐng)注明出處(http://www.reibang.com/p/7858b30a50cd)俗孝。歡迎大家留言共同探討匙奴，有誤的地方也希望指出俗冻。另如果有好的SLAM礁叔、ROS等相關(guān)交流群也希望可以留言給我，在此先謝過(guò)了迄薄。
參考：
1: How To: Get ground truth pose data in ROS Gazebo
2: 代碼
3: Rviz: fixed frame [world] does not exist
1 目標(biāo)
在Gazebo中可以使用模擬的機(jī)器人和預(yù)先建立好的地圖進(jìn)行AMCL的定位算法模擬琅关。但普通的AMCL是沒(méi)有機(jī)器人的實(shí)際位置輸出，僅有機(jī)器人的位姿輸出讥蔽，難以獲得機(jī)器人在定位過(guò)程中的誤差變化然后進(jìn)行相應(yīng)優(yōu)化涣易。因此需要增加一個(gè)topic來(lái)顯示機(jī)器人在該地圖上的真實(shí)位置，即Ground truth pose信息冶伞。
2 實(shí)現(xiàn)
根據(jù)1中的目標(biāo)新症，經(jīng)過(guò)一系列查找發(fā)現(xiàn)參考1中的文章提供了靠譜的解決方案，即通過(guò)在URDF文件中增加一個(gè)Plugin响禽，發(fā)布新的地圖topic /ground_truth/state徒爹。 具體的代碼在參考2中，復(fù)制后添加在所用的URDF文件末尾即可芋类。其會(huì)發(fā)布一個(gè)world frame和base_link的轉(zhuǎn)換隆嗅。
調(diào)整好后，gazebo與amcl侯繁，在rviz中出現(xiàn)如下故障：
發(fā)現(xiàn)并未有world frame胖喳。通過(guò)參考3，想到解決方向：建立新的static_transform_publisher贮竟。根據(jù)現(xiàn)在情況丽焊，需要的是world->bas_link體系的坐標(biāo)轉(zhuǎn)換较剃，因此根據(jù)實(shí)際情況添加world->map的坐標(biāo)轉(zhuǎn)換。
<node pkg="tf" type="static_transform_publisher" name="world_map"
args="0.0 0.0 0.0 0.0 0.0 0.0 /world /map 40" />
運(yùn)行后粹懒，可以正確得到1中要求的目標(biāo)重付。
如圖，藍(lán)色為機(jī)器的Ground truth凫乖，紅色箭頭為機(jī)器人的amcl定位位姿結(jié)果。
ps. 因給出的默認(rèn)Ground truth顯示模式是用紅色的里程計(jì)模型顯示的弓颈，推薦改為其他顏色帽芽，與機(jī)器人的amcl_pose結(jié)果作為區(qū)分比對(duì)。
