[jsk_pcl_ros] Add octomap contact

[mmurooka]

[mmurooka]

octomap のリンク占有領域と接触センサ情報に応じて更新するバージョンです．
こちらもrobot_self_filterに依存していますので，robot_self_filterが見つからない場合はコンパイルされないです．
ドキュメント追加します．

[garaemon]

nodeletにしてみましょう。
octomap関連から持ってきたのがある場合、ディレクトリをほってわかりやすく. 自分で書いたならライセンスとコーディングスタイル, 名前空間を整える。

[garaemon]

@ brief にする。他のコードのスタイルに合わせる

[garaemon]

コメントしてない部分も、スタイルが他のコードと合ってないので合わせるように.

[garaemon]

ここまでやるならstd::stringもconstで参照にする。
jsk_pcl_rosでは_tf_prefixという変数名は基本的に使っていない。

メンバ変数は_で終わるように、引数は_なし

[garaemon]

see jsk-ros-pkg/jsk_common#1202

[mmurooka]

ありがとうございます．修正してみます．

[mmurooka]

修正しました．
インデント修正のみ別コミットにしておけばよかったです．．
このcommitは確認して大丈夫そうでしたら，rebaseして mmurooka@793a6f9 と一緒にします．

全ファイルについて以下を修正しました．
・ if, for, catch, 関数定義 の括弧周りのスペース
・ ファイル先頭のライセンス
・ ヘッダのインクルードガード
・ 変数名
・ 関数の引数をconst参照渡しにする
・ 関数の説明で@briefを使う

ただ， src/OctomapServerContact.cpp の
・ 変数名
・ if, for, catch, 関数定義 の括弧周りのスペース
は，
https://github.com/OctoMap/octomap_mapping/blob/indigo-devel/octomap_server/src/OctomapServer.cpp
からコピペしたところがいくつかあって，
そもそもこのコードが変数の命名規則に統一性がないように思うので，
どうしようか迷い中です．

例えば
本家のpublishAll()という関数（ https://github.com/OctoMap/octomap_mapping/blob/indigo-devel/octomap_server/src/OctomapServer.cpp#L475 ）をオーバーライド（ https://github.com/jsk-ros-pkg/jsk_recognition/pull/1276/files#diff-2e1f85fd7b4a76c30ff8959d89b17303R252 ）していますが，
もともとの記述をコピペした上で未知領域のマーカをpublishするように追加しているので，
https://github.com/jsk-ros-pkg/jsk_recognition/pull/1276/files#diff-2e1f85fd7b4a76c30ff8959d89b17303R431
もとのソースからコピペした部分までは書き換えなくてもよろしいでしょうか．

src,includeへの追加ファイルは以下になります．ディレクトリ等は特に作らなくてよろしいでしょうか．

・ OcTreeクラスを継承したOcTreeContactクラスの定義
   接触センサのセンサモデルで使う確率に関する記述
    include/jsk_pcl_ros/OcTreeContact.h 
・ OctomapServerクラスを継承したOctomapServerContactクラスの定義
   接触センサのセンサモデルとそれを用いたmap更新に関する記述
    include/jsk_pcl_ros/OctomapServerContact.h
    src/OctomapServerContact.h
・ SelfMaskクラスを継承したSelfMaskNamedLinkクラスの定義
   containmentを調べる関数でリンク名を指定できるようにした
    include/jsk_pcl_ros/self_mask_named_link.h
・ ノード
    src/octomap_server_node.cpp

nodeletにするのはこれからやります．
CMakeLists.txtの書き方として jsk_pcl_nodelet を使うと，
robot_self_filterの都合上リンクしてほしくないものとリンクされてしまうところが難しいのですが，
jsk_pcl_nodeletを使わずにjsk_nodeletを使うのでよろしいでしょうか．

[garaemon]

robot_self_filterの都合上リンクしてほしくないものとリンクされてしまうところが難しいのですが，

すでにgeometric_shapes問題は解決していると思うんだけど，そんなことはないかな?

[garaemon]

&は型の方に付ける．

Hoge& hogeかHoge &hogeはjsk_pcl_rosは個人的な好みで前者．

どちらでもいいが，一つのコード内で揃える．このコードの後半の部分はHoge& hogeになっている．

[garaemon]

そもそもこのコードが変数の命名規則に統一性がないように思うので，
どうしようか迷い中です．

変数の命名則は統一しましょう．パッと見，m_FooBarのmsスタイルに合わせるのが良さそうに見えます．
上流クラスのメンバ変数の命名則が統一されていないときは，まぁ諦めるしかないですね．

ちなみに，変数の命名則を統一する狙いは，コードを見た時に各シンボルの種類がスグに判断できることです．
jsk_pcl_rosのスタイルだと，

• FooBar -> class
• fooBar -> function or method
• FOO_BAR -> macro
• foo_bar -> local variable
• foo_bar_ -> instance variable

[mmurooka]

src/OctomapServerContact.cpp の
・ 変数名（octomapオリジナルに合わせて，メンバ変数はm_fooBar, ローカル変数はfooBarとしました）
・ if, for, catch, 関数定義 の括弧周りのスペース
を直しました．

[mmurooka]

すでにgeometric_shapes問題は解決していると思うんだけど，そんなことはないかな?

とりあえず確認した結果のメモですが，

collision_detectorについては

