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..6f539aacc 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 @@
-
-
+
+
+ display_interactive_manipulator: true
+ display_interactive_manipulator_only_selected: true
+ display_description_only_selected: true
+
-
-
-
-
+
+
+
+ config_file: $(find jsk_2016_01_baxter_apc)/config/shelf_marker.yaml
+ config_auto_save: true
+
-
+
use_bin_info: false
@@ -14,7 +14,7 @@
-
+
use_bin_info: false
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 @@
-
+
index: 0
@@ -51,7 +51,7 @@
-
+
index: 2
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 @@
-
-
+
+
+ display_interactive_manipulator: true
+ display_interactive_manipulator_only_selected: true
+ display_description_only_selected: true
+
-
-
-
-
-
-
+
+
+
+ config_file: $(find jsk_2016_01_baxter_apc)/config/shelf_marker.yaml
+ config_auto_save: true
+