From 09140c57d05e914dc4c672e9f0f41918dde1d92a Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 17 Oct 2019 20:31:05 +0900 Subject: [PATCH 1/6] use proper distance for dock --- 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 a9e64485b1..c0c0eb17fa 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -32,7 +32,7 @@ (require :fetch-interface "package://fetcheus/fetch-interface.l") (fetch-init)) (send *fetch* :angle-vector (send *ri* :state :potentio-vector)) - (send *fetch* :head :look-at (float-vector 1500 0 500)) + (send *fetch* :head :look-at (float-vector 800 0 500)) (send *ri* :angle-vector (send *fetch* :angle-vector) 1000) (send *ri* :wait-interpolation) (simple-dock)) @@ -79,5 +79,5 @@ (send *ri* :move-to goal-pose :frame-id frame-id))) (defun auto-dock () - (go-to-spot "/eng2/7f/room73B2-fetch-dock-front" (make-coords :pos #f(0 -1500 0))) + (go-to-spot "/eng2/7f/room73B2-fetch-dock-front" (make-coords :pos #f(0 -800 0))) (dock)) From 1741103da6566b3618666f2b5ba694e81bdfd683 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 17 Oct 2019 22:11:45 +0900 Subject: [PATCH 2/6] add n-trial for auto-dock --- .../jsk_fetch_startup/euslisp/navigation-utils.l | 11 ++++++++--- 1 file changed, 8 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 c0c0eb17fa..19f58be93a 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -78,6 +78,11 @@ (setq goal-pose (send (ros::tf-pose->coords goal-pose) :transform relative-coords :world)) (send *ri* :move-to goal-pose :frame-id frame-id))) -(defun auto-dock () - (go-to-spot "/eng2/7f/room73B2-fetch-dock-front" (make-coords :pos #f(0 -800 0))) - (dock)) +(defun auto-dock (&key (n-trial 1)) + (let ((success nil) + (trial-count 0)) + (while (and (not success) (> n-trial trial-count)) + (go-to-spot "/eng2/7f/room73B2-fetch-dock-front" (make-coords :pos #f(0 -800 0))) + (setq success (dock)) + (incf trial-count)) + success)) From 1d39ed77407dbbba46e1cd7c1c61296a7607c20c Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 17 Oct 2019 23:14:23 +0900 Subject: [PATCH 3/6] refactor auto-dock in navigation-utils.l --- .../jsk_fetch_startup/euslisp/navigation-utils.l | 9 ++++----- 1 file changed, 4 insertions(+), 5 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 19f58be93a..83a0e56dd1 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -79,10 +79,9 @@ (send *ri* :move-to goal-pose :frame-id frame-id))) (defun auto-dock (&key (n-trial 1)) - (let ((success nil) - (trial-count 0)) - (while (and (not success) (> n-trial trial-count)) - (go-to-spot "/eng2/7f/room73B2-fetch-dock-front" (make-coords :pos #f(0 -800 0))) + (let ((success nil)) + (dotimes (i n-trial) + (go-to-spot *dock-spot* (make-coords :pos #f(0 -800 0))) (setq success (dock)) - (incf trial-count)) + (when success (return-from auto-dock success))) success)) From 604bc6ab4a6f5ae5c9e5836f3c39412926fd7933 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 9 Nov 2019 20:33:02 +0900 Subject: [PATCH 4/6] 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 83a0e56dd1..f831eb6164 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -79,6 +79,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) (go-to-spot *dock-spot* (make-coords :pos #f(0 -800 0))) From 8e1b76c2b2a69dc56fa7930c58290f03593f381e Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 9 Nov 2019 20:30:42 +0900 Subject: [PATCH 5/6] update fetch simple-dock to add goal_pose --- .../euslisp/navigation-utils.l | 56 +++++++++++++------ 1 file changed, 39 insertions(+), 17 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 f831eb6164..0cf5d15418 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -9,6 +9,21 @@ (defparameter *spots* nil) (defparameter *dock-spot* "/eng2/7f/room73B2-fetch-dock-front") (defparameter *is-charging* nil) +(defparameter *tfl* (instance ros::transform-listener :init)) + + +(defun get-spot-coords (name) + (unless *spots* + (setq *spots* (one-shot-subscribe "/spots_marker_array" visualization_msgs::MarkerArray))) + (let ((spot-coords nil) (frame-id nil)) + (dolist (x (send *spots* :markers)) + (if (equal (send x :text) name) + (progn + (setq spot-coords (send x :pose)) + (setq frame-id (send (send x :header) :frame_id))))) + (send (send spot-coords :position) :z 0) + (setq spot-coords (ros::tf-pose->coords spot-coords)) + (cons spot-coords frame-id))) (defun simple-dock () (unless *dock-action* @@ -18,13 +33,25 @@ (unless (send *dock-action* :wait-for-server 5) (ros::ros-error "/dock action server is not started") (return-from dock nil)) - - (send *dock-action* :send-goal - (instance fetch_auto_dock_msgs::DockActionGoal :init)) - (unless (send *dock-action* :wait-for-result :timeout 60) - (ros::ros-error "No result returned from /dock action server") - (return-from dock nil)) - (send (send *dock-action* :get-result) :docked)) + (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) + (send *dock-action* :send-goal dock-action-goal) + (unless (send *dock-action* :wait-for-result :timeout 60) + (send *dock-action* :cancel-all-goals) + (ros::ros-error "No result returned from /dock action server") + (return-from simple-dock nil)) + (send (send *dock-action* :get-result) :docked))) (defun dock () ;; look down @@ -66,16 +93,11 @@ (return)) (if (eq i 2) (progn (send *ri* :speak "Fail to undock") (ros::ros-error "Fail to undock")))) ;; go to spot - (unless *spots* - (setq *spots* (one-shot-subscribe "/spots_marker_array" visualization_msgs::MarkerArray))) - (let ((goal-pose nil) (frame-id nil)) - (dolist (x (send *spots* :markers)) - (if (equal (send x :text) name) - (progn - (setq goal-pose (send x :pose)) - (setq frame-id (send (send x :header) :frame_id))))) - (send (send goal-pose :position) :z 0) - (setq goal-pose (send (ros::tf-pose->coords goal-pose) :transform relative-coords :world)) + (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* :move-to goal-pose :frame-id frame-id))) (defun auto-dock (&key (n-trial 1)) From 84baee78a6dddba20f1be13d1f07c331c7868fe4 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 9 Nov 2019 22:55:23 +0900 Subject: [PATCH 6/6] move clear-costmap inside go-to-spot --- jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l | 5 +---- 1 file changed, 1 insertion(+), 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 0cf5d15418..d72ac1bb79 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l +++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l @@ -98,13 +98,10 @@ (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)) - (unless (boundp '*ri*) - (require :fetch-interface "package://fetcheus/fetch-interface.l") - (fetch-init)) - (send *ri* :clear-costmap) (let ((success nil)) (dotimes (i n-trial) (go-to-spot *dock-spot* (make-coords :pos #f(0 -800 0)))