[jsk_spot_startup] add sample_elevator server

jsk_spot_robot/jsk_spot_startup/apps/app.installed

@@ -3,4 +3,5 @@ apps:
   dispay: Hello World
 - app: jsk_spot_startup/head_lead_demo
   dispay: Head Lead Demo
-
+- app: jsk_spot_startup/sample_elevator
+  dispay: Sample Elevator

jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.app

@@ -0,0 +1,4 @@
+display: Sample Elevator
+platform: spot
+launch: jsk_spot_startup/sample_elevator.xml
+interface: jsk_spot_startup/sample_elevator.interface

jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.interface

@@ -0,0 +1,3 @@
+published_topics: {}
+subscribed_topics: {}
+

jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+
+import rospy
+from spot_ros_client.libspotros import SpotRosClient
+import actionlib
+from jsk_spot_behavior_msgs.msg import NavigationAction
+from jsk_spot_behavior_msgs.msg import NavigationActionGoal
+
+
+if __name__ == '__main__':
+
+    rospy.init_node("sample_elevator")
+    client = SpotRosClient()
+
+    behavior_client = actionlib.SimpleActionClient("/spot_behavior_manager_server/execute_behaviors", NavigationAction)
+    behavior_client.wait_for_server()
+
+    client.claim()
+    client.power_on()
+    client.undock()
+
+    behavior_client.send_goal_and_wait(NavigationActionGoal(target_node_id="eng2_3FElevator"))
+    result = behavior_client.get_result()
+
+    behavior_client.send_goal_and_wait(NavigationActionGoal(target_node_id="eng2_73B2_dock"))
+    result = behavior_client.get_result()
+
+    client.dock(521)

jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.xml

@@ -0,0 +1,9 @@
+<launch>
+  <node pkg="jsk_spot_startup" type="sample_elevator.py" name="sample_elevator"/>
+
+  <include
+      file="$(find jsk_spot_behavior_manager)/launch/demo.launch"
+      >
+      <arg name="initial_node_id" value="eng2_73B2_dock" />
+  </include>
+</launch>