From 8d25a85b45bd62288b8ceb0a8cd5060c446312cb Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Mon, 7 Aug 2017 23:18:34 +0900
Subject: [PATCH] use transformable_markers for kiva_pod

---
 .../config/shelf_marker.yaml                  | 73 +++++++++++++++++++
 .../euslisp/lib/baxter-interface.l            |  6 +-
 .../euslisp/lib/baxterrgv5-interface.l        |  6 +-
 jsk_2016_01_baxter_apc/launch/baxter.launch   | 22 ++++--
 .../launch/baxterrgv5.launch                  | 22 ++++--
 .../launch/in_bin_data_collection.launch      |  4 +-
 .../launch/setup_for_pick.launch              |  4 +-
 .../test/test_move_arm_to_bin.test            | 24 +++---
 8 files changed, 125 insertions(+), 36 deletions(-)
 create mode 100644 jsk_2016_01_baxter_apc/config/shelf_marker.yaml

diff --git a/jsk_2016_01_baxter_apc/config/shelf_marker.yaml b/jsk_2016_01_baxter_apc/config/shelf_marker.yaml
new file mode 100644
index 000000000..e9dacf402
--- /dev/null
+++ b/jsk_2016_01_baxter_apc/config/shelf_marker.yaml
@@ -0,0 +1,73 @@
+boxes:
+- dimensions: [0.37, 0.248, 0.218]
+  frame_id: base
+  name: bin_A
+  orientation: [-1.4221598218868449e-08, -9.368323958649108e-09, 0.012848515151612288,
+    0.9999174544223131]
+  position: [1.1806437969207764, 0.24124696850776672, 0.7747524976730347]
+- dimensions: [0.37, 0.306, 0.218]
+  frame_id: base
+  name: bin_B
+  orientation: [-8.619194072060711e-09, -2.7379195243035993e-08, 0.016088109136516454,
+    0.9998705779971779]
+  position: [1.1844762563705444, -0.04264336824417114, 0.7784229516983032]
+- dimensions: [0.37, 0.248, 0.218]
+  frame_id: base
+  name: bin_C
+  orientation: [1.2691212776250315e-08, -7.979128626810693e-09, 0.017129863019695716,
+    0.9998532731320762]
+  position: [1.181794285774231, -0.325874388217926, 0.7804300785064697]
+- dimensions: [0.37, 0.248, 0.218]
+  frame_id: base
+  name: bin_D
+  orientation: [-5.813541986857243e-06, -4.6613027489133287e-07, -0.003382295377040826,
+    0.9999942800056247]
+  position: [1.1884387731552124, 0.23623594641685486, 0.5623461604118347]
+- dimensions: [0.37, 0.306, 0.218]
+  frame_id: base
+  name: bin_E
+  orientation: [1.4296312688299295e-08, -1.404809523979114e-08, 0.019587814958728158,
+    0.999808140347508]
+  position: [1.1842076778411865, -0.041138894855976105, 0.5622233748435974]
+- dimensions: [0.37, 0.248, 0.218]
+  frame_id: base
+  name: bin_F
+  orientation: [-4.1806768326273164e-08, -1.4015075490417066e-08, 0.0141673713757229,
+    0.9998996377578604]
+  position: [1.1698460578918457, -0.32393547892570496, 0.5679965019226074]
+- dimensions: [0.37, 0.248, 0.218]
+  frame_id: base
+  name: bin_G
+  orientation: [-1.1006784172057532e-08, -2.4893073042505106e-08, -0.0016057364764762638,
+    0.9999987108043527]
+  position: [1.185474157333374, 0.23752033710479736, 0.3308442234992981]
+- dimensions: [0.37, 0.306, 0.218]
+  frame_id: base
+  name: bin_H
+  orientation: [1.768792709155667e-08, 6.808888381034825e-09, 0.012517532928541178,
+    0.9999216526155349]
+  position: [1.1667330265045166, -0.03685244545340538, 0.3453666567802429]
+- dimensions: [0.37, 0.248, 0.218]
+  frame_id: base
+  name: bin_I
+  orientation: [1.1113435380182779e-08, -5.808917725885156e-09, 0.0237319839311394,
+    0.9997183568079021]
+  position: [1.1659990549087524, -0.32885119318962097, 0.3448035717010498]
+- dimensions: [0.37, 0.248, 0.218]
+  frame_id: base
+  name: bin_J
+  orientation: [9.11331210457442e-09, 6.397741603338668e-09, 0.007465957845546083,
+    0.9999721293483376]
+  position: [1.1769664287567139, 0.24687521159648895, 0.10446316003799438]
+- dimensions: [0.37, 0.306, 0.218]
+  frame_id: base
+  name: bin_K
+  orientation: [-6.834654577141459e-09, 4.774725473898049e-09, 0.010019194271864634,
+    0.9999498066133833]
+  position: [1.16932213306427, -0.0358806736767292, 0.10993516445159912]
+- dimensions: [0.37, 0.248, 0.218]
+  frame_id: base
+  name: bin_L
+  orientation: [-5.9481323670782345e-09, 3.94564347244691e-09, 0.035749935563802786,
+    0.9993607667440142]
+  position: [1.1908793449401855, -0.3116720914840698, 0.11142793297767639]
diff --git a/jsk_2016_01_baxter_apc/euslisp/lib/baxter-interface.l b/jsk_2016_01_baxter_apc/euslisp/lib/baxter-interface.l
index e9895c32a..bd394e1c1 100644
--- a/jsk_2016_01_baxter_apc/euslisp/lib/baxter-interface.l
+++ b/jsk_2016_01_baxter_apc/euslisp/lib/baxter-interface.l
@@ -324,8 +324,8 @@
       (position box-size box-sizes-bak)))
   (:recognize-bin-boxes
     (&key (stamp (ros::time-now)))
-    (let ((box-topic (format nil "publish_bin_boxes/output"))
-          box-msg bin-list)
+    (let ((box-topic (format nil "transformable_bin_markers/output/boxes"))
+          box-msg box-list bin-list)
       (setq box-msg (one-shot-subscribe box-topic
                                         jsk_recognition_msgs::BoundingBoxArray
                                         :timeout 10000
@@ -336,7 +336,7 @@
           (setq box-list (send box-msg :boxes))
           (setq bin-list (list :a :b :c :d :e :f :g :h :i :j :k :l))
           (dolist (bin bin-list)
-            (setf (gethash bin bin-boxes-) (car box-list))
+            (sethash bin bin-boxes- (car box-list))
             (setq box-list (cdr box-list))))
         (ros::ros-fatal "[:recognize-bin-boxes] cannot recognize bin boxes"))))
   (:recognize-order-bin-box
diff --git a/jsk_2016_01_baxter_apc/euslisp/lib/baxterrgv5-interface.l b/jsk_2016_01_baxter_apc/euslisp/lib/baxterrgv5-interface.l
index dfa97d8d5..1cc7868a0 100644
--- a/jsk_2016_01_baxter_apc/euslisp/lib/baxterrgv5-interface.l
+++ b/jsk_2016_01_baxter_apc/euslisp/lib/baxterrgv5-interface.l
@@ -381,8 +381,8 @@
       (position box-size box-sizes-bak)))
   (:recognize-bin-boxes
     (&key (stamp (ros::time-now)))
-    (let ((box-topic (format nil "publish_bin_boxes/output"))
-          box-msg bin-list)
+    (let ((box-topic (format nil "transformable_bin_markers/output/boxes"))
+          box-msg box-list bin-list)
       (setq box-msg (one-shot-subscribe box-topic
                                         jsk_recognition_msgs::BoundingBoxArray
                                         :timeout 10000
@@ -393,7 +393,7 @@
           (setq box-list (send box-msg :boxes))
           (setq bin-list (list :a :b :c :d :e :f :g :h :i :j :k :l))
           (dolist (bin bin-list)
-            (setf (gethash bin _bin-boxes) (car box-list))
+            (sethash bin _bin-boxes (car box-list))
             (setq box-list (cdr box-list))))
         (ros::ros-fatal "[:recognize-bin-boxes] cannot recognize bin boxes"))))
   (:recognize-order-bin-box
diff --git a/jsk_2016_01_baxter_apc/launch/baxter.launch b/jsk_2016_01_baxter_apc/launch/baxter.launch
index 82500ca9f..4a587c59a 100644
--- a/jsk_2016_01_baxter_apc/launch/baxter.launch
+++ b/jsk_2016_01_baxter_apc/launch/baxter.launch
@@ -47,15 +47,21 @@
 
 
   <include file="$(find jsk_2015_05_baxter_apc)/launch/include/kiva_pod_state.launch" />
-  <node name="interactive_adjust_kiva_pod"
-        pkg="jsk_interactive_marker" type="marker_6dof">
-    <rosparam command="load" file="$(find jsk_2015_05_baxter_apc)/config/kiva_pod_interactive_marker.yaml" />
+  <node name="transformable_interactive_server"
+        pkg="jsk_interactive_marker" type="transformable_server_sample">
+    <rosparam subst_value="true">
+      display_interactive_manipulator: true
+      display_interactive_manipulator_only_selected: true
+      display_description_only_selected: true
+    </rosparam>
   </node>
-  <node name="publish_bin_boxes"
-        pkg="jsk_apc2016_common" type="publish_bin_bbox.py">
-    <remap from="~boxes" to="~output" />
-    <rosparam file="$(find jsk_apc2016_common)/config/bin_upper_shelf.yaml" command="load" ns="upper_shelf"/>
-    <rosparam file="$(find jsk_apc2016_common)/config/bin_lower_shelf.yaml" command="load" ns="lower_shelf"/>
+  <node name="transformable_bin_markers"
+        pkg="jsk_interactive_marker" type="transformable_markers_client.py">
+    <remap from="~server" to="transformable_interactive_server" />
+    <rosparam subst_value="true">
+      config_file: $(find jsk_2016_01_baxter_apc)/config/shelf_marker.yaml
+      config_auto_save: true  <!-- Use true to save config updated on rviz -->
+    </rosparam>
   </node>
 
   <node name="desktop_bg_publisher"
diff --git a/jsk_2016_01_baxter_apc/launch/baxterrgv5.launch b/jsk_2016_01_baxter_apc/launch/baxterrgv5.launch
index 8c0d00a2d..961e7b91d 100644
--- a/jsk_2016_01_baxter_apc/launch/baxterrgv5.launch
+++ b/jsk_2016_01_baxter_apc/launch/baxterrgv5.launch
@@ -54,15 +54,21 @@
 
 
   <include file="$(find jsk_2015_05_baxter_apc)/launch/include/kiva_pod_state.launch" />
-  <node name="interactive_adjust_kiva_pod"
-        pkg="jsk_interactive_marker" type="marker_6dof">
-    <rosparam command="load" file="$(find jsk_2015_05_baxter_apc)/config/kiva_pod_interactive_marker.yaml" />
+  <node name="transformable_interactive_server"
+        pkg="jsk_interactive_marker" type="transformable_server_sample">
+    <rosparam subst_value="true">
+      display_interactive_manipulator: true
+      display_interactive_manipulator_only_selected: true
+      display_description_only_selected: true
+    </rosparam>
   </node>
-  <node name="publish_bin_boxes"
-        pkg="jsk_apc2016_common" type="publish_bin_bbox.py">
-    <remap from="~boxes" to="~output" />
-    <rosparam file="$(find jsk_apc2016_common)/config/bin_upper_shelf.yaml" command="load" ns="upper_shelf"/>
-    <rosparam file="$(find jsk_apc2016_common)/config/bin_lower_shelf.yaml" command="load" ns="lower_shelf"/>
+  <node name="transformable_bin_markers"
+        pkg="jsk_interactive_marker" type="transformable_markers_client.py">
+    <remap from="~server" to="transformable_interactive_server" />
+    <rosparam subst_value="true">
+      config_file: $(find jsk_2016_01_baxter_apc)/config/shelf_marker.yaml
+      config_auto_save: true  <!-- Use true to save config updated on rviz -->
+    </rosparam>
   </node>
 
   <node name="desktop_bg_publisher"
diff --git a/jsk_2016_01_baxter_apc/launch/in_bin_data_collection.launch b/jsk_2016_01_baxter_apc/launch/in_bin_data_collection.launch
index af6b5a2e4..5df607d3d 100644
--- a/jsk_2016_01_baxter_apc/launch/in_bin_data_collection.launch
+++ b/jsk_2016_01_baxter_apc/launch/in_bin_data_collection.launch
@@ -6,7 +6,7 @@
   <node name="tf_bbox_to_mask_left_hand"
         pkg="jsk_apc2016_common" type="tf_bbox_to_mask.py">
     <remap from="~input" to="/left_hand_camera/rgb/camera_info"/>
-    <remap from="~input/boxes" to="publish_bin_boxes/output"/>
+    <remap from="~input/boxes" to="/transformable_bin_markers/output/boxes" />
     <rosparam>
       use_bin_info: false
     </rosparam>
@@ -14,7 +14,7 @@
   <node name="tf_bbox_to_mask_right_hand"
         pkg="jsk_apc2016_common" type="tf_bbox_to_mask.py">
     <remap from="~input" to="/right_hand_camera/rgb/camera_info"/>
-    <remap from="~input/boxes" to="publish_bin_bbox/boxes"/>
+    <remap from="~input/boxes" to="/transformable_bin_markers/output/boxes" />
     <rosparam>
       use_bin_info: false
     </rosparam>
diff --git a/jsk_2016_01_baxter_apc/launch/setup_for_pick.launch b/jsk_2016_01_baxter_apc/launch/setup_for_pick.launch
index 379d9c1f9..c9a414001 100644
--- a/jsk_2016_01_baxter_apc/launch/setup_for_pick.launch
+++ b/jsk_2016_01_baxter_apc/launch/setup_for_pick.launch
@@ -7,7 +7,7 @@
     <node name="bbox_array_to_bbox"
           pkg="nodelet" type="nodelet"
           args="load jsk_pcl_utils/BoundingBoxArrayToBoundingBox $(arg manager_left)">
-      <remap from="~input" to="/publish_bin_boxes/output" />
+      <remap from="~input" to="/transformable_bin_markers/output/boxes" />
       <rosparam>
         index: 0
       </rosparam>
@@ -51,7 +51,7 @@
     <node name="bbox_array_to_bbox"
           pkg="nodelet" type="nodelet"
           args="load jsk_pcl_utils/BoundingBoxArrayToBoundingBox $(arg manager_right)">
-      <remap from="~input" to="/publish_bin_boxes/output" />
+      <remap from="~input" to="/transformable_bin_markers/output/boxes" />
       <rosparam>
         index: 2
       </rosparam>
diff --git a/jsk_2016_01_baxter_apc/test/test_move_arm_to_bin.test b/jsk_2016_01_baxter_apc/test/test_move_arm_to_bin.test
index 653a05a44..7f1dc6cc5 100644
--- a/jsk_2016_01_baxter_apc/test/test_move_arm_to_bin.test
+++ b/jsk_2016_01_baxter_apc/test/test_move_arm_to_bin.test
@@ -4,17 +4,21 @@
 
   <!-- publish /tf of shelf -->
   <include file="$(find jsk_2015_05_baxter_apc)/launch/include/kiva_pod_state.launch" />
-  <node name="interactive_adjust_kiva_pod"
-        pkg="jsk_interactive_marker" type="marker_6dof">
-    <rosparam command="load" file="$(find jsk_2015_05_baxter_apc)/config/kiva_pod_interactive_marker.yaml" />
+  <node name="transformable_interactive_server"
+        pkg="jsk_interactive_marker" type="transformable_server_sample">
+    <rosparam subst_value="true">
+      display_interactive_manipulator: true
+      display_interactive_manipulator_only_selected: true
+      display_description_only_selected: true
+    </rosparam>
   </node>
-
-  <!-- publish shelf bin boxes -->
-  <node name="publish_bin_boxes"
-        pkg="jsk_apc2016_common" type="publish_bin_bbox.py">
-    <remap from="~boxes" to="~output" />
-    <rosparam command="load" file="$(find jsk_apc2016_common)/config/bin_upper_shelf.yaml" ns="upper_shelf" />
-    <rosparam command="load" file="$(find jsk_apc2016_common)/config/bin_lower_shelf.yaml" ns="lower_shelf" />
+  <node name="transformable_bin_markers"
+        pkg="jsk_interactive_marker" type="transformable_markers_client.py">
+    <remap from="~server" to="transformable_interactive_server" />
+    <rosparam subst_value="true">
+      config_file: $(find jsk_2016_01_baxter_apc)/config/shelf_marker.yaml
+      config_auto_save: true  <!-- Use true to save config updated on rviz -->
+    </rosparam>
   </node>
 
   <test test-name="test_move_arm_to_bin"