From 1c2afbe249fec144f144b15ab6fa1892e7f7590a Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 17 Oct 2019 22:33:46 +0900 Subject: [PATCH 01/54] add fetch-init --- jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l | 3 +++ 1 file changed, 3 insertions(+) diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l index c55912599f..5a06f835c6 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l @@ -3,6 +3,9 @@ (defun go-to-kitchen () ;; go to kitchen + (unless (boundp '*ri*) + (require :fetch-interface "package://fetcheus/fetch-interface.l") + (fetch-init)) (send *ri* :speak "go to kitchen.") (if (go-to-spot "/eng2/7f/room73B2-sink-front1" (make-coords :pos #f(100 -1000 0))) (progn ;; succeeded to go to kitchen From 5087f7545e9c2c368c89b7474b49da4c48062933 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 17 Oct 2019 23:13:52 +0900 Subject: [PATCH 02/54] refactor navigation-utils.l --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 cea8dfa210..ba5994c987 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -94,8 +94,7 @@ (ros::ros-warn "Skip undocking, so please make sure that Fetch is already undocked.") (return)) (setq *is-charging* (send msg :is_charging)) - (if *is-charging* - (progn (undock))) + (when *is-charging* (undock)) (unless *is-charging* (return)) (if (eq i 2) (progn (send *ri* :speak "Fail to undock") (ros::ros-error "Fail to undock"))))) ;; go to spot From a7ca736fc5ea0e75486120dab8a15b8a0297271d Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 17 Oct 2019 23:18:01 +0900 Subject: [PATCH 03/54] move go-to-kitchen in navigation-utils.l --- .../jsk_fetch_startup/euslisp/go-to-kitchen.l | 24 ++++--------------- .../euslisp/navigation-utils.l | 22 +++++++++++++++-- 2 files changed, 24 insertions(+), 22 deletions(-) diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l index 5a06f835c6..8db7d9e7ff 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l @@ -1,22 +1,6 @@ -(ros::roseus "go_to_kitchen") -(load "package://jsk_fetch_startup/euslisp/navigation-utils.l") - -(defun go-to-kitchen () - ;; go to kitchen - (unless (boundp '*ri*) - (require :fetch-interface "package://fetcheus/fetch-interface.l") - (fetch-init)) - (send *ri* :speak "go to kitchen.") - (if (go-to-spot "/eng2/7f/room73B2-sink-front1" (make-coords :pos #f(100 -1000 0))) - (progn ;; succeeded to go to kitchen - (unix:sleep 1) - (send *ri* :speak "arrived at kitchen." :wait t) - (unix:sleep 1) - (send *ri* :speak "return to dock." :wait t) - (auto-dock)) - (progn ;; failed to go to kitchen - (send *ri* :speak "I could not go to kitchen, so return to dock." :wait t) - (auto-dock)))) - +#!/usr/bin/env roseus +(load "package://jsk_fetch_startup/euslisp/navigation-utils.l") +(ros::roseus "go_to_kitchen") (go-to-kitchen) +(exit) 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 ba5994c987..5ba791f17b 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -1,5 +1,4 @@ -;; go-dock.l -;; Author: Yuki Furuta +(load "package://jsk_robot_startup/lifelog/tweet_client.l") (ros::load-ros-manifest "fetch_auto_dock_msgs") (ros::load-ros-manifest "power_msgs") @@ -113,3 +112,22 @@ (setq success (dock)) (when success (return-from auto-dock success))) success)) + +(defun go-to-kitchen (&key (tweet t)) + ;; go to kitchen + (unless (boundp '*ri*) + (require :fetch-interface "package://fetcheus/fetch-interface.l") + (fetch-init)) + (send *ri* :speak-jp "キッチンに向かいます。") + (if (go-to-spot "/eng2/7f/room73B2-sink-front1" (make-coords :pos #f(100 -1000 0))) + (progn ;; succeeded to go to kitchen + (unix:sleep 1) + (send *ri* :speak-jp "キッチンにつきました。" :wait t) + (unix:sleep 1) + (when tweet + (tweet-string "I took a photo at 73B2 Kitchen." :warning-time 3 :with-image t :speak t)) + (send *ri* :speak-jp "ドックに戻ります。" :wait t) + (auto-dock)) + (progn ;; failed to go to kitchen + (send *ri* :speak-jp "失敗しました。ドックに戻ります。" :wait t) + (auto-dock)))) From 86c755de48cbb1a4ed48334f88b02430c789447e Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 17 Oct 2019 23:47:35 +0900 Subject: [PATCH 04/54] add n-dock-trial for go-to-kitchen --- jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l | 2 +- .../jsk_fetch_startup/euslisp/navigation-utils.l | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l index 8db7d9e7ff..bc416c49ac 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l @@ -2,5 +2,5 @@ (load "package://jsk_fetch_startup/euslisp/navigation-utils.l") (ros::roseus "go_to_kitchen") -(go-to-kitchen) +(go-to-kitchen :tweet t :n-dock-trial 5) (exit) 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 5ba791f17b..21df805aaa 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -113,7 +113,7 @@ (when success (return-from auto-dock success))) success)) -(defun go-to-kitchen (&key (tweet t)) +(defun go-to-kitchen (&key (tweet t) (n-dock-trial 1)) ;; go to kitchen (unless (boundp '*ri*) (require :fetch-interface "package://fetcheus/fetch-interface.l") @@ -127,7 +127,7 @@ (when tweet (tweet-string "I took a photo at 73B2 Kitchen." :warning-time 3 :with-image t :speak t)) (send *ri* :speak-jp "ドックに戻ります。" :wait t) - (auto-dock)) + (auto-dock :n-trial n-dock-trial)) (progn ;; failed to go to kitchen (send *ri* :speak-jp "失敗しました。ドックに戻ります。" :wait t) - (auto-dock)))) + (auto-dock :n-trial n-dock-trial)))) From 17d1e02c882b2ac5c39bb997ea0add89ea0916cd Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 19 Oct 2019 01:32:40 +0900 Subject: [PATCH 05/54] rotate after undock when going to kitchen --- .../jsk_fetch_startup/euslisp/navigation-utils.l | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) 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 21df805aaa..5f0f144767 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -79,7 +79,7 @@ (return-from undock nil)) (send (send *undock-action* :get-result) :undocked)) -(defun go-to-spot (name &optional (relative-coords (make-coords))) +(defun go-to-spot (name &optional (relative-coords (make-coords)) &key (undock-rotate nil)) ;; undock if fetch is docking (unless (boundp '*ri*) (require :fetch-interface "package://fetcheus/fetch-interface.l") @@ -93,7 +93,10 @@ (ros::ros-warn "Skip undocking, so please make sure that Fetch is already undocked.") (return)) (setq *is-charging* (send msg :is_charging)) - (when *is-charging* (undock)) + (when *is-charging* + (undock) + ;; rotate after undock + (when undock-rotate (send *ri* :go-pos-unsafe 0 0 3.14))) (unless *is-charging* (return)) (if (eq i 2) (progn (send *ri* :speak "Fail to undock") (ros::ros-error "Fail to undock"))))) ;; go to spot @@ -119,7 +122,8 @@ (require :fetch-interface "package://fetcheus/fetch-interface.l") (fetch-init)) (send *ri* :speak-jp "キッチンに向かいます。") - (if (go-to-spot "/eng2/7f/room73B2-sink-front1" (make-coords :pos #f(100 -1000 0))) + (if (go-to-spot "/eng2/7f/room73B2-sink-front1" + (make-coords :pos #f(100 -1000 0)) :undock-rotate t) (progn ;; succeeded to go to kitchen (unix:sleep 1) (send *ri* :speak-jp "キッチンにつきました。" :wait t) From 4737f08ff170b4f1b0fbd36325ef6b9e313cd546 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 19 Oct 2019 01:33:18 +0900 Subject: [PATCH 06/54] clear costmap at the beginning of going to kitchen --- jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l index bc416c49ac..aeee6bf291 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l @@ -1,6 +1,10 @@ #!/usr/bin/env roseus +(require :fetch-interface "package://fetcheus/fetch-interface.l") (load "package://jsk_fetch_startup/euslisp/navigation-utils.l") + (ros::roseus "go_to_kitchen") +(fetch-init) +(send *ri* :clear-costmap) (go-to-kitchen :tweet t :n-dock-trial 5) (exit) From 8aa33797a040a6d38ccb78f1081551308bf82edc Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 19 Oct 2019 03:12:44 +0900 Subject: [PATCH 07/54] use edgetpu image for kitchen demo tweet --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 5f0f144767..a1bb2e9faf 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -129,7 +129,8 @@ (send *ri* :speak-jp "キッチンにつきました。" :wait t) (unix:sleep 1) (when tweet - (tweet-string "I took a photo at 73B2 Kitchen." :warning-time 3 :with-image t :speak t)) + (tweet-string "I took a photo at 73B2 Kitchen." :warning-time 3 + :with-image "/edgetpu_object_detector/output/image" :speak t)) (send *ri* :speak-jp "ドックに戻ります。" :wait t) (auto-dock :n-trial n-dock-trial)) (progn ;; failed to go to kitchen From 1d75d1f897c0325b0790b9a92310e7551d7b7c02 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 22 Oct 2019 11:09:49 +0900 Subject: [PATCH 08/54] add ros-info and ros-error for logging --- .../jsk_fetch_startup/euslisp/navigation-utils.l | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) 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 a1bb2e9faf..1bdcc0b47e 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -111,9 +111,10 @@ (defun auto-dock (&key (n-trial 1)) (let ((success nil)) (dotimes (i n-trial) - (go-to-spot *dock-spot* (make-coords :pos #f(0 -800 0))) - (setq success (dock)) - (when success (return-from auto-dock success))) + (when (go-to-spot *dock-spot* (make-coords :pos #f(0 -800 0))) + (ros::ros-info "arrived at the dock.") + (setq success (dock)) + (when success (return-from auto-dock success)))) success)) (defun go-to-kitchen (&key (tweet t) (n-dock-trial 1)) @@ -121,18 +122,23 @@ (unless (boundp '*ri*) (require :fetch-interface "package://fetcheus/fetch-interface.l") (fetch-init)) + (ros::ros-info "start going to the kitchen.") (send *ri* :speak-jp "キッチンに向かいます。") (if (go-to-spot "/eng2/7f/room73B2-sink-front1" (make-coords :pos #f(100 -1000 0)) :undock-rotate t) (progn ;; succeeded to go to kitchen (unix:sleep 1) + (ros::ros-info "arrived at the kitchen.") (send *ri* :speak-jp "キッチンにつきました。" :wait t) (unix:sleep 1) (when tweet (tweet-string "I took a photo at 73B2 Kitchen." :warning-time 3 :with-image "/edgetpu_object_detector/output/image" :speak t)) + (ros::ros-info "start going back to the dock.") (send *ri* :speak-jp "ドックに戻ります。" :wait t) (auto-dock :n-trial n-dock-trial)) (progn ;; failed to go to kitchen + (ros::ros-error "failed going to the kitchen.") (send *ri* :speak-jp "失敗しました。ドックに戻ります。" :wait t) + (ros::ros-info "start going back to the dock.") (auto-dock :n-trial n-dock-trial)))) From fc6d9a6fb38f6179c19d59c58b1ccdbb146d07c6 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 24 Oct 2019 04:50:13 +0900 Subject: [PATCH 09/54] update auto-dock.l --- jsk_fetch_robot/jsk_fetch_startup/euslisp/auto-dock.l | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/auto-dock.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/auto-dock.l index df7b44ee69..10ca08b546 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/auto-dock.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/auto-dock.l @@ -1,8 +1,10 @@ -;; go-dock.l -;; Author: Yuki Furuta +#!/usr/bin/env roseus -(ros::roseus "fetch_go_dock") +(require :fetch-interface "package://fetcheus/fetch-interface.l") (load "package://jsk_fetch_startup/euslisp/navigation-utils.l") +(ros::roseus "fetch_go_dock") +(fetch-init) +(send *ri* :clear-costmap) (auto-dock) (exit) From fb412653864e106613277c62c177e3858c0f297f Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 24 Oct 2019 06:29:08 +0900 Subject: [PATCH 10/54] go to stove and sink in kitchen demo --- .../euslisp/navigation-utils.l | 37 ++++++++++++++----- 1 file changed, 28 insertions(+), 9 deletions(-) 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 1bdcc0b47e..fedae1ebba 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -124,21 +124,40 @@ (fetch-init)) (ros::ros-info "start going to the kitchen.") (send *ri* :speak-jp "キッチンに向かいます。") - (if (go-to-spot "/eng2/7f/room73B2-sink-front1" - (make-coords :pos #f(100 -1000 0)) :undock-rotate t) + ;; stove + (if (go-to-spot "/eng2/7f/room73B2-sink-front0" + (make-coords :pos #f(100 -500 0)) :undock-rotate t) (progn ;; succeeded to go to kitchen (unix:sleep 1) - (ros::ros-info "arrived at the kitchen.") - (send *ri* :speak-jp "キッチンにつきました。" :wait t) + (ros::ros-info "arrived at the kitchen stove.") + (send *ri* :speak-jp "キッチンのコンロの前につきました。" :wait t) (unix:sleep 1) (when tweet - (tweet-string "I took a photo at 73B2 Kitchen." :warning-time 3 - :with-image "/edgetpu_object_detector/output/image" :speak t)) + (tweet-string "I took a photo at 73B2 Kitchen stove." :warning-time 3 + :with-image "/edgetpu_object_detector/output/image" :speak t))) + (progn ;; failed to go to kitchen + (ros::ros-error "failed going to the kitchen.") + (send *ri* :speak-jp "失敗しました。ドックに戻ります。" :wait t) (ros::ros-info "start going back to the dock.") - (send *ri* :speak-jp "ドックに戻ります。" :wait t) - (auto-dock :n-trial n-dock-trial)) + (auto-dock :n-trial n-dock-trial) + (return-from go-to-kitchen nil))) + ;; sink + (send *ri* :speak-jp "シンクに向かいます。") + (if (go-to-spot "/eng2/7f/room73B2-sink-front1" (make-coords :pos #f(100 -200 0))) + (progn ;; succeeded to go to kitchen + (unix:sleep 1) + (ros::ros-info "arrived at the kitchen.") + (send *ri* :speak-jp "キッチンのシンクの前につきました。" :wait t) + (unix:sleep 1) + (when tweet + (tweet-string "I took a photo at 73B2 Kitchen sink." :warning-time 3 + :with-image "/edgetpu_object_detector/output/image" :speak t))) (progn ;; failed to go to kitchen (ros::ros-error "failed going to the kitchen.") (send *ri* :speak-jp "失敗しました。ドックに戻ります。" :wait t) (ros::ros-info "start going back to the dock.") - (auto-dock :n-trial n-dock-trial)))) + (auto-dock :n-trial n-dock-trial) + (return-from go-to-kitchen nil))) + (ros::ros-info "start going back to the dock.") + (send *ri* :speak-jp "ドックに戻ります。" :wait t) + (auto-dock :n-trial n-dock-trial)) From 82fdb01d133b61fa83db06d753f6a272b926ea7c Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 24 Oct 2019 06:29:33 +0900 Subject: [PATCH 11/54] fix go-to-spot function --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 fedae1ebba..54e7efeda5 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -96,7 +96,7 @@ (when *is-charging* (undock) ;; rotate after undock - (when undock-rotate (send *ri* :go-pos-unsafe 0 0 3.14))) + (when undock-rotate (send *ri* :go-pos-unsafe 0 0 pi))) (unless *is-charging* (return)) (if (eq i 2) (progn (send *ri* :speak "Fail to undock") (ros::ros-error "Fail to undock"))))) ;; go to spot From 34c822a9d56395055fd3dfaa4fe687056d92dbd3 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 24 Oct 2019 08:14:13 +0900 Subject: [PATCH 12/54] do not go to in front of sink --- .../euslisp/navigation-utils.l | 37 +++++++++++-------- 1 file changed, 22 insertions(+), 15 deletions(-) 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 54e7efeda5..4ec36c6548 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -132,23 +132,13 @@ (ros::ros-info "arrived at the kitchen stove.") (send *ri* :speak-jp "キッチンのコンロの前につきました。" :wait t) (unix:sleep 1) + ;; stove (when tweet (tweet-string "I took a photo at 73B2 Kitchen stove." :warning-time 3 - :with-image "/edgetpu_object_detector/output/image" :speak t))) - (progn ;; failed to go to kitchen - (ros::ros-error "failed going to the kitchen.") - (send *ri* :speak-jp "失敗しました。ドックに戻ります。" :wait t) - (ros::ros-info "start going back to the dock.") - (auto-dock :n-trial n-dock-trial) - (return-from go-to-kitchen nil))) - ;; sink - (send *ri* :speak-jp "シンクに向かいます。") - (if (go-to-spot "/eng2/7f/room73B2-sink-front1" (make-coords :pos #f(100 -200 0))) - (progn ;; succeeded to go to kitchen - (unix:sleep 1) - (ros::ros-info "arrived at the kitchen.") - (send *ri* :speak-jp "キッチンのシンクの前につきました。" :wait t) - (unix:sleep 1) + :with-image "/edgetpu_object_detector/output/image" :speak t)) + (send *ri* :go-pos-unsafe-no-wait 0 0 (/ -pi 4)) + (send *ri* :go-pos-unsafe-wait :rotation-gain 2.0) + ;; sink (when tweet (tweet-string "I took a photo at 73B2 Kitchen sink." :warning-time 3 :with-image "/edgetpu_object_detector/output/image" :speak t))) @@ -158,6 +148,23 @@ (ros::ros-info "start going back to the dock.") (auto-dock :n-trial n-dock-trial) (return-from go-to-kitchen nil))) + ;; sink + ;; (send *ri* :speak-jp "シンクに向かいます。") + ;; (if (go-to-spot "/eng2/7f/room73B2-sink-front1" (make-coords :pos #f(100 -200 0))) + ;; (progn ;; succeeded to go to kitchen + ;; (unix:sleep 1) + ;; (ros::ros-info "arrived at the kitchen.") + ;; (send *ri* :speak-jp "キッチンのシンクの前につきました。" :wait t) + ;; (unix:sleep 1) + ;; (when tweet + ;; (tweet-string "I took a photo at 73B2 Kitchen sink." :warning-time 3 + ;; :with-image "/edgetpu_object_detector/output/image" :speak t))) + ;; (progn ;; failed to go to kitchen + ;; (ros::ros-error "failed going to the kitchen.") + ;; (send *ri* :speak-jp "失敗しました。ドックに戻ります。" :wait t) + ;; (ros::ros-info "start going back to the dock.") + ;; (auto-dock :n-trial n-dock-trial) + ;; (return-from go-to-kitchen nil))) (ros::ros-info "start going back to the dock.") (send *ri* :speak-jp "ドックに戻ります。" :wait t) (auto-dock :n-trial n-dock-trial)) From 942caa718034d530e94977c5245881079d655f70 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 26 Oct 2019 00:57:57 +0900 Subject: [PATCH 13/54] use normal go-pos-unsafe in go-to kitchen --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 4ec36c6548..801087a899 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -136,8 +136,7 @@ (when tweet (tweet-string "I took a photo at 73B2 Kitchen stove." :warning-time 3 :with-image "/edgetpu_object_detector/output/image" :speak t)) - (send *ri* :go-pos-unsafe-no-wait 0 0 (/ -pi 4)) - (send *ri* :go-pos-unsafe-wait :rotation-gain 2.0) + (send *ri* :go-pos-unsafe 0 0 (/ -pi 4)) ;; sink (when tweet (tweet-string "I took a photo at 73B2 Kitchen sink." :warning-time 3 From fbdb5e9dfc2031a181a2cd93e6bd458eb38f3470 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 26 Oct 2019 23:29:29 +0900 Subject: [PATCH 14/54] add n-trial-kitchen key --- .../jsk_fetch_startup/euslisp/go-to-kitchen.l | 2 +- .../euslisp/navigation-utils.l | 51 ++++++++++--------- 2 files changed, 29 insertions(+), 24 deletions(-) diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l index aeee6bf291..d9c1f0fa25 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l @@ -6,5 +6,5 @@ (ros::roseus "go_to_kitchen") (fetch-init) (send *ri* :clear-costmap) -(go-to-kitchen :tweet t :n-dock-trial 5) +(go-to-kitchen :tweet t :n-dock-trial 5 :n-kitchen-trial 3) (exit) 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 801087a899..e5bb0819d0 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -117,7 +117,7 @@ (when success (return-from auto-dock success)))) success)) -(defun go-to-kitchen (&key (tweet t) (n-dock-trial 1)) +(defun go-to-kitchen (&key (tweet t) (n-dock-trial 1) (n-kitchen-trial 1)) ;; go to kitchen (unless (boundp '*ri*) (require :fetch-interface "package://fetcheus/fetch-interface.l") @@ -125,28 +125,33 @@ (ros::ros-info "start going to the kitchen.") (send *ri* :speak-jp "キッチンに向かいます。") ;; stove - (if (go-to-spot "/eng2/7f/room73B2-sink-front0" - (make-coords :pos #f(100 -500 0)) :undock-rotate t) - (progn ;; succeeded to go to kitchen - (unix:sleep 1) - (ros::ros-info "arrived at the kitchen stove.") - (send *ri* :speak-jp "キッチンのコンロの前につきました。" :wait t) - (unix:sleep 1) - ;; stove - (when tweet - (tweet-string "I took a photo at 73B2 Kitchen stove." :warning-time 3 - :with-image "/edgetpu_object_detector/output/image" :speak t)) - (send *ri* :go-pos-unsafe 0 0 (/ -pi 4)) - ;; sink - (when tweet - (tweet-string "I took a photo at 73B2 Kitchen sink." :warning-time 3 - :with-image "/edgetpu_object_detector/output/image" :speak t))) - (progn ;; failed to go to kitchen - (ros::ros-error "failed going to the kitchen.") - (send *ri* :speak-jp "失敗しました。ドックに戻ります。" :wait t) - (ros::ros-info "start going back to the dock.") - (auto-dock :n-trial n-dock-trial) - (return-from go-to-kitchen nil))) + (let ((succcess-go-to-kitchen)) + (dotimes (i n-kitchen-trial) + (setq success-go-to-kitchen + (go-to-spot "/eng2/7f/room73B2-sink-front0" + (make-coords :pos #f(100 -500 0)) :undock-rotate t)) + (when success-go-to-kitchen (return))) + (if success-go-to-kitchen + (progn ;; succeeded to go to kitchen + (unix:sleep 1) + (ros::ros-info "arrived at the kitchen stove.") + (send *ri* :speak-jp "キッチンのコンロの前につきました。" :wait t) + (unix:sleep 1) + ;; stove + (when tweet + (tweet-string "I took a photo at 73B2 Kitchen stove." :warning-time 3 + :with-image "/edgetpu_object_detector/output/image" :speak t)) + (send *ri* :go-pos-unsafe 0 0 (/ -pi 4)) + ;; sink + (when tweet + (tweet-string "I took a photo at 73B2 Kitchen sink." :warning-time 3 + :with-image "/edgetpu_object_detector/output/image" :speak t))) + (progn ;; failed to go to kitchen + (ros::ros-error "failed going to the kitchen.") + (send *ri* :speak-jp "失敗しました。ドックに戻ります。" :wait t) + (ros::ros-info "start going back to the dock.") + (auto-dock :n-trial n-dock-trial) + (return-from go-to-kitchen nil)))) ;; sink ;; (send *ri* :speak-jp "シンクに向かいます。") ;; (if (go-to-spot "/eng2/7f/room73B2-sink-front1" (make-coords :pos #f(100 -200 0))) From 72ba470a869187404047bafb05242c851f7f7cbc Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Fri, 8 Nov 2019 23:58:51 +0900 Subject: [PATCH 15/54] update go-to-kitchen motion --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 e5bb0819d0..4bb4697b67 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -129,7 +129,7 @@ (dotimes (i n-kitchen-trial) (setq success-go-to-kitchen (go-to-spot "/eng2/7f/room73B2-sink-front0" - (make-coords :pos #f(100 -500 0)) :undock-rotate t)) + (make-coords :pos #f(200 -500 0)) :undock-rotate t)) (when success-go-to-kitchen (return))) (if success-go-to-kitchen (progn ;; succeeded to go to kitchen @@ -141,7 +141,7 @@ (when tweet (tweet-string "I took a photo at 73B2 Kitchen stove." :warning-time 3 :with-image "/edgetpu_object_detector/output/image" :speak t)) - (send *ri* :go-pos-unsafe 0 0 (/ -pi 4)) + (send *ri* :go-pos-unsafe 0 0 -45) ;; sink (when tweet (tweet-string "I took a photo at 73B2 Kitchen sink." :warning-time 3 From 789b77d168e03a999318f097e767358da0601ab0 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 9 Nov 2019 02:17:30 +0900 Subject: [PATCH 16/54] fix go-to-kitchen motion --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 4bb4697b67..4e373a3f38 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -96,7 +96,7 @@ (when *is-charging* (undock) ;; rotate after undock - (when undock-rotate (send *ri* :go-pos-unsafe 0 0 pi))) + (when undock-rotate (send *ri* :go-pos-unsafe 0 0 180))) (unless *is-charging* (return)) (if (eq i 2) (progn (send *ri* :speak "Fail to undock") (ros::ros-error "Fail to undock"))))) ;; go to spot From a511bdbf98109144a1759ab5df0ac5f0c0a2fd9b Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 9 Nov 2019 04:30:55 +0900 Subject: [PATCH 17/54] update go-to-kitchen move --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 4e373a3f38..d4de993a51 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -129,7 +129,7 @@ (dotimes (i n-kitchen-trial) (setq success-go-to-kitchen (go-to-spot "/eng2/7f/room73B2-sink-front0" - (make-coords :pos #f(200 -500 0)) :undock-rotate t)) + (make-coords :pos #f(100 -500 0)) :undock-rotate t)) (when success-go-to-kitchen (return))) (if success-go-to-kitchen (progn ;; succeeded to go to kitchen From 6c7b6976e1c974bc702a09286ea38a31f53fb353 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 9 Nov 2019 20:33:02 +0900 Subject: [PATCH 18/54] clear costmap before autodock --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 4 ++++ 1 file changed, 4 insertions(+) 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 d4de993a51..58c870ea7e 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -109,6 +109,10 @@ (send *ri* :move-to goal-pose :frame-id frame-id))) (defun auto-dock (&key (n-trial 1)) + (unless (boundp '*ri*) + (require :fetch-interface "package://fetcheus/fetch-interface.l") + (fetch-init)) + (send *ri* :clear-costmap) (let ((success nil)) (dotimes (i n-trial) (when (go-to-spot *dock-spot* (make-coords :pos #f(0 -800 0))) From 95fa42de0a585ab418b180ce87ff7e9c49dc0e67 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 9 Nov 2019 22:55:23 +0900 Subject: [PATCH 19/54] move clear-costmap inside go-to-spot --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 4 ---- 1 file changed, 4 deletions(-) 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 58c870ea7e..d4de993a51 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -109,10 +109,6 @@ (send *ri* :move-to goal-pose :frame-id frame-id))) (defun auto-dock (&key (n-trial 1)) - (unless (boundp '*ri*) - (require :fetch-interface "package://fetcheus/fetch-interface.l") - (fetch-init)) - (send *ri* :clear-costmap) (let ((success nil)) (dotimes (i n-trial) (when (go-to-spot *dock-spot* (make-coords :pos #f(0 -800 0))) From fef2aa8853fdcef2ec77737391b2a41248076d1c Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 28 Apr 2020 21:02:37 +0900 Subject: [PATCH 20/54] add use-pose tag in simpple-dock function --- .../euslisp/navigation-utils.l | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) 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 d4de993a51..c074b453ac 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -24,7 +24,7 @@ (setq spot-coords (ros::tf-pose->coords spot-coords)) (cons spot-coords frame-id))) -(defun simple-dock () +(defun simple-dock (&key (use-pose t)) (unless *dock-action* (setq *dock-action* (instance ros::simple-action-client :init @@ -32,19 +32,20 @@ (unless (send *dock-action* :wait-for-server 5) (ros::ros-error "/dock action server is not started") (return-from dock nil)) - (let* ((timestamp (ros::time-now)) - (cret (get-spot-coords *dock-spot*)) - (frame-to-dock (car cret)) - (frame-id (cdr cret)) - (lret (send *tfl* :wait-for-transform "base_link" frame-id timestamp 5)) - (base-to-frame (send *tfl* :lookup-transform "base_link" frame-id timestamp)) - (goal-pose (ros::coords->tf-pose (send frame-to-dock :transform base-to-frame :world))) - (pose-msg (instance geometry_msgs::PoseStamped :init)) - (dock-action-goal (instance fetch_auto_dock_msgs::DockActionGoal :init))) - (send pose-msg :header :stamp timestamp) - (send pose-msg :header :frame_id "base_link") - (send pose-msg :pose goal-pose) - (send dock-action-goal :goal :dock_pose pose-msg) + (let ((dock-action-goal (instance fetch_auto_dock_msgs::DockActionGoal :init))) + (when use-pose + (let* ((timestamp (ros::time-now)) + (cret (get-spot-coords *dock-spot*)) + (frame-to-dock (car cret)) + (frame-id (cdr cret)) + (lret (send *tfl* :wait-for-transform "base_link" frame-id timestamp 5)) + (base-to-frame (send *tfl* :lookup-transform "base_link" frame-id timestamp)) + (goal-pose (ros::coords->tf-pose (send frame-to-dock :transform base-to-frame :world))) + (pose-msg (instance geometry_msgs::PoseStamped :init))) + (send pose-msg :header :stamp timestamp) + (send pose-msg :header :frame_id "base_link") + (send pose-msg :pose goal-pose) + (send dock-action-goal :goal :dock_pose pose-msg))) (send *dock-action* :send-goal dock-action-goal) (unless (send *dock-action* :wait-for-result :timeout 60) (send *dock-action* :cancel-all-goals) From 851dd8968669dcb7b363f01f8599f4b0c5b0e0a7 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 28 Apr 2020 21:03:43 +0900 Subject: [PATCH 21/54] rotate before moves --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 c074b453ac..e0746d46f5 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -146,7 +146,8 @@ ;; sink (when tweet (tweet-string "I took a photo at 73B2 Kitchen sink." :warning-time 3 - :with-image "/edgetpu_object_detector/output/image" :speak t))) + :with-image "/edgetpu_object_detector/output/image" :speak t)) + (send *ri* :go-pos-unsafe 0 0 -135)) (progn ;; failed to go to kitchen (ros::ros-error "failed going to the kitchen.") (send *ri* :speak-jp "失敗しました。ドックに戻ります。" :wait t) From b16f3e3190e7b61f18f49c131275f6002d719957 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Fri, 1 May 2020 15:23:17 +0900 Subject: [PATCH 22/54] rotate before move --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 e0746d46f5..5cf0840c6d 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -147,7 +147,7 @@ (when tweet (tweet-string "I took a photo at 73B2 Kitchen sink." :warning-time 3 :with-image "/edgetpu_object_detector/output/image" :speak t)) - (send *ri* :go-pos-unsafe 0 0 -135)) + (send *ri* :go-pos-unsafe 0 0 -180)) (progn ;; failed to go to kitchen (ros::ros-error "failed going to the kitchen.") (send *ri* :speak-jp "失敗しました。ドックに戻ります。" :wait t) From 488c8a9026d7474a00ab163a92a6113f6c38a1ef Mon Sep 17 00:00:00 2001 From: jsk-fetchuser Date: Tue, 19 May 2020 23:00:25 +0900 Subject: [PATCH 23/54] rotate -90 + -90 to avoid rotating in couterclockwise --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 5cf0840c6d..2ebe8c80d0 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -147,7 +147,8 @@ (when tweet (tweet-string "I took a photo at 73B2 Kitchen sink." :warning-time 3 :with-image "/edgetpu_object_detector/output/image" :speak t)) - (send *ri* :go-pos-unsafe 0 0 -180)) + (send *ri* :go-pos-unsafe 0 0 -90) + (send *ri* :go-pos-unsafe 0 0 -90)) (progn ;; failed to go to kitchen (ros::ros-error "failed going to the kitchen.") (send *ri* :speak-jp "失敗しました。ドックに戻ります。" :wait t) From 9761b4813e53a0c9f06f839e37ca966bdbf5d760 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 17 Sep 2020 15:49:46 +0900 Subject: [PATCH 24/54] switch on/off room light by google home --- .../jsk_fetch_startup/euslisp/navigation-utils.l | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) 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 2ebe8c80d0..59ef9479be 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -124,7 +124,11 @@ (require :fetch-interface "package://fetcheus/fetch-interface.l") (fetch-init)) (ros::ros-info "start going to the kitchen.") - (send *ri* :speak-jp "キッチンに向かいます。") + (send *ri* :speak-jp "キッチンに向かいます。" :wait t) + (unix::sleep 1) + (send *ri* :speak-jp "オッケー、グーグル" :wait t) + (send *ri* :speak-jp "電気をつけて" :wait t) + (unix::sleep 1) ;; stove (let ((succcess-go-to-kitchen)) (dotimes (i n-kitchen-trial) @@ -174,4 +178,8 @@ ;; (return-from go-to-kitchen nil))) (ros::ros-info "start going back to the dock.") (send *ri* :speak-jp "ドックに戻ります。" :wait t) - (auto-dock :n-trial n-dock-trial)) + (auto-dock :n-trial n-dock-trial) + (unix::sleep 1) + (send *ri* :speak-jp "オッケー、グーグル" :wait t) + (send *ri* :speak-jp "電気を消して" :wait t) + (unix::sleep 1)) From 6393b6791dc89795ed02c30665cccb6ee01ba8e1 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 19 Nov 2020 00:26:56 +0900 Subject: [PATCH 25/54] return success or not in go-to-kitchen --- .../euslisp/navigation-utils.l | 85 ++++++++++++------- 1 file changed, 52 insertions(+), 33 deletions(-) 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 59ef9479be..c2c53fb21b 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -126,11 +126,31 @@ (ros::ros-info "start going to the kitchen.") (send *ri* :speak-jp "キッチンに向かいます。" :wait t) (unix::sleep 1) - (send *ri* :speak-jp "オッケー、グーグル" :wait t) - (send *ri* :speak-jp "電気をつけて" :wait t) - (unix::sleep 1) - ;; stove - (let ((succcess-go-to-kitchen)) + ;; Check if the lights are on in the room + (let ((initial-light-on nil) + (success-go-to-kitchen nil) + (success-auto-dock nil)) + (let* ((ros-img (one-shot-subscribe "/head_camera/rgb/quater/image_rect_color" sensor_msgs::Image)) + (eus-img (ros::sensor_msgs/Image->image ros-img)) + (luminance 0) rgb-oct r g b) + (dotimes (w (send eus-img :width)) + (dotimes (h (send eus-img :height)) + (setq rgb-oct (send eus-img :pixel-hex-string w h)) + (setq r (read-from-string (format nil "#X~A" (subseq rgb-oct 0 2)))) + (setq g (read-from-string (format nil "#X~A" (subseq rgb-oct 2 4)))) + (setq b (read-from-string (format nil "#X~A" (subseq rgb-oct 4 6)))) + (setq luminance (+ luminance (+ (* 0.299 r) (* 0.587 g) (* 0.114 b)))))) + (setq luminance (/ luminance (* (send eus-img :width) (send eus-img :height)))) + (if (> luminance 50) + (setq initial-light-on t) + (setq initial-light-on nil)) + (if initial-light-on + (send *ri* :speak-jp "すでに電気がついています。" :wait t) + (progn + (send *ri* :speak-jp "オッケー、グーグル" :wait t) + (send *ri* :speak-jp "電気をつけて" :wait t)))) + (unix::sleep 1) + ;; stove (dotimes (i n-kitchen-trial) (setq success-go-to-kitchen (go-to-spot "/eng2/7f/room73B2-sink-front0" @@ -155,31 +175,30 @@ (send *ri* :go-pos-unsafe 0 0 -90)) (progn ;; failed to go to kitchen (ros::ros-error "failed going to the kitchen.") - (send *ri* :speak-jp "失敗しました。ドックに戻ります。" :wait t) - (ros::ros-info "start going back to the dock.") - (auto-dock :n-trial n-dock-trial) - (return-from go-to-kitchen nil)))) - ;; sink - ;; (send *ri* :speak-jp "シンクに向かいます。") - ;; (if (go-to-spot "/eng2/7f/room73B2-sink-front1" (make-coords :pos #f(100 -200 0))) - ;; (progn ;; succeeded to go to kitchen - ;; (unix:sleep 1) - ;; (ros::ros-info "arrived at the kitchen.") - ;; (send *ri* :speak-jp "キッチンのシンクの前につきました。" :wait t) - ;; (unix:sleep 1) - ;; (when tweet - ;; (tweet-string "I took a photo at 73B2 Kitchen sink." :warning-time 3 - ;; :with-image "/edgetpu_object_detector/output/image" :speak t))) - ;; (progn ;; failed to go to kitchen - ;; (ros::ros-error "failed going to the kitchen.") - ;; (send *ri* :speak-jp "失敗しました。ドックに戻ります。" :wait t) - ;; (ros::ros-info "start going back to the dock.") - ;; (auto-dock :n-trial n-dock-trial) - ;; (return-from go-to-kitchen nil))) - (ros::ros-info "start going back to the dock.") - (send *ri* :speak-jp "ドックに戻ります。" :wait t) - (auto-dock :n-trial n-dock-trial) - (unix::sleep 1) - (send *ri* :speak-jp "オッケー、グーグル" :wait t) - (send *ri* :speak-jp "電気を消して" :wait t) - (unix::sleep 1)) + (send *ri* :speak-jp "失敗しました。" :wait t))) + ;; sink + ;; (send *ri* :speak-jp "シンクに向かいます。") + ;; (if (go-to-spot "/eng2/7f/room73B2-sink-front1" (make-coords :pos #f(100 -200 0))) + ;; (progn ;; succeeded to go to kitchen + ;; (unix:sleep 1) + ;; (ros::ros-info "arrived at the kitchen.") + ;; (send *ri* :speak-jp "キッチンのシンクの前につきました。" :wait t) + ;; (unix:sleep 1) + ;; (when tweet + ;; (tweet-string "I took a photo at 73B2 Kitchen sink." :warning-time 3 + ;; :with-image "/edgetpu_object_detector/output/image" :speak t))) + ;; (progn ;; failed to go to kitchen + ;; (ros::ros-error "failed going to the kitchen.") + ;; (send *ri* :speak-jp "失敗しました。ドックに戻ります。" :wait t) + ;; (ros::ros-info "start going back to the dock.") + ;; (auto-dock :n-trial n-dock-trial) + ;; (return-from go-to-kitchen nil))) + (ros::ros-info "start going back to the dock.") + (send *ri* :speak-jp "ドックに戻ります。" :wait t) + (setq success-auto-dock (auto-dock :n-trial n-dock-trial)) + (when (and success-auto-dock (not initial-light-on)) + (unix::sleep 1) + (send *ri* :speak-jp "オッケー、グーグル" :wait t) + (send *ri* :speak-jp "電気を消して" :wait t)) + (unix::sleep 1) + (and success-go-to-kitchen success-auto-dock))) From a6d7eda36be277bd21cb2a8c67b9c116ece202f0 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 23 Dec 2020 00:04:18 +0900 Subject: [PATCH 26/54] support fetch1075 for navigation-utils.l --- .../jsk_fetch_startup/euslisp/navigation-utils.l | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) 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 c2c53fb21b..595457ced6 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -6,11 +6,20 @@ (defparameter *dock-action* nil) (defparameter *undock-action* nil) (defparameter *spots* nil) -(defparameter *dock-spot* "/eng2/7f/room73B2-fetch-dock-front") (defparameter *is-charging* nil) (defparameter *tfl* (instance ros::transform-listener :init)) +(let ((robot-name (ros::get-param "/robot/name"))) + (defparameter *dock-spot* + (cond + ((equal robot-name "fetch15") + "/eng2/7f/room73B2-fetch-dock-front") + ((equal robot-name "fetch1075") + "/eng2/7f/room73B2-fetch-dock-entrance") + (t nil)))) + + (defun get-spot-coords (name) (unless *spots* (setq *spots* (one-shot-subscribe "/spots_marker_array" visualization_msgs::MarkerArray))) From 1434915bef7ff8b3408117dae8fb314414a55b3f Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 3 Apr 2021 14:06:11 +0900 Subject: [PATCH 27/54] update fetch1075 dock spots --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 595457ced6..0872c9ecfb 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -16,7 +16,7 @@ ((equal robot-name "fetch15") "/eng2/7f/room73B2-fetch-dock-front") ((equal robot-name "fetch1075") - "/eng2/7f/room73B2-fetch-dock-entrance") + "/eng2/7f/room73B2-fetch-dock2-front") (t nil)))) From c0fbd5e8c38a5a8945f92bcdd88e15470ecb2b5e Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 3 Apr 2021 14:22:15 +0900 Subject: [PATCH 28/54] fix navigation-utils for new fetch dock spot --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 0872c9ecfb..9a387bdcc0 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -121,7 +121,7 @@ (defun auto-dock (&key (n-trial 1)) (let ((success nil)) (dotimes (i n-trial) - (when (go-to-spot *dock-spot* (make-coords :pos #f(0 -800 0))) + (when (go-to-spot *dock-spot* (make-coords :pos #f(-800 0 0))) (ros::ros-info "arrived at the dock.") (setq success (dock)) (when success (return-from auto-dock success)))) From 4d32e06ba3d2d344fe82246b8e7904b6f0c26c2f Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 3 Apr 2021 16:09:36 +0900 Subject: [PATCH 29/54] do not move when undock failed --- .../euslisp/navigation-utils.l | 48 ++++++++++++------- 1 file changed, 32 insertions(+), 16 deletions(-) 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 9a387bdcc0..2b37e62a27 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -6,7 +6,6 @@ (defparameter *dock-action* nil) (defparameter *undock-action* nil) (defparameter *spots* nil) -(defparameter *is-charging* nil) (defparameter *tfl* (instance ros::transform-listener :init)) @@ -94,25 +93,34 @@ (unless (boundp '*ri*) (require :fetch-interface "package://fetcheus/fetch-interface.l") (fetch-init)) - (dotimes (i 3) - (let ((msg (one-shot-subscribe "/battery_state" power_msgs::batterystate :timeout 1500))) - ;; You may fail to subscribe /battery_state - ;; because of message md5 difference between melodic and indigo. - (unless msg - (ros::ros-warn "Failed to subscribe /battery_state") - (ros::ros-warn "Skip undocking, so please make sure that Fetch is already undocked.") - (return)) - (setq *is-charging* (send msg :is_charging)) - (when *is-charging* - (undock) - ;; rotate after undock - (when undock-rotate (send *ri* :go-pos-unsafe 0 0 180))) - (unless *is-charging* (return)) - (if (eq i 2) (progn (send *ri* :speak "Fail to undock") (ros::ros-error "Fail to undock"))))) + (let ((undock-success nil)) + (block go-to-spot-undock + (dotimes (i 3) + (let* ((msg (one-shot-subscribe "/battery_state" power_msgs::batterystate :timeout 1500)) + (is-charging (send msg :is_charging))) + ;; You may fail to subscribe /battery_state + ;; because of message md5 difference between melodic and indigo. + (unless msg + (ros::ros-warn "Failed to subscribe /battery_state") + (ros::ros-warn "Skip undocking, so please make sure that Fetch is already undocked.") + (return-from go-to-spot-undock t)) + (if is-charging + (progn + (setq undock-success (auto-undock :n-trial 3)) + ;; rotate after undock + (if (and undock-success undock-rotate) + (send *ri* :go-pos-unsafe 0 0 180))) + (return-from go-to-spot-undock t)) + (if (not undock-success) + (progn + (ros::ros-error "Failed to undock") + (send *ri* :speak "I failed to undock.")))))) + (if (not undock-success) (return-from go-to-spot nil))) ;; go to spot (let* ((ret (get-spot-coords name)) (goal-pose (car ret)) (frame-id (cdr ret))) + (when relative-coords (setq goal-pose (send goal-pose :transform relative-coords :world))) (send *ri* :clear-costmap) @@ -127,6 +135,14 @@ (when success (return-from auto-dock success)))) success)) +(defun auto-undock (&key (n-trial 1)) + (let ((success nil)) + (dotimes (i n-trial) + (ros::ros-info "trying to do undock.") + (setq success (undock)) + (when success (return-from auto-undock success))) + success)) + (defun go-to-kitchen (&key (tweet t) (n-dock-trial 1) (n-kitchen-trial 1)) ;; go to kitchen (unless (boundp '*ri*) From 81e6a66489cbe487e79b0b802bafa4783703a10b Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 3 Apr 2021 17:09:49 +0900 Subject: [PATCH 30/54] disable and enable base breaker when undock is failed --- .../jsk_fetch_startup/euslisp/navigation-utils.l | 14 ++++++++++++++ 1 file changed, 14 insertions(+) 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 2b37e62a27..c56e50dc58 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -137,10 +137,24 @@ (defun auto-undock (&key (n-trial 1)) (let ((success nil)) + (unless (boundp '*ri*) + (require :fetch-interface "package://fetcheus/fetch-interface.l") + (fetch-init)) (dotimes (i n-trial) (ros::ros-info "trying to do undock.") (setq success (undock)) (when success (return-from auto-undock success))) + (if (not success) + (let ((enable-request (instance power_msgs::BreakerCommandRequest :init :enable t)) + (disable-request (instance power_msgs::BreakerCommandRequest :init :enable nil)) + (breaker-service-name "base_breaker")) + (ros::wait-for-service breaker-service-name 5) + (ros::service-call breaker-service-name disable-request) + (ros::ros-error "Diable base breaker") + (send *ri* :speak "I disable and enable base breaker.") + (unix::sleep 5) + (ros::service-call breaker-service-name enable-request) + (ros::ros-error "Enable base breaker"))) success)) (defun go-to-kitchen (&key (tweet t) (n-dock-trial 1) (n-kitchen-trial 1)) From 0f6c124e694d701dddcb1e4fb5aa4a99840ada85 Mon Sep 17 00:00:00 2001 From: JSK fetch user Date: Sun, 4 Apr 2021 15:01:22 +0900 Subject: [PATCH 31/54] [jsk_fetch_startup] fix parenthesis --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 c56e50dc58..6747fc347a 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -114,8 +114,8 @@ (if (not undock-success) (progn (ros::ros-error "Failed to undock") - (send *ri* :speak "I failed to undock.")))))) - (if (not undock-success) (return-from go-to-spot nil))) + (send *ri* :speak "I failed to undock."))))) + (if (not undock-success) (return-from go-to-spot nil)))) ;; go to spot (let* ((ret (get-spot-coords name)) (goal-pose (car ret)) From aa518f7ada74116a438f47dea126ce6b786c5fe0 Mon Sep 17 00:00:00 2001 From: JSK fetch user Date: Sun, 4 Apr 2021 15:03:02 +0900 Subject: [PATCH 32/54] [jsk_fetch_startup] fix coordinate values of go-to-kitchen --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 6747fc347a..2b2d9d0116 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -193,7 +193,7 @@ (dotimes (i n-kitchen-trial) (setq success-go-to-kitchen (go-to-spot "/eng2/7f/room73B2-sink-front0" - (make-coords :pos #f(100 -500 0)) :undock-rotate t)) + (make-coords :pos #f(400 -500 0)) :undock-rotate t)) (when success-go-to-kitchen (return))) (if success-go-to-kitchen (progn ;; succeeded to go to kitchen @@ -210,8 +210,7 @@ (when tweet (tweet-string "I took a photo at 73B2 Kitchen sink." :warning-time 3 :with-image "/edgetpu_object_detector/output/image" :speak t)) - (send *ri* :go-pos-unsafe 0 0 -90) - (send *ri* :go-pos-unsafe 0 0 -90)) + (send *ri* :go-pos-unsafe 0 0 135)) (progn ;; failed to go to kitchen (ros::ros-error "failed going to the kitchen.") (send *ri* :speak-jp "失敗しました。" :wait t))) From 19ddf6b894b8e5ffb98fd7cac918a5a670dd019f Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 5 Apr 2021 19:11:17 +0900 Subject: [PATCH 33/54] return exit code in go-to-kitchen.l --- .../jsk_fetch_startup/euslisp/go-to-kitchen.l | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l index d9c1f0fa25..171da03639 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l @@ -3,8 +3,10 @@ (require :fetch-interface "package://fetcheus/fetch-interface.l") (load "package://jsk_fetch_startup/euslisp/navigation-utils.l") +(defun main () + (fetch-init) + (send *ri* :clear-costmap) + (go-to-kitchen :tweet t :n-dock-trial 3 :n-kitchen-trial 3)) + (ros::roseus "go_to_kitchen") -(fetch-init) -(send *ri* :clear-costmap) -(go-to-kitchen :tweet t :n-dock-trial 5 :n-kitchen-trial 3) -(exit) +(if (main) (unix::exit 0) (unix::exit 1)) From bb646b28ede6052121cf21bfbaa0b22b9682bf8e Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 8 Apr 2021 16:36:23 +0900 Subject: [PATCH 34/54] check msg is nil in navigation-utils.l --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 2b2d9d0116..fbeaa9bf11 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -97,7 +97,7 @@ (block go-to-spot-undock (dotimes (i 3) (let* ((msg (one-shot-subscribe "/battery_state" power_msgs::batterystate :timeout 1500)) - (is-charging (send msg :is_charging))) + (is-charging (if msg (send msg :is_charging)))) ;; You may fail to subscribe /battery_state ;; because of message md5 difference between melodic and indigo. (unless msg From 24365bc283f338f2169b02446c35d8dd79a8cdc2 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Sat, 10 Apr 2021 16:38:00 +0900 Subject: [PATCH 35/54] Use output of check_room_light.py in navigation-utils.l --- .../euslisp/navigation-utils.l | 32 ++++++------------- 1 file changed, 10 insertions(+), 22 deletions(-) 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 fbeaa9bf11..2b6dd6f7f3 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -1,6 +1,7 @@ (load "package://jsk_robot_startup/lifelog/tweet_client.l") (ros::load-ros-manifest "fetch_auto_dock_msgs") +(ros::load-ros-manifest "jsk_robot_startup") (ros::load-ros-manifest "power_msgs") (defparameter *dock-action* nil) @@ -166,28 +167,15 @@ (send *ri* :speak-jp "キッチンに向かいます。" :wait t) (unix::sleep 1) ;; Check if the lights are on in the room - (let ((initial-light-on nil) - (success-go-to-kitchen nil) - (success-auto-dock nil)) - (let* ((ros-img (one-shot-subscribe "/head_camera/rgb/quater/image_rect_color" sensor_msgs::Image)) - (eus-img (ros::sensor_msgs/Image->image ros-img)) - (luminance 0) rgb-oct r g b) - (dotimes (w (send eus-img :width)) - (dotimes (h (send eus-img :height)) - (setq rgb-oct (send eus-img :pixel-hex-string w h)) - (setq r (read-from-string (format nil "#X~A" (subseq rgb-oct 0 2)))) - (setq g (read-from-string (format nil "#X~A" (subseq rgb-oct 2 4)))) - (setq b (read-from-string (format nil "#X~A" (subseq rgb-oct 4 6)))) - (setq luminance (+ luminance (+ (* 0.299 r) (* 0.587 g) (* 0.114 b)))))) - (setq luminance (/ luminance (* (send eus-img :width) (send eus-img :height)))) - (if (> luminance 50) - (setq initial-light-on t) - (setq initial-light-on nil)) - (if initial-light-on - (send *ri* :speak-jp "すでに電気がついています。" :wait t) - (progn - (send *ri* :speak-jp "オッケー、グーグル" :wait t) - (send *ri* :speak-jp "電気をつけて" :wait t)))) + (let* ((room-light-msg (one-shot-subscribe "/check_room_light/output" jsk_robot_startup::RoomLight)) + (initial-light-on (send room-light-msg :light_on)) + (success-go-to-kitchen nil) + (success-auto-dock nil)) + (if initial-light-on + (send *ri* :speak-jp "すでに電気がついています。" :wait t) + (progn + (send *ri* :speak-jp "オッケー、グーグル" :wait t) + (send *ri* :speak-jp "電気をつけて" :wait t)))) (unix::sleep 1) ;; stove (dotimes (i n-kitchen-trial) From 9e5e3db1699437c66b8746f1269ec163996dc399 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Sat, 10 Apr 2021 18:06:58 +0900 Subject: [PATCH 36/54] Check if roo-light-msg is nil or not --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 2b6dd6f7f3..0d51abfd86 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -168,7 +168,7 @@ (unix::sleep 1) ;; Check if the lights are on in the room (let* ((room-light-msg (one-shot-subscribe "/check_room_light/output" jsk_robot_startup::RoomLight)) - (initial-light-on (send room-light-msg :light_on)) + (initial-light-on (if room-light-msg (send room-light-msg :light_on))) (success-go-to-kitchen nil) (success-auto-dock nil)) (if initial-light-on From aada728384c38027050bb3cdb91ee10015032243 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Tue, 18 May 2021 19:43:51 +0900 Subject: [PATCH 37/54] Fetch speaks to Google Home in English to improve recognition accuracy --- .../jsk_fetch_startup/euslisp/navigation-utils.l | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) 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 0d51abfd86..510b955e11 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -174,8 +174,11 @@ (if initial-light-on (send *ri* :speak-jp "すでに電気がついています。" :wait t) (progn - (send *ri* :speak-jp "オッケー、グーグル" :wait t) - (send *ri* :speak-jp "電気をつけて" :wait t)))) + ;; The accuracy of Google Home recognition is better in English than in Japanese. + ;; (send *ri* :speak-jp "オッケー、グーグル" :wait t) + ;; (send *ri* :speak-jp "電気をつけて" :wait t) + (send *ri* :speak "OK, Google" :wait t) + (send *ri* :speak "Turn on the light." :wait t)))) (unix::sleep 1) ;; stove (dotimes (i n-kitchen-trial) @@ -224,7 +227,10 @@ (setq success-auto-dock (auto-dock :n-trial n-dock-trial)) (when (and success-auto-dock (not initial-light-on)) (unix::sleep 1) - (send *ri* :speak-jp "オッケー、グーグル" :wait t) - (send *ri* :speak-jp "電気を消して" :wait t)) + ;; The accuracy of Google Home recognition is better in English than in Japanese. + ;; (send *ri* :speak-jp "オッケー、グーグル" :wait t) + ;; (send *ri* :speak-jp "電気を消して" :wait t) + (send *ri* :speak "OK, Google" :wait t) + (send *ri* :speak "Turn off the light." :wait t)) (unix::sleep 1) (and success-go-to-kitchen success-auto-dock))) From 8d53cc387bbbf605a2f92eb274604579c5934031 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sun, 23 May 2021 00:03:07 +0900 Subject: [PATCH 38/54] fix typo in navigation-utils.l --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 510b955e11..75beff2800 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -178,7 +178,7 @@ ;; (send *ri* :speak-jp "オッケー、グーグル" :wait t) ;; (send *ri* :speak-jp "電気をつけて" :wait t) (send *ri* :speak "OK, Google" :wait t) - (send *ri* :speak "Turn on the light." :wait t)))) + (send *ri* :speak "Turn on the light." :wait t))) (unix::sleep 1) ;; stove (dotimes (i n-kitchen-trial) From c4df38bc9eb29dc26d1f49d6333205762355eafe Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 31 May 2021 16:03:06 +0900 Subject: [PATCH 39/54] wait until light off --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 75beff2800..22d8c56748 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -232,5 +232,5 @@ ;; (send *ri* :speak-jp "電気を消して" :wait t) (send *ri* :speak "OK, Google" :wait t) (send *ri* :speak "Turn off the light." :wait t)) - (unix::sleep 1) + (unix::sleep 5) (and success-go-to-kitchen success-auto-dock))) From 838fc5e4536e08257c2f54c839562621dbf41cb8 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 31 May 2021 16:29:26 +0900 Subject: [PATCH 40/54] change inflation radius in go-to-kitchen app --- .../euslisp/navigation-utils.l | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) 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 22d8c56748..f3d1f23ae8 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -170,7 +170,11 @@ (let* ((room-light-msg (one-shot-subscribe "/check_room_light/output" jsk_robot_startup::RoomLight)) (initial-light-on (if room-light-msg (send room-light-msg :light_on))) (success-go-to-kitchen nil) - (success-auto-dock nil)) + (success-auto-dock nil) + (orig-global-inflation-radius + (ros::get-param "/move_base/global_costmap/inflater/inflation_radius")) + (orig-local-inflation-radius + (ros::get-param "/move_base/local_costmap/inflater/inflation_radius"))) (if initial-light-on (send *ri* :speak-jp "すでに電気がついています。" :wait t) (progn @@ -180,6 +184,11 @@ (send *ri* :speak "OK, Google" :wait t) (send *ri* :speak "Turn on the light." :wait t))) (unix::sleep 1) + ;; change the inflation_radius + (ros::set-dynamic-reconfigure-param + "/move_base/global_costmap/inflater" "inflation_radius" :double 0.7) + (ros::set-dynamic-reconfigure-param + "/move_base/local_costmap/inflater" "inflation_radius" :double 0.7) ;; stove (dotimes (i n-kitchen-trial) (setq success-go-to-kitchen @@ -233,4 +242,11 @@ (send *ri* :speak "OK, Google" :wait t) (send *ri* :speak "Turn off the light." :wait t)) (unix::sleep 5) + ;; change the inflation_radius + (ros::set-dynamic-reconfigure-param + "/move_base/global_costmap/inflater" "inflation_radius" + :double orig-global-inflation-radius) + (ros::set-dynamic-reconfigure-param + "/move_base/local_costmap/inflater" "inflation_radius" + :double orig-local-inflation-radius) (and success-go-to-kitchen success-auto-dock))) From 764bf9c5c203e3dd176d852b33de5b109e7037e3 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Wed, 2 Jun 2021 01:36:23 +0900 Subject: [PATCH 41/54] Look at Google Home before talking in go-to-kitchen --- .../euslisp/navigation-utils.l | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) 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 f3d1f23ae8..52833a1a18 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -178,11 +178,18 @@ (if initial-light-on (send *ri* :speak-jp "すでに電気がついています。" :wait t) (progn + ;; Look at Google Home + (send *fetch* :head :neck-y :joint-angle 89) + (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) + (send *ri* :wait-interpolation) ;; The accuracy of Google Home recognition is better in English than in Japanese. ;; (send *ri* :speak-jp "オッケー、グーグル" :wait t) ;; (send *ri* :speak-jp "電気をつけて" :wait t) (send *ri* :speak "OK, Google" :wait t) - (send *ri* :speak "Turn on the light." :wait t))) + (send *ri* :speak "Turn on the light." :wait t) + (send *fetch* :head :neck-y :joint-angle 0) + (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) + (send *ri* :wait-interpolation))) (unix::sleep 1) ;; change the inflation_radius (ros::set-dynamic-reconfigure-param @@ -236,11 +243,18 @@ (setq success-auto-dock (auto-dock :n-trial n-dock-trial)) (when (and success-auto-dock (not initial-light-on)) (unix::sleep 1) + ;; Look at Google Home + (send *fetch* :head :neck-y :joint-angle 89) + (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) + (send *ri* :wait-interpolation) ;; The accuracy of Google Home recognition is better in English than in Japanese. ;; (send *ri* :speak-jp "オッケー、グーグル" :wait t) ;; (send *ri* :speak-jp "電気を消して" :wait t) (send *ri* :speak "OK, Google" :wait t) - (send *ri* :speak "Turn off the light." :wait t)) + (send *ri* :speak "Turn off the light." :wait t) + (send *fetch* :head :neck-y :joint-angle 0) + (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) + (send *ri* :wait-interpolation)) (unix::sleep 5) ;; change the inflation_radius (ros::set-dynamic-reconfigure-param From bb503bf22f11df538498307000d23081a16aba98 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Wed, 2 Jun 2021 01:36:41 +0900 Subject: [PATCH 42/54] Smaller inflation radius in go-to-kitchen --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 52833a1a18..707c980883 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -192,10 +192,12 @@ (send *ri* :wait-interpolation))) (unix::sleep 1) ;; change the inflation_radius + ;; Increase the global inflation to generate a path that is less sensitive to differences between the map shape and the actual object placement. (ros::set-dynamic-reconfigure-param "/move_base/global_costmap/inflater" "inflation_radius" :double 0.7) + ;; Decrease the local inflation to generate a path that allows the robot to pass close to the object. (ros::set-dynamic-reconfigure-param - "/move_base/local_costmap/inflater" "inflation_radius" :double 0.7) + "/move_base/local_costmap/inflater" "inflation_radius" :double 0.35) ;; stove (dotimes (i n-kitchen-trial) (setq success-go-to-kitchen From a549d53681c141f518bcd2734be6337bb9086460 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Thu, 3 Jun 2021 22:49:34 +0900 Subject: [PATCH 43/54] Update go-to-kitchen demo --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 2 ++ 1 file changed, 2 insertions(+) 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 707c980883..c86e2da730 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -180,6 +180,7 @@ (progn ;; Look at Google Home (send *fetch* :head :neck-y :joint-angle 89) + (send *fetch* :head :neck-p :joint-angle 25) (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) (send *ri* :wait-interpolation) ;; The accuracy of Google Home recognition is better in English than in Japanese. @@ -247,6 +248,7 @@ (unix::sleep 1) ;; Look at Google Home (send *fetch* :head :neck-y :joint-angle 89) + (send *fetch* :head :neck-p :joint-angle 25) (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) (send *ri* :wait-interpolation) ;; The accuracy of Google Home recognition is better in English than in Japanese. From 0390857472d316f73aa600c2dfb517a40da8b3d8 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 10 Jun 2021 18:45:47 +0900 Subject: [PATCH 44/54] check battery charging state at the end of kitchen demo --- .../euslisp/navigation-utils.l | 24 ++++++++++++++----- 1 file changed, 18 insertions(+), 6 deletions(-) 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 c86e2da730..8bbdee1d0d 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -89,6 +89,16 @@ (return-from undock nil)) (send (send *undock-action* :get-result) :undocked)) + +(defun check-battery-charging-state (&key (timeout 1500)) + (let* ((msg (one-shot-subscribe "/battery_state" power_msgs::batterystate :timeout timeout)) + (is-charging (send msg :is_charging))) + ;; You may fail to subscribe /battery_state + ;; because of message md5 difference between melodic and indigo. + (if (not msg) (return-from check-battery-charging-state nil)) + (if is-charging :charging :discharging))) + + (defun go-to-spot (name &optional (relative-coords (make-coords)) &key (undock-rotate nil)) ;; undock if fetch is docking (unless (boundp '*ri*) @@ -97,15 +107,14 @@ (let ((undock-success nil)) (block go-to-spot-undock (dotimes (i 3) - (let* ((msg (one-shot-subscribe "/battery_state" power_msgs::batterystate :timeout 1500)) - (is-charging (if msg (send msg :is_charging)))) + (let* ((battery-charging-state (check-battery-charging-state))) ;; You may fail to subscribe /battery_state ;; because of message md5 difference between melodic and indigo. - (unless msg + (unless battery-charging-state (ros::ros-warn "Failed to subscribe /battery_state") (ros::ros-warn "Skip undocking, so please make sure that Fetch is already undocked.") (return-from go-to-spot-undock t)) - (if is-charging + (if (equal battery-charging-state :charging) (progn (setq undock-success (auto-undock :n-trial 3)) ;; rotate after undock @@ -115,7 +124,7 @@ (if (not undock-success) (progn (ros::ros-error "Failed to undock") - (send *ri* :speak "I failed to undock."))))) + (send *ri* :speak "I failed to undock."))))) (if (not undock-success) (return-from go-to-spot nil)))) ;; go to spot (let* ((ret (get-spot-coords name)) @@ -171,6 +180,7 @@ (initial-light-on (if room-light-msg (send room-light-msg :light_on))) (success-go-to-kitchen nil) (success-auto-dock nil) + (success-battery-charging) (orig-global-inflation-radius (ros::get-param "/move_base/global_costmap/inflater/inflation_radius")) (orig-local-inflation-radius @@ -267,4 +277,6 @@ (ros::set-dynamic-reconfigure-param "/move_base/local_costmap/inflater" "inflation_radius" :double orig-local-inflation-radius) - (and success-go-to-kitchen success-auto-dock))) + (setq success-battery-charging + (equal (check-battery-charging-state) :charging)) + (and success-go-to-kitchen success-auto-dock success-battery-charging))) From 03262c777da65bbc6b4c864f5d5306fcf73c71eb Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Fri, 11 Jun 2021 12:16:44 +0900 Subject: [PATCH 45/54] check msg is nil or not before accessing slot --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 8bbdee1d0d..23bd35d41f 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -92,7 +92,7 @@ (defun check-battery-charging-state (&key (timeout 1500)) (let* ((msg (one-shot-subscribe "/battery_state" power_msgs::batterystate :timeout timeout)) - (is-charging (send msg :is_charging))) + (is-charging (if msg (send msg :is_charging)))) ;; You may fail to subscribe /battery_state ;; because of message md5 difference between melodic and indigo. (if (not msg) (return-from check-battery-charging-state nil)) From e8b6fab5f61c115d3d0c7b44d8e8a5ea83137502 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Thu, 17 Jun 2021 19:04:00 +0900 Subject: [PATCH 46/54] Turn on/off switchbots from switchbot_ros API --- .../euslisp/navigation-utils.l | 56 ++++++++++++------- 1 file changed, 36 insertions(+), 20 deletions(-) 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 23bd35d41f..052399baa6 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -1,4 +1,5 @@ (load "package://jsk_robot_startup/lifelog/tweet_client.l") +(load "package://switchbot_ros/scripts/switchbot.l") (ros::load-ros-manifest "fetch_auto_dock_msgs") (ros::load-ros-manifest "jsk_robot_startup") @@ -167,7 +168,7 @@ (ros::ros-error "Enable base breaker"))) success)) -(defun go-to-kitchen (&key (tweet t) (n-dock-trial 1) (n-kitchen-trial 1)) +(defun go-to-kitchen (&key (tweet t) (n-dock-trial 1) (n-kitchen-trial 1) (control-switchbot :api)) ;; go to kitchen (unless (boundp '*ri*) (require :fetch-interface "package://fetcheus/fetch-interface.l") @@ -187,20 +188,27 @@ (ros::get-param "/move_base/local_costmap/inflater/inflation_radius"))) (if initial-light-on (send *ri* :speak-jp "すでに電気がついています。" :wait t) - (progn + (cond + ((or (eq control-switchbot :en) (eq control-switchbot :jp)) ;; Look at Google Home (send *fetch* :head :neck-y :joint-angle 89) (send *fetch* :head :neck-p :joint-angle 25) (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) (send *ri* :wait-interpolation) ;; The accuracy of Google Home recognition is better in English than in Japanese. - ;; (send *ri* :speak-jp "オッケー、グーグル" :wait t) - ;; (send *ri* :speak-jp "電気をつけて" :wait t) - (send *ri* :speak "OK, Google" :wait t) - (send *ri* :speak "Turn on the light." :wait t) + (if (eq control-switchbot :en) + (progn + (send *ri* :speak "OK, Google" :wait t) + (send *ri* :speak "Turn on the light." :wait t)) + (progn + (send *ri* :speak-jp "オッケー、グーグル" :wait t) + (send *ri* :speak-jp "電気をつけて" :wait t))) (send *fetch* :head :neck-y :joint-angle 0) (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) - (send *ri* :wait-interpolation))) + (send *ri* :wait-interpolation)) + ((eq control-switchbot :api) + (control-device "/eng2/7f/73b2/light/upper/switch" "turnOn") + (control-device "/eng2/7f/73b2/light/lower/switch" "turnOn")))) (unix::sleep 1) ;; change the inflation_radius ;; Increase the global inflation to generate a path that is less sensitive to differences between the map shape and the actual object placement. @@ -256,19 +264,27 @@ (setq success-auto-dock (auto-dock :n-trial n-dock-trial)) (when (and success-auto-dock (not initial-light-on)) (unix::sleep 1) - ;; Look at Google Home - (send *fetch* :head :neck-y :joint-angle 89) - (send *fetch* :head :neck-p :joint-angle 25) - (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) - (send *ri* :wait-interpolation) - ;; The accuracy of Google Home recognition is better in English than in Japanese. - ;; (send *ri* :speak-jp "オッケー、グーグル" :wait t) - ;; (send *ri* :speak-jp "電気を消して" :wait t) - (send *ri* :speak "OK, Google" :wait t) - (send *ri* :speak "Turn off the light." :wait t) - (send *fetch* :head :neck-y :joint-angle 0) - (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) - (send *ri* :wait-interpolation)) + (cond + ((or (eq control-switchbot :en) (eq control-switchbot :jp)) + ;; Look at Google Home + (send *fetch* :head :neck-y :joint-angle 89) + (send *fetch* :head :neck-p :joint-angle 25) + (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) + (send *ri* :wait-interpolation) + ;; The accuracy of Google Home recognition is better in English than in Japanese. + (if (eq control-switchbot :en) + (progn + (send *ri* :speak "OK, Google" :wait t) + (send *ri* :speak "Turn off the light." :wait t)) + (progn + (send *ri* :speak-jp "オッケー、グーグル" :wait t) + (send *ri* :speak-jp "電気を消して" :wait t))) + (send *fetch* :head :neck-y :joint-angle 0) + (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) + (send *ri* :wait-interpolation)) + ((eq control-switchbot :api) + (control-device "/eng2/7f/73b2/light/upper/switch" "turnOff") + (control-device "/eng2/7f/73b2/light/lower/switch" "turnOff")))) (unix::sleep 5) ;; change the inflation_radius (ros::set-dynamic-reconfigure-param From d057754c4b8bd455d2186ac3eb8fca15a1e571d0 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Thu, 17 Jun 2021 19:06:14 +0900 Subject: [PATCH 47/54] wait and speak at switchbot control-device --- .../jsk_fetch_startup/euslisp/navigation-utils.l | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) 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 052399baa6..90a9338a60 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -207,8 +207,9 @@ (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) (send *ri* :wait-interpolation)) ((eq control-switchbot :api) - (control-device "/eng2/7f/73b2/light/upper/switch" "turnOn") - (control-device "/eng2/7f/73b2/light/lower/switch" "turnOn")))) + (control-device "/eng2/7f/73b2/light/upper/switch" "turnOn" :wait t) + (control-device "/eng2/7f/73b2/light/lower/switch" "turnOn" :wait t) + (send *ri* :speak-jp "電気をつけました" :wait t)))) (unix::sleep 1) ;; change the inflation_radius ;; Increase the global inflation to generate a path that is less sensitive to differences between the map shape and the actual object placement. @@ -283,8 +284,9 @@ (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) (send *ri* :wait-interpolation)) ((eq control-switchbot :api) - (control-device "/eng2/7f/73b2/light/upper/switch" "turnOff") - (control-device "/eng2/7f/73b2/light/lower/switch" "turnOff")))) + (control-device "/eng2/7f/73b2/light/upper/switch" "turnOff" :wait t) + (control-device "/eng2/7f/73b2/light/lower/switch" "turnOff" :wait t) + (send *ri* :speak-jp "電気を消しました" :wait t)))) (unix::sleep 5) ;; change the inflation_radius (ros::set-dynamic-reconfigure-param From 74cd9f03cd14291670b466471ce1f05d4a460e73 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 19 Jun 2021 16:50:21 +0900 Subject: [PATCH 48/54] refactor navigation-utils to add functions --- .../euslisp/navigation-utils.l | 304 +++++++++++------- 1 file changed, 180 insertions(+), 124 deletions(-) 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 90a9338a60..91dcfd623f 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -21,6 +21,38 @@ (t nil)))) +(defun store-params () + (defparameter *global-inflation-radius* + (ros::get-param "/move_base/global_costmap/inflater/inflation_radius")) + (defparameter *local-inflation-radius* + (ros::get-param "/move_base/local_costmap/inflater/inflation_radius")) + t) + + +(defun restore-params () + (if (boundp '*global-inflation-radius*) + (ros::set-dynamic-reconfigure-param + "/move_base/global_costmap/inflater" "inflation_radius" + :double *global-inflation-radius*)) + (if (boundp '*local-inflation-radius*) + (ros::set-dynamic-reconfigure-param + "/move_base/local_costmap/inflater" "inflation_radius" + :double *local-inflation-radius*)) + t) + + +(defun inflation-loose () + ;; Increase the global inflation to generate a path + ;; that is less sensitive to differences between the map shape and the actual object placement. + (ros::set-dynamic-reconfigure-param + "/move_base/global_costmap/inflater" "inflation_radius" :double 0.7) + ;; Decrease the local inflation to generate a path + ;; that allows the robot to pass close to the object. + (ros::set-dynamic-reconfigure-param + "/move_base/local_costmap/inflater" "inflation_radius" :double 0.35) + t) + + (defun get-spot-coords (name) (unless *spots* (setq *spots* (one-shot-subscribe "/spots_marker_array" visualization_msgs::MarkerArray))) @@ -34,6 +66,7 @@ (setq spot-coords (ros::tf-pose->coords spot-coords)) (cons spot-coords frame-id))) + (defun simple-dock (&key (use-pose t)) (unless *dock-action* (setq *dock-action* @@ -63,6 +96,7 @@ (return-from simple-dock nil)) (send (send *dock-action* :get-result) :docked))) + (defun dock () ;; look down (unless (boundp '*ri*) @@ -74,6 +108,7 @@ (send *ri* :wait-interpolation) (simple-dock)) + (defun undock () (unless *undock-action* (setq *undock-action* @@ -82,7 +117,6 @@ (unless (send *undock-action* :wait-for-server 5) (ros::ros-error "/undock action server is not started") (return-from undock nil)) - (send *undock-action* :send-goal (instance fetch_auto_dock_msgs::UndockActionGoal :init)) (unless (send *undock-action* :wait-for-result :timeout 60) @@ -91,12 +125,12 @@ (send (send *undock-action* :get-result) :undocked)) -(defun check-battery-charging-state (&key (timeout 1500)) +(defun get-battery-charging-state (&key (timeout 1500)) (let* ((msg (one-shot-subscribe "/battery_state" power_msgs::batterystate :timeout timeout)) (is-charging (if msg (send msg :is_charging)))) ;; You may fail to subscribe /battery_state ;; because of message md5 difference between melodic and indigo. - (if (not msg) (return-from check-battery-charging-state nil)) + (if (not msg) (return-from get-battery-charging-state nil)) (if is-charging :charging :discharging))) @@ -108,7 +142,7 @@ (let ((undock-success nil)) (block go-to-spot-undock (dotimes (i 3) - (let* ((battery-charging-state (check-battery-charging-state))) + (let* ((battery-charging-state (get-battery-charging-state))) ;; You may fail to subscribe /battery_state ;; because of message md5 difference between melodic and indigo. (unless battery-charging-state @@ -131,12 +165,12 @@ (let* ((ret (get-spot-coords name)) (goal-pose (car ret)) (frame-id (cdr ret))) - (when relative-coords (setq goal-pose (send goal-pose :transform relative-coords :world))) (send *ri* :clear-costmap) (send *ri* :move-to goal-pose :frame-id frame-id))) + (defun auto-dock (&key (n-trial 1)) (let ((success nil)) (dotimes (i n-trial) @@ -146,6 +180,7 @@ (when success (return-from auto-dock success)))) success)) + (defun auto-undock (&key (n-trial 1)) (let ((success nil)) (unless (boundp '*ri*) @@ -168,133 +203,154 @@ (ros::ros-error "Enable base breaker"))) success)) -(defun go-to-kitchen (&key (tweet t) (n-dock-trial 1) (n-kitchen-trial 1) (control-switchbot :api)) + +(defun get-light-on () + (let* ((room-light-msg + (one-shot-subscribe "/check_room_light/output" jsk_robot_startup::RoomLight)) + (light-on (if room-light-msg (send room-light-msg :light_on)))) + light-on)) + + +(defun room-light-on (&key (control-switchbot :api)) + (cond + ((or (eq control-switchbot :en) (eq control-switchbot :jp)) + ;; Look at Google Home + (send *fetch* :head :neck-y :joint-angle 89) + (send *fetch* :head :neck-p :joint-angle 25) + (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) + (send *ri* :wait-interpolation) + ;; The accuracy of Google Home recognition is better in English than in Japanese. + (if (eq control-switchbot :en) + (progn + (send *ri* :speak "OK, Google" :wait t) + (send *ri* :speak "Turn on the light." :wait t)) + (progn + (send *ri* :speak-jp "オッケー、グーグル" :wait t) + (send *ri* :speak-jp "電気をつけて" :wait t))) + (send *fetch* :head :neck-y :joint-angle 0) + (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) + (send *ri* :wait-interpolation)) + ((eq control-switchbot :api) + (control-device "/eng2/7f/73b2/light/upper/switch" "turnOn" :wait t) + (control-device "/eng2/7f/73b2/light/lower/switch" "turnOn" :wait t) + (send *ri* :speak-jp "電気をつけました" :wait t)))) + + +(defun room-light-off (&key (control-switchbot :api)) + (cond + ((or (eq control-switchbot :en) (eq control-switchbot :jp)) + ;; Look at Google Home + (send *fetch* :head :neck-y :joint-angle 89) + (send *fetch* :head :neck-p :joint-angle 25) + (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) + (send *ri* :wait-interpolation) + ;; The accuracy of Google Home recognition is better in English than in Japanese. + (if (eq control-switchbot :en) + (progn + (send *ri* :speak "OK, Google" :wait t) + (send *ri* :speak "Turn off the light." :wait t)) + (progn + (send *ri* :speak-jp "オッケー、グーグル" :wait t) + (send *ri* :speak-jp "電気を消して" :wait t))) + (send *fetch* :head :neck-y :joint-angle 0) + (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) + (send *ri* :wait-interpolation)) + ((eq control-switchbot :api) + (control-device "/eng2/7f/73b2/light/upper/switch" "turnOff" :wait t) + (control-device "/eng2/7f/73b2/light/lower/switch" "turnOff" :wait t) + (send *ri* :speak-jp "電気を消しました" :wait t)))) + + +(defun report-auto-dock () + (ros::ros-info "start going back to the dock.") + (send *ri* :speak-jp "ドックに戻ります。" :wait t)) + + +(defun report-move-to-sink-front-success () + (ros::ros-info "arrived at the kitchen stove.") + (send *ri* :speak-jp "キッチンのコンロの前につきました。" :wait t)) + + +(defun report-move-to-sink-front-failure () + (ros::ros-error "failed going to the kitchen.") + (send *ri* :speak-jp "キッチンのコンロの前に行くのに失敗しました。" :wait t)) + + +(defun report-start-go-to-kitchen () + (ros::ros-info "start going to the kitchen.") + (send *ri* :speak-jp "キッチンに向かいます。" :wait t)) + + +(defun report-light-on () + (ros::ros-info "room light is already on.") + (send *ri* :speak-jp "すでに電気がついています。" :wait t)) + + +(defun report-light-off () + (ros::ros-info "room light is off.") + (send *ri* :speak-jp "電気が消えています。" :wait t)) + + +(defun inspect-kitchen (&key (tweet t)) + (report-move-to-sink-front-success) + (if tweet + (progn + ;; stove + (tweet-string "I took a photo at 73B2 Kitchen stove." :warning-time 3 + :with-image "/edgetpu_object_detector/output/image" :speak t) + (send *ri* :go-pos-unsafe 0 0 -45) + ;; sink + (tweet-string "I took a photo at 73B2 Kitchen sink." :warning-time 3 + :with-image "/edgetpu_object_detector/output/image" :speak t) + (send *ri* :go-pos-unsafe 0 0 135)) + (progn + (send *ri* :go-pos-unsafe 0 0 90)))) + + +(defun move-to-sink-front (&key (n-trial 1) (offset #f(400 -500 0))) + (let ((success-move-to-sink-front nil)) + (dotimes (i n-trial) + (setq success-move-to-sink-front + (go-to-spot "/eng2/7f/room73B2-sink-front0" + (make-coords :pos offset) :undock-rotate t)) + (when success-move-to-sink-front (return))) + success-move-to-sink-front)) + + +(defun go-to-kitchen (&key (tweet t) (n-dock-trial 1) (n-kitchen-trial 1) + (control-switchbot :api)) ;; go to kitchen (unless (boundp '*ri*) (require :fetch-interface "package://fetcheus/fetch-interface.l") (fetch-init)) - (ros::ros-info "start going to the kitchen.") - (send *ri* :speak-jp "キッチンに向かいます。" :wait t) - (unix::sleep 1) + (report-start-go-to-kitchen) ;; Check if the lights are on in the room - (let* ((room-light-msg (one-shot-subscribe "/check_room_light/output" jsk_robot_startup::RoomLight)) - (initial-light-on (if room-light-msg (send room-light-msg :light_on))) - (success-go-to-kitchen nil) - (success-auto-dock nil) - (success-battery-charging) - (orig-global-inflation-radius - (ros::get-param "/move_base/global_costmap/inflater/inflation_radius")) - (orig-local-inflation-radius - (ros::get-param "/move_base/local_costmap/inflater/inflation_radius"))) + (let ((initial-light-on (get-light-on)) + (success-go-to-kitchen nil) + (success-auto-dock nil) + (success-battery-charging nil)) + (store-params) + ;; turn on light (if initial-light-on - (send *ri* :speak-jp "すでに電気がついています。" :wait t) - (cond - ((or (eq control-switchbot :en) (eq control-switchbot :jp)) - ;; Look at Google Home - (send *fetch* :head :neck-y :joint-angle 89) - (send *fetch* :head :neck-p :joint-angle 25) - (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) - (send *ri* :wait-interpolation) - ;; The accuracy of Google Home recognition is better in English than in Japanese. - (if (eq control-switchbot :en) - (progn - (send *ri* :speak "OK, Google" :wait t) - (send *ri* :speak "Turn on the light." :wait t)) - (progn - (send *ri* :speak-jp "オッケー、グーグル" :wait t) - (send *ri* :speak-jp "電気をつけて" :wait t))) - (send *fetch* :head :neck-y :joint-angle 0) - (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) - (send *ri* :wait-interpolation)) - ((eq control-switchbot :api) - (control-device "/eng2/7f/73b2/light/upper/switch" "turnOn" :wait t) - (control-device "/eng2/7f/73b2/light/lower/switch" "turnOn" :wait t) - (send *ri* :speak-jp "電気をつけました" :wait t)))) - (unix::sleep 1) + (report-light-on) + (progn + (report-light-off) + (room-light-on :control-switchbot control-switchbot))) ;; change the inflation_radius - ;; Increase the global inflation to generate a path that is less sensitive to differences between the map shape and the actual object placement. - (ros::set-dynamic-reconfigure-param - "/move_base/global_costmap/inflater" "inflation_radius" :double 0.7) - ;; Decrease the local inflation to generate a path that allows the robot to pass close to the object. - (ros::set-dynamic-reconfigure-param - "/move_base/local_costmap/inflater" "inflation_radius" :double 0.35) - ;; stove - (dotimes (i n-kitchen-trial) - (setq success-go-to-kitchen - (go-to-spot "/eng2/7f/room73B2-sink-front0" - (make-coords :pos #f(400 -500 0)) :undock-rotate t)) - (when success-go-to-kitchen (return))) + (inflation-loose) + ;; go to kitchen sink + (setq success-go-to-kitchen (move-to-sink-front :n-trial n-kitchen-trial)) + ;; report result to go to kitchen (if success-go-to-kitchen - (progn ;; succeeded to go to kitchen - (unix:sleep 1) - (ros::ros-info "arrived at the kitchen stove.") - (send *ri* :speak-jp "キッチンのコンロの前につきました。" :wait t) - (unix:sleep 1) - ;; stove - (when tweet - (tweet-string "I took a photo at 73B2 Kitchen stove." :warning-time 3 - :with-image "/edgetpu_object_detector/output/image" :speak t)) - (send *ri* :go-pos-unsafe 0 0 -45) - ;; sink - (when tweet - (tweet-string "I took a photo at 73B2 Kitchen sink." :warning-time 3 - :with-image "/edgetpu_object_detector/output/image" :speak t)) - (send *ri* :go-pos-unsafe 0 0 135)) - (progn ;; failed to go to kitchen - (ros::ros-error "failed going to the kitchen.") - (send *ri* :speak-jp "失敗しました。" :wait t))) - ;; sink - ;; (send *ri* :speak-jp "シンクに向かいます。") - ;; (if (go-to-spot "/eng2/7f/room73B2-sink-front1" (make-coords :pos #f(100 -200 0))) - ;; (progn ;; succeeded to go to kitchen - ;; (unix:sleep 1) - ;; (ros::ros-info "arrived at the kitchen.") - ;; (send *ri* :speak-jp "キッチンのシンクの前につきました。" :wait t) - ;; (unix:sleep 1) - ;; (when tweet - ;; (tweet-string "I took a photo at 73B2 Kitchen sink." :warning-time 3 - ;; :with-image "/edgetpu_object_detector/output/image" :speak t))) - ;; (progn ;; failed to go to kitchen - ;; (ros::ros-error "failed going to the kitchen.") - ;; (send *ri* :speak-jp "失敗しました。ドックに戻ります。" :wait t) - ;; (ros::ros-info "start going back to the dock.") - ;; (auto-dock :n-trial n-dock-trial) - ;; (return-from go-to-kitchen nil))) - (ros::ros-info "start going back to the dock.") - (send *ri* :speak-jp "ドックに戻ります。" :wait t) + (inspect-kitchen :tweet tweet) + (report-move-to-sink-front-failure)) + ;; go back from dock + (report-auto-dock) (setq success-auto-dock (auto-dock :n-trial n-dock-trial)) - (when (and success-auto-dock (not initial-light-on)) - (unix::sleep 1) - (cond - ((or (eq control-switchbot :en) (eq control-switchbot :jp)) - ;; Look at Google Home - (send *fetch* :head :neck-y :joint-angle 89) - (send *fetch* :head :neck-p :joint-angle 25) - (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) - (send *ri* :wait-interpolation) - ;; The accuracy of Google Home recognition is better in English than in Japanese. - (if (eq control-switchbot :en) - (progn - (send *ri* :speak "OK, Google" :wait t) - (send *ri* :speak "Turn off the light." :wait t)) - (progn - (send *ri* :speak-jp "オッケー、グーグル" :wait t) - (send *ri* :speak-jp "電気を消して" :wait t))) - (send *fetch* :head :neck-y :joint-angle 0) - (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) - (send *ri* :wait-interpolation)) - ((eq control-switchbot :api) - (control-device "/eng2/7f/73b2/light/upper/switch" "turnOff" :wait t) - (control-device "/eng2/7f/73b2/light/lower/switch" "turnOff" :wait t) - (send *ri* :speak-jp "電気を消しました" :wait t)))) - (unix::sleep 5) + ;; turn off light + (if (and success-auto-dock (not initial-light-on)) + (room-light-off :control-switchbot control-switchbot)) ;; change the inflation_radius - (ros::set-dynamic-reconfigure-param - "/move_base/global_costmap/inflater" "inflation_radius" - :double orig-global-inflation-radius) - (ros::set-dynamic-reconfigure-param - "/move_base/local_costmap/inflater" "inflation_radius" - :double orig-local-inflation-radius) - (setq success-battery-charging - (equal (check-battery-charging-state) :charging)) + (restore-params) + (setq success-battery-charging (equal (get-battery-charging-state) :charging)) (and success-go-to-kitchen success-auto-dock success-battery-charging))) From 8b537dc749f769fb9d5c089a0cf8e2b7a6d12deb Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 19 Jun 2021 17:13:41 +0900 Subject: [PATCH 49/54] chmod +x, shebang and add exit at last --- jsk_fetch_robot/jsk_fetch_startup/euslisp/auto-dock.l | 2 +- jsk_fetch_robot/jsk_fetch_startup/euslisp/call-k-okada.l | 4 ++-- jsk_fetch_robot/jsk_fetch_startup/euslisp/dock.l | 4 +++- .../jsk_fetch_startup/euslisp/fetch-look-at-human.l | 1 + .../euslisp/go-to-kitchen-with-mail-notify.l | 1 + jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l | 0 jsk_fetch_robot/jsk_fetch_startup/euslisp/speak-battery.l | 0 jsk_fetch_robot/jsk_fetch_startup/euslisp/tweet.l | 2 +- jsk_fetch_robot/jsk_fetch_startup/euslisp/undock.l | 4 +++- 9 files changed, 12 insertions(+), 6 deletions(-) mode change 100644 => 100755 jsk_fetch_robot/jsk_fetch_startup/euslisp/auto-dock.l mode change 100644 => 100755 jsk_fetch_robot/jsk_fetch_startup/euslisp/call-k-okada.l mode change 100644 => 100755 jsk_fetch_robot/jsk_fetch_startup/euslisp/dock.l mode change 100644 => 100755 jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l mode change 100644 => 100755 jsk_fetch_robot/jsk_fetch_startup/euslisp/speak-battery.l mode change 100644 => 100755 jsk_fetch_robot/jsk_fetch_startup/euslisp/tweet.l mode change 100644 => 100755 jsk_fetch_robot/jsk_fetch_startup/euslisp/undock.l diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/auto-dock.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/auto-dock.l old mode 100644 new mode 100755 index 10ca08b546..fa4dbaa212 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/auto-dock.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/auto-dock.l @@ -7,4 +7,4 @@ (fetch-init) (send *ri* :clear-costmap) (auto-dock) -(exit) +(sys::exit 0) diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/call-k-okada.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/call-k-okada.l old mode 100644 new mode 100755 index e215325c5c..69dda0e3b2 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/call-k-okada.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/call-k-okada.l @@ -1,5 +1,4 @@ -;; go-dock.l -;; Author: Yuki Furuta +#!/usr/bin/env roseus (ros::roseus "call_k_okada") (load "package://jsk_fetch_startup/euslisp/navigation-utils.l") @@ -16,3 +15,4 @@ (auto-dock)) (call-k-okada) +(sys::exit 0) diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/dock.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/dock.l old mode 100644 new mode 100755 index 4cd923d501..7031b1fa46 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/dock.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/dock.l @@ -1,5 +1,7 @@ +#!/usr/bin/env roseus + (load "package://jsk_fetch_startup/euslisp/navigation-utils.l") (ros::roseus "fetch_dock") (simple-dock) -(exit) +(sys::exit 0) diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/fetch-look-at-human.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/fetch-look-at-human.l index b3b6127a4f..ab6c2e9bd1 100755 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/fetch-look-at-human.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/fetch-look-at-human.l @@ -7,3 +7,4 @@ (ros::roseus "fetch_look_at_human") (fetch-init) (main *fetch* (send *fetch* :head-rgb)) +(sys::exit 0) diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen-with-mail-notify.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen-with-mail-notify.l index 714e6a1a88..7bb07c9f17 100755 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen-with-mail-notify.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen-with-mail-notify.l @@ -56,3 +56,4 @@ (auto-dock)) (go-to-kitchen) +(sys::exit 0) diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l old mode 100644 new mode 100755 diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/speak-battery.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/speak-battery.l old mode 100644 new mode 100755 diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/tweet.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/tweet.l old mode 100644 new mode 100755 index 786f5d988b..40e8283bf5 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/tweet.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/tweet.l @@ -3,4 +3,4 @@ (load "package://jsk_robot_startup/lifelog/tweet_client.l") (tweet-string "I took a photo." :warning-time 3 :with-image t :speak t) -(exit) +(sys::exit 0) diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/undock.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/undock.l old mode 100644 new mode 100755 index 764142eb10..d29cfc6a2c --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/undock.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/undock.l @@ -1,5 +1,7 @@ +#!/usr/bin/env roseus + (load "package://jsk_fetch_startup/euslisp/navigation-utils.l") (ros::roseus "fetch_undock") (undock) -(exit) +(sys::exit 0) From 2be649c1403d53fe8f6c5cb830711717466d3dac Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 19 Jun 2021 18:38:38 +0900 Subject: [PATCH 50/54] use smach state machine in go-to-kitchen.l --- .../jsk_fetch_startup/euslisp/go-to-kitchen.l | 16 +++- .../euslisp/navigation-utils.l | 88 +++++++++++++++++++ 2 files changed, 100 insertions(+), 4 deletions(-) diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l index 171da03639..24bcd3cd75 100755 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l @@ -3,10 +3,18 @@ (require :fetch-interface "package://fetcheus/fetch-interface.l") (load "package://jsk_fetch_startup/euslisp/navigation-utils.l") -(defun main () - (fetch-init) - (send *ri* :clear-costmap) - (go-to-kitchen :tweet t :n-dock-trial 3 :n-kitchen-trial 3)) +(defun main (&key (tweet t) (n-dock-trial 3) (n-kitchen-trial 3) (control-switchbot :api)) + (when (not (boundp '*sm*)) + (go-to-kitchen-state-machine)) + (exec-state-machine + *sm* `((tweet . ,tweet) + (n-kitchen-trial . ,n-kitchen-trial) + (n-dock-trial . ,n-dock-trial) + (control-switchbot . ,control-switchbot) + (initial-light-on . nil) + (success-go-to-kitchen . nil) + (success-auto-dock . nil)) + :hz 2.0)) (ros::roseus "go_to_kitchen") (if (main) (unix::exit 0) (unix::exit 1)) 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 91dcfd623f..477d911beb 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -1,5 +1,8 @@ (load "package://jsk_robot_startup/lifelog/tweet_client.l") (load "package://switchbot_ros/scripts/switchbot.l") +(require :state-machine "package://roseus_smach/src/state-machine.l") +(require :state-machine-ros "package://roseus_smach/src/state-machine-ros.l") +(require :state-machine-utils "package://roseus_smach/src/state-machine-utils.l") (ros::load-ros-manifest "fetch_auto_dock_msgs") (ros::load-ros-manifest "jsk_robot_startup") @@ -354,3 +357,88 @@ (restore-params) (setq success-battery-charging (equal (get-battery-charging-state) :charging)) (and success-go-to-kitchen success-auto-dock success-battery-charging))) + + +(defun go-to-kitchen-state-machine () + (setq *sm* + (make-state-machine + '((:init -> :report-start-go-to-kitchen) + (:report-start-go-to-kitchen -> :get-light-on) + (:get-light-on -> :report-light-on) + (:get-light-on !-> :room-light-on) + (:report-light-on -> :move-to-sink-front) + (:room-light-on -> :move-to-sink-front) + (:move-to-sink-front -> :inspect-kitchen) + (:move-to-sink-front !-> :report-move-to-sink-front-failure) + (:inspect-kitchen -> :auto-dock) + (:report-move-to-sink-front-failure -> :auto-dock) + (:auto-dock -> :room-light-off) + (:room-light-off -> :finish) + (:finish -> t) + (:finish !-> nil)) + '((:init + '(lambda (userdata) + (fetch-init) + (send *ri* :clear-costmap) + (store-params) + (inflation-loose) + t)) + (:report-start-go-to-kitchen + '(lambda (userdata) + (report-start-go-to-kitchen) + t)) + (:get-light-on + '(lambda (userdata) + (let ((light-on (get-light-on))) + (setf (cdr (assoc 'initial-light-on userdata)) light-on) + light-on))) + (:report-light-on + '(lambda (userdata) + (report-light-on) + t)) + (:room-light-on + '(lambda (userdata) + (let ((control-switchbot (cdr (assoc 'control-switchbot userdata)))) + (report-light-off) + (room-light-on :control-switchbot control-switchbot) + t))) + (:move-to-sink-front + '(lambda (userdata) + (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) + success))) + (:inspect-kitchen + '(lambda (userdata) + (inspect-kitchen :tweet (cdr (assoc 'tweet userdata))) + t)) + (:report-move-to-sink-front-failure + '(lambda (userdata) + (report-move-to-sink-front-failure) + t)) + (:auto-dock + '(lambda (userdata) + (report-auto-dock) + (let* ((n-trial (cdr (assoc 'n-dock-trial userdata))) + (success (auto-dock :n-trial n-trial))) + (setf (cdr (assoc 'success-auto-dock userdata)) success) + success))) + (:room-light-off + '(lambda (userdata) + (let ((success-auto-dock (cdr (assoc 'success-auto-dock userdata))) + (initial-light-on (cdr (assoc 'initial-light-on userdata))) + (control-switchbot (cdr (assoc 'control-switchbot userdata)))) + (if (and success-auto-dock (not initial-light-on)) + (room-light-off :control-switchbot control-switchbot))) + t)) + (:finish + '(lambda (userdata) + (let ((success-battery-charging + (equal (get-battery-charging-state) :charging)) + (success-auto-dock (cdr (assoc 'success-auto-dock userdata))) + (success-go-to-kitchen + (cdr (assoc 'success-go-to-kitchen userdata)))) + (restore-params) + (and success-go-to-kitchen success-auto-dock success-battery-charging))))) + '(:init) + '(t nil)))) From 9641962317fdef1635094697a46b3e260a3e13d9 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 23 Jun 2021 12:37:06 +0900 Subject: [PATCH 51/54] return name of result state in go-to-kitchen-smach --- .../jsk_fetch_startup/euslisp/go-to-kitchen.l | 21 +++++++++++-------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l index 24bcd3cd75..f7addae82d 100755 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l @@ -3,18 +3,21 @@ (require :fetch-interface "package://fetcheus/fetch-interface.l") (load "package://jsk_fetch_startup/euslisp/navigation-utils.l") + (defun main (&key (tweet t) (n-dock-trial 3) (n-kitchen-trial 3) (control-switchbot :api)) (when (not (boundp '*sm*)) (go-to-kitchen-state-machine)) - (exec-state-machine - *sm* `((tweet . ,tweet) - (n-kitchen-trial . ,n-kitchen-trial) - (n-dock-trial . ,n-dock-trial) - (control-switchbot . ,control-switchbot) - (initial-light-on . nil) - (success-go-to-kitchen . nil) - (success-auto-dock . nil)) - :hz 2.0)) + (let ((result-state + (exec-state-machine *sm* `((tweet . ,tweet) + (n-kitchen-trial . ,n-kitchen-trial) + (n-dock-trial . ,n-dock-trial) + (control-switchbot . ,control-switchbot) + (initial-light-on . nil) + (success-go-to-kitchen . nil) + (success-auto-dock . nil)) + :hz 2.0))) + (send result-state :name))) + (ros::roseus "go_to_kitchen") (if (main) (unix::exit 0) (unix::exit 1)) From 8376c432b731d689287e13692e5f6346af015327 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 10 Jul 2021 18:34:05 +0900 Subject: [PATCH 52/54] add light-off.l --- jsk_fetch_robot/jsk_fetch_startup/euslisp/light-off.l | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100755 jsk_fetch_robot/jsk_fetch_startup/euslisp/light-off.l diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/light-off.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/light-off.l new file mode 100755 index 0000000000..9fdef16494 --- /dev/null +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/light-off.l @@ -0,0 +1,9 @@ +#!/usr/bin/env roseus + +(require :fetch-interface "package://fetcheus/fetch-interface.l") +(load "package://jsk_fetch_startup/euslisp/navigation-utils.l") + +(ros::roseus "light_off") +(fetch-init) +(room-light-off :control-switchbot :api) +(sys::exit 0) From b6602cfe39dea2f8608d9b677c49e3d241e90eea Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 10 Jul 2021 18:34:18 +0900 Subject: [PATCH 53/54] add light-on.l --- jsk_fetch_robot/jsk_fetch_startup/euslisp/light-on.l | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100755 jsk_fetch_robot/jsk_fetch_startup/euslisp/light-on.l diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/light-on.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/light-on.l new file mode 100755 index 0000000000..9a9f2e86de --- /dev/null +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/light-on.l @@ -0,0 +1,9 @@ +#!/usr/bin/env roseus + +(require :fetch-interface "package://fetcheus/fetch-interface.l") +(load "package://jsk_fetch_startup/euslisp/navigation-utils.l") + +(ros::roseus "light_on") +(fetch-init) +(room-light-on :control-switchbot :api) +(sys::exit 0) From 4de2866df27f736baa1a1178a74d758cb9a6c917 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 24 Dec 2020 21:03:24 +0900 Subject: [PATCH 54/54] exit in speak-battery ap --- jsk_fetch_robot/jsk_fetch_startup/euslisp/speak-battery.l | 1 + 1 file changed, 1 insertion(+) diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/speak-battery.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/speak-battery.l index a2e96176a9..bee0869447 100755 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/speak-battery.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/speak-battery.l @@ -19,3 +19,4 @@ (ros::roseus "speak_battery") (speak-battery) +(sys::exit 0)