Update footstep planner sample

[snozawa]

@YoheiKakiuchi さん footstep plannerをeusから呼ぶサンプルを更新して、テストなどを追加しました。 各コミットは以下です。

• optimistic_footstep_planner.launchでuse_obstacle_modelできるようにしました（デフォルトfalse == 挙動かわらない）。またrvizでobstacleてんぐんを表示するようにしました
• use_lazy_perceptionのパラメタは、USE_PERCEPTION引数を使うようにしました（デフォルトtrue == 挙動かわらない）
• footstep client sampleの関数を、utilsに移動させて、関数名をながくてわかりやすくしました
• （jskeusglのsample_possionのかわり）簡易ですが、引数のbodiesから点群をつくるようにしました（内包をチェックしてる）
• footstep_graph.cppで、use_obstacle_modelがtrueでかつ点群サイズが０のときにセグフォしていたので、点群サイズが0のときにはisCollideをぬけて干渉ナシにするようにしました。
• testを追加して、cmakeでよばれるようにしました。

[snozawa]

testを追加して、cmakeでよばれるようにしました。

testは追加しましたが、諸々確認中ですので https://github.com/jsk-ros-pkg/geneus/issues/51 一旦、まだcmakeでaddrostestしてtravis上で呼ばれるようにはしなくしました。