From 4f75b8ce6a098b65c5bfdb7918f289d06b93c84d Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 8 Aug 2023 00:08:14 +0900 Subject: [PATCH 1/7] [jsk_spot_startup] add sample_elevator server --- .../jsk_spot_startup/apps/app.installed | 3 +- .../apps/sample_elevator/sample_elevator.app | 4 +++ .../sample_elevator/sample_elevator.interface | 3 ++ .../apps/sample_elevator/sample_elevator.py | 28 +++++++++++++++++++ .../apps/sample_elevator/sample_elevator.xml | 9 ++++++ 5 files changed, 46 insertions(+), 1 deletion(-) create mode 100644 jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.app create mode 100644 jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.interface create mode 100755 jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.py create mode 100644 jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.xml 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..43788e9318 --- /dev/null +++ b/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 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..2381891b02 --- /dev/null +++ b/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) 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..52ccfb8eda --- /dev/null +++ b/jsk_spot_robot/jsk_spot_startup/apps/sample_elevator/sample_elevator.xml @@ -0,0 +1,9 @@ + + + + + + + From 6ad6ce34c1befeb41ff9ad513122d6eb9dec3cf8 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 8 Aug 2023 00:55:19 +0900 Subject: [PATCH 2/7] [jsk_spot_startup] update sample_elevator demo --- .../apps/sample_elevator/sample_elevator.py | 8 ++++---- .../apps/sample_elevator/sample_elevator.xml | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) 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 index 2381891b02..dd92393d26 100755 --- 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 @@ -1,10 +1,10 @@ -#!/usr/bin/env python +#!/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 NavigationActionGoal +from jsk_spot_behavior_msgs.msg import NavigationGoal if __name__ == '__main__': @@ -19,10 +19,10 @@ client.power_on() client.undock() - behavior_client.send_goal_and_wait(NavigationActionGoal(target_node_id="eng2_3FElevator")) + behavior_client.send_goal_and_wait(NavigationGoal(target_node_id="eng2_3FElevator")) result = behavior_client.get_result() - behavior_client.send_goal_and_wait(NavigationActionGoal(target_node_id="eng2_73B2_dock")) + behavior_client.send_goal_and_wait(NavigationGoal(target_node_id="eng2_73B2_dock")) result = behavior_client.get_result() 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 index 52ccfb8eda..a893cf0bbd 100644 --- 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 @@ -1,5 +1,5 @@ - + Date: Tue, 8 Aug 2023 01:43:18 +0900 Subject: [PATCH 3/7] [jsk_spot_startup] add app plugin --- .../apps/sample_elevator/sample_elevator.app | 62 +++++++++++++++++++ 1 file changed, 62 insertions(+) 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 index 43788e9318..9bb2e091e6 100644 --- 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 @@ -2,3 +2,65 @@ 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 + - 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 From 5d84e27a70f85335e6745582730f2e1bfd27fee1 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 8 Aug 2023 02:29:03 +0900 Subject: [PATCH 4/7] [jsk_spot_startup] update app --- .../apps/sample_elevator/sample_elevator.app | 9 +++++++++ .../apps/sample_elevator/sample_elevator.py | 2 +- 2 files changed, 10 insertions(+), 1 deletion(-) 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 index 9bb2e091e6..e94307dbce 100644 --- 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 @@ -22,6 +22,15 @@ plugins: - /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: 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 index dd92393d26..30219ebd30 100755 --- 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 @@ -19,7 +19,7 @@ client.power_on() client.undock() - behavior_client.send_goal_and_wait(NavigationGoal(target_node_id="eng2_3FElevator")) + behavior_client.send_goal_and_wait(NavigationGoal(target_node_id="eng2_7FElevator")) result = behavior_client.get_result() behavior_client.send_goal_and_wait(NavigationGoal(target_node_id="eng2_73B2_dock")) From 095a30acac56bfb8a85e3be15c1b4bf1b4945e3e Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 8 Aug 2023 02:54:59 +0900 Subject: [PATCH 5/7] [jsk_spot_startup] fix app --- .../apps/sample_elevator/sample_elevator.app | 16 ++++++++-------- .../apps/sample_elevator/sample_elevator.py | 2 ++ 2 files changed, 10 insertions(+), 8 deletions(-) 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 index e94307dbce..2fdff70260 100644 --- 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 @@ -22,14 +22,14 @@ plugins: - /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 \ + - /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 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 index 30219ebd30..8cb8630e24 100755 --- 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 @@ -21,8 +21,10 @@ behavior_client.send_goal_and_wait(NavigationGoal(target_node_id="eng2_7FElevator")) result = behavior_client.get_result() + rospy.loginfo("Result forward: {} {}".format(result.success, result.message)) behavior_client.send_goal_and_wait(NavigationGoal(target_node_id="eng2_73B2_dock")) result = behavior_client.get_result() + rospy.loginfo("Result backward: {} {}".format(result.success, result.message)) client.dock(521) From 0c82ff74bcca72f8af8a161a7a93a7b63480db4c Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 8 Aug 2023 03:41:15 +0900 Subject: [PATCH 6/7] [jsk_spot_startup] fix app --- .../jsk_spot_startup/apps/sample_elevator/sample_elevator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 8cb8630e24..aba406b27c 100755 --- 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 @@ -19,7 +19,7 @@ client.power_on() client.undock() - behavior_client.send_goal_and_wait(NavigationGoal(target_node_id="eng2_7FElevator")) + behavior_client.send_goal_and_wait(NavigationGoal(target_node_id="eng2_3FElevator")) result = behavior_client.get_result() rospy.loginfo("Result forward: {} {}".format(result.success, result.message)) From eeb4e2aabc516bad0d6706740b394c5b727b5db3 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 10 Nov 2023 15:34:19 +0900 Subject: [PATCH 7/7] [sample_elevator update --- .../apps/sample_elevator/sample_elevator.py | 10 ++++++---- .../apps/sample_elevator/sample_elevator.xml | 6 +++--- 2 files changed, 9 insertions(+), 7 deletions(-) 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 index aba406b27c..00372e57e8 100755 --- 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 @@ -19,12 +19,14 @@ client.power_on() client.undock() - behavior_client.send_goal_and_wait(NavigationGoal(target_node_id="eng2_3FElevator")) - result = behavior_client.get_result() + 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)) - behavior_client.send_goal_and_wait(NavigationGoal(target_node_id="eng2_73B2_dock")) - result = behavior_client.get_result() + 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 index a893cf0bbd..f3497f43d5 100644 --- 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 @@ -2,8 +2,8 @@ - + file="$(find elevator_operation)/launch/elevator_state_publisher.launch"> + +