diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/auto-dock.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/auto-dock.l
old mode 100644
new mode 100755
index df7b44ee69..fa4dbaa212
--- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/auto-dock.l
+++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/auto-dock.l
@@ -1,8 +1,10 @@
-;; go-dock.l
-;; Author: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>
+#!/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)
diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/call-k-okada.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/call-k-okada.l
old mode 100644
new mode 100755
index e215325c5c..69dda0e3b2
--- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/call-k-okada.l
+++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/call-k-okada.l
@@ -1,5 +1,4 @@
-;; go-dock.l
-;; Author: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>
+#!/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)
diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/dock.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/dock.l
old mode 100644
new mode 100755
index 4cd923d501..7031b1fa46
--- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/dock.l
+++ b/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)
diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/fetch-look-at-human.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/fetch-look-at-human.l
index b3b6127a4f..ab6c2e9bd1 100755
--- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/fetch-look-at-human.l
+++ b/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)
diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen-with-mail-notify.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen-with-mail-notify.l
index 714e6a1a88..7bb07c9f17 100755
--- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen-with-mail-notify.l
+++ b/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)
diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l
old mode 100644
new mode 100755
index c55912599f..f7addae82d
--- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/go-to-kitchen.l
+++ b/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))
diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/light-off.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/light-off.l
new file mode 100755
index 0000000000..9fdef16494
--- /dev/null
+++ b/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)
diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/light-on.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/light-on.l
new file mode 100755
index 0000000000..9a9f2e86de
--- /dev/null
+++ b/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)
diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l
index b66e7b1a54..477d911beb 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l
+++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/navigation-utils.l
@@ -1,13 +1,16 @@
-;; go-dock.l
-;; Author: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>
+(load "package://jsk_robot_startup/lifelog/tweet_client.l")
+(load "package://switchbot_ros/scripts/switchbot.l")
+(require :state-machine "package://roseus_smach/src/state-machine.l")
+(require :state-machine-ros "package://roseus_smach/src/state-machine-ros.l")
+(require :state-machine-utils "package://roseus_smach/src/state-machine-utils.l")
 
 (ros::load-ros-manifest "fetch_auto_dock_msgs")
+(ros::load-ros-manifest "jsk_robot_startup")
 (ros::load-ros-manifest "power_msgs")
 
 (defparameter *dock-action* nil)
 (defparameter *undock-action* nil)
 (defparameter *spots* nil)
-(defparameter *is-charging* nil)
 (defparameter *tfl* (instance ros::transform-listener :init))
 
 
@@ -17,10 +20,42 @@
       ((equal robot-name "fetch15")
        "/eng2/7f/room73B2-fetch-dock-front")
       ((equal robot-name "fetch1075")
-       "/eng2/7f/room73B2-fetch-dock-entrance")
+       "/eng2/7f/room73B2-fetch-dock2-front")
       (t nil))))
 
 
