[WIP] add 3d bbox recognition

[Kanazawanaoaki]

eusのプログラムで指定した範囲内の点群だけをクラスタリングするtabletopの3次元bbox認識の認識部分を試すプログラム部分だけ試しに書いてみました．
使う場合はプログラムの名前とか中身も自由に変更してお好きに使って下さい．
できれば日曜日に，kitchen-spotでおたまを決めた位置に置く→arrange-spotに移動→記憶している位置の周辺を指定して3dbbox認識→把持をする．
のようなテストプログラムを作ってみようかなと思います．

サンプル

rosbagを再生

rosbagの場所: https://drive.google.com/file/d/17js3jA5gwsm99I02SqxaoVlHfAXYnXRN/view?usp=sharing

roslaunch jsk_2021_10_soup_from_boil rosbag_play.launch rosbag:=/home/kanazawa/Downloads/20220311_kanazawa_kitchen_test/20220311_kanazawa_kitchen_test_06.bag

tabletop認識のlaunchを立ち上げる．

roslaunch jsk_2021_10_soup_from_boil tabletop_recognition_with_dynamic_attention_clipper.launch run_rviz:=true

範囲を指定するtopicをpublish.

roscd jsk_2021_10_soup_from_boil/euslisp/util/
roseus 3dbbox.l 
(test-cup1)

もしくはコマンドラインから

$ rostopic pub /dynamic_attention_clipper/input/box jsk_recognition_msgs/BoundingBox "header:
  seq: 0
  stamp: {secs: 0, nsecs: 0}
  frame_id: 'base_link'
pose:
  position: {x: 0.8, y: -0.07, z: 0.8}
  orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}
dimensions: {x: 0.2, y: 0.2, z: 0.2}
value: 0.0
label: 0"

[708yamaguchi]

めっちゃ良い感じだと思います！