update fetch simple-dock to add goal_pose

jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l

@@ -8,6 +8,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*
@@ -17,13 +32,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 simple-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
@@ -67,16 +94,9 @@
     (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 (ros::tf-pose->coords goal-pose))
+  (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)))