move clear-costmap inside go-to-spot

jsk-ros-pkg · Dec 5, 2019 · 6a2c524 · 6a2c524
1 parent 1653c1b
commit 6a2c524
Showing 1 changed file with 1 addition and 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
@@ -105,13 +105,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)
       (when (go-to-spot *dock-spot* (make-coords :pos #f(0 -800 0)))