Skip to content

Commit

Permalink
add spin-once before image set-alist
Browse files Browse the repository at this point in the history
  • Loading branch information
@knorth55
knorth55 committed Oct 24, 2022
1 parent 3752a9b commit dd4249a
Showing 1 changed file with 30 additions and 0 deletions.
30 changes: 30 additions & 0 deletions jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand All @@ -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)))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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))
Expand All @@ -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)))))
Expand Down

0 comments on commit dd4249a

Please sign in to comment.