Navigationの際のCost mapのlayerについて

[Hoshito Kudo]

いつもお世話になっております、工藤です。

連投に近い形となってしまい、申し訳ございません。

最終的な目的は前回の質問と同様ではありますが、アプローチが異なると感じたため、新しくトピックを立てさせていただきました。

再び、皆様のお知恵を貸していただければ幸いです。

今回お聞きしたいのは、コストマップのレイヤーについてです。

先日、人がいる可能性が高い場所を座標で管理したいという内容で質問させていただいたのですが、

座標ではなくコストマップで管理してみるのはどうだろうということを思いつきました。

調べてみると、http://wiki.ros.org/costmap_2d/Tutorials/Creating%20a%20New%20Layer で紹介されているように、新しくコストマップのレイヤーを作れることを知りました。

しかし思ったよりも参考例が少なく、頭を抱えています。

今考えているものとしましては、大まかではありますが、

①人がいる可能性が高い場所をコストマップとして管理したい。

②Rviz等で表示する際には、人がいる可能性の高さによって対象のセル(グリッド?)の色を変えたい。

③そのセルの場所を座標に変換し、Navigationの目的地として指定したい。

といった内容になります。

あくまで空想に過ぎず、おそらく不可能である項目もあると思われますが、

もしこれらに近しい内容をC++で実現出来るような案がございましたら、ご教示いただければと存じます。

稚拙な文章で申し訳ございません。

イメージは出来ているのですが、やりたいことが上手く文章にまとまらず、しどろもどろなものとなってしまっているかと思われます。

どうかご助力いただければ幸いです。

よろしくお願いいたします。

開発環境

ROS: Hydro

Ubuntu: 12.04 LTS

Robot: Turtlebot2 kobuki

Camera: Kinect V1