costmap_2dの障害物情報のクリアについて - うごくものづくりのために

勘違いしてました。

costmap_2dは、センサ情報としてPointCloudまたはLaserScan型のメッセージを受け取ります。

RoboCup SSLでは、フィールド上部のカメラからロボット及びボールの絶対座標が常に取得できるので、その値を使ってロボットの輪郭をなぞったPointCloudを渡してやればいいだろう、と思っていました。

が、うまくいかず。障害物は確かに認識されるのですが、障害物が動いてPointCloudのデータが変化した後も、前の場所に障害物の情報が残ってしまう、という現象が起きました。

これでしばらくハマりましたが、おそらく原因がわかりました。

costmap_2dが障害物をクリアする条件は、

以前観測された点が、直近に観測された点とロボット本体の間にある場合

のようです。

ですから、障害物情報を消去したい場合は、ロボット本体と観測の間に障害物が入るような観測を与えてやる必要があります。

costmap_2dは、RoboCupSSLのようなワールド座標に固定されたセンサ情報は前提としておらず、ロボット本体に乗っかったセンサのみを扱うように設計されているみたいです。

障害物情報を消去する方法としてもうひとつ、 move_baseのサービスである、"clear_costmap"を呼ぶという方法もあります。

しかしながら、"clear_costmap"サービスは、高速に頻繁に呼ばれることを想定していないみたいです。あんまり頻繁に呼びすぎるとクラッシュするとか。