Merge pull request #1327 from knorth55/fetch-euslisp-navigation

Update Fetch navigation euslisp scripts

jsk_fetch_robot/jsk_fetch_startup/euslisp/auto-dock.l

@@ -1,8 +1,10 @@
-;; go-dock.l
-;; Author: Yuki Furuta <[email protected]>
+#!/usr/bin/env roseus
 
-(ros::roseus "fetch_go_dock")
+(require :fetch-interface "package://fetcheus/fetch-interface.l")
 (load "package://jsk_fetch_startup/euslisp/navigation-utils.l")
 
+(ros::roseus "fetch_go_dock")
+(fetch-init)
+(send *ri* :clear-costmap)
 (auto-dock)
-(exit)
+(sys::exit 0)

jsk_fetch_robot/jsk_fetch_startup/euslisp/call-k-okada.l

@@ -1,5 +1,4 @@
-;; go-dock.l
-;; Author: Yuki Furuta <[email protected]>
+#!/usr/bin/env roseus
 
 (ros::roseus "call_k_okada")
 (load "package://jsk_fetch_startup/euslisp/navigation-utils.l")
@@ -16,3 +15,4 @@
   (auto-dock))
 
 (call-k-okada)
+(sys::exit 0)

jsk_fetch_robot/jsk_fetch_startup/euslisp/dock.l

@@ -1,5 +1,7 @@
+#!/usr/bin/env roseus
+
 (load "package://jsk_fetch_startup/euslisp/navigation-utils.l")
 
 (ros::roseus "fetch_dock")
 (simple-dock)
-(exit)
+(sys::exit 0)

jsk_fetch_robot/jsk_fetch_startup/euslisp/fetch-look-at-human.l

@@ -7,3 +7,4 @@
 (ros::roseus "fetch_look_at_human")
 (fetch-init)
 (main *fetch* (send *fetch* :head-rgb))
+(sys::exit 0)

jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen-with-mail-notify.l

@@ -56,3 +56,4 @@
   (auto-dock))
 
 (go-to-kitchen)
+(sys::exit 0)

jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l

@@ -1,19 +1,23 @@
-(ros::roseus "go_to_kitchen")
+#!/usr/bin/env roseus
+
+(require :fetch-interface "package://fetcheus/fetch-interface.l")
 (load "package://jsk_fetch_startup/euslisp/navigation-utils.l")
 
-(defun go-to-kitchen ()
-  ;; go to kitchen
-  (send *ri* :speak "go to kitchen.")
-  (if (go-to-spot "/eng2/7f/room73B2-sink-front1" (make-coords :pos #f(100 -1000 0)))
-    (progn ;; succeeded to go to kitchen
-      (unix:sleep 1)
-      (send *ri* :speak "arrived at kitchen." :wait t)
-      (unix:sleep 1)
-      (send *ri* :speak "return to dock." :wait t)
-      (auto-dock))
-    (progn ;; failed to go to kitchen
-      (send *ri* :speak "I could not go to kitchen, so return to dock." :wait t)
-      (auto-dock))))
 
+(defun main (&key (tweet t) (n-dock-trial 3) (n-kitchen-trial 3) (control-switchbot :api))
+  (when (not (boundp '*sm*))
+    (go-to-kitchen-state-machine))
+  (let ((result-state
+          (exec-state-machine *sm* `((tweet . ,tweet)
+                                     (n-kitchen-trial . ,n-kitchen-trial)
+                                     (n-dock-trial . ,n-dock-trial)
+                                     (control-switchbot . ,control-switchbot)
+                                     (initial-light-on . nil)
+                                     (success-go-to-kitchen . nil)
+                                     (success-auto-dock . nil))
+                              :hz 2.0)))
+    (send result-state :name)))
 
-(go-to-kitchen)
+
+(ros::roseus "go_to_kitchen")
+(if (main) (unix::exit 0) (unix::exit 1))

jsk_fetch_robot/jsk_fetch_startup/euslisp/light-off.l

@@ -0,0 +1,9 @@
+#!/usr/bin/env roseus
+
+(require :fetch-interface "package://fetcheus/fetch-interface.l")
+(load "package://jsk_fetch_startup/euslisp/navigation-utils.l")
+
+(ros::roseus "light_off")
+(fetch-init)
+(room-light-off :control-switchbot :api)
+(sys::exit 0)

jsk_fetch_robot/jsk_fetch_startup/euslisp/light-on.l

@@ -0,0 +1,9 @@
+#!/usr/bin/env roseus
+
+(require :fetch-interface "package://fetcheus/fetch-interface.l")
+(load "package://jsk_fetch_startup/euslisp/navigation-utils.l")
+
+(ros::roseus "light_on")
+(fetch-init)
+(room-light-on :control-switchbot :api)
+(sys::exit 0)