target_link_libraries(collision_detector ${pcl_ros_LIBRARIES} ${robot_self_filter_LIBRARIES} ${kdl_parser_LIBRARIES} ${kdl_conversions_LIBRARIES} ${tf_conversions_LIBRARIES})

と

target_link_libraries(collision_detector ${catkin_LIBRARIES} ${robot_self_filter_LIBRARIES})

の両方で正常に動いて，

octomapについては，

target_link_libraries(octomap_server_contact ${robot_self_filter_LIBRARIES} ${octomap_server_LIBRARIES})

で正常に動いているのが

target_link_libraries(octomap_server_contact ${catkin_LIBRARIES} ${robot_self_filter_LIBRARIES})       

では，動きませんでした．（セグフォル訳ではないがリンク内に全く含まれていることにならない）

ソースを十分追えていないですが，
collision_detectorとoctomapの違いは，
octomapでは，
https://github.com/jsk-ros-pkg/jsk_recognition/pull/1276/files#diff-2e1f85fd7b4a76c30ff8959d89b17303R189
https://github.com/jsk-ros-pkg/jsk_recognition/pull/1276/files#diff-01f9384e1ceb2a0176d1af0884f71e48R95
のように継承された先で呼んでいますが，
collision_detectorでは，
https://github.com/jsk-ros-pkg/jsk_recognition/blob/master/jsk_pcl_ros/src/collision_detector.cpp#L161
のようにひとつ外側の関数を呼んでいます．

[mmurooka]

ひとつ前のコメントで，"ソースを十分追えていないですが，"の後に書いた内容は的外れだったようでした．

target_link_libraries(octomap_server_contact ${robot_self_filter_LIBRARIES} ${octomap_server_LIBRARIES})

では問題なく動いているのが，

target_link_libraries(octomap_server_contact ${catkin_LIBRARIES} ${robot_self_filter_LIBRARIES})       

にすると動かなくなり，
print文で調べると，

https://github.com/jsk-ros-pkg/jsk_recognition/pull/1276/files#diff-2e1f85fd7b4a76c30ff8959d89b17303R45
で，親クラスであるOctomapServerのコンストラクタが呼ばれなくなっているようです．
（OctomapServerのコンストラクタ内のROS_ERRORが表示されず，中でセットしている変数も反映されていないことを確認しました．）

[mmurooka]

${catkin_LIBRARIES}をリンクすると動かなくなる問題は，
#1281 (comment) が原因で，
#1282 で解決しました．

[mmurooka]

nodeletにしてみましょう。

しました．

[mmurooka]

jadeでのみocotmap_serverが見つからないエラー

jsk_pcl_ros: Cannot locate rosdep definition for [octomap_server]

...

[jsk_pcl_ros] CMake Error at /opt/ros/jade/share/catkin/cmake/catkinConfig.cmake:75 (find_package):
[jsk_pcl_ros]   Could not find a package configuration file provided by "octomap_server"
[jsk_pcl_ros]   with any of the following names:
[jsk_pcl_ros]
[jsk_pcl_ros]     octomap_serverConfig.cmake
[jsk_pcl_ros]     octomap_server-config.cmake
[jsk_pcl_ros]
[jsk_pcl_ros]   Add the installation prefix of "octomap_server" to CMAKE_PREFIX_PATH or set
[jsk_pcl_ros]   "octomap_server_DIR" to a directory containing one of the above files.  If
[jsk_pcl_ros]   "octomap_server" provides a separate development package or SDK, be sure it
[jsk_pcl_ros]   has been installed.
[jsk_pcl_ros] Call Stack (most recent call first):
[jsk_pcl_ros]   CMakeLists.txt:26 (find_package)
[jsk_pcl_ros]
[jsk_pcl_ros]
[jsk_pcl_ros] -- Configuring incomplete, errors occurred!
[jsk_pcl_ros] See also "/workspace/ros/ws_jsk_recognition/build/jsk_pcl_ros/CMakeFiles/CMakeOutput.log".
[jsk_pcl_ros] See also "/workspace/ros/ws_jsk_recognition/build/jsk_pcl_ros/CMakeFiles/CMakeError.log".
[jsk_pcl_ros] <== '/workspace/ros/ws_jsk_recognition/build/jsk_pcl_ros/build_env.sh /usr/bin/cmake /workspace/ros/ws_jsk_recognition/src/jsk_recognition/jsk_pcl_ros -DCATKIN_DEVEL_PREFIX=/workspace/ros/ws_jsk_recognition/devel -DCMAKE_INSTALL_PREFIX=/workspace/ros/ws_jsk_recognition/install' failed with return code '1'
Failed   <== jsk_pcl_ros              [ 4.9 seconds ]

[mmurooka]

@wkentaro に聞いて，.travis.rosinstall.jadeにoctomap_serverを加え，
ソースから入れるようにしました．

[k-okada]

octmap サーバをreleaseするようにissueを書いて，ここにリンクをはっておきましょう．

◉ Kei Okada

On Fri, Oct 30, 2015 at 5:47 PM, Masaki Murooka notifications@github.com
wrote:

@wkentaro https://github.com/wkentaro
に聞いて，.travis.rosinstall.jadeにoctomap_serverを加え，
ソースから入れるようにしました．

—
Reply to this email directly or view it on GitHub
#1276 (comment)
.

[mmurooka]

はい，頑張って英語でコメント書いておけばよかったと思いました．

[mmurooka]

Passed. @garaemon, If no problem, please merge.