Merge branch 'PR/spot_arm/add-elevator-demo' into develop/spot_arm

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,75 @@
+display: Sample Elevator
+platform: spot
+launch: jsk_spot_startup/sample_elevator.xml
+interface: jsk_spot_startup/sample_elevator.interface
+plugins:
+  - name: panorama_video_recorder_plugin
+    type: app_recorder/video_recorder_plugin
+    launch_args:
+      video_path: /tmp
+      video_title: sample_elevator_panorama.avi
+      video_topic_name: /dual_fisheye_to_panorama/output
+      video_fps: 1.0
+  - name: rosbag_recorder_plugin
+    type: app_recorder/rosbag_recorder_plugin
+    launch_args:
+      rosbag_path: /tmp
+      rosbag_title: sample_elevator_rosbag.bag
+      compress: true
+      rosbag_topic_names:
+        - /rosout
+        - /tf
+        - /tf_static
+        - /joint_states
+        - /odom
+        - /elevator_accel
+        - /elevator_accel_filtered
+        - /elevator_altitude
+        - /elevator_state_publisher/current_floor
+        - /elevator_state_publisher/elevator_movement
+        - /elevator_state_publisher/rest_elevator
+        - /m5stack_core2_driver/imu
+        - /m5stack_core2_driver/pressure
+        - /m5stack_core2_driver/temperature
+  - name: result_recorder_plugin
+    type: app_recorder/result_recorder_plugin
+    plugin_args:
+      result_path: /tmp
+      result_title: sample_elevator_result.yaml
+  - name: gdrive_uploader_plugin
+    type: app_uploader/gdrive_uploader_plugin
+    plugin_args:
+      upload_file_paths:
+        - /tmp/sample_elevator_result.yaml
+        - /tmp/sample_elevator_panorama.avi
+        - /tmp/sample_elevator_rosbag.bag
+      upload_file_titles:
+        - sample_elevator_result.yaml
+        - sample_elevator_panorama.avi
+        - sample_elevator_rosbag.bag
+      upload_parents_path: spot_sample_elevator
+      upload_server_name: /gdrive_server
+  - name: speech_notifier_plugin
+    type: app_notifier/speech_notifier_plugin
+    plugin_args:
+      client_name: /sound_play
+  - name: mail_notifier_plugin
+    type: app_notifier/mail_notifier_plugin
+    plugin_args:
+      mail_title: Spot Sample Elevator demo
+      use_timestamp_title: true
+      sender_address: [email protected]
+      receiver_address: [email protected]
+plugin_order:
+  start_plugin_order:
+    - panorama_video_recorder_plugin
+    - rosbag_recorder_plugin
+    - result_recorder_plugin
+    - gdrive_uploader_plugin
+    - mail_notifier_plugin
+  stop_plugin_order:
+    - panorama_video_recorder_plugin
+    - rosbag_recorder_plugin
+    - result_recorder_plugin
+    - gdrive_uploader_plugin
+    - mail_notifier_plugin

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,32 @@
+#!/usr/bin/env python3
+
+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 NavigationGoal
+
+
+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()
+
+    client.reset_current_node('eng2_73B2_dock')
+    client.execute_behaviors(target_node_id="eng2_3FElevator")
+    client.wait_execute_behaviors_result()
+    result = client.get_execute_behaviors_result()
+    rospy.loginfo("Result forward: {} {}".format(result.success, result.message))
+
+    client.execute_behaviors(NavigationGoal(target_node_id="eng2_73B2_dock"))
+    result = client.get_execute_behaviors_result()
+    rospy.loginfo("Result backward: {} {}".format(result.success, result.message))
+
+    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" output="screen" required="True"/>
+
+  <include
+      file="$(find elevator_operation)/launch/elevator_state_publisher.launch">
+      <arg name="initial_floor" value="7" />
+      <arg name="robot_type" value="spot" />
+  </include>
+</launch>