separate hand-over from move-to

detect_cans_in_fridge_201202/apps/pick_object/pick_object.l

@@ -15,7 +15,6 @@
       ((find object (list "wonda" "georgia" "boss" "iemon") :test #'string=)
        (send *ri* :clear-costmap)
        (fridge-demo-smach-main :can-type object
-                               :finish-type :pass-to-human
                                :use-arm :larm
                                :use-base nil))
       (t (speak-jp "ごめんなさい。いまちょっと手が離せないです。")

detect_cans_in_fridge_201202/euslisp/fridge-demo-smach-main.l

@@ -33,6 +33,7 @@
 
 (def-state-func move-to-fridge-func (userdata)
   (let ((res (goto-front-of-fridge)))
+    (ros::spin-once)
     (if res
       (set-alist 'description "冷蔵庫の前に移動したよ" userdata)
       (set-alist 'description "冷蔵庫へ行けなかったよ" userdata))
@@ -44,6 +45,7 @@
   (let* ((use-arm (cdr (assoc 'use-arm userdata)))
          (use-base (cdr (assoc 'use-base userdata)))
          (res (open-fridge-door :use-arm use-arm :use-base use-base)))
+    (ros::spin-once)
     (if res
       (set-alist 'description "冷蔵庫の扉を開けたよ" userdata)
       (set-alist 'description "冷蔵庫の扉を開けられなかったよ" userdata))
@@ -55,6 +57,7 @@
   (let* ((can-type (cdr (assoc 'can-type userdata)))
          (use-arm (cdr (assoc 'use-arm userdata)))
          (res (grasp-can :use-arm use-arm)))
+    (ros::spin-once)
     (if res
       (set-alist 'description (format nil "~Aを取り出せたよ" can-type) userdata)
       (set-alist 'description (format nil "~Aを取り出せなかったよ" can-type) userdata))
@@ -65,6 +68,7 @@
 (def-state-func close-fridge-func (userdata)
   (let* ((use-arm (cdr (assoc 'use-arm userdata)))
          (res (close-fridge :use-arm use-arm)))
+    (ros::spin-once)
     (if res
       (set-alist 'description "冷蔵庫の扉を閉めたよ" userdata)
       (set-alist 'description "冷蔵庫の扉を閉められなかったよ" userdata))
@@ -74,16 +78,29 @@
 
 (def-state-func move-to-initial-position-func (userdata)
   (let* ((can-type (cdr (assoc 'can-type userdata)))
-         (finish-type (cdr (assoc 'finish-type userdata)))
          (use-arm (cdr (assoc 'use-arm userdata)))
-         (res (goto-initial-position :finish-type finish-type :use-arm use-arm)))
+         (res (goto-initial-position :use-arm use-arm)))
+    (ros::spin-once)
     (if res
       (set-alist 'description (format nil "~Aを持っていったよ" can-type) userdata)
       (set-alist 'description (format nil "~Aを持っていけなかったよ" can-type) userdata))
     (if *image* (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata))
     res))
 
 
+(def-state-func hand-over-can-func (userdata)
+  (let* ((can-type (cdr (assoc 'can-type userdata)))
+         (use-arm (cdr (assoc 'use-arm userdata)))
+         (res (pass-to-human :use-arm use-arm)))
+    ;; we dont need this here for better photo 
+    ;; (ros::spin-once)
+    (if res
+      (set-alist 'description (format nil "~Aを渡したよ" can-type) userdata)
+      (set-alist 'description (format nil "~Aを渡せなかったよ" can-type) userdata))
+    (if *image* (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata))
+    res))
+
+
 (def-state-func finish-func (userdata) t)
 
 
@@ -102,8 +119,10 @@
             (:grasp-can !-> :finish-failure)
             (:close-fridge -> :move-to-initial-position)
             (:close-fridge !-> :finish-failure)
-            (:move-to-initial-position -> :finish)
+            (:move-to-initial-position -> :hand-over-can)
             (:move-to-initial-position !-> :finish-failure)
+            (:hand-over-can -> :finish)
+            (:hand-over-can !-> :finish-failure)
             (:finish -> t)
             (:finish-failure -> nil))
           '((:init 'init-func)
@@ -112,14 +131,14 @@
             (:grasp-can 'grasp-can-func)
             (:close-fridge 'close-fridge-func)
             (:move-to-initial-position 'move-to-initial-position-func)
+            (:hand-over-can 'hand-over-can-func)
             (:finish 'finish-func)
             (:finish-failure 'finish-failure-func))
           '(:init)
           '(t nil))))
 
 
 (defun fridge-demo-smach-main (&key (can-type "georgia")
-                                    (finish-type :pass-to-human)
                                     (use-arm :larm)
                                     (use-base nil))
   (when (not (boundp '*sm*))
@@ -129,7 +148,6 @@
             *sm* `((description . ,(format nil "冷蔵庫に~Aを取りに行ったよ" can-type))
                    (image . "")
                    (can-type . ,can-type)
-                   (finish-type . ,finish-type)
                    (use-arm . ,use-arm)
                    (use-base . ,use-base))
             :hz 2.0)))

detect_cans_in_fridge_201202/euslisp/main.l

@@ -66,7 +66,7 @@
 
 (defun goto-initial-position (&key (return-to-initial-position t)
                                    (use-arm :rarm)
-                                   (turnp t) (finish-type :pass-to-human))
+                                   (turnp t))
   (when turnp
     (send *ri* :go-pos-unsafe 0 0 -90)
     (send *ri* :wait-interpolation))
@@ -78,22 +78,17 @@
           (speak-jp (format nil "I will bring  ~a." *type*)))
          (t (speak-jp (format nil "~aを持って行きます。" *type*))))
       (speak-jp (format nil "~a を もって いきます" *type*)))
-    (send *ri* :move-to *opose*))
+    (send *ri* :move-to *opose*)))
 
-  (case finish-type
-    (:put-on-turtlebot
-     (put-can-on-turtlebot))
-    ((:pass-to-human t)
-     (if (and (boundp '*use-voicetext*) *use-voicetext*)
-         (cond
-          ((and (boundp '*use-english*) *use-english*)
-           (speak-jp (format nil "Please take ~a." *type*)))
-          (t (speak-jp (format nil "~aをどうぞ。" *type*))))
-       (speak-jp (format nil "~a を どうぞ" *type*)))
-     (hand-over use-arm :wait-shock t :tuckarm :rarm))
-    )
-  t
-  )
+(defun pass-to-human (&key (use-arm :rarm))
+  (if (and (boundp '*use-voicetext*) *use-voicetext*)
+      (cond
+       ((and (boundp '*use-english*) *use-english*)
+        (speak-jp (format nil "Please take ~a." *type*)))
+       (t (speak-jp (format nil "~aをどうぞ。" *type*))))
+    (speak-jp (format nil "~a を どうぞ" *type*)))
+  (hand-over use-arm :wait-shock t :tuckarm :rarm)
+  t)
 
 (defun enable-behavior-server ()
   (if (ros::wait-for-service "/interactive_behavior_enable" 1)
@@ -168,15 +163,18 @@
 
   (case demo-type
     (:map
-     (goto-initial-position :finish-type finish-type :use-arm use-arm)
-     )
+     (unless (goto-initial-position :use-arm use-arm)
+       (return-from demo)))
     (t
      (unless (goto-initial-position
               :return-to-initial-position nil :turnp t
-              :use-arm use-arm
-              :finish-type finish-type)
-       (return-from demo)))
-    )
+              :use-arm use-arm)
+       (return-from demo))))
+
+  (case finish-type
+    (:pass-to-human (pass-to-human :use-arm use-arm))
+    (:put-on-turtlebot (put-can-on-turtlebot)))
+
   ; (enable-behavior-server)
   (if (eq app-manager :true) (ros::exit) t)
   )