clear costmap before autodock

jsk-ros-pkg · Nov 9, 2019 · 27bfcf3 · 27bfcf3
1 parent bfac76f
commit 27bfcf3
Showing 1 changed file with 4 additions and 0 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
@@ -102,6 +102,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)
       (when (go-to-spot *dock-spot* (make-coords :pos #f(0 -800 0)))