From 20a74f864ca6a04d026425dab67e6cabaa8de668 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Wed, 21 Jun 2017 11:33:09 +0900
Subject: [PATCH] Add check-grasp-style state in pick task

---
 jsk_arc2017_baxter/euslisp/lib/pick-interface.l | 3 ++-
 jsk_arc2017_baxter/euslisp/pick-main.l          | 8 +++++++-
 jsk_arc2017_baxter/node_scripts/state_server.py | 5 +++--
 3 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
index 84e0434f2..742c4cd46 100644
--- a/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
+++ b/jsk_arc2017_baxter/euslisp/lib/pick-interface.l
@@ -47,8 +47,9 @@
       (ros::ros-info "[:wait-for-user-input] received user input: ~a" arm)
       (setq order nil)
       can-start))
+  (:check-grasp-style (arm)
+    (if (eq grasp-style :suction) nil t))
   (:set-target (arm)
-    (if (oddp picking-fail-count) (return-from :set-target t))
     (setq label-names
           (ros::get-param (format nil "/~a_hand_camera/label_names"
                                   (arm2str arm))))
diff --git a/jsk_arc2017_baxter/euslisp/pick-main.l b/jsk_arc2017_baxter/euslisp/pick-main.l
index b424a4ce9..ac0be78bc 100755
--- a/jsk_arc2017_baxter/euslisp/pick-main.l
+++ b/jsk_arc2017_baxter/euslisp/pick-main.l
@@ -15,8 +15,10 @@
             (:recognize-bin-boxes -> :wait-for-user-input)
             (:wait-for-user-input -> :wait-for-opposite-arm)
             (:wait-for-user-input !-> :finish)
-            (:wait-for-opposite-arm -> :set-target)
+            (:wait-for-opposite-arm -> :check-grasp-style)
             (:wait-for-opposite-arm !-> :wait-for-opposite-arm)
+            (:check-grasp-style -> :set-target)
+            (:check-grasp-style !-> :recognize-object)
             (:set-target -> :recognize-object)
             (:set-target !-> :wait-for-user-input)
             (:recognize-object -> :set-grasp-style)
@@ -56,6 +58,10 @@
                  (send *ti* :update-state *arm* :wait-for-opposite-arm)
                  (send *ti* :check-can-start *arm* :set-target
                        :wait-for-opposite-arm)))
+            (:check-grasp-style
+              '(lambda (userdata)
+                 (send *ti* :update-state *arm* :check-grasp-style)
+                 (send *ti* :check-grasp-style *arm*)))
             (:set-target
               '(lambda (userdata)
                  (send *ti* :update-state *arm* :set-target)
diff --git a/jsk_arc2017_baxter/node_scripts/state_server.py b/jsk_arc2017_baxter/node_scripts/state_server.py
index 71739d07c..50323be6b 100755
--- a/jsk_arc2017_baxter/node_scripts/state_server.py
+++ b/jsk_arc2017_baxter/node_scripts/state_server.py
@@ -86,13 +86,14 @@ def _get_can_start(self, state, opposite_state):
         if self.is_pick:
             # pick task: wait condition
             if state == 'wait-for-opposite-arm-start-picking':
-                if opposite_state == 'set-target' \
+                if opposite_state == 'check-grasp-style' \
                         or opposite_state == 'wait-for-user-input':
                     can_start = True
                 else:
                     can_start = False
             elif state == 'wait-for-opposite-arm':
-                if opposite_state == 'set-target' \
+                if opposite_state == 'check-grasp-style' \
+                        or opposite_state == 'set-target' \
                         or opposite_state == 'recognize-object' \
                         or opposite_state == 'set-grasp-style' \
                         or opposite_state == 'pick-object' \