From 8449a8765b02a86f368563ee9ca60bdee01d9e9e Mon Sep 17 00:00:00 2001 From: Naoto Tsukamoto Date: Wed, 15 Sep 2021 13:43:18 +0900 Subject: [PATCH] update trashcan front position --- .../jsk_fetch_startup/euslisp/navigation-utils.l | 16 ++++++++++++---- 1 file changed, 12 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 32ea5097e11..d3cf1b7ca33 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -358,13 +358,21 @@ Args: (send *ri* :wait-interpolation) (if tweet (progn - (tweet-string "I took a photo of 73B2 trash cans." :warning-time 3 - :with-image "/edgetpu_object_detector/output/image" :speak t) - (notify-recognition :location "trash cans")) + (tweet-string "I took a photo of 73B2 trash cans." :warning-time 3 + :with-image "/edgetpu_object_detector/output/image" :speak t) + (notify-recognition :location "trash cans") + (send *ri* :go-pos-unsafe 0.2 0 0) + (send *fetch* :head :neck-p :joint-angle 50) + (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller) + (send *ri* :wait-interpolation) + (tweet-string "I took a photo of 73B2 trash can inside." :warning-time 3 + :with-image "/edgetpu_object_detector/output/image" :speak t) + (notify-recognition :location "trash cans inside") + (send *ri* :go-pos-unsafe -0.2 0 0)) (progn (notify-recognition :location "trash cans")))) -(defun move-to-trashcan-front (&key (n-trial 1) (offset #f(-300 -300 0))) +(defun move-to-trashcan-front (&key (n-trial 1) (offset #f(-400 -300 0))) (let ((success-move-to-trashcan-front nil)) (dotimes (i n-trial) (setq success-move-to-trashcan-front