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

[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軸方向の高さが起因している恐れがある．

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次元地図を作り直す必要がある．

原因の調査

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

[yasuohayashibara]

現在のレポジトリがどれかが分からないところはありますが，コストマップに反映するLiDARの計測データの高さを指定するパラメータがあります．
こちらを緩和するとおそらくどの高さでも反映されるようになりますが，緩くしすぎると屋根の下や坂が走行できなくなりますので，お気をつけください．

https://github.com/open-rdc/orne_navigation/blob/be6cfccb57a76b3b8afa3fcf79fcef881bf06861/orne_navigation_executor/param/costmap_common_params.yaml#L13

[masakifujiwara1]

コメントありがとうございます．
確認はしていて，obstacle_heightは±.infになっています．

orne-box/orne_box_navigation_executor/config/move_base/box2/costmap_common.yaml

Lines 41 to 42 in 98eef6c

	min_obstacle_height: -.inf
	max_obstacle_height: .inf

[yasuohayashibara]

このパラメータが無視されているおそれがありますね．
もしくはこれを上書きするようなパラメータがあるということでしょうか．
下限を制限していない場合は地面も含めて障害物として登録されると思いますが，それが起きていないということは反映されていないということですね．

ソースコードは公開されていると思いますので，それを読んで問題を解決するのが一番近道な気がします．

[masakifujiwara1]

このトピックは地面をクロップしているものなので, 反映はされていそうです.

[yasuohayashibara]

local_obstacles_layerに関しては以下のようになっています．
それぞれのレイヤーがどのような役割になっているかを把握していませんが，障害物検出をこちらのレイヤーで行っているとしたら，これが影響しているおそれがあります．

orne-box/orne_box_navigation_executor/config/move_base/box2/costmap_common.yaml

Lines 16 to 17 in 98eef6c

	min_obstacle_height: 0.1
	max_obstacle_height: 30.0

[masakifujiwara1]

以下のような役割になっていて，別々のパラメータなので正しく機能していそうです．

• local_obstacles_layer: 2d（hokuyo 低層障害物用）
• local_cost_cloud_layer: 3d（rfans 一般障害物用）

orne-box/orne_box_navigation_executor/config/move_base/box2/local_costmap.yaml

Lines 19 to 25 in 98eef6c

	plugins:
	- name: local_obstacles_layer
	type: costmap_2d::ObstacleLayer
	- name: local_cost_cloud_layer
	type: costmap_2d::ObstacleLayer
	- name: inflation_layer
	type: costmap_2d::InflationLayer

[yasuohayashibara]

@masakifujiwara1
こちら解決したと聞いていますので，時間のある時で構いませんので，解決法を記載して閉じていただければと思います．