+(defun store-params ()
+  (defparameter *global-inflation-radius*
+    (ros::get-param "/move_base/global_costmap/inflater/inflation_radius"))
+  (defparameter *local-inflation-radius*
+    (ros::get-param "/move_base/local_costmap/inflater/inflation_radius"))
+  t)
+
+
+(defun restore-params ()
+  (if (boundp '*global-inflation-radius*)
+    (ros::set-dynamic-reconfigure-param
+      "/move_base/global_costmap/inflater" "inflation_radius"
+      :double *global-inflation-radius*))
+  (if (boundp '*local-inflation-radius*)
+    (ros::set-dynamic-reconfigure-param
+      "/move_base/local_costmap/inflater" "inflation_radius"
+      :double *local-inflation-radius*))
+  t)
+
+
+(defun inflation-loose ()
+  ;; Increase the global inflation to generate a path
+  ;; that is less sensitive to differences between the map shape and the actual object placement.
+  (ros::set-dynamic-reconfigure-param
+    "/move_base/global_costmap/inflater" "inflation_radius" :double 0.7)
+  ;; Decrease the local inflation to generate a path
+  ;; that allows the robot to pass close to the object.
+  (ros::set-dynamic-reconfigure-param
+    "/move_base/local_costmap/inflater" "inflation_radius" :double 0.35)
+  t)
+
+
 (defun get-spot-coords (name)
   (unless *spots*
     (setq *spots* (one-shot-subscribe "/spots_marker_array" visualization_msgs::MarkerArray)))
@@ -34,16 +69,8 @@
     (setq spot-coords (ros::tf-pose->coords spot-coords))
     (cons spot-coords frame-id)))
 
-(let ((robot-name (ros::get-param "/robot/name")))
-  (defparameter *dock-spot*
-    (cond
-      ((equal robot-name "fetch15")
-       "/eng2/7f/room73B2-fetch-dock-front")
-      ((equal robot-name "fetch1075")
-       "/eng2/7f/room73B2-fetch-dock-entrance")
-      (t nil))))
 
-(defun simple-dock ()
+(defun simple-dock (&key (use-pose t))
   (unless *dock-action*
     (setq *dock-action*
           (instance ros::simple-action-client :init
@@ -51,19 +78,20 @@
   (unless (send *dock-action* :wait-for-server 5)
     (ros::ros-error "/dock action server is not started")
     (return-from dock nil))
-  (let* ((timestamp (ros::time-now))
-         (cret (get-spot-coords *dock-spot*))
-         (frame-to-dock (car cret))
-         (frame-id (cdr cret))
-         (lret (send *tfl* :wait-for-transform "base_link" frame-id timestamp 5))
-         (base-to-frame (send *tfl* :lookup-transform "base_link" frame-id timestamp))
-         (goal-pose (ros::coords->tf-pose (send frame-to-dock :transform base-to-frame :world)))
-         (pose-msg (instance geometry_msgs::PoseStamped :init))
-         (dock-action-goal (instance fetch_auto_dock_msgs::DockActionGoal :init)))
-    (send pose-msg :header :stamp timestamp)
-    (send pose-msg :header :frame_id "base_link")
-    (send pose-msg :pose goal-pose)
-    (send dock-action-goal :goal :dock_pose pose-msg)
+  (let ((dock-action-goal (instance fetch_auto_dock_msgs::DockActionGoal :init)))
+    (when use-pose
+      (let* ((timestamp (ros::time-now))
+             (cret (get-spot-coords *dock-spot*))
+             (frame-to-dock (car cret))
+             (frame-id (cdr cret))
+             (lret (send *tfl* :wait-for-transform "base_link" frame-id timestamp 5))
+             (base-to-frame (send *tfl* :lookup-transform "base_link" frame-id timestamp))
+             (goal-pose (ros::coords->tf-pose (send frame-to-dock :transform base-to-frame :world)))
+             (pose-msg (instance geometry_msgs::PoseStamped :init)))
+        (send pose-msg :header :stamp timestamp)
+        (send pose-msg :header :frame_id "base_link")
+        (send pose-msg :pose goal-pose)
+        (send dock-action-goal :goal :dock_pose pose-msg)))
     (send *dock-action* :send-goal dock-action-goal)
     (unless (send *dock-action* :wait-for-result :timeout 60)
       (send *dock-action* :cancel-all-goals)
@@ -71,6 +99,7 @@
       (return-from simple-dock nil))
     (send (send *dock-action* :get-result) :docked)))
 
+
 (defun dock ()
   ;; look down
   (unless (boundp '*ri*)
@@ -82,6 +111,7 @@
   (send *ri* :wait-interpolation)
   (simple-dock))
 
+
 (defun undock ()
   (unless *undock-action*
     (setq *undock-action*
@@ -90,7 +120,6 @@
   (unless (send *undock-action* :wait-for-server 5)
     (ros::ros-error "/undock action server is not started")
     (return-from undock nil))
-
   (send *undock-action* :send-goal
         (instance fetch_auto_dock_msgs::UndockActionGoal :init))
   (unless (send *undock-action* :wait-for-result :timeout 60)
@@ -98,24 +127,43 @@
     (return-from undock nil))
   (send (send *undock-action* :get-result) :undocked))
 
-(defun go-to-spot (name &optional (relative-coords (make-coords)))
+
+(defun get-battery-charging-state (&key (timeout 1500))
+   (let* ((msg (one-shot-subscribe "/battery_state" power_msgs::batterystate :timeout timeout))
+          (is-charging (if msg (send msg :is_charging))))
+     ;; You may fail to subscribe /battery_state
+     ;; because of message md5 difference between melodic and indigo.
+     (if (not msg) (return-from get-battery-charging-state nil))
+     (if is-charging :charging :discharging)))
+
+
+(defun go-to-spot (name &optional (relative-coords (make-coords)) &key (undock-rotate nil))
   ;; undock if fetch is docking
   (unless (boundp '*ri*)
     (require :fetch-interface "package://fetcheus/fetch-interface.l")
     (fetch-init))
-  (dotimes (i 3)
-    (let ((msg (one-shot-subscribe "/battery_state" power_msgs::batterystate :timeout 1500)))
-      ;; You may fail to subscribe /battery_state
-      ;; because of message md5 difference between melodic and indigo.
-      (unless msg
-        (ros::ros-warn "Failed to subscribe /battery_state")
-        (ros::ros-warn "Skip undocking, so please make sure that Fetch is already undocked.")
-        (return))
-      (setq *is-charging* (send msg :is_charging))
-      (if *is-charging*
-        (progn (undock)))
-      (unless *is-charging* (return))
-      (if (eq i 2) (progn (send *ri* :speak "Fail to undock") (ros::ros-error "Fail to undock")))))
+  (let ((undock-success nil))
+    (block go-to-spot-undock
+      (dotimes (i 3)
+        (let* ((battery-charging-state (get-battery-charging-state)))
+          ;; You may fail to subscribe /battery_state
+          ;; because of message md5 difference between melodic and indigo.
+          (unless battery-charging-state
+            (ros::ros-warn "Failed to subscribe /battery_state")
+            (ros::ros-warn "Skip undocking, so please make sure that Fetch is already undocked.")
+            (return-from go-to-spot-undock t))
+          (if (equal battery-charging-state :charging)
+            (progn
+              (setq undock-success (auto-undock :n-trial 3))
+              ;; rotate after undock
+              (if (and undock-success undock-rotate)
+                (send *ri* :go-pos-unsafe 0 0 180)))
+            (return-from go-to-spot-undock t))
+          (if (not undock-success)
+            (progn
+              (ros::ros-error "Failed to undock")
+            (send *ri* :speak "I failed to undock.")))))
+      (if (not undock-success) (return-from go-to-spot nil))))
   ;; go to spot
   (let* ((ret (get-spot-coords name))
          (goal-pose (car ret))
@@ -125,10 +173,272 @@
     (send *ri* :clear-costmap)
     (send *ri* :move-to goal-pose :frame-id frame-id)))
 
+
 (defun auto-dock (&key (n-trial 1))
   (let ((success nil))
     (dotimes (i n-trial)
-      (go-to-spot *dock-spot* (make-coords :pos #f(0 -800 0)))
-      (setq success (dock))
-      (when success (return-from auto-dock success)))
+      (when (go-to-spot *dock-spot* (make-coords :pos #f(-800 0 0)))
+        (ros::ros-info "arrived at the dock.")
+        (setq success (dock))
+        (when success (return-from auto-dock success))))
+    success))
+
+
+(defun auto-undock (&key (n-trial 1))
+  (let ((success nil))
+    (unless (boundp '*ri*)
+      (require :fetch-interface "package://fetcheus/fetch-interface.l")
+      (fetch-init))
+    (dotimes (i n-trial)
+      (ros::ros-info "trying to do undock.")
+      (setq success (undock))
+      (when success (return-from auto-undock success)))
+    (if (not success)
+      (let ((enable-request (instance power_msgs::BreakerCommandRequest :init :enable t))
+            (disable-request (instance power_msgs::BreakerCommandRequest :init :enable nil))
+            (breaker-service-name "base_breaker"))
+        (ros::wait-for-service breaker-service-name 5)
+        (ros::service-call breaker-service-name disable-request)
+        (ros::ros-error "Diable base breaker")
+        (send *ri* :speak "I disable and enable base breaker.")
+        (unix::sleep 5)
+        (ros::service-call breaker-service-name enable-request)
+        (ros::ros-error "Enable base breaker")))
     success))
+
+
+(defun get-light-on ()
+  (let* ((room-light-msg
+           (one-shot-subscribe "/check_room_light/output" jsk_robot_startup::RoomLight))
+         (light-on (if room-light-msg (send room-light-msg :light_on))))
+    light-on))
+
+
+(defun room-light-on (&key (control-switchbot :api))
+  (cond
+   ((or (eq control-switchbot :en) (eq control-switchbot :jp))
+    ;; Look at Google Home
+    (send *fetch* :head :neck-y :joint-angle 89)
+    (send *fetch* :head :neck-p :joint-angle 25)
+    (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller)
+    (send *ri* :wait-interpolation)
+    ;; The accuracy of Google Home recognition is better in English than in Japanese.
+    (if (eq control-switchbot :en)
+      (progn
+        (send *ri* :speak "OK, Google" :wait t)
+        (send *ri* :speak "Turn on the light." :wait t))
+      (progn
+        (send *ri* :speak-jp "オッケー、グーグル" :wait t)
+        (send *ri* :speak-jp "電気をつけて" :wait t)))
+    (send *fetch* :head :neck-y :joint-angle 0)
+    (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller)
+    (send *ri* :wait-interpolation))
+   ((eq control-switchbot :api)
+    (control-device "/eng2/7f/73b2/light/upper/switch" "turnOn" :wait t)
+    (control-device "/eng2/7f/73b2/light/lower/switch" "turnOn" :wait t)
+    (send *ri* :speak-jp "電気をつけました" :wait t))))
+
+
+(defun room-light-off (&key (control-switchbot :api))
+  (cond
+   ((or (eq control-switchbot :en) (eq control-switchbot :jp))
+    ;; Look at Google Home
+    (send *fetch* :head :neck-y :joint-angle 89)
+    (send *fetch* :head :neck-p :joint-angle 25)
+    (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller)
+    (send *ri* :wait-interpolation)
+    ;; The accuracy of Google Home recognition is better in English than in Japanese.
+    (if (eq control-switchbot :en)
+      (progn
+        (send *ri* :speak "OK, Google" :wait t)
+        (send *ri* :speak "Turn off the light." :wait t))
+      (progn
+        (send *ri* :speak-jp "オッケー、グーグル" :wait t)
+        (send *ri* :speak-jp "電気を消して" :wait t)))
+    (send *fetch* :head :neck-y :joint-angle 0)
+    (send *ri* :angle-vector-raw (send *fetch* :angle-vector) 3000 :head-controller)
+    (send *ri* :wait-interpolation))
+   ((eq control-switchbot :api)
+    (control-device "/eng2/7f/73b2/light/upper/switch" "turnOff" :wait t)
+    (control-device "/eng2/7f/73b2/light/lower/switch" "turnOff" :wait t)
+    (send *ri* :speak-jp "電気を消しました" :wait t))))
+
+
+(defun report-auto-dock ()
+  (ros::ros-info "start going back to the dock.")
+  (send *ri* :speak-jp "ドックに戻ります。" :wait t))
+
+
+(defun report-move-to-sink-front-success ()
+  (ros::ros-info "arrived at the kitchen stove.")
+  (send *ri* :speak-jp "キッチンのコンロの前につきました。" :wait t))
+
+
+(defun report-move-to-sink-front-failure ()
+  (ros::ros-error "failed going to the kitchen.")
+  (send *ri* :speak-jp "キッチンのコンロの前に行くのに失敗しました。" :wait t))
+
+
+(defun report-start-go-to-kitchen ()
+  (ros::ros-info "start going to the kitchen.")
+  (send *ri* :speak-jp "キッチンに向かいます。" :wait t))
+
+
+(defun report-light-on ()
+  (ros::ros-info "room light is already on.")
+  (send *ri* :speak-jp "すでに電気がついています。" :wait t))
+
+
+(defun report-light-off ()
+  (ros::ros-info "room light is off.")
+  (send *ri* :speak-jp "電気が消えています。" :wait t))
+
+
+(defun inspect-kitchen (&key (tweet t))
+  (report-move-to-sink-front-success)
+  (if tweet
+    (progn
+      ;; stove
+      (tweet-string "I took a photo at 73B2 Kitchen stove." :warning-time 3
+                    :with-image "/edgetpu_object_detector/output/image" :speak t)
+      (send *ri* :go-pos-unsafe 0 0 -45)
+      ;; sink
+      (tweet-string "I took a photo at 73B2 Kitchen sink." :warning-time 3
+                    :with-image "/edgetpu_object_detector/output/image" :speak t)
+      (send *ri* :go-pos-unsafe 0 0 135))
+    (progn
+      (send *ri* :go-pos-unsafe 0 0 90))))
+
+
+(defun move-to-sink-front (&key (n-trial 1) (offset #f(400 -500 0)))
+  (let ((success-move-to-sink-front nil))
+    (dotimes (i n-trial)
+      (setq success-move-to-sink-front
+            (go-to-spot "/eng2/7f/room73B2-sink-front0"
+                        (make-coords :pos offset) :undock-rotate t))
+      (when success-move-to-sink-front (return)))
+    success-move-to-sink-front))
+
+
+(defun go-to-kitchen (&key (tweet t) (n-dock-trial 1) (n-kitchen-trial 1)
+                           (control-switchbot :api))
+  ;; go to kitchen
+  (unless (boundp '*ri*)
+    (require :fetch-interface "package://fetcheus/fetch-interface.l")
+    (fetch-init))
+  (report-start-go-to-kitchen)
+  ;; Check if the lights are on in the room
+  (let ((initial-light-on (get-light-on))
+        (success-go-to-kitchen nil)
+        (success-auto-dock nil)
+        (success-battery-charging nil))
+    (store-params)
+    ;; turn on light
+    (if initial-light-on
+      (report-light-on)
+      (progn
+        (report-light-off)
+        (room-light-on :control-switchbot control-switchbot)))
+    ;; change the inflation_radius
+    (inflation-loose)
+    ;; go to kitchen sink
+    (setq success-go-to-kitchen (move-to-sink-front :n-trial n-kitchen-trial))
+    ;; report result to go to kitchen
+    (if success-go-to-kitchen
+      (inspect-kitchen :tweet tweet)
+      (report-move-to-sink-front-failure))
+    ;; go back from dock
+    (report-auto-dock)
+    (setq success-auto-dock (auto-dock :n-trial n-dock-trial))
+    ;; turn off light
+    (if (and success-auto-dock (not initial-light-on))
+      (room-light-off :control-switchbot control-switchbot))
+    ;; change the inflation_radius
+    (restore-params)
+    (setq success-battery-charging (equal (get-battery-charging-state) :charging))
+    (and success-go-to-kitchen success-auto-dock success-battery-charging)))
+
+
+(defun go-to-kitchen-state-machine ()
+  (setq *sm*
+        (make-state-machine
+          '((:init -> :report-start-go-to-kitchen)
+            (:report-start-go-to-kitchen -> :get-light-on)
+            (:get-light-on -> :report-light-on)
+            (:get-light-on !-> :room-light-on)
+            (:report-light-on -> :move-to-sink-front)
+            (:room-light-on -> :move-to-sink-front)
+            (:move-to-sink-front -> :inspect-kitchen)
+            (:move-to-sink-front !-> :report-move-to-sink-front-failure)
+            (:inspect-kitchen -> :auto-dock)
+            (:report-move-to-sink-front-failure -> :auto-dock)
+            (:auto-dock -> :room-light-off)
+            (:room-light-off -> :finish)
+            (:finish -> t)
+            (:finish !-> nil))
+          '((:init
+              '(lambda (userdata)
+                 (fetch-init)
+                 (send *ri* :clear-costmap)
+                 (store-params)
+                 (inflation-loose)
+                 t))
+            (:report-start-go-to-kitchen
+              '(lambda (userdata)
+                 (report-start-go-to-kitchen)
+                 t))
+            (:get-light-on
+              '(lambda (userdata)
+                 (let ((light-on (get-light-on)))
+                   (setf (cdr (assoc 'initial-light-on userdata)) light-on)
+                   light-on)))
+            (:report-light-on
+              '(lambda (userdata)
+                 (report-light-on)
+                 t))
+            (:room-light-on
+              '(lambda (userdata)
+                 (let ((control-switchbot (cdr (assoc 'control-switchbot userdata))))
+                   (report-light-off)
+                   (room-light-on :control-switchbot control-switchbot)
+                   t)))
+            (:move-to-sink-front
+              '(lambda (userdata)
+                 (let* ((n-trial (cdr (assoc 'n-kitchen-trial userdata)))
+                        (success (move-to-sink-front :n-trial n-trial)))
+                   (setf (cdr (assoc 'success-go-to-kitchen userdata)) success)
+                   success)))
+            (:inspect-kitchen
+              '(lambda (userdata)
+                 (inspect-kitchen :tweet (cdr (assoc 'tweet userdata)))
+                 t))
+            (:report-move-to-sink-front-failure
+              '(lambda (userdata)
+                 (report-move-to-sink-front-failure)
+                 t))
+            (:auto-dock
+              '(lambda (userdata)
+                 (report-auto-dock)
+                 (let* ((n-trial (cdr (assoc 'n-dock-trial userdata)))
+                        (success (auto-dock :n-trial n-trial)))
+                   (setf (cdr (assoc 'success-auto-dock userdata)) success)
+                   success)))
+            (:room-light-off
+              '(lambda (userdata)
+                 (let ((success-auto-dock (cdr (assoc 'success-auto-dock userdata)))
+                       (initial-light-on (cdr (assoc 'initial-light-on userdata)))
+                       (control-switchbot (cdr (assoc 'control-switchbot userdata))))
+                   (if (and success-auto-dock (not initial-light-on))
+                     (room-light-off :control-switchbot control-switchbot)))
+                 t))
+            (:finish
+              '(lambda (userdata)
+                 (let ((success-battery-charging
+                         (equal (get-battery-charging-state) :charging))
+                       (success-auto-dock (cdr (assoc 'success-auto-dock userdata)))
+                       (success-go-to-kitchen
+                         (cdr (assoc 'success-go-to-kitchen userdata))))
+                   (restore-params)
+                   (and success-go-to-kitchen success-auto-dock success-battery-charging)))))
+          '(:init)
+          '(t nil))))
diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/speak-battery.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/speak-battery.l
old mode 100644
new mode 100755
diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/tweet.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/tweet.l
old mode 100644
new mode 100755
index 786f5d988b..40e8283bf5
--- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/tweet.l
+++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/tweet.l
@@ -3,4 +3,4 @@
 (load "package://jsk_robot_startup/lifelog/tweet_client.l")
 
 (tweet-string "I took a photo." :warning-time 3 :with-image t :speak t)
-(exit)
+(sys::exit 0)
diff --git a/jsk_fetch_robot/jsk_fetch_startup/euslisp/undock.l b/jsk_fetch_robot/jsk_fetch_startup/euslisp/undock.l
old mode 100644
new mode 100755
index 764142eb10..d29cfc6a2c
--- a/jsk_fetch_robot/jsk_fetch_startup/euslisp/undock.l
+++ b/jsk_fetch_robot/jsk_fetch_startup/euslisp/undock.l
@@ -1,5 +1,7 @@
+#!/usr/bin/env roseus
+
 (load "package://jsk_fetch_startup/euslisp/navigation-utils.l")
 
 (ros::roseus "fetch_undock")
 (undock)
-(exit)
+(sys::exit 0)