Merge pull request #1160 from knorth55/fetch-fix-dock

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,21 +33,33 @@
   (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
   (unless (boundp '*ri*)
     (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))
@@ -72,18 +99,18 @@
       (unless *is-charging* (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* :clear-costmap)
     (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)))
-  (dock))
+(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)))
+    success))