つくばの特定の場所でコストマップが出ない

[masakifujiwara1]

概要

特定の場所でコストマップが出ない問題について調査を行う．

問題

以下の動画のように，確認走行エリア内の市役所の裏に差し掛かる坂付近でコストマップが消えてしまう問題があった．（再現性あり） https://youtu.be/wnRaBaflOwE?si=EyttGmvgtAx4_i8W&t=128

仮説

以下の画像のように，pcdマップの端になるにつれて反っているため，map座標系に対してロボットがz軸方向にかなり高く（約1.7m）なっている．

直接的な原因ではないが，問題の再現性があることから高さがトリガーになっている可能性がある．

• コストマップが消える坂付近

• pcdマップの地面を構成している点の高さ

[masakifujiwara1]

検証1

津田沼2号館3階のpcdマップの高さをいくつか変更し，コストマップが正常に出るか確認する． （つくばでは約1.7ｍ付近でコストマップが消えている）

1.1m

コストマップが出ることを確認 https://youtu.be/vxMypUYvF38

1.6m

コストマップが出ることを確認 https://youtu.be/kkIoBscZC5o

1.8m

コストマップが出なかった https://youtu.be/ChAeoodlmEg

[masakifujiwara1]

検証2

つくばのpcdマップの高さを変更して，元の高さのときと比較を行う． 具体的には，つくばで取ったrosbagのmap->odom間のtfだけを削除し，再度mcl_3dlで自己位置推定しながら，コストマップを確認する．

元の高さ

https://youtu.be/wnRaBaflOwE?si=EyttGmvgtAx4_i8W&t=128

pcdマップの高さを変更

坂付近でもコストマップが出ていることが確認できた． 編集したpcdファイル https://youtu.be/UBJrGLyjtHE?si=Wfd84wGCMmDxXh1q&t=173

[masakifujiwara1]

検証を踏まえた考察

以下の動画 2:35では，ロボット自体の高さが1.7mを超えているが，ロボットに対して前方左側のコストマップは出ている．これは，ロボットが傾いており，rfansのpointcloudが1.7mを下回っている部分において，コストマップが正しく反映されていると思われる．つまり，ロボット自体の高さに起因しているのではなく，障害物としてセンシングするpointcloudのmap座標系におけるz軸方向の高さが起因している恐れがある．

コストマップのfameがbase_link基準であることが要因だと考え，後述するframeの変更でframeを変更し，確認した．

local_cost_cloud_layerのsensor_frameがsurestarであることが要因だと考え，後述するframeの変更でframeを変更し，確認した．

https://youtu.be/wnRaBaflOwE?si=hhz7BPKoLb-5fi-9&t=155

frameの変更

以下のノードを用いて，base_linkの座標のz成分を0にしたリンクをコストマップのframeに設定したが，コストマップは出なかった．

https://github.com/open-rdc/orne-box/blob/TsukubaChallenge2021/orne_box_bringup/src/projected_transform_publisher_node.cpp

応急処置

確認走行エリアに関しては，pcdマップの高さを変更し，対処する． 今後，3次元地図を作り直す必要がある．

原因の調査

根本的な原因は特定できておらず，継続して調査が必要である．