Merge pull request #396 from YutoUchimi/fix_retval_clear-costmap

Fix return value of clear-costmap function

pr2eus/robot-interface.l

@@ -1801,11 +1801,10 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
    )
   (:clear-costmap
    ()
-   "Send signal to clear costmap for obstacle avoidance to move_base."
+   "Send signal to clear costmap for obstacle avoidance to move_base and return t if succeeded."
    (let ((srv-name (format nil "~A/clear_costmaps" (send move-base-action :name))))
-     (if (ros::wait-for-service srv-name 0)
-         (call-empty-service srv-name)))
-   t)
+     (call-empty-service srv-name))
+   )
   (:change-inflation-range
    (&optional (range 0.2)
     &key (node-name "/move_base_node")
@@ -1939,14 +1938,13 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
 ;;
 
 (defun clear-costmap (&key (node-name "/move_base_node"))
-  "reset local costmap and clear unknown grid around robot"
-  (if (and (boundp '*ri*) (derivedp *ri* robot-move-base-interface))
-      (send *ri* :clear-costmap)
+  "reset local costmap, clear unknown grid around robot and return t if succeeded"
+  (if (and (and (boundp '*ri*) (derivedp *ri* robot-move-base-interface))
+           (send *ri* :clear-costmap))
+      t
       ;; for backward compatibility
       (let ((srv-name (format nil "/~A/clear_costmaps" node-name)))
-        (if (ros::wait-for-service srv-name 0)
-            (call-empty-service srv-name))))
-  t)
+        (call-empty-service srv-name))))
 
 (defun change-inflation-range (&optional (range 0.2)
                                          &key (node-name "/move_base_node")