diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l index 7134603ce2..f28e0bb526 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -760,6 +760,7 @@ Args: '(lambda (userdata) (let ((light-on (get-light-on))) (setf (cdr (assoc 'initial-light-on userdata)) light-on) + (ros::spin-once) (if light-on (set-alist 'description "電気がついていたよ" userdata) (set-alist 'description "電気がついていなかったよ" userdata)) @@ -774,6 +775,7 @@ Args: (let ((control-switchbot (cdr (assoc 'control-switchbot userdata)))) (report-light-off) (room-light-on :control-switchbot control-switchbot) + (ros::spin-once) (set-alist 'description "電気をつけたよ" userdata) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t))) @@ -782,6 +784,7 @@ Args: (let* ((n-trial (cdr (assoc 'n-dock-front-trial userdata))) (success (move-to-dock-front :n-trial n-trial))) (setf (cdr (assoc 'success-go-to-dock-front userdata)) success) + (ros::spin-once) (if success (set-alist 'description "ドックの前に移動したよ" userdata) (set-alist 'description "ドックの前に移動しようとしたけど,迷子になっちゃった" userdata)) @@ -790,12 +793,14 @@ Args: (:inspect-dock-front '(lambda (userdata) (inspect-dock-front :tweet (cdr (assoc 'tweet userdata))) + (ros::spin-once) (set-alist 'description "ドックの前の様子を見たよ" userdata) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t)) (:report-move-to-dock-front-failure '(lambda (userdata) (report-move-to-dock-front-failure) + (ros::spin-once) (set-alist 'description "ドックの前に移動できなかったよ" userdata) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t)) @@ -804,6 +809,7 @@ Args: (let* ((n-trial (cdr (assoc 'n-tv-front-trial userdata))) (success (move-to-tv-front :n-trial n-trial))) (setf (cdr (assoc 'success-go-to-tv-front userdata)) success) + (ros::spin-once) (if success (set-alist 'description "テレビの前に移動したよ" userdata) (set-alist 'description "テレビの前に移動しようとしたけど,迷子になっちゃった" userdata)) @@ -812,12 +818,14 @@ Args: (:inspect-tv-front '(lambda (userdata) (inspect-tv-front :tweet (cdr (assoc 'tweet userdata))) + (ros::spin-once) (set-alist 'description "テレビの前の様子を見たよ" userdata) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t)) (:report-move-to-tv-front-failure '(lambda (userdata) (report-move-to-tv-front-failure) + (ros::spin-once) (set-alist 'description "テレビの前に移動できなかったよ" userdata) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t)) @@ -826,6 +834,7 @@ Args: (let* ((n-trial (cdr (assoc 'n-tv-desk-trial userdata))) (success (move-to-tv-desk :n-trial n-trial))) (setf (cdr (assoc 'success-go-to-tv-desk userdata)) success) + (ros::spin-once) (if success (set-alist 'description "机の前に移動したよ" userdata) (set-alist 'description "机の前に移動しようとしたけど,迷子になっちゃった" userdata)) @@ -834,12 +843,14 @@ Args: (:inspect-tv-desk '(lambda (userdata) (inspect-tv-desk :tweet (cdr (assoc 'tweet userdata))) + (ros::spin-once) (set-alist 'description "机の様子を確認したよ" userdata) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t)) (:report-move-to-tv-desk-failure '(lambda (userdata) (report-move-to-tv-desk-failure) + (ros::spin-once) (set-alist 'description "机の前に移動できなかったよ" userdata) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t)) @@ -848,6 +859,7 @@ Args: (let* ((n-trial (cdr (assoc 'n-desk-back-trial userdata))) (success (move-to-desk-back :n-trial n-trial))) (setf (cdr (assoc 'success-go-to-desk-back userdata)) success) + (ros::spin-once) (if success (set-alist 'description "部屋の後ろに移動したよ" userdata) (set-alist 'description "部屋の後ろに移動しようとしたけど,迷子になっちゃった" userdata)) @@ -856,12 +868,14 @@ Args: (:inspect-desk-back '(lambda (userdata) (inspect-desk-back :tweet (cdr (assoc 'tweet userdata))) + (ros::spin-once) (set-alist 'description "部屋の後ろを確認したよ" userdata) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t)) (:report-move-to-desk-back-failure '(lambda (userdata) (report-move-to-desk-back-failure) + (ros::spin-once) (set-alist 'description "部屋の後ろに移動できなかったよ" userdata) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t)) @@ -870,6 +884,7 @@ Args: (let* ((n-trial (cdr (assoc 'n-desk-front-trial userdata))) (success (move-to-desk-front :n-trial n-trial))) (setf (cdr (assoc 'success-go-to-desk-front userdata)) success) + (ros::spin-once) (if success (set-alist 'description "部屋の前に移動したよ" userdata) (set-alist 'description "部屋の前に移動しようとしたけど,迷子になっちゃった" userdata)) @@ -878,12 +893,14 @@ Args: (:inspect-desk-front '(lambda (userdata) (inspect-desk-front :tweet (cdr (assoc 'tweet userdata))) + (ros::spin-once) (set-alist 'description "部屋の前を確認したよ" userdata) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t)) (:report-move-to-desk-front-failure '(lambda (userdata) (report-move-to-desk-front-failure) + (ros::spin-once) (set-alist 'description "部屋の前に移動できなかったよ" userdata) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t)) @@ -892,6 +909,7 @@ Args: (let* ((n-trial (cdr (assoc 'n-kitchen-door-front-trial userdata))) (success (move-to-kitchen-door-front :n-trial n-trial))) (setf (cdr (assoc 'success-go-to-tv-desk userdata)) success) + (ros::spin-once) (if success (set-alist 'description "ドアの前に移動したよ" userdata) (set-alist 'description "ドアの前に移動しようとしたけど,迷子になっちゃった" userdata)) @@ -900,12 +918,14 @@ Args: (:inspect-kitchen-door-front '(lambda (userdata) (inspect-kitchen-door-front :tweet (cdr (assoc 'tweet userdata))) + (ros::spin-once) (set-alist 'description "ドアの前からキッチンの様子を見たよ" userdata) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t)) (:report-move-to-kitchen-door-front-failure '(lambda (userdata) (report-move-to-kitchen-door-front-failure) + (ros::spin-once) (set-alist 'description "ドアの前に移動できなかったよ" userdata) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t)) @@ -914,6 +934,7 @@ Args: (let* ((n-trial (cdr (assoc 'n-kitchen-trial userdata))) (success (move-to-sink-front :n-trial n-trial))) (setf (cdr (assoc 'success-go-to-kitchen userdata)) success) + (ros::spin-once) (if success (set-alist 'description "キッチンに移動したよ" userdata) (set-alist 'description "キッチンに移動しようとしたけど,迷子になっちゃった" userdata)) @@ -924,12 +945,14 @@ Args: (let* ((label-names (notify-recognition :location "kitchen")) (notify-text (if label-names (format nil "~Aがあったよ" label-names) ""))) (inspect-kitchen :tweet (cdr (assoc 'tweet userdata))) + (ros::spin-once) (set-alist 'description (format nil "キッチンの様子を見たよ。~A" notify-text) userdata)) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t)) (:report-move-to-sink-front-failure '(lambda (userdata) (report-move-to-sink-front-failure) + (ros::spin-once) (set-alist 'description "キッチンに行けなかったよ" userdata) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t)) @@ -938,6 +961,7 @@ Args: (let* ((n-trial (cdr (assoc 'n-trashcan-trial userdata))) (success (move-to-trashcan-front :n-trial n-trial))) (setf (cdr (assoc 'success-go-to-trashcan userdata)) success) + (ros::spin-once) (if success (set-alist 'description "ゴミ箱の前に移動したよ" userdata) (set-alist 'description "ゴミ箱の前に移動しようとしたけど,迷子になっちゃった" userdata)) @@ -946,12 +970,14 @@ Args: (:inspect-trashcan '(lambda (userdata) (inspect-trashcan :tweet (cdr (assoc 'tweet userdata))) + (ros::spin-once) (set-alist 'description "ゴミ箱の様子を確認したよ" userdata) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t)) (:report-move-to-trashcan-front-failure '(lambda (userdata) (report-move-to-trashcan-front-failure) + (ros::spin-once) (set-alist 'description "ゴミ箱の前に行けなかったよ" userdata) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t)) @@ -961,6 +987,7 @@ Args: (let* ((n-trial (cdr (assoc 'n-dock-trial userdata))) (success (auto-dock :n-trial n-trial :clear-costmap nil))) (setf (cdr (assoc 'success-auto-dock userdata)) success) + (ros::spin-once) (if success (set-alist 'description "帰ってきたよ" userdata) (set-alist 'description "帰ってこようとしたけど,迷子になっちゃった" userdata)) @@ -969,6 +996,7 @@ Args: (:auto-dock-failure '(lambda (userdata) (report-auto-dock-failure) + (ros::spin-once) (set-alist 'description "帰ってこられなかったよ" userdata) (set-alist 'image (remove #\newline (base64encode (send *image* :serialize))) userdata) t)) @@ -980,6 +1008,7 @@ Args: (if (and success-auto-dock (not initial-light-on)) (progn (room-light-off :control-switchbot control-switchbot) + (ros::spin-once) (set-alist 'description "電気を消したよ" userdata) (set-alist 'image "" userdata)))) t)) @@ -992,6 +1021,7 @@ Args: (success-go-to-kitchen (cdr (assoc 'success-go-to-kitchen userdata)))) (restore-params) + (ros::spin-once) (set-alist 'description "キッチンデモを終えるよ" userdata) (set-alist 'image "" userdata) (and success-go-to-kitchen success-auto-dock success-battery-charging)))))