add spin-once before image set-alist

Author: knorth55

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)))))