diff --git a/jsk_spot_robot/jsk_spot_startup/apps/app.installed b/jsk_spot_robot/jsk_spot_startup/apps/app.installed index 2bf50ef955..eb14ab712a 100644 --- a/jsk_spot_robot/jsk_spot_startup/apps/app.installed +++ b/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 - \ No newline at end of file +- app: jsk_spot_startup/sample_elevator + dispay: Sample Elevator diff --git a/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.app b/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.app new file mode 100644 index 0000000000..2fdff70260 --- /dev/null +++ b/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: belka@jsk.imi.i.u-tokyo.ac.jp + receiver_address: spot@jsk.imi.i.u-tokyo.ac.jp +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 diff --git a/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.interface b/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.interface new file mode 100644 index 0000000000..c27c9c296e --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.interface @@ -0,0 +1,3 @@ +published_topics: {} +subscribed_topics: {} + diff --git a/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.py b/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.py new file mode 100755 index 0000000000..00372e57e8 --- /dev/null +++ b/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) diff --git a/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.xml b/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.xml new file mode 100644 index 0000000000..f3497f43d5 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.xml @@ -0,0 +1,9 @@ + + + + + + + +