set timestamp and wait transform

jsk-ros-pkg · Nov 8, 2019 · 4f71c14 · 4f71c14
1 parent a450bef
commit 4f71c14
Showing 1 changed file with 9 additions and 3 deletions.
diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l
@@ -1402,17 +1402,23 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
      (ros::ros-info "move-to : ~A" (if ret 'succeeded 'failed))
      (when ret
        (dotimes (i 2)
-         (let* ((map-goal-coords
+         (let* ((timestamp (ros::time-now))
+                (mret (send *tfl* :wait-for-transform "map" frame-id timestamp 5))
+                (map-goal-coords
                   (if (string= frame-id base-frame-id)
                       (send (send map-to-frame :copy-worldcoords) :transform (send coords :worldcoords))
-                    (send (send *tfl* :lookup-transform "map" frame-id (ros::time 0))
+                    (send (send *tfl* :lookup-transform "map" frame-id timestamp)
                           :transform (send coords :copy-worldcoords)))) ;; goal-coords in /map coordinates
-                (timestamp (ros::time-now))
                 (lret (send *tfl* :wait-for-transform "map" base-frame-id timestamp 5))
                 (current-coords (send *tfl* :lookup-transform "map" base-frame-id timestamp))
                 (diff (send current-coords :transformation map-goal-coords))
                 (diff-len (norm (subseq (send diff :worldpos) 0 2))))
+           (ros::ros-warn ":move-to wait-for transform map to ~A -> ~A" frame-id mret)
            (ros::ros-warn ":move-to wait-for transform map to ~A -> ~A" base-frame-id lret)
+           (when (null mret)
+             (ros::ros-error ":move-to wait-for transform map to ~A failed" frame-id)
+             (setq move-base-goal-msg nil)
+             (return-from :move-to-wait nil))
            (when (null lret)
              (ros::ros-error ":move-to wait-for transform map to ~A failed" base-frame-id)
              (setq move-base-goal-msg nil)