diff --git a/detect_cans_in_fridge_201202/apps/pick_object/pick_object.l b/detect_cans_in_fridge_201202/apps/pick_object/pick_object.l index 129328f994..91c8f76457 100755 --- a/detect_cans_in_fridge_201202/apps/pick_object/pick_object.l +++ b/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 "ごめんなさい。いまちょっと手が離せないです。") diff --git a/detect_cans_in_fridge_201202/euslisp/fridge-demo-smach-main.l b/detect_cans_in_fridge_201202/euslisp/fridge-demo-smach-main.l index 8600c05134..bf6e50a33b 100644 --- a/detect_cans_in_fridge_201202/euslisp/fridge-demo-smach-main.l +++ b/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,9 +78,9 @@ (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)) @@ -84,6 +88,19 @@ 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,6 +131,7 @@ (: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) @@ -119,7 +139,6 @@ (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))) diff --git a/detect_cans_in_fridge_201202/euslisp/main.l b/detect_cans_in_fridge_201202/euslisp/main.l index 4805e3c63a..9a42cb58ac 100644 --- a/detect_cans_in_fridge_201202/euslisp/main.l +++ b/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) )