From f97aff06f4a4b2d24d77f30c534474b9ccb9ce47 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 5 Jul 2023 09:29:16 +0900 Subject: [PATCH 01/40] Add spot_ros_client, jsk_spot_behaviors and spot_autowalk_data --- jsk_spot_robot/jsk_spot_behaviors/README.md | 49 ++ .../spot_basic_behaviors/CMakeLists.txt | 17 + .../spot_basic_behaviors/README.md | 124 +++++ .../config/apriltag/settings.yaml | 9 + .../config/apriltag/tags.yaml | 94 ++++ .../config/switchbot_ros/token.yaml | 1 + .../launch/crosswalk_detection.launch | 24 + .../launch/elevator_detection.launch | 215 +++++++ .../node_scripts/car_tracker.py | 233 ++++++++ .../spot_basic_behaviors/package.xml | 37 ++ .../spot_basic_behaviors/setup.py | 9 + .../src/spot_basic_behaviors/__init__.py | 3 + .../crosswalk_behavior.py | 161 ++++++ .../spot_basic_behaviors/elevator_behavior.py | 295 ++++++++++ .../src/spot_basic_behaviors/walk_behavior.py | 84 +++ .../spot_behavior_graph/CMakeLists.txt | 15 + .../spot_behavior_graph/README.md | 25 + .../spot_behavior_graph/config/edges.yaml | 389 +++++++++++++ .../config/edges_person_lead.yaml | 295 ++++++++++ .../spot_behavior_graph/config/nodes.yaml | 421 ++++++++++++++ .../spot_behavior_graph/package.xml | 25 + .../scripts/visualize_map.py | 84 +++ .../spot_behavior_manager/CMakeLists.txt | 9 + .../spot_behavior_manager/package.xml | 38 ++ .../spot_behavior_manager/setup.py | 9 + .../src/spot_behavior_manager/__init__.py | 2 + .../spot_behavior_manager/base_behavior.py | 51 ++ .../behavior_manager_node.py | 249 +++++++++ .../support_behavior_graph.py | 99 ++++ .../spot_behavior_manager_msgs/CMakeLists.txt | 21 + .../action/LeadPerson.action | 6 + .../spot_behavior_manager_msgs/package.xml | 25 + .../srv/ResetCurrentNode.srv | 3 + .../CMakeLists.txt | 15 + .../spot_behavior_manager_server/README.md | 16 + .../launch/demo.launch | 65 +++ .../node_scripts/behavior_manager_server.py | 14 + .../interactive_behavior_executor.py | 66 +++ .../spot_behavior_manager_server/package.xml | 24 + .../spot_person_lead_behaviors/CMakeLists.txt | 17 + .../launch/crosswalk_detection.launch | 47 ++ .../launch/elevator_detection.launch | 213 +++++++ .../launch/narrow_detection.launch | 96 ++++ .../launch/stair_detection.launch | 19 + .../launch/walk_detection.launch | 19 + .../node_scripts/car_tracker.py | 219 ++++++++ .../node_scripts/person_tracker.py | 94 ++++ .../spot_person_lead_behaviors/package.xml | 18 + .../spot_person_lead_behaviors/setup.py | 9 + .../spot_person_lead_behaviors/__init__.py | 5 + .../crosswalk_behavior.py | 201 +++++++ .../elevator_behavior.py | 288 ++++++++++ .../narrow_behavior.py | 231 ++++++++ .../stair_behavior.py | 194 +++++++ .../walk_behavior.py | 157 ++++++ .../spot_autowalk_data/CMakeLists.txt | 16 + jsk_spot_robot/spot_autowalk_data/README.md | 116 ++++ jsk_spot_robot/spot_autowalk_data/package.xml | 18 + .../scripts/download_autowalk_data.py | 306 ++++++++++ .../spot_autowalk_data/scripts/view_map.py | 405 ++++++++++++++ jsk_spot_robot/spot_ros_client/CMakeLists.txt | 12 + jsk_spot_robot/spot_ros_client/README.md | 9 + jsk_spot_robot/spot_ros_client/package.xml | 24 + .../spot_ros_client/scripts/demo.py | 99 ++++ jsk_spot_robot/spot_ros_client/setup.py | 9 + .../src/spot_ros_client/__init__.py | 1 + .../src/spot_ros_client/libspotros.py | 523 ++++++++++++++++++ 67 files changed, 6686 insertions(+) create mode 100644 jsk_spot_robot/jsk_spot_behaviors/README.md create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/README.md create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/settings.yaml create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/tags.yaml create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/switchbot_ros/token.yaml create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/crosswalk_detection.launch create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch create mode 100755 jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/node_scripts/car_tracker.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/setup.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/crosswalk_behavior.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/CMakeLists.txt create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/README.md create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/edges.yaml create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/edges_person_lead.yaml create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/nodes.yaml create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/package.xml create mode 100755 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/scripts/visualize_map.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/CMakeLists.txt create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/package.xml create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/setup.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/__init__.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/base_behavior.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/behavior_manager_node.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/support_behavior_graph.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/CMakeLists.txt create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/action/LeadPerson.action create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/package.xml create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/srv/ResetCurrentNode.srv create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/CMakeLists.txt create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/README.md create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/launch/demo.launch create mode 100755 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/node_scripts/behavior_manager_server.py create mode 100755 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/node_scripts/interactive_behavior_executor.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/package.xml create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/CMakeLists.txt create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/crosswalk_detection.launch create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/elevator_detection.launch create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/narrow_detection.launch create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/stair_detection.launch create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/walk_detection.launch create mode 100755 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/node_scripts/car_tracker.py create mode 100755 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/node_scripts/person_tracker.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/package.xml create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/setup.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/__init__.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/crosswalk_behavior.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/elevator_behavior.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/narrow_behavior.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/stair_behavior.py create mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/walk_behavior.py create mode 100644 jsk_spot_robot/spot_autowalk_data/CMakeLists.txt create mode 100644 jsk_spot_robot/spot_autowalk_data/README.md create mode 100644 jsk_spot_robot/spot_autowalk_data/package.xml create mode 100755 jsk_spot_robot/spot_autowalk_data/scripts/download_autowalk_data.py create mode 100755 jsk_spot_robot/spot_autowalk_data/scripts/view_map.py create mode 100644 jsk_spot_robot/spot_ros_client/CMakeLists.txt create mode 100644 jsk_spot_robot/spot_ros_client/README.md create mode 100644 jsk_spot_robot/spot_ros_client/package.xml create mode 100755 jsk_spot_robot/spot_ros_client/scripts/demo.py create mode 100644 jsk_spot_robot/spot_ros_client/setup.py create mode 100644 jsk_spot_robot/spot_ros_client/src/spot_ros_client/__init__.py create mode 100644 jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py diff --git a/jsk_spot_robot/jsk_spot_behaviors/README.md b/jsk_spot_robot/jsk_spot_behaviors/README.md new file mode 100644 index 0000000000..648d8bfc09 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/README.md @@ -0,0 +1,49 @@ +# jsk_spot_behaviors + +These packages enable for Spot to execute locomotoion behaviors to reach a desired position. + +## Concept + +In this framework, knowledge about positions and transtion behaviors between them are represented as a digraph like below. + +Each node represents specified positions and each edge represents a behavior to transition between them. + +![example_graph](https://user-images.githubusercontent.com/9410362/124147589-cc8ce700-dac9-11eb-930f-1c00c2a4777e.png) + +A graph is defined by a yaml file ( e.g. [node.yaml in spot_behavior_graph](./spot_behavior_graph/config/node.yaml) and [edge.yaml in spot_behavior_graph](./spot_behavior_graph/config/edge.yaml) ) +Please see nodes and edges format section below. + +Knowledge representation and execution process of behaviors are separated from actual behavior implementation. +The implementation of former is in spot_behavior_manager and spot_behavior_manager_server, but actual behaviors like walk and elevator are in spot_basic_behaviors. +behavior_manager_server node will dynamically load each behaviors defined in map.yaml so you can easilly add your behavior without editing these core implementation. +Please see spot_basic_behaviors package for behavior examples. + +## How to use it + +To run demo, please make Spot stand in front of the fiducial in 73B2 and run + +```bash +roslaunch spot_behavior_graph demo.launch +``` + +```bash +$ rostopic pub -1 /spot_behavior_manager_server/lead_person/execute_behaviors spot_behavior_manager_msgs/LeadPersonActionGoal "header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '' +goal_id: + stamp: + secs: 0 + nsecs: 0 + id: '' +goal: + target_node_id: 'eng2_2FElevator'" +``` + +Then Spot will go to 2FElevator of eng2 by walk behavior and elevator behavior implemented in spot_basic_behaviors packages. + +https://user-images.githubusercontent.com/9410362/124338016-aad25380-dbe0-11eb-962f-b9a27e1e08cb.mp4 + +For more details, please see [spot_behavior_manager](./spot_behavior_manager), [spot_behavior_manager_server](./spot_behavior_manager_server) and each behavior documentation. (e.g. [spot_basic_behaviors](./spot_basic_behaviors) ) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt new file mode 100644 index 0000000000..d0b53fd0b6 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 2.8.3) +project(spot_basic_behaviors) + +find_package(catkin REQUIRED) + +catkin_python_setup() + +catkin_package( +) + +catkin_install_python(PROGRAMS node_scripts/car_tracker.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY config launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/README.md b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/README.md new file mode 100644 index 0000000000..fa341940b3 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/README.md @@ -0,0 +1,124 @@ +# spot_basic_behaviors + +This package includes exaple implementation of base_behavior for spot_behavior_manager. + +## walk behavior + +This behavior enables Spot to walk a specified route in a autowalk data from a given waypoint to another waypoint. + +behavior name: `spot_basic_behaviors.walk_behavior.WalkBehavior` + +https://user-images.githubusercontent.com/9410362/124337233-896f6880-dbdc-11eb-9588-30d4a5193bbd.mp4 + +### Required Configuration + +Before using this behavior, belows are required. + +- autowalk data containing a route for the behavior +- edge and nodes configuration for the behavior + +#### Edge + +Fields below are required in args of a edge + +- `graph`: path to autowalk data + +e.g. + +```yaml +- from: 'eng2_73B2' + to: 'eng2_73A2' + behavior_type: 'spot_basic_behavior.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' +``` + +#### Start and End Node + +Fields below are required for nodes + +- `waypoints_on_graph`: list of dict. each dict has `graph`, `id`, `localization_method` + + `graph`: path to a autowalk data + + `id`: waypoint id of the start node in the graph + + `localization_method`: localization_method to be used when starting autowalk, this must be 'fiducial' or 'waypoint' + +e.g. + +```yaml + 'eng2_73B2': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'dyed-bat-t00VKo5XixLihCvpsZPRqw==' + localization_method: 'fiducial' +``` + +## elevator behavior + +This behavior enables Spot to use an elevator to move to anothor floor. + +behavior name: `spot_basic_behaviors.elevator_behavior.ElevatorBehavior` + +https://user-images.githubusercontent.com/9410362/124337679-cd636d00-dbde-11eb-8db9-c5fedfda4ad9.mp4 + +### Required Configuration + +Before using this behavior, belows are required. + +- autowalk data containing a route for the behavior. +- apriltag pose information for elevator door frame detection. +- edge and nodes configuration for the behavior + +#### autowalk data + +To use this behavior, you need to record autowalk data while Spot riding on the elevator and getting off. + +Here is an example. + +![124134162-86318b00-dabd-11eb-86e3-092fcc5e8719](https://user-images.githubusercontent.com/9410362/124337765-364ae500-dbdf-11eb-83d3-a99e4025102e.png) + +#### apriltag pose information + +To use this behavior, an Fiducial (april tag) must be placed on the wall near the elevator doors. +And for elevator door opening and closing detection, transform from elevator foor frame ( center point on the ground ) to the fiducial is required. +Please see https://github.com/sktometometo/jsk_robot/blob/b166ef04cb954b175bedd5653af808be35e42121/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/tags.yaml#L44-L54 for examples + +#### Edge + +Fields below are required in args of a edge + +- `graph`: path to autowalk data +- `rest_waypoint_id`: waypoint of rest position in a elevator + +e.g. + +```yaml +- from: 'eng2_7FElevator' + to: 'eng2_2FElevator' + behavior_type: 'spot_basic_behavior.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A==' +``` + +#### Start and End Node + +Fields below are required for nodes + +- `waypoints_on_graph`: list of dict. each dict has `graph`, `id`, `localization_method` + + `graph`: path to a autowalk data + + `id`: waypoint id of the start node in the graph + + `localization_method`: localization_method to be used when starting autowalk, this must be 'fiducial' or 'waypoint' +- `switchbot_device`: switchbot device name for elevator button + +e.g. + +```yaml + 'eng2_7FElevator': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + id: 'sly-chetah-IZ4pVY7vrqO36OoKCYk9Zg==' + localization_method: 'fiducial' + switchbot_device: '/eng2/7f/elevator/down/button' +``` diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/settings.yaml b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/settings.yaml new file mode 100644 index 0000000000..9bd12af3d9 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/settings.yaml @@ -0,0 +1,9 @@ +tag_family: "tag36h11" # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12 +tag_threads: 2 # default: 2 +tag_decimate: 1.0 # default: 1.0 +tag_blur: 0.0 # default: 0.0 +tag_refine_edges: 1 # default: 1 +tag_debug: 0 # default: 0 +max_hamming_dist: 2 # default: 2 (Tunable parameter with 2 being a good choice - values >=3 consume large amounts of memory. Choose the largest value possible.) +# Other parameters +publish_tf: true # default: false diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/tags.yaml b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/tags.yaml new file mode 100644 index 0000000000..6828fdd222 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/tags.yaml @@ -0,0 +1,94 @@ +# # Definitions of tags to detect +# +# ## General remarks +# +# - All length in meters +# - Ellipsis (...) signifies that the previous element can be repeated multiple times. +# +# ## Standalone tag definitions +# ### Remarks +# +# - name is optional +# +# ### Syntax +# +# standalone_tags: +# [ +# {id: ID, size: SIZE, name: NAME}, +# ... +# ] +standalone_tags: [] +# ## Tag bundle definitions +# ### Remarks +# +# - name is optional +# - x, y, z have default values of 0 thus they are optional +# - qw has default value of 1 and qx, qy, qz have default values of 0 thus they are optional +# +# ### Syntax +# +# tag_bundles: +# [ +# { +# name: 'CUSTOM_BUNDLE_NAME', +# layout: +# [ +# {id: ID, size: SIZE, x: X_POS, y: Y_POS, z: Z_POS, qw: QUAT_W_VAL, qx: QUAT_X_VAL, qy: QUAT_Y_VAL, qz: QUAT_Z_VAL}, +# ... +# ] +# }, +# ... +# ] +tag_bundles: + [ + { + name: "elevator_door_frame_raw", + layout: + [ + { + id: 219, + size: 0.146, + x: -1.7, + y: -1.0, + z: 0.6, + qx: 0.0, + qy: 0.7071067811865475, + qz: 0.7071067811865476, + qw: 0.0, + }, + { + id: 211, + size: 0.146, + x: -1.7, + y: -1.0, + z: 0.4, + qx: 0.0, + qy: 0.7071067811865475, + qz: 0.7071067811865476, + qw: 0.0, + }, + { + id: 202, + size: 0.146, + x: -1.7, + y: -1.0, + z: 0.4, + qx: 0.0, + qy: 0.7071067811865475, + qz: 0.7071067811865476, + qw: 0.0, + }, + { + id: 216, + size: 0.146, + x: -1.7, + y: -1.0, + z: 0.4, + qx: 0.0, + qy: 0.7071067811865475, + qz: 0.7071067811865476, + qw: 0.0, + }, + ], + }, + ] diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/switchbot_ros/token.yaml b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/switchbot_ros/token.yaml new file mode 100644 index 0000000000..bfc61225f6 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/switchbot_ros/token.yaml @@ -0,0 +1 @@ +token: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/crosswalk_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/crosswalk_detection.launch new file mode 100644 index 0000000000..69499d7307 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/crosswalk_detection.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + frame_fixed: 'odom' + frame_robot: 'body' + timeout_transform: 0.05 + num_max_track: 10 + thresholds_distance: + '2': 50 + '5': 50 + '7': 50 + threshold_move_velocity: 2 + threshold_lost_duration: 1.0 + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch new file mode 100644 index 0000000000..529eb1538e --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch @@ -0,0 +1,215 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + max_duration: 5.0 + timeout_duration: 0.05 + timer_duration: 0.1 + reference_frame_id: elevator_door_frame_raw + output_frame_id: elevator_door_frame + fixed_frame_id: odom + + + + + + + + approximate_sync: true + output_frame: body + input_topics: + - $(arg TOPIC_DEPTH_FRONT_RIGHT)/points + - $(arg TOPIC_DEPTH_FRONT_LEFT)/points + - $(arg TOPIC_DEPTH_RIGHT)/points + - $(arg TOPIC_DEPTH_LEFT)/points + - $(arg TOPIC_DEPTH_BACK)/points + + + + + + + + initial_pos: [0, 0, 0.5] + initial_rot: [0, 0, 0] + dimension_x: 0.5 + dimension_y: 0.5 + dimension_z: 0.8 + frame_id: elevator_door_frame + + + + + + + + + + + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/node_scripts/car_tracker.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/node_scripts/car_tracker.py new file mode 100755 index 0000000000..01ab2037c8 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/node_scripts/car_tracker.py @@ -0,0 +1,233 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import threading + +import PyKDL +import rospy +import message_filters +import tf2_ros +import tf2_geometry_msgs + +from std_msgs.msg import Bool +from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray +from jsk_recognition_msgs.msg import Label, LabelArray + + +def convert_msg_point_to_kdl_vector(point): + return PyKDL.Vector(point.x, point.y, point.z) + + +class TrackedObject(object): + + UNKNOWN = 0 + FOUND = 1 + MOVING = 2 + LOST = 4 + + def __init__(self, + label, + position, + observed_time + ): + + self.label = label + self.state = self.FOUND + self.position = position + self.velocity = PyKDL.Vector() + self.last_observed = observed_time + self.lock = threading.Lock() + + def checkLost(self, + observed_time, + threshold_lost): + + with self.lock: + if (observed_time - self.last_observed).to_sec() > threshold_lost: + self.state = self.LOST + return True + else: + return False + + def update(self, + observed_position, + observed_time, + robot_position_fixedbased, + threshold_velocity, + threshold_distance): + + with self.lock: + if observed_time < self.last_observed: + rospy.logerr( + 'observed time is earlier than last observed time, dropped.') + return + elif observed_time == self.last_observed: + rospy.loginfo('this object is already observed') + return + + # Update position and velocity + if self.state == self.FOUND or self.state == self.MOVING or self.state == self.APPROCHING: + self.velocity = (observed_position - self.position) / \ + (observed_time - self.last_observed).to_sec() + else: + self.velocity = PyKDL.Vector() + self.position = observed_position + self.last_observed = observed_time + + # Update State + if (self.state == self.LOST or self.state == self.UNKNOWN) or \ + (self.velocity.Norm() < threshold_velocity) or \ + ((robot_position_fixedbased - self.position).Norm() > threshold_distance): + self.state = self.FOUND + else: + self.state = self.MOVING + + def getDistance(self, robot_position): + return (self.position - robot_position).Norm() + + +class MultiObjectTracker(object): + + def __init__(self): + + self._frame_fixed = rospy.get_param('~frame_fixed', 'fixed_frame') + self._frame_robot = rospy.get_param('~frame_robot', 'base_link') + self._timeout_transform = rospy.get_param('~timeout_transform', 0.05) + slop = rospy.get_param('~slop', 0.1) + + # parameters for multi object trackers + # maximum number of tracking objects + self._num_max_track = rospy.get_param('~num_max_track', 10) + # threshold for MOT state update + self._thresholds_distance = rospy.get_param('~thresholds_distance', {}) + self._threshold_velocity = rospy.get_param( + '~threshold_move_velocity', 1.0) + self._threshold_lost = rospy.get_param('~threshold_lost_duration', 1.0) + + # members + self._lock_for_dict_objects = threading.RLock() + self._dict_objects = {} + + # TF + self._tf_buffer = tf2_ros.Buffer() + self._tf_listener = tf2_ros.TransformListener(self._tf_buffer) + + # Publisher + # publsher for if there is a moving object or not + self._pub_visible = rospy.Publisher( + '~visible', + Bool, + queue_size=1 + ) + + # Subscriber + rate = rospy.Rate(1) + while not rospy.is_shutdown(): + try: + rospy.wait_for_message( + '~input_bbox_array', BoundingBoxArray, 3) + rospy.wait_for_message('~input_tracking_labels', LabelArray, 3) + break + except (rospy.ROSException, rospy.ROSInterruptException) as e: + rospy.logerr('subscribing topic seems not to be published.') + rospy.logerr('Error: {}'.format(e)) + rate.sleep() + mf_sub_bbbox_array = message_filters.Subscriber( + '~input_bbox_array', BoundingBoxArray) + mf_sub_tracking_labels = message_filters.Subscriber( + '~input_tracking_labels', LabelArray) + ts = message_filters.ApproximateTimeSynchronizer( + [mf_sub_bbbox_array, mf_sub_tracking_labels], 10, slop) + ts.registerCallback(self._cb_object) + + def _cb_object(self, + msg_bbox_array, + msg_tracking_labels): + + if msg_bbox_array.header.frame_id != self._frame_fixed: + rospy.logerr('frame_id of bbox (which is {}) array must be the same `~frame_fixed` which is {}'.format( + msg_bbox_array.header.frame_id, + self._frame_fixed)) + return + + time_observed = msg_tracking_labels.header.stamp + + try: + pykdl_transform_fixed_to_robot = tf2_geometry_msgs.transform_to_kdl( + self._tf_buffer.lookup_transform( + self._frame_fixed, + self._frame_robot, + time_observed, + timeout=rospy.Duration(self._timeout_transform) + ) + ) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + rospy.logerr('lookup transform failed. {}'.format(e)) + return + + with self._lock_for_dict_objects: + # add new object and update existing object state + for bbox, tracking_label in zip(msg_bbox_array.boxes, msg_tracking_labels.labels): + + if bbox.label not in self._thresholds_distance.keys(): + continue + + if tracking_label.id not in self._dict_objects: + if len(self._dict_objects) < self._num_max_track: + self._dict_objects[tracking_label.id] = \ + TrackedObject( + bbox.label, + convert_msg_point_to_kdl_vector( + bbox.pose.position), + time_observed + ) + else: + rospy.logwarn( + 'number of objects exceeds max track. dropped.') + else: + self._dict_objects[tracking_label.id].update( + convert_msg_point_to_kdl_vector(bbox.pose.position), + time_observed, + pykdl_transform_fixed_to_robot.p, + self._threshold_velocity, + self._thresholds_distance[str(bbox.label)] + ) + # check if there is lost object + to_be_removed = [] + for key in self._dict_objects: + is_lost = self._dict_objects[key].checkLost( + time_observed, + self._threshold_lost + ) + if is_lost: + to_be_removed.append(key) + # remove lost object from dict + for key in to_be_removed: + self._dict_objects.pop(key) + + def spin(self): + + rate = rospy.Rate(2) + while not rospy.is_shutdown(): + + rate.sleep() + + with self._lock_for_dict_objects: + + exist_moving_object = False + for key in self._dict_objects: + if self._dict_objects[key].state == TrackedObject.MOVING: + exist_moving_object = True + + self._pub_visible.publish(Bool(exist_moving_object)) + + +def main(): + + rospy.init_node('person_tracker') + tracker = MultiObjectTracker() + tracker.spin() + + +if __name__ == '__main__': + main() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml new file mode 100644 index 0000000000..48b7ae10f5 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml @@ -0,0 +1,37 @@ + + + spot_basic_behaviors + 1.1.0 + The spot_basic_behaviors package + + Kei Okada + Koki Shinjo + Koki Shinjo + BSD + + catkin + + spot_behavior_manager + + PyKDL + apriltag_ros + depth_image_proc + geometry_msgs + jsk_pcl_ros + jsk_recognition_msgs + jsk_spot_startup + message_filters + pcl + roslaunch + rospy + sensor_msgs + spinal + std_msgs + switchbot_ros + tf2_geometry_msgs + tf2_ros + tf_relay + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/setup.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/setup.py new file mode 100644 index 0000000000..5518911cd8 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/setup.py @@ -0,0 +1,9 @@ +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['spot_basic_behaviors'], + package_dir={'': 'src'} +) + +setup(**d) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py new file mode 100644 index 0000000000..2e6a9eefe6 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py @@ -0,0 +1,3 @@ +import spot_basic_behaviors.walk_behavior as walk_behavior +import spot_basic_behaviors.crosswalk_behavior as crosswalk_behavior +import spot_basic_behaviors.elevator_behavior as elevator_behavior diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/crosswalk_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/crosswalk_behavior.py new file mode 100644 index 0000000000..6a6444929b --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/crosswalk_behavior.py @@ -0,0 +1,161 @@ +# -*- coding: utf-8 -*- + +from spot_behavior_manager.base_behavior import BaseBehavior + +import roslaunch +import rospkg +import rospy + +import threading + +from std_msgs.msg import Bool + + +class CrosswalkBehavior(BaseBehavior): + + def callback_car_visible(self, msg): + + if self.car_state_visible != msg.data: + self.car_starttime_visibility = rospy.Time.now() + self.car_duration_visibility = rospy.Duration() + self.car_state_visible = msg.data + else: + self.car_duration_visibility = rospy.Time.now() - self.car_starttime_visibility + + def run_initial(self, start_node, end_node, edge, pre_edge): + + rospy.logdebug('run_initial() called') + + self.silent_mode = rospy.get_param('~silent_mode', True) + + # launch recognition launch + uuid = roslaunch.rlutil.get_or_generate_uuid(None, True) + roslaunch_path = rospkg.RosPack().get_path('spot_basic_behaviors') +\ + '/launch/crosswalk_detection.launch' + roslaunch_cli_args = [roslaunch_path] + roslaunch_file = roslaunch.rlutil.resolve_launch_arguments( + roslaunch_cli_args) + self.roslaunch_parent = roslaunch.parent.ROSLaunchParent( + uuid, + roslaunch_file + ) + self.roslaunch_parent.start() + + # value for car checker + self.subscriber_car_visible = None + self.car_state_visible = False + self.car_starttime_visibility = rospy.Time.now() + self.car_duration_visibility = rospy.Duration(10) + + # start subscribers + try: + self.subscriber_car_visible = rospy.Subscriber( + '/crosswalk_detection_car_tracker/visible', + Bool, + self.callback_car_visible + ) + except Exception as e: + rospy.logerr('{}'.format(e)) + return False + + return True + + def run_main(self, start_node, end_node, edge, pre_edge): + + rospy.logdebug('run_main() called') + + graph_name = edge.properties['graph'] + start_id = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['id'] + end_id = filter( + lambda x: x['graph'] == graph_name, + end_node.properties['waypoints_on_graph'] + )[0]['id'] + localization_method = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['localization_method'] + + # graph uploading and localization + if pre_edge is not None and \ + graph_name == pre_edge.properties['graph']: + rospy.loginfo('graph upload and localization skipped.') + else: + # Upload + ret = self.spot_client.upload_graph(graph_name) + if ret[0]: + rospy.loginfo('graph {} uploaded.'.format(graph_name)) + else: + rospy.logerr('graph uploading failed: {}'.format(ret[1])) + return False + # Localization + if localization_method == 'fiducial': + ret = self.spot_client.set_localization_fiducial() + elif localization_method == 'waypoint': + ret = self.spot_client.set_localization_waypoint(start_id) + else: + ret = (False, 'Unknown localization method') + if ret[0]: + rospy.loginfo('robot is localized on the graph.') + else: + rospy.logwarn('Localization failed: {}'.format(ret[1])) + return False + + # checking if there is a moving car visible or not + if not self.silent_mode: + self.sound_client.say('車が通るかみています', blocking=True) + safety_count = 0 + rate = rospy.Rate(1) + while not rospy.is_shutdown(): + rate.sleep() + rospy.loginfo('safety_count: {}'.format(safety_count)) + if safety_count > 10: + break + try: + rospy.loginfo('car visible: {}'.format(self.car_state_visible)) + if self.car_state_visible == True: + safety_count = 0 + if not self.silent_mode: + self.sound_client.say('車が通ります', blocking=True) + else: + safety_count += 1 + except Exception as e: + rospy.logwarn('{}'.format(e)) + safety_count = 0 + + # start leading + success = False + rate = rospy.Rate(10) + if not self.silent_mode: + self.sound_client.say('移動します', blocking=True) + self.spot_client.navigate_to(end_id, blocking=False) + while not rospy.is_shutdown(): + rate.sleep() + if self.spot_client.wait_for_navigate_to_result(rospy.Duration(0.1)): + result = self.spot_client.get_navigate_to_result() + success = result.success + rospy.loginfo('result: {}'.format(result)) + break + + # recovery on failure + if not success: + if not self.silent_mode: + self.sound_client.say('失敗したので元に戻ります', blocking=True) + self.spot_client.navigate_to(start_id, blocking=True) + self.spot_client.wait_for_navigate_to_result() + + return success + + def run_final(self, start_node, end_node, edge, pre_edge): + + rospy.logdebug('run_finalize() called') + + self.spot_client.cancel_navigate_to() + + if self.subscriber_car_visible != None: + self.subscriber_car_visible.unregister() + self.subscriber_car_visible = None + + self.roslaunch_parent.shutdown() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py new file mode 100644 index 0000000000..9823177b03 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py @@ -0,0 +1,295 @@ +# -*- coding: utf-8 -*- + +from spot_behavior_manager.base_behavior import BaseBehavior + +import actionlib +import roslaunch +import rospkg +import rospy + +import math + +from switchbot_ros.msg import SwitchBotCommandGoal, SwitchBotCommandAction +from sensor_msgs.msg import PointCloud2 +from spinal.msg import Barometer, Imu +from geometry_msgs.msg import Quaternion + + +class ElevatorBehavior(BaseBehavior): + + def door_point_cloud_callback(self, msg): + if len(msg.data) == 0: + self.door_is_open = True + else: + self.door_is_open = False + + def barometer_callback(self, msg): + + rospy.logdebug('altitude: {}'.format(msg.altitude)) + if math.fabs(msg.altitude - self.target_altitude) < self.threshold_altitude: + self.is_target_floor = True + else: + self.is_target_floor = False + + def imu_callback(self, msg): + + rospy.logdebug('acc z: {}'.format(msg.acc_data[2])) + if math.fabs(msg.acc_data[2] - self.stable_acc_z) < self.threshold_acc: + self.elevator_stop_acc = True + else: + self.elevator_stop_acc = False + + def run_initial(self, start_node, end_node, edge, pre_edge): + + rospy.logdebug('run_initial() called') + + self.silent_mode = rospy.get_param('~silent_mode', True) + + # launch recognition launch + uuid = roslaunch.rlutil.get_or_generate_uuid(None, True) + roslaunch_path = rospkg.RosPack().get_path('spot_basic_behaviors') +\ + '/launch/elevator_detection.launch' + roslaunch_cli_args = [roslaunch_path] + roslaunch_file = roslaunch.rlutil.resolve_launch_arguments( + roslaunch_cli_args) + self.roslaunch_parent = roslaunch.parent.ROSLaunchParent( + uuid, + roslaunch_file + ) + self.roslaunch_parent.start() + + # value for door openring checker + self.door_is_open = False + self.subscriber_door_check = None + + # value for floor detection + self.threshold_altitude = rospy.get_param( + '/elevator_behavior/threshold_altitude', 2) + self.is_target_floor = False + self.target_altitude = 0 + self.subscriber_floor_detection = None + + # + self.threshold_acc = rospy.get_param( + '/elevator_behavior/threshold_acc', 0.2) + self.stable_acc_z = 0 + self.elevator_stop_acc = False + self.subscriber_imu = None + + # value for switchbot + self.action_client_switchbot = actionlib.SimpleActionClient( + '/switchbot_ros/switch', + SwitchBotCommandAction + ) + + return True + + def run_main(self, start_node, end_node, edge, pre_edge): + + rospy.logdebug('run_main() called') + + graph_name = edge.properties['graph'] + start_id = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['id'] + end_id = edge.properties['goal_waypoint_id'] + rest_waypoint_id = edge.properties['rest_waypoint_id'] + localization_method = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['localization_method'] + + start_altitude = start_node.properties['floor_height'] + end_altitude = end_node.properties['floor_height'] + end_floor = end_node.properties['floor'] + + # graph uploading and localization + if pre_edge is not None and \ + graph_name == pre_edge.properties['graph']: + rospy.loginfo('graph upload and localization skipped.') + else: + # Upload + ret = self.spot_client.upload_graph(graph_name) + if ret[0]: + rospy.loginfo('graph {} uploaded.'.format(graph_name)) + else: + rospy.logerr('graph uploading failed: {}'.format(ret[1])) + return False + # Localization + if localization_method == 'fiducial': + ret = self.spot_client.set_localization_fiducial() + elif localization_method == 'waypoint': + ret = self.spot_client.set_localization_waypoint(start_id) + else: + ret = (False, 'Unknown localization method') + if ret[0]: + rospy.loginfo('robot is localized on the graph.') + else: + rospy.logwarn('Localization failed: {}'.format(ret[1])) + return False + + # start floor detection + try: + current_altitude = rospy.wait_for_message( + '/spinal/baro', Barometer, rospy.Duration(5)).altitude + self.target_altitude = current_altitude - \ + (start_altitude - end_altitude) + self.subscriber_floor_detection = rospy.Subscriber( + '/spinal/baro', + Barometer, + self.barometer_callback) + except Exception as e: + rospy.logerr('{}'.format(e)) + return False + + # start imu + try: + self.stable_acc_z = rospy.wait_for_message( + '/spinal/imu', Imu, rospy.Duration(5)).acc_data[2] + self.subscriber_imu = rospy.Subscriber( + '/spinal/imu', + Imu, + self.imu_callback) + except Exception as e: + rospy.logerr('{}'.format(e)) + return False + + # start door opening check from outside + self.subscriber_door_check = rospy.Subscriber( + '/spot_recognition/elevator_door_points', + PointCloud2, + self.door_point_cloud_callback) + + # push button with switchbot + rospy.loginfo('calling elevator when riding...') + success_calling = False + if not self.action_client_switchbot.wait_for_server(rospy.Duration(10)): + rospy.logerr('switchbot server seems to fail.') + return False + else: + switchbot_goal = SwitchBotCommandGoal() + switchbot_goal.device_name = start_node.properties['switchbot_device'] + switchbot_goal.command = 'press' + count = 0 + while True: + self.action_client_switchbot.send_goal(switchbot_goal) + if self.action_client_switchbot.wait_for_result(rospy.Duration(10)): + break + count += 1 + if count >= 3: + rospy.logerr('switchbot calling failed.') + return False + result = self.action_client_switchbot.get_result() + rospy.loginfo('switchbot result: {}'.format(result)) + if not result.done: + rospy.logerr('switchbot calling failed.') + return False + rospy.loginfo('elevator calling when riding on has succeeded') + + # wait for elevator + rate = rospy.Rate(1) + door_open_count = 0 + while not rospy.is_shutdown(): + rate.sleep() + if self.door_is_open: + door_open_count += 1 + else: + door_open_count = 0 + if door_open_count >= 2: + break + rospy.loginfo('elevator door opened.') + self.subscriber_door_check.unregister() + self.subscriber_door_check = None + + # start navigation to rest point + rate = rospy.Rate(10) + if not self.silent_mode: + self.sound_client.say('エレベータに乗り込みます。ご注意ください。', blocking=False) + self.spot_client.navigate_to(rest_waypoint_id, blocking=False) + # call elevator from destination floor while + rospy.loginfo('calling elevator when getting off...') + switchbot_goal = SwitchBotCommandGoal() + switchbot_goal.device_name = end_node.properties['switchbot_device'] + switchbot_goal.command = 'press' + self.action_client_switchbot.send_goal(switchbot_goal) + ## + if not self.action_client_switchbot.wait_for_result(timeout=rospy.Duration(20)): + rospy.logerr('Switchbot timeout') + self.spot_client.wait_for_navigate_to_result() + self.spot_client.navigate_to(start_id, blocking=True) + self.spot_client.wait_for_navigate_to_result() + return False + result_switchbot = self.action_client_switchbot.get_result() + self.spot_client.wait_for_navigate_to_result() + result_navigation = self.spot_client.get_navigate_to_result() + # recovery when riding on + if not result_navigation.success or not result_switchbot.done: + rospy.logerr('Failed to ride on a elevator. result_navigation: {}, result_switchbot: {}'.format( + result_navigation, result_switchbot)) + self.spot_client.navigate_to(start_id, blocking=True) + self.spot_client.wait_for_navigate_to_result() + return False + else: + rospy.loginfo('Riding on succeded.') + + if not self.silent_mode: + self.sound_client.say('{}階に行きます'.format(end_floor), blocking=False) + + # start door openning check from inside + self.subscriber_door_check = rospy.Subscriber( + '/spot_recognition/elevator_door_points', + PointCloud2, + self.door_point_cloud_callback) + + # check if the door is closed + rate = rospy.Rate(2) + while not rospy.is_shutdown(): + rate.sleep() + if not self.door_is_open: + break + rospy.loginfo('elevator door closed') + + # check if the door is open and at the target floor + rate = rospy.Rate(1) + while not rospy.is_shutdown(): + rate.sleep() + rospy.loginfo('door_is_open: {}, is_target_floor: {}, stop from acc: {}'.format( + self.door_is_open, self.is_target_floor, self.elevator_stop_acc)) + if self.door_is_open and self.is_target_floor and self.elevator_stop_acc: + break + rospy.loginfo('elevator door opened and at the target_floor') + + # dance before starting to move + self.spot_client.pub_body_pose(0.0, Quaternion(w=1)) + self.spot_client.stand() + rospy.sleep(0.5) + self.spot_client.pub_body_pose(-0.2, Quaternion(w=1)) + self.spot_client.stand() + rospy.sleep(0.5) + self.spot_client.pub_body_pose(0.0, Quaternion(w=1)) + self.spot_client.stand() + + # get off the elevator + self.spot_client.navigate_to(end_id, blocking=True) + result = self.spot_client.get_navigate_to_result() + + return result.success + + def run_final(self, start_node, end_node, edge, pre_edge): + + rospy.logdebug('run_finalize() called') + + if self.subscriber_door_check != None: + self.subscriber_door_check.unregister() + self.subscriber_door_check = None + + if self.subscriber_floor_detection != None: + self.subscriber_floor_detection.unregister() + self.subscriber_floor_detection = None + + if self.subscriber_imu != None: + self.subscriber_imu.unregister() + self.subscriber_imu = None + + self.roslaunch_parent.shutdown() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py new file mode 100644 index 0000000000..aaac0c3e5f --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py @@ -0,0 +1,84 @@ +# -*- coding: utf-8 -*- + +from spot_behavior_manager.base_behavior import BaseBehavior + +import rospy + + +class WalkBehavior(BaseBehavior): + + def run_initial(self, start_node, end_node, edge, pre_edge): + + rospy.logdebug('run_initial() called') + self.silent_mode = rospy.get_param('~silent_mode', True) + return True + + def run_main(self, start_node, end_node, edge, pre_edge): + + rospy.logdebug('run_main() called') + + graph_name = edge.properties['graph'] + start_id = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['id'] + end_id = filter( + lambda x: x['graph'] == graph_name, + end_node.properties['waypoints_on_graph'] + )[0]['id'] + localization_method = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['localization_method'] + + # graph uploading and localization + if pre_edge is not None and \ + graph_name == pre_edge.properties['graph']: + rospy.loginfo('graph upload and localization skipped.') + else: + # Upload + ret = self.spot_client.upload_graph(graph_name) + if ret[0]: + rospy.loginfo('graph {} uploaded.'.format(graph_name)) + else: + rospy.logerr('graph uploading failed: {}'.format(ret[1])) + return False + # Localization + if localization_method == 'fiducial': + ret = self.spot_client.set_localization_fiducial() + elif localization_method == 'waypoint': + ret = self.spot_client.set_localization_waypoint(start_id) + else: + ret = (False, 'Unknown localization method') + if ret[0]: + rospy.loginfo('robot is localized on the graph.') + else: + rospy.logwarn('Localization failed: {}'.format(ret[1])) + return False + + # start navigation + success = False + rate = rospy.Rate(10) + if not self.silent_mode: + self.sound_client.say('移動します', blocking=True) + self.spot_client.navigate_to(end_id, blocking=False) + while not rospy.is_shutdown(): + rate.sleep() + if self.spot_client.wait_for_navigate_to_result(rospy.Duration(0.1)): + result = self.spot_client.get_navigate_to_result() + success = result.success + rospy.loginfo('result: {}'.format(result)) + break + + # recovery on failure + if not success: + if not self.silent_mode: + self.sound_client.say('失敗したので元に戻ります', blocking=True) + self.spot_client.navigate_to(start_id, blocking=True) + self.spot_client.wait_for_navigate_to_result() + + return success + + def run_final(self, start_node, end_node, edge, pre_edge): + + rospy.logdebug('run_finalize() called') diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/CMakeLists.txt new file mode 100644 index 0000000000..f28f925292 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 2.8.3) +project(spot_behavior_graph) + +find_package(catkin REQUIRED) + +catkin_package( +) + +install(DIRECTORY config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +catkin_install_python(PROGRAMS scripts/visualize_map.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/README.md b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/README.md new file mode 100644 index 0000000000..ffc335bf96 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/README.md @@ -0,0 +1,25 @@ +# spot_behavior_graph + +This package provides graph and node data for spot_behavior_manager. + +## scripts + +### visualize_map.py + +Visualizer script of map file of spot_behavior_manager + +Befor using this script, you need to install + +``` +pip3 install graphviz xdot +``` + +#### Usage + +``` +rosrun spot_behavior_graph visualize_map.py --filename +``` + +#### Example Output + +![map](https://user-images.githubusercontent.com/9410362/132942120-4a4e652b-3d25-43df-a678-fd3c09782284.png) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/edges.yaml b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/edges.yaml new file mode 100644 index 0000000000..13455ad03f --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/edges.yaml @@ -0,0 +1,389 @@ +edges: + - from: 'eng2_JSKEntrance' + to: 'eng2_73A2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73A2' + to: 'eng2_JSKEntrance' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_JSKEntrance' + to: 'eng2_73B1' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73B1' + to: 'eng2_JSKEntrance' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_JSKEntrance' + to: 'eng2_73B2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73B2' + to: 'eng2_JSKEntrance' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_JSKEntrance' + to: 'eng2_73A4' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73A4' + to: 'eng2_JSKEntrance' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73B2' + to: 'eng2_73B2_belka_home' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73B2_belka_home' + to: 'eng2_73B2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73B2' + to: 'eng2_73B2_strelka_home' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73B2_strelka_home' + to: 'eng2_73B2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_73B2' + to: 'eng2_73B2_shinjo_desk' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/73b2_inside.walk' + - from: 'eng2_73B2_shinjo_desk' + to: 'eng2_73B2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/73b2_inside.walk' + - from: 'eng2_73B2' + to: 'eng2_81C1' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_81C1.walk' + - from: 'eng2_81C1' + to: 'eng2_73B2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_81C1.walk' + - from: 'eng2_73B2' + to: 'eng2_83A2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_83A2.walk' + - from: 'eng2_83A2' + to: 'eng2_73B2' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_83A2.walk' + - from: 'eng2_JSKEntrance' + to: 'eng2_7FElevator' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_7FElevator' + to: 'eng2_JSKEntrance' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_7FElevator' + to: 'eng2_2FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A==' + goal_waypoint_id: 'shelfy-dassie-vK9Tf3cAQpdA.25gPJl8OQ==' + - from: 'eng2_2FElevator' + to: 'eng2_7FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A==' + goal_waypoint_id: 'sly-chetah-IZ4pVY7vrqO36OoKCYk9Zg==' + - from: 'eng2_2FElevator' + to: 'eng_TelephoneBox' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + - from: 'eng_TelephoneBox' + to: 'eng2_2FElevator' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + - from: 'eng_TelephoneBox' + to: 'eng8_Piloti_Center' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + - from: 'eng8_Piloti_Center' + to: 'eng_TelephoneBox' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + - from: 'eng2_3FElevator' + to: 'eng2_2FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_3FElevator_to_2FElevator.walk' + rest_waypoint_id: 'akimbo-scarab-RoO1pzYYARw+EquQk1IIbg==' + goal_waypoint_id: 'dashed-virus-QgetwIkinypWecGNMSS4Yw==' + - from: 'eng2_2FElevator' + to: 'eng2_3FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_3FElevator_to_2FElevator.walk' + rest_waypoint_id: 'akimbo-scarab-RoO1pzYYARw+EquQk1IIbg==' + goal_waypoint_id: 'eighth-ant-0Ot3VoMm0uiJNBHnbJOF9w==' + - from: 'eng2_3FElevator' + to: 'eng2_3F_door_inside' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3F_door_inside' + to: 'eng2_3FElevator' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3F_door_inside' + to: 'eng2_3F_door_outside' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3F_door_outside' + to: 'eng2_3F_door_inside' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3F_door_outside' + to: 'eng2_Mech_Office' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_Mech_Office' + to: 'eng2_3F_door_outside' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_Mech_Office' + to: 'eng2_Mailbox' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + - from: 'eng2_Mailbox' + to: 'eng2_Mech_Office' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + - from: 'eng2_Mech_Office' + to: 'eng2_CampusMail' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + - from: 'eng2_CampusMail' + to: 'eng2_Mech_Office' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + - from: 'eng2_Mailbox' + to: 'eng2_CampusMail' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + - from: 'eng2_CampusMail' + to: 'eng2_Mailbox' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + - from: 'eng2_3FElevator' + to: 'eng2_7FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_3FElevator_to_2FElevator.walk' + rest_waypoint_id: 'akimbo-scarab-RoO1pzYYARw+EquQk1IIbg==' + goal_waypoint_id: 'dashed-virus-QgetwIkinypWecGNMSS4Yw==' + - from: 'eng2_7FElevator' + to: 'eng2_3FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A==' + goal_waypoint_id: 'shelfy-dassie-vK9Tf3cAQpdA.25gPJl8OQ==' + - from: 'eng2_2FElevator' + to: 'eng2_subway_pillar' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_subway.walk' + - from: 'eng2_subway_pillar' + to: 'eng2_2FElevator' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_subway.walk' + - from: 'eng2_subway_pillar' + to: 'eng2_subway_initial' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_pillar' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_00' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_table_00' + to: 'eng2_subway_initial' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_2FElevator' + to: 'eng2_MainEntrance_upper' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_MainEntrance.walk' + - from: 'eng2_MainEntrance_upper' + to: 'eng2_2FElevator' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_MainEntrance.walk' + - from: 'eng2_MainEntrance_upper' + to: 'eng2_MainEntrance_lower' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'eng2_MainEntrance_lower' + to: 'eng2_MainEntrance_upper' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'eng2_MainEntrance_lower' + to: 'eng2_MainEntrance_outer' + behavior_type: 'spot_basic_behaviors.crosswalk_behavior.CrosswalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'eng2_MainEntrance_outer' + to: 'eng2_MainEntrance_lower' + behavior_type: 'spot_basic_behaviors.crosswalk_behavior.CrosswalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'eng2_MainEntrance_outer' + to: 'HongoMainGate' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'HongoMainGate' + to: 'eng2_MainEntrance_outer' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'eng2_7FElevator' + to: 'eng2_1FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_1FElevator.walk' + rest_waypoint_id: 'snubbed-puma-5v8XtjdA17XhPNLhsH2WEg==' + goal_waypoint_id: 'meager-fox-bT4DU3uCHBJFWuYILkopVg==' + - from: 'eng2_1FElevator' + to: 'eng2_7FElevator' + behavior_type: 'spot_basic_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_1FElevator.walk' + rest_waypoint_id: 'snubbed-puma-5v8XtjdA17XhPNLhsH2WEg==' + goal_waypoint_id: 'meager-fox-bT4DU3uCHBJFWuYILkopVg==' + - from: 'eng2_1FElevator' + to: 'eng2_GarbageCollectionArea_east' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaEast.walk' + - from: 'eng2_1FElevator' + to: 'eng2_GarbageCollectionArea_south' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaSouth.walk' + - from: 'eng2_GarbageCollectionArea_east' + to: 'eng2_1FElevator' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaEast.walk' + - from: 'eng2_GarbageCollectionArea_south' + to: 'eng2_1FElevator' + behavior_type: 'spot_basic_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaSouth.walk' diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/edges_person_lead.yaml b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/edges_person_lead.yaml new file mode 100644 index 0000000000..64632e982f --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/edges_person_lead.yaml @@ -0,0 +1,295 @@ +edges: + - from: 'eng2_73B2' + to: 'eng2_7FElevator' + behavior_type: 'spot_person_lead_behaviors.narrow_behavior.NarrowBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_7FElevator' + to: 'eng2_73B2' + behavior_type: 'spot_person_lead_behaviors.narrow_behavior.NarrowBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + - from: 'eng2_7FElevator' + to: 'eng2_2FElevator' + behavior_type: 'spot_person_lead_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A==' + goal_waypoint_id: 'shelfy-dassie-vK9Tf3cAQpdA.25gPJl8OQ==' + - from: 'eng2_2FElevator' + to: 'eng2_7FElevator' + behavior_type: 'spot_person_lead_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A==' + goal_waypoint_id: 'sly-chetah-IZ4pVY7vrqO36OoKCYk9Zg==' + - from: 'eng2_2FElevator' + to: 'eng2_MainEntrance_upper' + behavior_type: 'spot_person_lead_behaviors.narrow_behavior.NarrowBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_MainEntrance.walk' + - from: 'eng2_MainEntrance_upper' + to: 'eng2_2FElevator' + behavior_type: 'spot_person_lead_behaviors.narrow_behavior.NarrowBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_MainEntrance.walk' + - from: 'eng2_MainEntrance_upper' + to: 'eng2_MainEntrance_lower' + behavior_type: 'spot_person_lead_behaviors.stair_behavior.StairBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + stair_nums: + - -4 + - -8 + - from: 'eng2_MainEntrance_lower' + to: 'eng2_MainEntrance_upper' + behavior_type: 'spot_person_lead_behaviors.stair_behavior.StairBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + stair_nums: + - 8 + - 4 + - from: 'eng2_MainEntrance_lower' + to: 'eng2_MainEntrance_outer' + behavior_type: 'spot_person_lead_behaviors.crosswalk_behavior.CrosswalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'eng2_MainEntrance_outer' + to: 'eng2_MainEntrance_lower' + behavior_type: 'spot_person_lead_behaviors.crosswalk_behavior.CrosswalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'eng2_MainEntrance_outer' + to: 'HongoMainGate' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'HongoMainGate' + to: 'eng2_MainEntrance_outer' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + - from: 'eng2_3FElevator' + to: 'eng2_2FElevator' + behavior_type: 'spot_person_lead_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_3FElevator_to_2FElevator.walk' + rest_waypoint_id: 'akimbo-scarab-RoO1pzYYARw+EquQk1IIbg==' + goal_waypoint_id: 'dashed-virus-QgetwIkinypWecGNMSS4Yw==' + - from: 'eng2_2FElevator' + to: 'eng2_3FElevator' + behavior_type: 'spot_person_lead_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_3FElevator_to_2FElevator.walk' + rest_waypoint_id: 'akimbo-scarab-RoO1pzYYARw+EquQk1IIbg==' + goal_waypoint_id: 'eighth-ant-0Ot3VoMm0uiJNBHnbJOF9w==' + - from: 'eng2_3FElevator' + to: 'eng2_3F_door_inside' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3F_door_inside' + to: 'eng2_3FElevator' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3F_door_inside' + to: 'eng2_3F_door_outside' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3F_door_outside' + to: 'eng2_3F_door_inside' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3F_door_outside' + to: 'eng2_Mech_Office' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_Mech_Office' + to: 'eng2_3F_door_outside' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + - from: 'eng2_3FElevator' + to: 'eng2_7FElevator' + behavior_type: 'spot_person_lead_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_3FElevator_to_2FElevator.walk' + rest_waypoint_id: 'akimbo-scarab-RoO1pzYYARw+EquQk1IIbg==' + goal_waypoint_id: 'dashed-virus-QgetwIkinypWecGNMSS4Yw==' + - from: 'eng2_7FElevator' + to: 'eng2_3FElevator' + behavior_type: 'spot_person_lead_behaviors.elevator_behavior.ElevatorBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A==' + goal_waypoint_id: 'shelfy-dassie-vK9Tf3cAQpdA.25gPJl8OQ==' + - from: 'eng2_2FElevator' + to: 'eng2_subway_pillar' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_subway.walk' + - from: 'eng2_subway_pillar' + to: 'eng2_2FElevator' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_subway.walk' + - from: 'eng2_subway_pillar' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_pillar' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_00' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_table_00' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_01' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_table_01' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_02' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_table_02' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_03' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_table_03' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_04' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_table_04' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_10' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_table_10' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_11' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_table_11' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_12' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_table_12' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_13' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_table_13' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_initial' + to: 'eng2_subway_table_14' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + - from: 'eng2_subway_table_14' + to: 'eng2_subway_initial' + behavior_type: 'spot_person_lead_behaviors.walk_behavior.WalkBehavior' + cost: 10 + args: + graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/nodes.yaml b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/nodes.yaml new file mode 100644 index 0000000000..49006a7fde --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/nodes.yaml @@ -0,0 +1,421 @@ +nodes: + 'dummy': + name_en: 'dummy' + name_jp: 'ダミー' + 'eng2_JSKEntrance': + name_en: '73B1' + name_jp: + - 'JSK' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'hated-salmon-+oAmVbVKzwfer6BexXKNog==' + localization_method: 'waypoint' + 'eng2_73B1': + name_en: '73B1' + name_jp: + - '73 B 1' + - '73B1' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'unkept-scarab-pfr5U1CBRCXx9EOt6eQDCw==' + localization_method: 'waypoint' + 'eng2_73B2': + name_en: '73B2' + name_jp: + - '73 B 2' + - '73B2' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'arty-hyrax-hiViwXJwUrkJ2LR5UAuKyw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/73b2_inside.walk' + id: 'goodly-hogg-cEjt+0aMeZdksVRXrP7.uw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_81C1.walk' + id: 'fuzzed-medusa-wyXWCEjmWas1kMo8EJW8Bw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_83A2.walk' + id: 'dyed-viper-HVQRLVeklEUaE2aXJW+d6Q==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_7FElevator_to_73B2_trash.walk' + id: 'tined-mouse-D.2ws5hStb8AE.C.J5lKEw==' + localization_method: 'fiducial' + 'eng2_73B2_belka_home': + name_en: 'belka' + name_jp: + - 'ベルカ' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'uppity-cayman-rm8aTrilycp7soPeorOdUw==' + localization_method: 'waypoint' + 'eng2_73B2_strelka_home': + name_en: 'strelka' + name_jp: + - 'ストレルカ' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'proof-llama-9slE0zml9BYcNFbpQkmNcg==' + localization_method: 'waypoint' + 'eng2_73B2_shinjo_desk': + name_en: 'shinjo' + name_jp: + - '新城' + - '新城の席' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/73b2_inside.walk' + id: 'cogent-vole-wTZdgPPyjZIObFh2dr7EqA==' + localization_method: 'waypoint' + 'eng2_73A2': + name_en: '73A2' + name_jp: + - '73 A 2' + - '73A2' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'enough-python-pqYBEhDZHuCEapcDxLK32w==' + localization_method: 'waypoint' + 'eng2_73A4': + name_en: '73A4' + name_jp: + - '73 A 4' + - '73A4' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'dashed-kodiak-bKNHSR3S7fXE11UeXeeekA==' + localization_method: 'waypoint' + 'eng2_81C1': + name_en: '81C1' + name_jp: + - '81 C 1' + - '81C1' + - 'プロジェクト部屋' + - 'プロジェクト 部屋' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_81C1.walk' + id: 'sable-narwal-o7qEo7543BUTj4sNKWcQ6g==' + localization_method: 'waypoint' + 'eng2_83A2': + name_en: '83A2' + name_jp: + - '83 A 2' + - '83A2' + - '工作室' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_83A2.walk' + id: 'shaved-koala-aLy+bUIQpAHBQbi5DxeKJA==' + localization_method: 'waypoint' + 'eng2_7FElevator': + name_en: 'Elevator' + name_jp: + - '7階エレベーター' + - '7階 エレベーター' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk' + id: 'looted-cougar-RWT2C0zLJXG.ezYnCuLUeQ==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_7FElevator_to_2FElevator.walk' + id: 'larger-mudcat-0x3bt3Dif.1QU9JfXpyQPw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + id: 'sly-chetah-IZ4pVY7vrqO36OoKCYk9Zg==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_1FElevator.walk' + id: 'meager-fox-bT4DU3uCHBJFWuYILkopVg==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_73B2_trash.walk' + id: 'spruce-frog-bNtHHEv469wvlNxlIxBV5w==' + localization_method: 'fiducial' + switchbot_device: '/eng2/7f/elevator_bot_down' + floor: 7 + floor_height: 24 + 'eng2_2FElevator': + name_en: 'Elevator' + name_jp: + - '2階 エレベーター' + - '2階エレベーター' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_7FElevator_to_2FElevator.walk' + id: 'rainy-collie-uwFW2KVYg2YCUi8FF6N00g==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_2FEntrance.walk' + id: 'snaky-beagle-.fmRm2JflzlV6Dv5fTdnIQ==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_MainGate.walk' + id: 'banner-oxen-UrEwTUbZYL0IQnY5TCFwrA==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk' + id: 'shelfy-dassie-vK9Tf3cAQpdA.25gPJl8OQ==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + id: 'inland-remora-FqtX8ScP+.inEd4xFYKq4A==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_3FElevator_to_2FElevator.walk' + id: 'dashed-virus-QgetwIkinypWecGNMSS4Yw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_MainEntrance.walk' + id: 'mythic-leech-Uslc00eTIhygFsCdH2q1Qg==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_subway.walk' + id: 'brumal-gerbil-NLVgQc0oBXbc8BCZev57LQ==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2fentrance_to_1fentrance.walk' + id: 'felted-weasel-kFVXZpw1.DvEm6FCSgC5XQ==' + localization_method: 'fiducial' + switchbot_device: '/eng2/2f/elevator_bot_up' + floor: 2 + floor_height: 0 + 'eng2_3FElevator': + name_en: 'Elevator' + name_jp: + - '3階 エレベーター' + - '3階エレベーター' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_3FElevator_to_2FElevator.walk' + id: 'eighth-ant-0Ot3VoMm0uiJNBHnbJOF9w==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + id: 'doughy-vulture-kMie2F0bAno9STnKFjNadA==' + localization_method: 'fiducial' + switchbot_device: '/eng2/3f/elevator_bot_up' + floor: 3 + floor_height: 4.5 + 'eng2_1FElevator': + name_en: 'Elevator' + name_jp: + - '1階 エレベーター' + - '1階エレベーター' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_1FElevator.walk' + id: 'brag-eagle-Of.kPg1gvRBhSWzrP+G3Yw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaEast.walk' + id: 'sunlit-iguana-cXGwqooA99O95Hzqas19dw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaSouth.walk' + id: 'gilled-bee-ve3JJ8B2sMTnRL3s8p.SZA==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaSouth_via213.walk' + id: 'oafish-vervet-w.bCe7FjEILCP8oBQg6Miw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_1fentrance_to_1FElevator.walk' + id: 'anemic-spider-TimPFBe0OMBdd+kWxYBW2Q==' + localization_method: 'fiducial' + switchbot_device: '/eng2/1f/elevator_bot_up' + floor: 1 + floor_height: -4.5 + 'eng2_Mech_Office': + name_en: 'Mech Office' + name_jp: + - '機械系 事務室' + - '機械系事務室' + - '事務室' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + id: 'foiled-ibis-dAio3Ld6rCSWQgy9ERDulw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + id: 'askant-beef-JQLXogu60cJgJiSXcyjKUQ==' + localization_method: 'fiducial' + 'eng2_Mailbox': + name_en: 'Mailbox' + name_jp: + - '郵便入れ' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + id: 'sacked-tomcat-wpzX0UCSooUeKol.CShTqQ==' + localization_method: 'waypoint' + 'eng2_CampusMail': + name_en: 'CampusMail' + name_jp: + - '学内便' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MechOffice_inside.walk' + id: 'jovial-civet-OPcK4EyGMTkxjZg04nuO.Q==' + localization_method: 'waypoint' + 'eng2_3F_door_inside': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + id: 'silty-llama-gdnA1Y.5ugUuDuZ9RL9upQ==' + localization_method: 'waypoint' + 'eng2_3F_door_outside': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_3FElevator_to_Mech_Office.walk' + id: 'plane-marlin-gaEqSGH8WX35X1619X0NwQ==' + localization_method: 'waypoint' + 'eng_TelephoneBox': + name_en: 'telephone box' + name_jp: '電話ボックス' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + id: 'slain-pika-VdtPz3lWcp5T+pEArUB3PA==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng_TelephoneBox_to_HongoMainGate.walk' + id: 'aired-hyrax-BF+hXHaUufi.DLd3pEWm5Q==' + localization_method: 'fiducial' + 'eng8_Piloti_Center': + name_en: 'Piloti' + name_jp: '8号館 ピロティ' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_eng8_2FElevator.walk' + id: 'azoic-lion-Iapbto7rzlzuFF0MvXdyPA==' + localization_method: 'fiducial' + 'eng2_subway_pillar': + name_en: 'Piloti' + name_jp: 'サブウェイ' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_subway.walk' + id: 'quack-snail-Zm7LgLSyql.xcoqi9OeCQA==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'risen-bowfin-2he29MpsZLJRToJDhZ3.3g==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + id: 'unread-donkey-2pdTiY6qfMnAfHD8RNH2ZA==' + localization_method: 'fiducial' + 'eng2_subway_initial': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'skimpy-vole-2plogvKFUrjXTCoxsCsNGA==' + localization_method: 'waypoint' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation_02.walk' + id: 'fired-minnow-Pw+F4RFHFQwQolGeuHgkEQ==' + localization_method: 'waypoint' + 'eng2_subway_table_00': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'perky-crake-iZWT3e+cQtLKUCo8iVTu7Q==' + localization_method: 'waypoint' + 'eng2_subway_table_01': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'spacy-eland-WlC6wgdJeo5u.TY6T3hA8Q==' + localization_method: 'waypoint' + 'eng2_subway_table_02': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'apodal-drum-o2pzrnIBJdLVWMX.ElaQOQ==' + localization_method: 'waypoint' + 'eng2_subway_table_03': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'formed-gerbil-yZerC+XIahSrvCJK5Duk6A==' + localization_method: 'waypoint' + 'eng2_subway_table_04': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'ruled-otter-2zSijP2KPICZpqU80utSPw==' + localization_method: 'waypoint' + 'eng2_subway_table_10': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'spiral-falcon-bG1t8To8x.p7BWwOWjH2Lg==' + localization_method: 'waypoint' + 'eng2_subway_table_11': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'manky-mite-EvhV0ln+iiG+pTCfeEnArQ==' + localization_method: 'waypoint' + 'eng2_subway_table_12': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'brash-sheep-NYE7iE0HuaqR0i5xnzYE1A==' + localization_method: 'waypoint' + 'eng2_subway_table_13': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'swampy-jay-jEYEQ+FOe4eMKEhoGco9LA==' + localization_method: 'waypoint' + 'eng2_subway_table_14': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_subway_tablenavigation.walk' + id: 'terse-crest-Toj82ugJt2Y029d7Afhq0A==' + localization_method: 'waypoint' + 'eng2_MainEntrance_upper': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2FElevator_to_MainEntrance.walk' + id: 'violet-ibis-HtCPm39SIDSzoJFVX88S+A==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate.walk' + id: 'about-goat-EHRLqkk7XWmMXAdfcXHP5g==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + id: 'ahead-mink-Fml31k5vw.Y2XR.7v1BSeg==' + localization_method: 'fiducial' + 'eng2_MainEntrance_lower': + name_en: 'Entrance' + name_jp: + - '2号館 正面入口' + - '2号館正面入口' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate.walk' + id: 'aft-tick-clWAq8obxtWm64bTthYwMQ==' + localization_method: 'waypoint' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + id: 'rested-grouse-GzVp6htKyt6kdIcS+Tu6aA==' + localization_method: 'waypoint' + 'eng2_MainEntrance_outer': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate.walk' + id: 'tilled-weevil-slAZLtX3U2NcqaINkGY0Sw==' + localization_method: 'waypoint' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + id: 'sown-pincer-NcgfKhWf9H+B3yWIKA8W7A==' + localization_method: 'waypoint' + 'eng2_1FEntrance_outer': + name_en: '1F Entrance' + name_jp: + - '2号館1階入口' + - '2号館 1階入口' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_1fentrance_to_1FElevator.walk' + id: 'madcap-racoon-WmDiG93hDjlU6eDOUX2Jbg==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2fentrance_to_1fentrance.walk' + id: 'jammed-deer-L6mHZb2lmu+iQwGexsoAvw==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_outside.walk' + id: 'bully-walrus-xsZTCuUHJ0dvzFGGmEZJxQ==' + localization_method: 'fiducial' + 'eng2_corner_northwest': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_outside.walk' + id: 'setose-angus-zflUwvXh2LTV96LNrrAHFg==' + localization_method: 'fiducial' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_2fentrance_to_1fentrance.walk' + id: 'honey-agouti-3CfK3qU9XiErSV9gttqSKA==' + localization_method: 'fiducial' + 'eng2_GarbageCollectionArea_east': + name_en: 'Garbage collection area' + name_jp: 'ゴミ捨て場' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaEast.walk' + id: 'podgy-oxen-t+KIiX.QiVaFGcphBsrekA==' + localization_method: 'waypoint' + 'eng2_GarbageCollectionArea_south': + name_en: 'Garbage collection area' + name_jp: 'ゴミ捨て場' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaSouth.walk' + id: 'atonic-frog-uCPybjcZAN3fQZHNx71GRw==' + localization_method: 'waypoint' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaSouth_via213.walk' + id: 'boxy-hound-2E2IbEf63lXIM91zP.P2Ww==' + localization_method: 'waypoint' + 'eng2_7F_trashcan': + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_7FElevator_to_73B2_trash.walk' + id: 'faddy-cuckoo-ZSHi6wGNKFCQhYetW6iLew==' + localization_method: 'waypoint' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_7f_around.walk' + id: '' + localization_method: 'waypoint' + 'HongoMainGate': + name_en: 'Maingate' + name_jp: '正門' + waypoints_on_graph: + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate.walk' + id: 'aglow-gadfly-tSusAUqUNtqVaKFO49pDbQ==' + localization_method: 'waypoint' + - graph: '$(find spot_autowalk_data)/autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk' + id: 'regal-bison-qmAA7fyZh10L2bzoTNqUFQ==' + localization_method: 'waypoint' diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/package.xml b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/package.xml new file mode 100644 index 0000000000..8dda04d6cd --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/package.xml @@ -0,0 +1,25 @@ + + + spot_behavior_graph + 1.1.0 + The spot_behavior_graph package + + Kei Okada + Koki Shinjo + Koki Shinjo + + BSD + + catkin + + rospy + spot_basic_behaviors + spot_person_lead_behaviors + spot_autowalk_data + + python-graphviz-pip + python-xdot + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/scripts/visualize_map.py b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/scripts/visualize_map.py new file mode 100755 index 0000000000..f26b7742da --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/scripts/visualize_map.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +# -*- encoding: utf-8 -*- + +import argparse +import yaml + +from graphviz import Digraph + +import gi +gi.require_version('Gtk','3.0') +from gi.repository import Gtk + +import xdot + + +class MyDotwindow(xdot.DotWindow): + + def __init__(self): + xdot.DotWindow.__init__(self) + self.dotwidget.connect('clicked', self.on_url_clicked) + + def on_url_clicked(self, widget, url, event): + dialog = Gtk.MessageDialog( + parent=self, + buttons=Gtk.ButtonsType.OK, + message_format='{} clicked'.format(url)) + dialog.connect('response', lambda dialog, response: dialog.destroy()) + dialog.run() + return True + + +def main(): + + parser = argparse.ArgumentParser() + parser.add_argument('filename') + parser.add_argument('--output') + args = parser.parse_args() + + filename = args.filename + + with open(filename, 'r') as f: + map_data = yaml.load(f, Loader=yaml.FullLoader) + + edges = map_data['edges'] + + list_node = [] + list_behavior_type = [] + list_color_for_bt = [] + + for edge in edges: + + if edge['from'] not in list_node: + list_node.append(edge['from']) + + if edge['to'] not in list_node: + list_node.append(edge['to']) + + if edge['behavior_type'] not in list_behavior_type: + list_behavior_type.append(edge['behavior_type']) + + for index, behavior_type in enumerate(list_behavior_type): + list_color_for_bt.append('{} 1.0 1.0'.format(1.0 * index / len(list_behavior_type))) + + dg = Digraph(format='svg') + + for node in list_node: + dg.node(node) + + for edge in edges: + dg.attr('edge', color=list_color_for_bt[list_behavior_type.index(edge['behavior_type'])]) + dg.edge(edge['from'], edge['to'], label=edge['behavior_type'].split('.')[2]) + + + if args.output: + dg.render(args.output) + else: + window = MyDotwindow() + window.set_dotcode(dg.source.encode('utf-8')) + window.connect('delete-event', Gtk.main_quit) + Gtk.main() + + +if __name__ == '__main__': + main() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/CMakeLists.txt new file mode 100644 index 0000000000..ec7345df41 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 2.8.3) +project(spot_behavior_manager) + +find_package(catkin REQUIRED) + +catkin_python_setup() + +catkin_package( +) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/package.xml b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/package.xml new file mode 100644 index 0000000000..02ed8c9256 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/package.xml @@ -0,0 +1,38 @@ + + + spot_behavior_manager + 1.1.0 + The spot_behavior_manager package + + Kei Okada + Koki Shinjo + Koki Shinjo + + BSD + + catkin + + rospy + + actionlib + networkx + rospkg + rospy + roslaunch + sound_play + spot_behavior_manager_msgs + spot_ros_client + std_msgs + + actionlib + networkx + rospkg + rospy + roslaunch + sound_play + spot_behavior_manager_msgs + spot_ros_client + std_msgs + + + \ No newline at end of file diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/setup.py b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/setup.py new file mode 100644 index 0000000000..41e9829c62 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/setup.py @@ -0,0 +1,9 @@ +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['spot_behavior_manager'], + package_dir={'': 'src'} +) + +setup(**d) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/__init__.py b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/__init__.py new file mode 100644 index 0000000000..defc635b8b --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/__init__.py @@ -0,0 +1,2 @@ +import spot_behavior_manager.support_behavior_graph as support_behavior_graph +import spot_behavior_manager.base_behavior as base_behavior diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/base_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/base_behavior.py new file mode 100644 index 0000000000..5c3fa4266f --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/base_behavior.py @@ -0,0 +1,51 @@ +# -*- coding: utf-8 -*- + +import rospy +import roslaunch + + +def load_behavior_class(class_string): + package_name = class_string.split('.')[0] + module_name = class_string.split('.')[1] + class_name = class_string.split('.')[2] + behavior_class = getattr( + getattr(__import__(package_name), module_name), class_name) + return behavior_class + + +class BaseBehavior(object): + + def __init__(self, spot_client, sound_client): + + self.spot_client = spot_client + self.sound_client = sound_client + + def run_initial(self, start_node, end_node, edge, pre_edge): + pass + + def run_main(self, start_node, end_node, edge, pre_edge): + pass + + def run_final(self, start_node, end_node, edge, pre_edge): + pass + + +class SimpleBehavior(BaseBehavior): + + def run_initial(self, start_node, end_node, edge, pre_edge): + + rospy.loginfo('__run_initial() called') + return True + + def run_main(self, start_node, end_node, edge, pre_edge): + + rospy.loginfo('__run_main() called') + rospy.loginfo('start_node: {}'.format(start_node)) + rospy.loginfo('end_node: {}'.format(end_node)) + rospy.loginfo('edge: {}'.format(edge)) + rospy.loginfo('pre_edge: {}'.format(pre_edge)) + return True + + def run_final(self, start_node, end_node, edge, pre_edge): + + rospy.loginfo('__run_finalize() called') diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/behavior_manager_node.py b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/behavior_manager_node.py new file mode 100644 index 0000000000..d3e09497c4 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/behavior_manager_node.py @@ -0,0 +1,249 @@ +# -*- coding: utf-8 -*- + +import copy + +import actionlib +import rospy +import roslaunch +import tf2_ros +import tf2_geometry_msgs +import PyKDL + +from sound_play.libsoundplay import SoundClient +from spot_ros_client.libspotros import SpotRosClient + +from spot_behavior_manager.support_behavior_graph import SupportBehaviorGraph +from spot_behavior_manager.base_behavior import BaseBehavior, load_behavior_class + +from std_msgs.msg import String +from spot_behavior_manager_msgs.msg import LeadPersonAction, LeadPersonFeedback, LeadPersonResult, LeadPersonActionFeedback +from spot_behavior_manager_msgs.srv import ResetCurrentNode, ResetCurrentNodeResponse + + +class BehaviorManagerNode(object): + + def __init__(self): + + # navigation dictonary + raw_edges = rospy.get_param('~map/edges') + raw_nodes = rospy.get_param('~map/nodes') + self.graph = SupportBehaviorGraph(raw_edges, raw_nodes) + self.current_node_id = rospy.get_param('~initial_node_id') + self.pre_edge = None + self.anchor_pose = None + + # silent mode + self.silent_mode = rospy.get_param('~silent_mode', False) + + # + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) + + # get target action name list for synchronizer + self.list_action_name_synchronizer = rospy.get_param( + '~action_names_synchronizer', []) + + # action clients + self.spot_client = SpotRosClient() + self.sound_client = SoundClient( + blocking=False, + sound_action='/robotsound_jp', + sound_topic='/robotsound_jp' + ) + + # publisher + self.pub_current_node_id = rospy.Publisher( + '~current_node_id', String, queue_size=1) + + # reset service + self.service_reset_current_node_id = rospy.Service( + '~reset_current_node_id', + ResetCurrentNode, + self.handler_reset_current_node_id + ) + + # + self.set_anchor_pose() + + # + roslaunch.pmon._init_signal_handlers() + + # subscribers + self.list_behaviors_execution_actions = [] + for action_name in self.list_action_name_synchronizer: + self.list_behaviors_execution_actions.append( + rospy.Subscriber('{}/feedback'.format(action_name), LeadPersonActionFeedback, self.callback_synchronizer) + ) + + # action server + self.server_execute_behaviors = actionlib.SimpleActionServer( + '~execute_behaviors', + LeadPersonAction, + execute_cb=self.handler_execute_behaviors, + auto_start=False + ) + self.server_execute_behaviors.start() + + rospy.loginfo('Initialized!') + + def get_robot_pose(self): + + try: + frame_odom_to_base = tf2_geometry_msgs.transform_to_kdl( + self.tf_buffer.lookup_transform( + 'odom', + 'base_link', + rospy.Time() + ) + ) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + return None + return frame_odom_to_base + + def set_anchor_pose(self): + rospy.loginfo('Set new anchor pose') + self.anchor_pose = self.get_robot_pose() + + def go_back_to_anchor_pose(self): + rospy.loginfo('Going back to an anchor pose') + current_pose = self.get_robot_pose() + if current_pose is None or self.anchor_pose is None: + return False + frame_current_to_anchor = current_pose.Inverse() * self.anchor_pose + self.spot_client.trajectory( + frame_current_to_anchor.p[0], + frame_current_to_anchor.p[1], + frame_current_to_anchor.M.GetRPY()[2], + 10, + blocking=True + ) + + def callback_synchronizer(self, msg): + + rospy.loginfo('Current node is updated to {}'.format(msg.feedback.current_node_id)) + self.current_node_id = msg.feedback.current_node_id + self.pre_edge = None + + def run(self): + + rate = rospy.Rate(1) + while not rospy.is_shutdown(): + rate.sleep() + self.pub_current_node_id.publish(String(data=self.current_node_id)) + + def handler_reset_current_node_id(self, req): + + rospy.loginfo('current_node_id is reset to {}'.format( + req.current_node_id)) + self.current_node_id = req.current_node_id + self.pre_edge = None + self.set_anchor_pose() + return ResetCurrentNodeResponse(success=True) + + def handler_execute_behaviors(self, goal): + + rospy.loginfo('Lead Action started. goal: {}'.format(goal)) + + current_graph = copy.deepcopy(self.graph) + while True: + # path calculation + path = current_graph.calcPath( + self.current_node_id, goal.target_node_id) + if path is None: + rospy.logerr('No path from {} to {}'.format( + self.current_node_id, goal.target_node_id)) + if not self.silent_mode: + self.sound_client.say('パスが見つかりませんでした') + result = LeadPersonResult( + success=False, message='No path found') + self.server_execute_behaviors.set_aborted(result) + return + else: + # navigation of edges in the path + if not self.silent_mode: + self.sound_client.say('目的地に向かいます', blocking=True) + self.go_back_to_anchor_pose() + success_navigation = True + for edge in path: + rospy.loginfo('Navigating Edge {}...'.format(edge)) + try: + if self.navigate_edge(edge): + rospy.loginfo('Edge {} succeeded.'.format(edge)) + self.current_node_id = edge.node_id_to + self.server_execute_behaviors.publish_feedback(LeadPersonFeedback(current_node_id=self.current_node_id)) + self.pre_edge = edge + self.set_anchor_pose() + else: + rospy.logwarn('Edge {} failed'.format(edge)) + if not self.silent_mode: + self.sound_client.say( + '移動に失敗しました。経路を探索し直します。', blocking=True) + self.server_execute_behaviors.publish_feedback(LeadPersonFeedback(current_node_id=self.current_node_id)) + current_graph.remove_edge( + edge.node_id_from, edge.node_id_to) + success_navigation = False + break + except Exception as e: + rospy.logerr( + 'Got an error while navigating edge {}: {}'.format(edge, e)) + if not self.silent_mode: + self.sound_client.say('エラーが発生しました', blocking=True) + self.pre_edge = None + result = LeadPersonResult( + success=False, + message='Got an error while navigating edge {}: {}'.format(edge, e)) + self.server_execute_behaviors.set_aborted(result) + self.anchor_pose = None + return + + if success_navigation: + break + + rospy.loginfo('Goal Reached!') + if not self.silent_mode: + self.sound_client.say('目的地に到着しました.', blocking=True) + result = LeadPersonResult(success=True, message='Goal Reached.') + self.server_execute_behaviors.set_succeeded(result) + self.set_anchor_pose() + return + + def navigate_edge(self, edge): + + # start node id validation + if self.current_node_id != edge.node_id_from: + rospy.logwarn( + 'current_node_id {} does not match node_id_from of edge ({})'.format( + self.current_node_id, + edge.node_id_from) + ) + return False + + # load behavior class + try: + behavior_class = load_behavior_class(edge.behavior_type) + behavior = behavior_class( + self.spot_client, + self.sound_client + ) + except Exception as e: + rospy.logerr( + 'Failed to load and initialize behavior class: {}'.format(e)) + if not self.silent_mode: + self.sound_client.say('行動クラスを読み込めませんでした', blocking=True) + self.pre_edge = None + return False + + node_from = self.graph.nodes[edge.node_id_from] + node_to = self.graph.nodes[edge.node_id_to] + + # Exception from behavior will be caught in handler + success_initial = behavior.run_initial( + node_from, node_to, edge, self.pre_edge) + if success_initial is False: + behavior.run_final(node_from, node_to, edge, self.pre_edge) + return False + else: + success_main = behavior.run_main( + node_from, node_to, edge, self.pre_edge) + behavior.run_final(node_from, node_to, edge, self.pre_edge) + return success_main diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/support_behavior_graph.py b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/support_behavior_graph.py new file mode 100644 index 0000000000..7c61102556 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/support_behavior_graph.py @@ -0,0 +1,99 @@ +# -*- coding: utf-8 -*- + +import networkx as nx + + +class GraphEdge: + + def __init__(self, + node_id_from, + node_id_to, + behavior_type, + cost, + properties + ): + self.node_id_from = node_id_from + self.node_id_to = node_id_to + self.behavior_type = behavior_type + self.cost = cost + self.properties = properties + + +class GraphNode: + + def __init__(self, + node_id, + properties + ): + self.node_id = node_id + self.properties = properties + + +class SupportBehaviorGraph: + + # 現在の SupportBehaviorGraph の仕様 + # 重み付きの有向グラフ + # あるノードからあるノードまでのエッジの数は 0 or 1 + + def __init__(self, raw_edges=[], raw_nodes={}): + + self.edges = {} + self.nodes = {} + self.network = nx.DiGraph() + + for raw_edge in raw_edges: + self.add_edge(raw_edge['from'], + raw_edge['to'], + raw_edge['behavior_type'], + int(raw_edge['cost']), + raw_edge['args']) + + for key, raw_node in raw_nodes.items(): + self.add_node(key, raw_node) + + def calcPath(self, node_id_from, node_id_to): + + try: + node_id_list = nx.shortest_path( + self.network, node_id_from, node_id_to) + except nx.NetworkXNoPath as e: + return None + path = [] + for index in range(len(node_id_list)-1): + path.append(self.edges[node_id_list[index], node_id_list[index+1]]) + return path + + def add_node(self, + node_id, + properties): + # TODO: もしすでにnodeがあれば,適切に上書きするようにする + # NOTICE: node と edge で整合性を保つようには実装されていない + self.nodes[node_id] = GraphNode(node_id, properties) + + def add_edge(self, + node_id_from, + node_id_to, + behavior_type, + cost, + args): + # TODO: もしすでにedgeがあれば,適切に上書きするようにする + # NOTICE: node と edge で整合性を保つようには実装されていない + edge = GraphEdge(node_id_from, + node_id_to, + behavior_type, + cost, + args) + self.edges[node_id_from, node_id_to] = edge + self.network.add_edge(node_id_from, + node_id_to, + weight=cost) + + def remove_node(self, + node_id): + del self.nodes[node_id] + + def remove_edge(self, + node_id_from, + node_id_to): + del self.edges[node_id_from, node_id_to] + self.network.remove_edge(node_id_from, node_id_to) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/CMakeLists.txt new file mode 100644 index 0000000000..f3eb72c71c --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 2.8.3) +project(spot_behavior_manager_msgs) + +find_package(catkin REQUIRED COMPONENTS genmsg actionlib actionlib_msgs) + +add_action_files( + DIRECTORY action + FILES LeadPerson.action +) + +add_service_files( + DIRECTORY srv + FILES ResetCurrentNode.srv +) + +generate_messages(DEPENDENCIES actionlib_msgs) + +catkin_package( + LIBRARIES spot_behavior_manager_msgs + CATKIN_DEPENDS message_runtime +) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/action/LeadPerson.action b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/action/LeadPerson.action new file mode 100644 index 0000000000..8f41dfdaf9 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/action/LeadPerson.action @@ -0,0 +1,6 @@ +string target_node_id +--- +bool success +string message +--- +string current_node_id diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/package.xml b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/package.xml new file mode 100644 index 0000000000..98f45f148c --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/package.xml @@ -0,0 +1,25 @@ + + + spot_behavior_manager_msgs + 1.1.0 + The spot_behavior_manager_msgs package + + Kei Okada + Koki Shinjo + Koki Shinjo + + BSD + + catkin + + actionlib + actionlib_msgs + message_generation + + actionlib + actionlib_msgs + message_runtime + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/srv/ResetCurrentNode.srv b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/srv/ResetCurrentNode.srv new file mode 100644 index 0000000000..8fa01bd4c0 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/srv/ResetCurrentNode.srv @@ -0,0 +1,3 @@ +string current_node_id +--- +bool success diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/CMakeLists.txt new file mode 100644 index 0000000000..e592051190 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 2.8.3) +project(spot_behavior_manager_server) + +find_package(catkin REQUIRED) + +catkin_package( +) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +catkin_install_python(PROGRAMS node_scripts/behavior_manager_server.py node_scripts/interactive_behavior_executor.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/README.md b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/README.md new file mode 100644 index 0000000000..21fbfa82c8 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/README.md @@ -0,0 +1,16 @@ +# spot_behavior_manager_server + +This package provides ros nodes for spot_behavior_manager. + +## ROS Node + +### behavior_manager_server.py + +ROS Node script for spot_behavior_manager. + +See [the source of behavior_manager_node](../spot_behavior_manager/src/spot_behavior_manager/behavior_manager_node.py). + +### interactive_behavior_executor.py + +ROS Node script to execute action of spot_behavior_manager by language. + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/launch/demo.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/launch/demo.launch new file mode 100644 index 0000000000..57bb98f92b --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/launch/demo.launch @@ -0,0 +1,65 @@ + + + + + + + + /narrow_behavior/use_person_detection: $(arg use_person_detection) + /narrow_behavior/use_obstacle_detection: $(arg use_obstacle_detection) + + + + + + + + + + initial_node_id: '$(arg initial_node_id)' + action_names_synchronizer: + - '/spot_person_lead_server/execute_behaviors' + silent_mode: true + + + + + + + + + + + initial_node_id: '$(arg initial_node_id)' + action_names_synchronizer: + - '/spot_behavior_manager_server/execute_behaviors' + silent_mode: true + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/node_scripts/behavior_manager_server.py b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/node_scripts/behavior_manager_server.py new file mode 100755 index 0000000000..cb1a90adc1 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/node_scripts/behavior_manager_server.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import rospy +from spot_behavior_manager.behavior_manager_node import BehaviorManagerNode + +def main(): + + rospy.init_node('behavior_manager_node') + node = BehaviorManagerNode() + node.run() + +if __name__ == '__main__': + main() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/node_scripts/interactive_behavior_executor.py b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/node_scripts/interactive_behavior_executor.py new file mode 100755 index 0000000000..a06d2a75e4 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/node_scripts/interactive_behavior_executor.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python +# -*- encoding: utf-8 -*- + +import actionlib +import rospy + +from spot_behavior_manager_msgs.msg import LeadPersonAction, LeadPersonGoal + +from sound_play.libsoundplay import SoundClient +from ros_speech_recognition import SpeechRecognitionClient + + +def main(): + + rospy.init_node('interactive_behavior_executor') + + speech_recognition_client = SpeechRecognitionClient() + sound_client = SoundClient(sound_action='/robotsound_jp', sound_topic='/robotsound_jp') + action_server_lead_to = actionlib.SimpleActionClient('~execute_behaviors', LeadPersonAction) + + node_list = rospy.get_param('~nodes', {}) + + while not rospy.is_shutdown(): + + rospy.loginfo('Asking package information') + while not rospy.is_shutdown(): + sound_client.say('行き先を教えてください。', blocking=True) + recogntion_result = speech_recognition_client.recognize() + if len(recogntion_result.transcript) == 0: + rospy.logerr('No matching node found from spoken \'{}\''.format(recogntion_result)) + sound_client.say('行き先がわかりませんでした', blocking=True) + continue + recognized_destination = recogntion_result.transcript[0] + target_node_candidates = {} + for node_id, value in node_list.items(): + try: + if not value.has_key('name_jp'): + continue + if type(value['name_jp']) is list: + # DO HOGE + for name in value['name_jp']: + if name.encode('utf-8') == recognized_destination: + target_node_candidates[node_id] = value + else: + if value['name_jp'].encode('utf-8') == recognized_destination: + target_node_candidates[node_id] = value + except Exception as e: + rospy.logerr('Error: {}'.format(e)) + if len(target_node_candidates) == 0: + rospy.logerr('No matching node found from spoken \'{}\''.format(recogntion_result)) + sound_client.say('行き先がわかりませんでした', blocking=True) + else: + rospy.loginfo('target_node_candidates: {}'.format(target_node_candidates)) + break + + target_node_id = target_node_candidates.keys()[0] + target_node_name_jp = node_list[target_node_id]['name_jp'].encode('utf-8') + + sound_client.say('{} へ移動します'.format(target_node_name_jp)) + + rospy.loginfo('executing behaviors to {}'.format(target_node_name_jp)) + action_server_lead_to.send_goal_and_wait(LeadPersonGoal(target_node_id=target_node_id)) + + +if __name__ == '__main__': + main() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/package.xml b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/package.xml new file mode 100644 index 0000000000..084333f92e --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/package.xml @@ -0,0 +1,24 @@ + + + spot_behavior_manager_server + 1.1.0 + The spot_behavior_manager_server package + + Kei Okada + Koki Shinjo + Koki Shinjo + + BSD + + catkin + + rospy + spot_behavior_manager + spot_behavior_map + spot_basic_behaviors + spot_person_lead_behaviors + spot_autowalk_data + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/CMakeLists.txt new file mode 100644 index 0000000000..a01c3a362d --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 2.8.3) +project(spot_person_lead_behaviors) + +find_package(catkin REQUIRED) + +catkin_python_setup() + +catkin_package( +) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +catkin_install_python(PROGRAMS node_scripts/car_tracker.py node_scripts/person_tracker.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/crosswalk_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/crosswalk_detection.launch new file mode 100644 index 0000000000..5ac7cceff2 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/crosswalk_detection.launch @@ -0,0 +1,47 @@ + + + + + + + + frame_id_robot: 'body' + label_person: 0 + dist_visible: 5.0 + timeout_observation: 5.0 + + + + + + + + + + + + + frame_fixed: 'odom' + frame_robot: 'body' + timeout_transform: 0.05 + num_max_track: 10 + thresholds_distance: + '2': 50 + '5': 50 + '7': 50 + threshold_move_velocity: 2 + threshold_lost_duration: 1.0 + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/elevator_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/elevator_detection.launch new file mode 100644 index 0000000000..4693e60ca2 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/elevator_detection.launch @@ -0,0 +1,213 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + max_duration: 5.0 + timeout_duration: 0.05 + timer_duration: 0.1 + reference_frame_id: elevator_door_frame_raw + output_frame_id: elevator_door_frame + fixed_frame_id: odom + + + + + + + + approximate_sync: true + output_frame: body + input_topics: + - $(arg TOPIC_DEPTH_FRONT_RIGHT)/points + - $(arg TOPIC_DEPTH_FRONT_LEFT)/points + - $(arg TOPIC_DEPTH_RIGHT)/points + - $(arg TOPIC_DEPTH_LEFT)/points + - $(arg TOPIC_DEPTH_BACK)/points + + + + + + + + initial_pos: [0, 0, 0.5] + initial_rot: [0, 0, 0] + dimension_x: 0.5 + dimension_y: 0.5 + dimension_z: 0.8 + frame_id: elevator_door_frame + + + + + + + + + + + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/narrow_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/narrow_detection.launch new file mode 100644 index 0000000000..fc9644a5b6 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/narrow_detection.launch @@ -0,0 +1,96 @@ + + + + + + + + + + + frame_id_robot: 'body' + label_person: 0 + dist_visible: 5.0 + timeout_observation: 5.0 + + + + + + + + + + + + + initial_pos: [1.0, 0, 0] + initial_rot: [0, 0, 0] + dimension_x: 1.0 + dimension_y: 1.5 + dimension_z: 0.5 + frame_id: "body" + + + + + + + + + + + + + + + + + + initial_pos: [1.0, 0, 0] + initial_rot: [0, 0, 0] + dimension_x: 1.0 + dimension_y: 1.5 + dimension_z: 0.5 + frame_id: "body" + + + + + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/stair_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/stair_detection.launch new file mode 100644 index 0000000000..bea1639b05 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/stair_detection.launch @@ -0,0 +1,19 @@ + + + + + + + + frame_id_robot: 'body' + label_person: 0 + dist_visible: 5.0 + timeout_observation: 5.0 + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/walk_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/walk_detection.launch new file mode 100644 index 0000000000..6b7f8850c0 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/walk_detection.launch @@ -0,0 +1,19 @@ + + + + + + + + frame_id_robot: 'body' + label_person: 0 + dist_visible: 5.0 + timeout_observation: 5.0 + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/node_scripts/car_tracker.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/node_scripts/car_tracker.py new file mode 100755 index 0000000000..2763078a42 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/node_scripts/car_tracker.py @@ -0,0 +1,219 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import threading + +import PyKDL +import rospy +import message_filters +import tf2_ros +import tf2_geometry_msgs + +from std_msgs.msg import Bool +from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray +from jsk_recognition_msgs.msg import Label, LabelArray + +def convert_msg_point_to_kdl_vector(point): + return PyKDL.Vector(point.x,point.y,point.z) + +class TrackedObject(object): + + UNKNOWN = 0 + FOUND = 1 + MOVING = 2 + LOST = 4 + + def __init__(self, + label, + position, + observed_time + ): + + self.label = label + self.state = self.FOUND + self.position = position + self.velocity = PyKDL.Vector() + self.last_observed = observed_time + self.lock = threading.Lock() + + def checkLost(self, + observed_time, + threshold_lost): + + with self.lock: + if (observed_time - self.last_observed).to_sec() > threshold_lost: + self.state = self.LOST + return True + else: + return False + + def update(self, + observed_position, + observed_time, + robot_position_fixedbased, + threshold_velocity, + threshold_distance): + + with self.lock: + if observed_time < self.last_observed: + rospy.logerr('observed time is earlier than last observed time, dropped.') + return + elif observed_time == self.last_observed: + rospy.loginfo('this object is already observed') + return + + # Update position and velocity + if self.state == self.FOUND or self.state == self.MOVING or self.state == self.APPROCHING: + self.velocity = (observed_position - self.position) / (observed_time - self.last_observed).to_sec() + else: + self.velocity = PyKDL.Vector() + self.position = observed_position + self.last_observed = observed_time + + # Update State + if (self.state == self.LOST or self.state == self.UNKNOWN) or \ + (self.velocity.Norm() < threshold_velocity) or \ + ((robot_position_fixedbased - self.position).Norm() > threshold_distance): + self.state = self.FOUND + else: + self.state = self.MOVING + + def getDistance(self, robot_position): + return (self.position - robot_position).Norm() + +class MultiObjectTracker(object): + + def __init__(self): + + self._frame_fixed = rospy.get_param('~frame_fixed', 'fixed_frame') + self._frame_robot = rospy.get_param('~frame_robot', 'base_link') + self._timeout_transform = rospy.get_param('~timeout_transform',0.05) + slop = rospy.get_param('~slop', 0.1) + + # parameters for multi object trackers + ## maximum number of tracking objects + self._num_max_track = rospy.get_param('~num_max_track', 10) + ## threshold for MOT state update + self._thresholds_distance = rospy.get_param('~thresholds_distance', {}) + self._threshold_velocity = rospy.get_param('~threshold_move_velocity', 1.0) + self._threshold_lost = rospy.get_param('~threshold_lost_duration', 1.0) + + # members + self._lock_for_dict_objects = threading.RLock() + self._dict_objects = {} + + # TF + self._tf_buffer = tf2_ros.Buffer() + self._tf_listener = tf2_ros.TransformListener(self._tf_buffer) + + # Publisher + ## publsher for if there is a moving object or not + self._pub_visible = rospy.Publisher( + '~visible', + Bool, + queue_size=1 + ) + + # Subscriber + rate = rospy.Rate(1) + while not rospy.is_shutdown(): + try: + rospy.wait_for_message('~input_bbox_array', BoundingBoxArray, 3) + rospy.wait_for_message('~input_tracking_labels', LabelArray, 3) + break + except (rospy.ROSException, rospy.ROSInterruptException) as e: + rospy.logerr('subscribing topic seems not to be published.') + rospy.logerr('Error: {}'.format(e)) + rate.sleep() + mf_sub_bbbox_array = message_filters.Subscriber('~input_bbox_array', BoundingBoxArray) + mf_sub_tracking_labels = message_filters.Subscriber('~input_tracking_labels', LabelArray) + ts = message_filters.ApproximateTimeSynchronizer([mf_sub_bbbox_array, mf_sub_tracking_labels], 10, slop) + ts.registerCallback(self._cb_object) + + def _cb_object(self, + msg_bbox_array, + msg_tracking_labels): + + if msg_bbox_array.header.frame_id != self._frame_fixed: + rospy.logerr('frame_id of bbox (which is {}) array must be the same `~frame_fixed` which is {}'.format( + msg_bbox_array.header.frame_id, + self._frame_fixed)) + return + + time_observed = msg_tracking_labels.header.stamp + + try: + pykdl_transform_fixed_to_robot = tf2_geometry_msgs.transform_to_kdl( + self._tf_buffer.lookup_transform( + self._frame_fixed, + self._frame_robot, + time_observed, + timeout=rospy.Duration(self._timeout_transform) + ) + ) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + rospy.logerr('lookup transform failed. {}'.format(e)) + return + + with self._lock_for_dict_objects: + # add new object and update existing object state + for bbox, tracking_label in zip(msg_bbox_array.boxes, msg_tracking_labels.labels): + + if bbox.label not in self._thresholds_distance.keys(): + continue + + if tracking_label.id not in self._dict_objects: + if len(self._dict_objects) < self._num_max_track: + self._dict_objects[tracking_label.id] = \ + TrackedObject( + bbox.label, + convert_msg_point_to_kdl_vector(bbox.pose.position), + time_observed + ) + else: + rospy.logwarn('number of objects exceeds max track. dropped.') + else: + self._dict_objects[tracking_label.id].update( + convert_msg_point_to_kdl_vector(bbox.pose.position), + time_observed, + pykdl_transform_fixed_to_robot.p, + self._threshold_velocity, + self._thresholds_distance[str(bbox.label)] + ) + # check if there is lost object + to_be_removed = [] + for key in self._dict_objects: + is_lost = self._dict_objects[key].checkLost( + time_observed, + self._threshold_lost + ) + if is_lost: + to_be_removed.append(key) + # remove lost object from dict + for key in to_be_removed: + self._dict_objects.pop(key) + + def spin(self): + + rate = rospy.Rate(2) + while not rospy.is_shutdown(): + + rate.sleep() + + with self._lock_for_dict_objects: + + exist_moving_object = False + for key in self._dict_objects: + if self._dict_objects[key].state == TrackedObject.MOVING: + exist_moving_object = True + + self._pub_visible.publish(Bool(exist_moving_object)) + +def main(): + + rospy.init_node('person_tracker') + tracker = MultiObjectTracker() + tracker.spin() + +if __name__=='__main__': + main() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/node_scripts/person_tracker.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/node_scripts/person_tracker.py new file mode 100755 index 0000000000..7ecdc56219 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/node_scripts/person_tracker.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import PyKDL +import actionlib +import rospy +import tf2_ros +import tf2_geometry_msgs + +from jsk_recognition_msgs.msg import BoundingBoxArray +from geometry_msgs.msg import PoseArray, Pose +from std_msgs.msg import Bool + +def convert_msg_point_to_kdl_vector(point): + return PyKDL.Vector(point.x,point.y,point.z) + +class PersonTracker(object): + + def __init__(self): + + self._frame_id_robot = rospy.get_param('~frame_id_robot','body') + self._label_person = rospy.get_param('~label_person',0) + self._dist_visible = rospy.get_param('~dist_visible',1.0) + self._timeout_transform = rospy.get_param('~timeout_transform',0.05) + + self._tf_buffer = tf2_ros.Buffer() + self._tf_listener = tf2_ros.TransformListener(self._tf_buffer) + + self._sub_bbox_array = rospy.Subscriber( + '~bbox_array', + BoundingBoxArray, + self._cb_bbox_array + ) + self._pub_visible = rospy.Publisher( + '~visible', + Bool, + queue_size=1 + ) + self._pub_people_pose_array = rospy.Publisher( + '~people_pose_array', + PoseArray, + queue_size=1 + ) + + def _cb_bbox_array(self,msg): + + frame_id_msg = msg.header.frame_id + frame_id_robot = self._frame_id_robot + + try: + frame_fixed_to_robot = tf2_geometry_msgs.transform_to_kdl( + self._tf_buffer.lookup_transform( + frame_id_robot, + frame_id_msg, + msg.header.stamp, + timeout=rospy.Duration(self._timeout_transform) + ) + ) + except (tf2_ros.LookupException, + tf2_ros.ConnectivityException, + tf2_ros.ExtrapolationException) as e: + rospy.logerr('lookup transform failed. {}'.format(e)) + return + + # check if there is a person + is_person_visible = False + people_pose_array = PoseArray() + people_pose_array.header.stamp = msg.header.stamp + people_pose_array.header.frame_id = frame_id_robot + for bbox in msg.boxes: + if bbox.label == self._label_person: + vector_bbox = convert_msg_point_to_kdl_vector(bbox.pose.position) + vector_bbox_robotbased = frame_fixed_to_robot * vector_bbox + rospy.logdebug('Person found at the distance {}'.format(vector_bbox_robotbased.Norm())) + if vector_bbox_robotbased.Norm() < self._dist_visible: + pose = Pose() + pose.position.x = vector_bbox_robotbased[0] + pose.position.y = vector_bbox_robotbased[1] + pose.position.z = vector_bbox_robotbased[2] + pose.orientation.w = 1.0 + people_pose_array.poses.append(pose) + is_person_visible = True + break + self._pub_people_pose_array.publish(people_pose_array) + self._pub_visible.publish(Bool(is_person_visible)) + +def main(): + + rospy.init_node('person_tracker') + person_tracker = PersonTracker() + rospy.spin() + +if __name__=='__main__': + main() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/package.xml b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/package.xml new file mode 100644 index 0000000000..b2fdca62b2 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/package.xml @@ -0,0 +1,18 @@ + + + spot_person_lead_behaviors + 1.1.0 + The spot_person_lead_behaviors package + + Kei Okada + Koki Shinjo + Koki Shinjo + BSD + + catkin + + spot_behavior_manager + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/setup.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/setup.py new file mode 100644 index 0000000000..ed21b775e2 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/setup.py @@ -0,0 +1,9 @@ +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['spot_person_lead_behaviors'], + package_dir={'': 'src'} + ) + +setup(**d) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/__init__.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/__init__.py new file mode 100644 index 0000000000..cc1bd31158 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/__init__.py @@ -0,0 +1,5 @@ +import spot_person_lead_behaviors.walk_behavior as walk_behavior +import spot_person_lead_behaviors.narrow_behavior as narrow_behavior +import spot_person_lead_behaviors.crosswalk_behavior as crosswalk_behavior +import spot_person_lead_behaviors.stair_behavior as stair_behavior +import spot_person_lead_behaviors.elevator_behavior as elevator_behavior diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/crosswalk_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/crosswalk_behavior.py new file mode 100644 index 0000000000..2ed96d7ab2 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/crosswalk_behavior.py @@ -0,0 +1,201 @@ +# -*- coding: utf-8 -*- + +from spot_behavior_manager.base_behavior import BaseBehavior + +import roslaunch +import rospkg +import rospy + +import threading + +from std_msgs.msg import Bool + +class CrosswalkBehavior(BaseBehavior): + + def callback_person_visible(self, msg): + + if self.person_state_visible != msg.data: + self.person_starttime_visibility = rospy.Time.now() + self.person_duration_visibility = rospy.Duration() + self.person_state_visible = msg.data + else: + self.person_duration_visibility = rospy.Time.now() - self.person_starttime_visibility + + def callback_car_visible(self, msg): + + if self.car_state_visible != msg.data: + self.car_starttime_visibility = rospy.Time.now() + self.car_duration_visibility = rospy.Duration() + self.car_state_visible = msg.data + else: + self.car_duration_visibility = rospy.Time.now() - self.car_starttime_visibility + + def run_initial(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_initial() called') + + # launch recognition launch + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch_path = rospkg.RosPack().get_path('spot_person_lead_behaviors') +\ + '/launch/crosswalk_detection.launch' + roslaunch_cli_args = [roslaunch_path] + roslaunch_file = roslaunch.rlutil.resolve_launch_arguments(roslaunch_cli_args) + self.roslaunch_parent = roslaunch.parent.ROSLaunchParent( + uuid, + roslaunch_file + ) + self.roslaunch_parent.start() + + # value for person checker + self.subscriber_person_visible = None + self.person_state_visible = False + self.person_starttime_visibility = rospy.Time.now() + self.person_duration_visibility = rospy.Duration(10) + + # value for car checker + self.subscriber_car_visible = None + self.car_state_visible = False + self.car_starttime_visibility = rospy.Time.now() + self.car_duration_visibility = rospy.Duration(10) + + # start subscribers + try: + self.subscriber_person_visible = rospy.Subscriber( + '/crosswalk_detection_person_tracker/visible', + Bool, + self.callback_person_visible + ) + self.subscriber_car_visible = rospy.Subscriber( + '/crosswalk_detection_car_tracker/visible', + Bool, + self.callback_car_visible + ) + except Exception as e: + rospy.logerr('{}'.format(e)) + return False + + return True + + def run_main(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_main() called') + + graph_name = edge.properties['graph'] + start_id = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['id'] + end_id = filter( + lambda x: x['graph'] == graph_name, + end_node.properties['waypoints_on_graph'] + )[0]['id'] + localization_method = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['localization_method'] + + # graph uploading and localization + if pre_edge is not None and \ + graph_name == pre_edge.properties['graph']: + rospy.loginfo('graph upload and localization skipped.') + else: + # Upload + ret = self.spot_client.upload_graph(graph_name) + if ret[0]: + rospy.loginfo('graph {} uploaded.'.format(graph_name)) + else: + rospy.logerr('graph uploading failed: {}'.format(ret[1])) + return False + # Localization + if localization_method == 'fiducial': + ret = self.spot_client.set_localization_fiducial() + elif localization_method == 'waypoint': + ret = self.spot_client.set_localization_waypoint(start_id) + else: + ret = (False,'Unknown localization method') + if ret[0]: + rospy.loginfo('robot is localized on the graph.') + else: + rospy.logwarn('Localization failed: {}'.format(ret[1])) + return False + + # checking if there is a moving car visible or not + self.sound_client.say('車が通るかみています',blocking=True) + safety_count = 0 + rate = rospy.Rate(1) + while not rospy.is_shutdown(): + rate.sleep() + rospy.loginfo('safety_count: {}'.format(safety_count)) + if safety_count > 10: + break; + try: + rospy.loginfo('car visible: {}'.format(self.car_state_visible)) + if self.car_state_visible == True: + safety_count = 0 + self.sound_client.say('車が通ります',blocking=True) + else: + safety_count += 1 + except Exception as e: + rospy.logwarn('{}'.format(e)) + safety_count = 0 + + # start leading + success = False + rate = rospy.Rate(10) + self.sound_client.say('ついてきてください',blocking=True) + self.spot_client.navigate_to( end_id, blocking=False) + while not rospy.is_shutdown(): + rate.sleep() + + if self.spot_client.wait_for_navigate_to_result(rospy.Duration(0.1)): + result = self.spot_client.get_navigate_to_result() + success = result.success + rospy.loginfo('result: {}'.format(result)) + break + + if not self.person_state_visible and self.person_duration_visibility > rospy.Duration(0.5): + flag_speech = False + def notify_visibility(): + self.sound_client.say( + '近くに人が見えません', + volume=1.0, + blocking=True + ) + flag_speech = False + speech_thread = threading.Thread(target=notify_visibility) + speech_thread.start() + while not self.person_state_visible and self.person_duration_visibility > rospy.Duration(0.5): + rate.sleep() + self.spot_client.pub_cmd_vel(0,0,0) + if not flag_speech: + flag_speech = True + speech_thread = threading.Thread(target=notify_visibility) + speech_thread.start() + if not self.person_state_visible and self.person_duration_visibility > rospy.Duration(5.0): + self.spot_client.cancel_navigate_to() + if self.person_state_visible: + self.spot_client.navigate_to( end_id, blocking=False) + + # recovery on failure + if not success: + self.sound_client.say('失敗したので元に戻ります', blocking=True) + self.spot_client.navigate_to( start_id, blocking=True) + self.spot_client.wait_for_navigate_to_result() + + return success + + def run_final(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_finalize() called') + + self.spot_client.cancel_navigate_to() + + if self.subscriber_person_visible != None: + self.subscriber_person_visible.unregister() + self.subscriber_person_visible = None + + if self.subscriber_car_visible != None: + self.subscriber_car_visible.unregister() + self.subscriber_car_visible = None + + self.roslaunch_parent.shutdown() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/elevator_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/elevator_behavior.py new file mode 100644 index 0000000000..fe68d2d10b --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/elevator_behavior.py @@ -0,0 +1,288 @@ +# -*- coding: utf-8 -*- + +from spot_behavior_manager.base_behavior import BaseBehavior + +import actionlib +import roslaunch +import rospkg +import rospy + +import math + +from switchbot_ros.msg import SwitchBotCommandGoal, SwitchBotCommandAction +from sensor_msgs.msg import PointCloud2 +from spinal.msg import Barometer, Imu +from geometry_msgs.msg import Quaternion + +class ElevatorBehavior(BaseBehavior): + + def door_point_cloud_callback(self, msg): + if len(msg.data) == 0: + self.door_is_open = True + else: + self.door_is_open = False + + def barometer_callback(self, msg): + + rospy.logdebug('altitude: {}'.format(msg.altitude)) + if math.fabs(msg.altitude - self.target_altitude) < self.threshold_altitude: + self.is_target_floor = True + else: + self.is_target_floor = False + + def imu_callback(self, msg): + + rospy.logdebug('acc z: {}'.format(msg.acc_data[2])) + if math.fabs(msg.acc_data[2] - self.stable_acc_z) < self.threshold_acc: + self.elevator_stop_acc = True + else: + self.elevator_stop_acc = False + + def run_initial(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_initial() called') + + # launch recognition launch + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch_path = rospkg.RosPack().get_path('spot_person_lead_behaviors') +\ + '/launch/elevator_detection.launch' + roslaunch_cli_args = [roslaunch_path] + roslaunch_file = roslaunch.rlutil.resolve_launch_arguments(roslaunch_cli_args) + self.roslaunch_parent = roslaunch.parent.ROSLaunchParent( + uuid, + roslaunch_file + ) + self.roslaunch_parent.start() + + # value for door openring checker + self.door_is_open = False + self.subscriber_door_check = None + + # value for floor detection + self.threshold_altitude = rospy.get_param('/elevator_behavior/threshold_altitude', 2) + self.is_target_floor = False + self.target_altitude = 0 + self.subscriber_floor_detection = None + + # + self.threshold_acc = rospy.get_param('/elevator_behavior/threshold_acc', 0.2) + self.stable_acc_z = 0 + self.elevator_stop_acc = False + self.subscriber_imu = None + + # value for switchbot + self.action_client_switchbot = actionlib.SimpleActionClient( + '/switchbot_ros/switch', + SwitchBotCommandAction + ) + + return True + + def run_main(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_main() called') + + graph_name = edge.properties['graph'] + start_id = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['id'] + end_id = edge.properties['goal_waypoint_id'] + rest_waypoint_id = edge.properties['rest_waypoint_id'] + localization_method = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['localization_method'] + + start_altitude = start_node.properties['floor_height'] + end_altitude = end_node.properties['floor_height'] + end_floor = end_node.properties['floor'] + + # graph uploading and localization + if pre_edge is not None and \ + graph_name == pre_edge.properties['graph']: + rospy.loginfo('graph upload and localization skipped.') + else: + # Upload + ret = self.spot_client.upload_graph(graph_name) + if ret[0]: + rospy.loginfo('graph {} uploaded.'.format(graph_name)) + else: + rospy.logerr('graph uploading failed: {}'.format(ret[1])) + return False + # Localization + if localization_method == 'fiducial': + ret = self.spot_client.set_localization_fiducial() + elif localization_method == 'waypoint': + ret = self.spot_client.set_localization_waypoint(start_id) + else: + ret = (False,'Unknown localization method') + if ret[0]: + rospy.loginfo('robot is localized on the graph.') + else: + rospy.logwarn('Localization failed: {}'.format(ret[1])) + return False + + # start floor detection + try: + current_altitude = rospy.wait_for_message( '/spinal/baro', Barometer, rospy.Duration(5)).altitude + self.target_altitude = current_altitude - ( start_altitude - end_altitude ) + self.subscriber_floor_detection = rospy.Subscriber( + '/spinal/baro', + Barometer, + self.barometer_callback) + except Exception as e: + rospy.logerr('{}'.format(e)) + return False + + # start imu + try: + self.stable_acc_z = rospy.wait_for_message( '/spinal/imu', Imu, rospy.Duration(5)).acc_data[2] + self.subscriber_imu = rospy.Subscriber( + '/spinal/imu', + Imu, + self.imu_callback) + except Exception as e: + rospy.logerr('{}'.format(e)) + return False + + # start door opening check from outside + self.subscriber_door_check = rospy.Subscriber( + '/spot_recognition/elevator_door_points', + PointCloud2, + self.door_point_cloud_callback) + + self.sound_client.say('エレベーターに乗ります', blocking=True) + + # push button with switchbot + rospy.loginfo('calling elevator when riding...') + self.sound_client.say('エレベーターを呼んでいます', blocking=True) + success_calling = False + if not self.action_client_switchbot.wait_for_server(rospy.Duration(10)): + rospy.logerr('switchbot server seems to fail.') + return False + else: + switchbot_goal = SwitchBotCommandGoal() + switchbot_goal.device_name = start_node.properties['switchbot_device'] + switchbot_goal.command = 'press' + count = 0 + while True: + self.action_client_switchbot.send_goal(switchbot_goal) + if self.action_client_switchbot.wait_for_result(rospy.Duration(10)): + break + count += 1 + if count >= 3: + rospy.logerr('switchbot calling failed.') + return False + result = self.action_client_switchbot.get_result() + rospy.loginfo('switchbot result: {}'.format(result)) + if not result.done: + rospy.logerr('switchbot calling failed.') + return False + rospy.loginfo('elevator calling when riding on has succeeded') + + # wait for elevator + rate = rospy.Rate(1) + door_open_count = 0 + while not rospy.is_shutdown(): + rate.sleep() + if self.door_is_open: + door_open_count += 1 + else: + door_open_count = 0 + if door_open_count >= 2: + break + rospy.loginfo('elevator door opened.') + self.subscriber_door_check.unregister() + self.subscriber_door_check = None + + # start navigation to rest point + rate = rospy.Rate(10) + self.sound_client.say('エレベータに乗り込みます。ご注意ください。', blocking=False) + self.spot_client.navigate_to( rest_waypoint_id, blocking=False) + ## call elevator from destination floor while + rospy.loginfo('calling elevator when getting off...') + switchbot_goal = SwitchBotCommandGoal() + switchbot_goal.device_name = end_node.properties['switchbot_device'] + switchbot_goal.command = 'press' + self.action_client_switchbot.send_goal(switchbot_goal) + ## + if not self.action_client_switchbot.wait_for_result(timeout=rospy.Duration(20)): + rospy.logerr('Switchbot timeout') + self.spot_client.wait_for_navigate_to_result() + self.spot_client.navigate_to( start_id, blocking=True) + self.spot_client.wait_for_navigate_to_result() + return False + result_switchbot = self.action_client_switchbot.get_result() + self.spot_client.wait_for_navigate_to_result() + result_navigation = self.spot_client.get_navigate_to_result() + ## recovery when riding on + if not result_navigation.success or not result_switchbot.done: + rospy.logerr('Failed to ride on a elevator. result_navigation: {}, result_switchbot: {}'.format(result_navigation,result_switchbot)) + self.spot_client.navigate_to( start_id, blocking=True) + self.spot_client.wait_for_navigate_to_result() + return False + else: + rospy.loginfo('Riding on succeded.') + + self.sound_client.say('{}階に行きます'.format(end_floor), blocking=False) + + # start door openning check from inside + self.subscriber_door_check = rospy.Subscriber( + '/spot_recognition/elevator_door_points', + PointCloud2, + self.door_point_cloud_callback) + + # check if the door is closed + rate = rospy.Rate(2) + while not rospy.is_shutdown(): + rate.sleep() + if not self.door_is_open: + break + rospy.loginfo('elevator door closed') + + # check if the door is open and at the target floor + rate = rospy.Rate(1) + while not rospy.is_shutdown(): + rate.sleep() + rospy.loginfo('door_is_open: {}, is_target_floor: {}, stop from acc: {}'.format( + self.door_is_open, self.is_target_floor, self.elevator_stop_acc)) + if self.door_is_open and self.is_target_floor and self.elevator_stop_acc: + break + rospy.loginfo('elevator door opened and at the target_floor') + self.sound_client.say('エレベーターが{}階に到着しました'.format(end_floor), blocking=False) + + # dance before starting to move + self.spot_client.pub_body_pose(0.0,Quaternion(w=1)) + self.spot_client.stand() + rospy.sleep(0.5) + self.spot_client.pub_body_pose(-0.2,Quaternion(w=1)) + self.spot_client.stand() + rospy.sleep(0.5) + self.spot_client.pub_body_pose(0.0,Quaternion(w=1)) + self.spot_client.stand() + + # get off the elevator + self.sound_client.say('エレベーターからおります', blocking=False) + self.spot_client.navigate_to(end_id, blocking=True) + result = self.spot_client.get_navigate_to_result() + + return result.success + + def run_final(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_finalize() called') + + if self.subscriber_door_check != None: + self.subscriber_door_check.unregister() + self.subscriber_door_check = None + + if self.subscriber_floor_detection != None: + self.subscriber_floor_detection.unregister() + self.subscriber_floor_detection = None + + if self.subscriber_imu != None: + self.subscriber_imu.unregister() + self.subscriber_imu = None + + self.roslaunch_parent.shutdown() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/narrow_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/narrow_behavior.py new file mode 100644 index 0000000000..b7a90ecf26 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/narrow_behavior.py @@ -0,0 +1,231 @@ +# -*- coding: utf-8 -*- + +from spot_behavior_manager.base_behavior import BaseBehavior + +import roslaunch +import rospkg +import rospy + +import threading + +from std_msgs.msg import Bool +from sensor_msgs.msg import PointCloud2 + +class NarrowBehavior(BaseBehavior): + + def callback_visible(self, msg): + + if self.state_visible != msg.data: + self.starttime_visibility = rospy.Time.now() + self.duration_visibility = rospy.Duration() + self.state_visible = msg.data + else: + self.duration_visibility = rospy.Time.now() - self.starttime_visibility + + def callback_obstacle_right(self, msg): + + if len(msg.data) > 0: + self.last_obstacle_right = msg.header.stamp + + def callback_obstacle_left(self, msg): + + if len(msg.data) > 0: + self.last_obstacle_left = msg.header.stamp + + def run_initial(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_initial() called') + + # get parameters + self.use_person_detection = rospy.get_param('/narrow_behavior/use_person_detection', True) + self.use_obstacle_detection = rospy.get_param('/narrow_behavior/use_obstacle_detection', True) + + # launch recognition launch + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch_path = rospkg.RosPack().get_path('spot_person_lead_behaviors') +\ + '/launch/narrow_detection.launch' + roslaunch_cli_args = [roslaunch_path] + roslaunch_file = roslaunch.rlutil.resolve_launch_arguments(roslaunch_cli_args) + self.roslaunch_parent = roslaunch.parent.ROSLaunchParent( + uuid, + roslaunch_file + ) + self.roslaunch_parent.start() + + # value for person checker + self.subscriber_visible = None + self.state_visible = False + self.starttime_visibility = rospy.Time.now() + self.duration_visibility = rospy.Duration(10) + + # value for obstacle detection + self.subscriber_obstacle_right = None + self.subscriber_obstacle_left = None + self.last_obstacle_right = rospy.Time() + self.last_obstacle_left = rospy.Time() + + # start subscribers + try: + self.subscriber_obstacle_right = rospy.Subscriber( + '/spot_recognition/right_obstacle', + PointCloud2, + self.callback_obstacle_right + ) + self.subscriber_obstacle_left = rospy.Subscriber( + '/spot_recognition/left_obstacle', + PointCloud2, + self.callback_obstacle_left + ) + self.subscriber_visible = rospy.Subscriber( + '/narrow_detection_person_tracker/visible', + Bool, + self.callback_visible + ) + except Exception as e: + rospy.logerr('{}'.format(e)) + return False + + return True + + def run_main(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_main() called') + + graph_name = edge.properties['graph'] + start_id = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['id'] + end_id = filter( + lambda x: x['graph'] == graph_name, + end_node.properties['waypoints_on_graph'] + )[0]['id'] + localization_method = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['localization_method'] + + # graph uploading and localization + if pre_edge is not None and \ + graph_name == pre_edge.properties['graph']: + rospy.loginfo('graph upload and localization skipped.') + else: + # Upload + ret = self.spot_client.upload_graph(graph_name) + if ret[0]: + rospy.loginfo('graph {} uploaded.'.format(graph_name)) + else: + rospy.logerr('graph uploading failed: {}'.format(ret[1])) + return False + # Localization + if localization_method == 'fiducial': + ret = self.spot_client.set_localization_fiducial() + elif localization_method == 'waypoint': + ret = self.spot_client.set_localization_waypoint(start_id) + else: + ret = (False,'Unknown localization method') + if ret[0]: + rospy.loginfo('robot is localized on the graph.') + else: + rospy.logwarn('Localization failed: {}'.format(ret[1])) + return False + + # thread for obstacle notification + flag_valid_obstacle_notification = True + def notify_obstacle(): + rate = rospy.Rate(2) + while flag_valid_obstacle_notification and not rospy.is_shutdown(): + current_time = rospy.Time.now() + if current_time - self.last_obstacle_right < rospy.Duration(1.0) \ + and current_time - self.last_obstacle_left < rospy.Duration(1.0): + self.sound_client.say( + '周囲にご注意ください', + blocking=True + ) + elif current_time - self.last_obstacle_right < rospy.Duration(1.0): + self.sound_client.say( + '右にご注意ください', + blocking=True + ) + elif current_time - self.last_obstacle_left < rospy.Duration(1.0): + self.sound_client.say( + '左にご注意ください', + blocking=True + ) + rospy.logwarn('notify_obstacle finished.') + if self.use_obstacle_detection: + thread_notify_obstacle = threading.Thread(target=notify_obstacle) + else: + thread_notify_obstacle = None + + # start leading + success = False + rate = rospy.Rate(10) + self.sound_client.say('ついてきてください',blocking=True) + self.spot_client.navigate_to( end_id, blocking=False) + if thread_notify_obstacle is not None: + thread_notify_obstacle.start() + while not rospy.is_shutdown(): + rate.sleep() + + if self.spot_client.wait_for_navigate_to_result(rospy.Duration(0.1)): + result = self.spot_client.get_navigate_to_result() + success = result.success + rospy.loginfo('result: {}'.format(result)) + break + + if self.use_person_detection and not self.state_visible and self.duration_visibility > rospy.Duration(0.5): + flag_speech = False + def notify_visibility(): + self.sound_client.say( + '近くに人が見えません', + volume=1.0, + blocking=True + ) + flag_speech = False + speech_thread = threading.Thread(target=notify_visibility) + speech_thread.start() + while not self.state_visible and self.duration_visibility > rospy.Duration(0.5): + rate.sleep() + self.spot_client.pub_cmd_vel(0,0,0) + if not flag_speech: + flag_speech = True + speech_thread = threading.Thread(target=notify_visibility) + speech_thread.start() + if not self.state_visible and self.duration_visibility > rospy.Duration(5.0): + self.spot_client.cancel_navigate_to() + if self.state_visible: + self.spot_client.navigate_to( end_id, blocking=False) + + # stop notification thread + flag_valid_obstacle_notification = False + if thread_notify_obstacle is not None: + thread_notify_obstacle.join() + + # recovery on failure + if not success: + self.sound_client.say('失敗したので元に戻ります', blocking=True) + self.spot_client.navigate_to( start_id, blocking=True) + self.spot_client.wait_for_navigate_to_result() + + return success + + def run_final(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_finalize() called') + + self.spot_client.cancel_navigate_to() + + if self.subscriber_obstacle_right != None: + self.subscriber_obstacle_right.unregister() + self.subscriber_obstacle_right = None + + if self.subscriber_obstacle_left != None: + self.subscriber_obstacle_left.unregister() + self.subscriber_obstacle_left = None + + if self.subscriber_visible != None: + self.subscriber_visible.unregister() + self.subscriber_visible = None + + self.roslaunch_parent.shutdown() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/stair_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/stair_behavior.py new file mode 100644 index 0000000000..83d0613980 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/stair_behavior.py @@ -0,0 +1,194 @@ +# -*- coding: utf-8 -*- + +from spot_behavior_manager.base_behavior import BaseBehavior + +import roslaunch +import rospkg +import rospy + +from std_msgs.msg import Bool +from geometry_msgs.msg import PoseArray + +import threading + +class StairBehavior(BaseBehavior): + + def callback_visible(self, msg): + + if self.state_visible != msg.data: + self.starttime_visibility = rospy.Time.now() + self.duration_visibility = rospy.Duration() + self.state_visible = msg.data + else: + self.duration_visibility = rospy.Time.now() - self.starttime_visibility + + def callback_people_pose_array(self, msg): + + self.exist_person_down = False + for pose in msg.poses: + if pose.position.z < -0.5: + self.exist_person_down = True + + def run_initial(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_initial() called') + + # launch recognition launch + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch_path = rospkg.RosPack().get_path('spot_person_lead_behaviors') +\ + '/launch/stair_detection.launch' + roslaunch_cli_args = [roslaunch_path] + roslaunch_file = roslaunch.rlutil.resolve_launch_arguments(roslaunch_cli_args) + self.roslaunch_parent = roslaunch.parent.ROSLaunchParent( + uuid, + roslaunch_file + ) + self.roslaunch_parent.start() + + # value for person checker + self.subscriber_visible = None + self.state_visible = False + self.starttime_visibility = rospy.Time.now() + self.duration_visibility = rospy.Duration(10) + + # value for people pose array + self.subscriber_people_pose_array = None + self.msg_people_pose_array = None + self.exist_person_down = False + + # start subscribers + try: + self.subscriber_visible = rospy.Subscriber( + '/stair_detection_person_tracker/visible', + Bool, + self.callback_visible + ) + self.subscriber_people_pose_array = rospy.Subscriber( + '/stair_detection_person_tracker/people_pose_array', + PoseArray, + self.callback_people_pose_array + ) + except Exception as e: + rospy.logerr('{}'.format(e)) + return False + + return True + + def run_main(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_main() called') + + graph_name = edge.properties['graph'] + start_id = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['id'] + end_id = filter( + lambda x: x['graph'] == graph_name, + end_node.properties['waypoints_on_graph'] + )[0]['id'] + localization_method = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['localization_method'] + stair_nums = edge.properties['stair_nums'] + + # graph uploading and localization + if pre_edge is not None and \ + graph_name == pre_edge.properties['graph']: + rospy.loginfo('graph upload and localization skipped.') + else: + # Upload + ret = self.spot_client.upload_graph(graph_name) + if ret[0]: + rospy.loginfo('graph {} uploaded.'.format(graph_name)) + else: + rospy.logerr('graph uploading failed: {}'.format(ret[1])) + return False + # Localization + if localization_method == 'fiducial': + ret = self.spot_client.set_localization_fiducial() + elif localization_method == 'waypoint': + ret = self.spot_client.set_localization_waypoint(start_id) + else: + ret = (False,'Unknown localization method') + if ret[0]: + rospy.loginfo('robot is localized on the graph.') + else: + rospy.logwarn('Localization failed: {}'.format(ret[1])) + return False + + self.sound_client.say('階段を移動します。',blocking=True) + for index, num in enumerate(stair_nums): + if num < 0: + self.sound_client.say('{}段の下り階段があります'.format(-num),blocking=True) + else: + self.sound_client.say('{}段の登り階段があります'.format(num),blocking=True) + + if index != len(stair_nums) - 1: + self.sound_client.say('その後踊り場があった後',blocking=True) + + # start leading + success = False + rate = rospy.Rate(10) + self.sound_client.say('ついてきてください',blocking=True) + self.spot_client.navigate_to( end_id, blocking=False) + while not rospy.is_shutdown(): + rate.sleep() + + if self.spot_client.wait_for_navigate_to_result(rospy.Duration(0.1)): + result = self.spot_client.get_navigate_to_result() + success = result.success + rospy.loginfo('result: {}'.format(result)) + break + + if not self.state_visible and self.duration_visibility > rospy.Duration(0.5): + flag_speech = False + def notify_visibility(): + self.sound_client.say( + '近くに人が見えません', + blocking=True + ) + flag_speech = False + speech_thread = threading.Thread(target=notify_visibility) + speech_thread.start() + while not self.state_visible and self.duration_visibility > rospy.Duration(0.5): + rate.sleep() + self.spot_client.pub_cmd_vel(0,0,0) + if not flag_speech: + flag_speech = True + speech_thread = threading.Thread(target=notify_visibility) + speech_thread.start() + if not self.state_visible and self.duration_visibility > rospy.Duration(5.0): + self.spot_client.cancel_navigate_to() + if self.state_visible: + self.spot_client.navigate_to( end_id, blocking=False) + + # recovery on failure + if not result.success: + self.sound_client.say('失敗したので元に戻ります', blocking=True) + self.spot_client.navigate_to( start_id, blocking=True) + self.spot_client.wait_for_navigate_to_result() + return result.success + else: + rate = rospy.Rate(1) + while not self.state_visible: + self.sound_client.say('ひとが見えるまでお待ちしています。',blocking=True) + rate.sleep() + return result.success + + def run_final(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_finalize() called') + + self.spot_client.cancel_navigate_to() + + if self.subscriber_visible != None: + self.subscriber_visible.unregister() + self.subscriber_visible = None + + if self.subscriber_people_pose_array != None: + self.subscriber_people_pose_array.unregister() + self.subscriber_people_pose_array = None + + self.roslaunch_parent.shutdown() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/walk_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/walk_behavior.py new file mode 100644 index 0000000000..e825a4b9d9 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/walk_behavior.py @@ -0,0 +1,157 @@ +# -*- coding: utf-8 -*- + +from spot_behavior_manager.base_behavior import BaseBehavior + +import roslaunch +import rospkg +import rospy + +import threading + +from std_msgs.msg import Bool + +class WalkBehavior(BaseBehavior): + + def callback_visible(self, msg): + + if self.state_visible != msg.data: + self.starttime_visibility = rospy.Time.now() + self.duration_visibility = rospy.Duration() + self.state_visible = msg.data + else: + self.duration_visibility = rospy.Time.now() - self.starttime_visibility + + def run_initial(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_initial() called') + + # launch recognition launch + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch_path = rospkg.RosPack().get_path('spot_person_lead_behaviors') +\ + '/launch/walk_detection.launch' + roslaunch_cli_args = [roslaunch_path] + roslaunch_file = roslaunch.rlutil.resolve_launch_arguments(roslaunch_cli_args) + self.roslaunch_parent = roslaunch.parent.ROSLaunchParent( + uuid, + roslaunch_file + ) + self.roslaunch_parent.start() + + # value for person checker + self.subscriber_visible = None + self.state_visible = False + self.starttime_visibility = rospy.Time.now() + self.duration_visibility = rospy.Duration(10) + + # start subscriber + try: + self.subscriber_visible = rospy.Subscriber( + '/walk_detection_person_tracker/visible', + Bool, + self.callback_visible + ) + except Exception as e: + rospy.logerr('{}'.format(e)) + return False + + return True + + def run_main(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_main() called') + + graph_name = edge.properties['graph'] + start_id = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['id'] + end_id = filter( + lambda x: x['graph'] == graph_name, + end_node.properties['waypoints_on_graph'] + )[0]['id'] + localization_method = filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + )[0]['localization_method'] + + # graph uploading and localization + if pre_edge is not None and \ + graph_name == pre_edge.properties['graph']: + rospy.loginfo('graph upload and localization skipped.') + else: + # Upload + ret = self.spot_client.upload_graph(graph_name) + if ret[0]: + rospy.loginfo('graph {} uploaded.'.format(graph_name)) + else: + rospy.logerr('graph uploading failed: {}'.format(ret[1])) + return False + # Localization + if localization_method == 'fiducial': + ret = self.spot_client.set_localization_fiducial() + elif localization_method == 'waypoint': + ret = self.spot_client.set_localization_waypoint(start_id) + else: + ret = (False,'Unknown localization method') + if ret[0]: + rospy.loginfo('robot is localized on the graph.') + else: + rospy.logwarn('Localization failed: {}'.format(ret[1])) + return False + + # start leading + success = False + rate = rospy.Rate(10) + self.sound_client.say('ついてきてください',blocking=True) + self.spot_client.navigate_to( end_id, blocking=False) + while not rospy.is_shutdown(): + rate.sleep() + + if self.spot_client.wait_for_navigate_to_result(rospy.Duration(0.1)): + result = self.spot_client.get_navigate_to_result() + success = result.success + rospy.loginfo('result: {}'.format(result)) + break + + if not self.state_visible and self.duration_visibility > rospy.Duration(0.5): + flag_speech = False + def notify_visibility(): + self.sound_client.say( + '近くに人が見えません', + volume=1.0, + blocking=True + ) + flag_speech = False + speech_thread = threading.Thread(target=notify_visibility) + speech_thread.start() + while not self.state_visible and self.duration_visibility > rospy.Duration(0.5): + rate.sleep() + self.spot_client.pub_cmd_vel(0,0,0) + if not flag_speech: + flag_speech = True + speech_thread = threading.Thread(target=notify_visibility) + speech_thread.start() + if not self.state_visible and self.duration_visibility > rospy.Duration(5.0): + self.spot_client.cancel_navigate_to() + if self.state_visible: + self.spot_client.navigate_to( end_id, blocking=False) + + # recovery on failure + if not success: + self.sound_client.say('失敗したので元に戻ります', blocking=True) + self.spot_client.navigate_to( start_id, blocking=True) + self.spot_client.wait_for_navigate_to_result() + + return success + + def run_final(self, start_node, end_node, edge, pre_edge ): + + rospy.logdebug('run_finalize() called') + + self.spot_client.cancel_navigate_to() + + if self.subscriber_visible != None: + self.subscriber_visible.unregister() + self.subscriber_visible = None + + self.roslaunch_parent.shutdown() diff --git a/jsk_spot_robot/spot_autowalk_data/CMakeLists.txt b/jsk_spot_robot/spot_autowalk_data/CMakeLists.txt new file mode 100644 index 0000000000..056cb9480f --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 2.8.3) +project(spot_autowalk_data) + +find_package(catkin REQUIRED) + +add_custom_target(${PROJECT_NAME}_download_autowalk_data ALL COMMAND python$ENV{ROS_PYTHON_VERSION} ${PROJECT_SOURCE_DIR}/scripts/download_autowalk_data.py) + +catkin_package() + +install(DIRECTORY autowalk + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +catkin_install_python(PROGRAMS scripts/view_map.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/jsk_spot_robot/spot_autowalk_data/README.md b/jsk_spot_robot/spot_autowalk_data/README.md new file mode 100644 index 0000000000..31552a9930 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/README.md @@ -0,0 +1,116 @@ +# spot_autowalk_data + +This package includes autowalk data downloaded from public Googledrive folder with jsk_data. + +## scripts + +### download_autowalk_data.py + +this scripts download autowalk data from google drive. this script will be run when catkin build. + +### view_map.py + +this script visualize autowalk data and each waypoint id + +#### Usage + +when you want to visualize autowalk data without waypoint id, just run + +``` +rosrun spot_autowalk_data view_map.py +``` + +when you want to visualize autowalk data with waypoint id, run the script with `--draw-id` option + +``` +rosrun spot_autowalk_data view_map.py --draw-id +``` + +## autowalk data in this packages + +### eng2_2FElevator_to_2FEntrance.walk + +![eng2_2FElevator_to_2FEntrance](https://user-images.githubusercontent.com/9410362/124133634-fbe92700-dabc-11eb-90d5-84b434729233.png) + +### eng2_2FElevator_to_MainGate + +![eng2_2FElevator_to_MainGate](https://user-images.githubusercontent.com/9410362/124133994-55e9ec80-dabd-11eb-8ba1-1bd118146244.png) + +### eng2_2Felevator_to_eng8_2FElevator + +![eng2_2FElevator_to_eng8_2FElevator](https://user-images.githubusercontent.com/9410362/124134033-600beb00-dabd-11eb-823c-483e75318f8b.png) + +### eng2_73B2_to_2FElevator + +![eng2_73B2_to_7FElevator](https://user-images.githubusercontent.com/9410362/135750882-3d72e538-316c-40a4-b209-cffd06a774a4.png) + +### eng2_73B2_to_81C1 + +![eng2_73B2_to_81C1](https://user-images.githubusercontent.com/9410362/124134118-7b76f600-dabd-11eb-829b-c36f105d7051.png) + +### eng2_elevator_7FElevator_to_2FElevator + +![eng2_elevator_7FElevator_to_2FElevator](https://user-images.githubusercontent.com/9410362/124134162-86318b00-dabd-11eb-86e3-092fcc5e8719.png) + +### eng_TelephoneBox_to_HongoMainGate + +![eng_TelephoneBox_to_HongoMainGate](https://user-images.githubusercontent.com/9410362/124134207-90ec2000-dabd-11eb-988b-62ffbf98ca67.png) + +### eng2_elevator_3FElevator_to_2FElevator.walk + +![eng2_elevator_3FElevator_to_2FElevator walk](https://user-images.githubusercontent.com/9410362/124587250-6077fd80-de92-11eb-8014-2852dfa7f60a.png) + +### eng2_3FElevator_to_Mech_Office.walk + +![eng2_3FElevator_to_Mech_Office walk](https://user-images.githubusercontent.com/9410362/124587299-6f5eb000-de92-11eb-9427-0ed88e6ca414.png) + +### eng2_2FElevator_to_MainEntrance.walk + +![eng2_2FElevator_to_MainEntrance](https://user-images.githubusercontent.com/9410362/129509474-c3429db1-6a6f-4076-bbbb-c3fb27bc3167.png) + +### eng2_MainEntrance_to_HongoMainGate.walk + +![eng2_MainEntrance_to_HongoMainGate](https://user-images.githubusercontent.com/9410362/129509500-1bde2818-66c7-4770-a87d-88c4f9d65ae3.png) + +### eng2_MainEntrance_to_HongoMainGate_Daylight.walk + +![eng2_MainEntrance_to_HongoMainGate_Daylight](https://user-images.githubusercontent.com/9410362/129997430-620ee1dc-6659-4028-acd7-f80b7539364a.png) + +### eng2_2FElevator_to_subway.walk + +![eng2_2FElevator_to_subway](https://user-images.githubusercontent.com/9410362/132933793-63c627bc-e210-4922-9fa2-4c1538b6f45e.png) + +### eng2_subway_tablenavigation.walk + +![eng2_subway_tablenavigation](https://user-images.githubusercontent.com/9410362/132933800-6f68566e-8471-4ac4-a030-94414f4a1b83.png) + +### eng2_subway_tablenavigation_02.walk + +![eng2_subway_tablenavigation_02](https://user-images.githubusercontent.com/9410362/132933805-f3c10dc3-01da-429c-96b8-74225401d703.png) + +### eng2_73b2kitchen.walk + +![eng2_73b2kitchen.walk](https://user-images.githubusercontent.com/67531577/136342909-7979a058-538e-4f76-a705-0186439c478a.png) + +### eng2_73b2trashcans.walk + +![eng2_73b2trashcans.walk](https://user-images.githubusercontent.com/67531577/136343017-74229050-1b36-41e3-bba1-fcad53e92df7.png) + +## How to add autowalk data in this package + +1. record a new autowalk data +2. archive and compress an autowalk data directory (e.g. `autowalk.walk`) to `autowalk.walk.tar.gz` by `tar -zcvf autowalk.walk.tar.gz autowalk.walk` +3. upload `autowalk.walk.tar.gz` to [jsk_data public google drive folger](https://drive.google.com/drive/folders/1PPOZDoWbJnxM7XkPMPP3iKQqrji56Vgq?usp=sharing) +4. add new entry to download_autowalk_data.py like + +``` + download_data( + pkg_name=PKG, + path='autowalk/autowalk.walk.tar.gz', + url='https://drive.google.com/uc?id=', + md5='', + extract=True + ) +``` + +5. update README.md diff --git a/jsk_spot_robot/spot_autowalk_data/package.xml b/jsk_spot_robot/spot_autowalk_data/package.xml new file mode 100644 index 0000000000..715f494d26 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/package.xml @@ -0,0 +1,18 @@ + + + spot_autowalk_data + 1.1.0 + The spot_autowalk_data package + + Kei Okada + Koki Shinjo + Koki Shinjo + + BSD + + catkin + jsk_data + + + + diff --git a/jsk_spot_robot/spot_autowalk_data/scripts/download_autowalk_data.py b/jsk_spot_robot/spot_autowalk_data/scripts/download_autowalk_data.py new file mode 100755 index 0000000000..78a5b07b06 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/scripts/download_autowalk_data.py @@ -0,0 +1,306 @@ +#!/usr/bin/env python + +from jsk_data import download_data + + +def main(): + PKG = 'spot_autowalk_data' + + download_data( + pkg_name=PKG, + path='autowalk/eng2_73B2_to_81C1.walk.tar.gz', + url='https://drive.google.com/uc?id=1IDTCP7n4LCowizW3mFQvTOQtE4qOH9tx', + md5='65f09629c0ac2aa21df6f179c9875bd0', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_73B2_to_7FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=1O8o6voq2v8WenfaUYcmpSU-IwJSsXW5_', + md5='b75ff21caf475a044e7ea197ffe9e378', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_7FElevator_to_2FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=12MOg5okckmQlYiM6flkdeMYxqjn9-C9l', + md5='67ae3210cbfb55791fff6494f84abb3b', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_elevator_7FElevator_to_2FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=1iyx0y1dPu4HUPMNepR_VaZd_WEaC5Lku', + md5='915916d084abd54c2c17f0738a726da3', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_2FElevator_to_2FEntrance.walk.tar.gz', + url='https://drive.google.com/uc?id=1cYUn_qnRslWuH0ZMEBN6ovqmdcUB0MzY', + md5='78c6e1e8e5967b216c9f53e38893750e', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_2FElevator_to_MainGate.walk.tar.gz', + url='https://drive.google.com/uc?id=1CIuStpjxIA188MLxUsfUjI-47Ev36Wiv', + md5='47d669bcb1394b97c95e5d77f78da3e5', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_2FElevator_to_eng8_2FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=1oyU1ufqy9gryPXw8YZ45Ff7AR8K825dT', + md5='ac9a67567104df2ddd78b8c53fa61ba2', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng_TelephoneBox_to_HongoMainGate.walk.tar.gz', + url='https://drive.google.com/uc?id=120WC6SE4C_9XIvy0j3HEraljIigjcJ5U', + md5='6f89cd74bea3934171e0ae720747da24', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_3FElevator_to_Mech_Office.walk.tar.gz', + url='https://drive.google.com/uc?id=10pWP1qnbr5TCfQVUeOHnDacdwQ3X3Awh', + md5='52564afd6471c9551c8f77ac031a366d', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_elevator_3FElevator_to_2FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=1OFB38ISYjTtu9A87QtfYyVLhtcGnpu2a', + md5='e8ed341b22c5eb66373d29b580bede05', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_2FElevator_to_MainEntrance.walk.tar.gz', + url='https://drive.google.com/uc?id=1e5hCLtfOlDYWhrQoefYGxz2AnjaSfs_f', + md5='e79f25769b702c203d4f9229c81a1f89', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_MainEntrance_to_HongoMainGate.walk.tar.gz', + url='https://drive.google.com/uc?id=13mG7iFMkFWN04S7BeR2rPwvpL-0cTgko', + md5='e0260203d5a3fa4958014b4149c3749a', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_MainEntrance_to_HongoMainGate_Daylight.walk.tar.gz', + url='https://drive.google.com/uc?id=1EHpKaMBa8rnRZRyK8KwTK3727cxNqph4', + md5='43cff60ed9dc444d3d7de0e4ef304d0f', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/73b2_inside.walk.tar.gz', + url='https://drive.google.com/uc?id=14BPDOJBNS2v5EoUyIZ9gDwm4VFXyj-FD', + md5='5b45fcd7271f42f32414602145b77bd8', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaSouth.walk.tar.gz', + url='https://drive.google.com/uc?id=1eZbBLwzl9nVpSAebtux5nbN1B4UfGEoh', + md5='01ef288775d6c21a634a17fa427e4900', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaEast.walk.tar.gz', + url='https://drive.google.com/uc?id=1_zySu-e94wzHLaLYTbQuUd9A0Sb8x9NQ', + md5='fc9fb3161b745ff5f7b62be8977f7e56', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_elevator_7FElevator_to_1FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=1kLSNCn2lhkcdOFBaYSVHUFzklhVz_jL8', + md5='fa5a26677d9136ef4671d7a32ca59097', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_1FElevator_to_1FGarbageCollectionAreaSouth_via213.walk.tar.gz', + url='https://drive.google.com/uc?id=1oSFjkwG0EcVXapMVgLlc_i1VtcWXwEwE', + md5='72ba498fe56700bee31c8ce166e77008', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_2FElevator_to_subway.walk.tar.gz', + url='https://drive.google.com/uc?id=1uRZA0JnFyenVcZDtXhQGzqGAIcO9lwop', + md5='ef40ee8677ef68b4beee4e516a11d1f1', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_subway_tablenavigation.walk.tar.gz', + url='https://drive.google.com/uc?id=1Lye5ZfFLgmU-G8TdevSiHG72kN04XnFt', + md5='e65a06fbb7b2037659bf657ebb4e5d36', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_subway_tablenavigation_02.walk.tar.gz', + url='https://drive.google.com/uc?id=1GgIra90yX21IMYYL8VgrY9YJpGevA1ek', + md5='08eca6ac0e1310236f32f229370aa161', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_1FElevator_to_GarbageCollectionArea_inside_clockwise.walk.tar.gz', + url='https://drive.google.com/uc?id=1Ur6pZZKxZQsP_3zsbikWFkOWOB1KsKlb', + md5='da542fc9b4376378dc2a644454175e06', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_GarbageCollectionArea_inside_clockwise.walk.tar.gz', + url='https://drive.google.com/uc?id=1ipkw696vWe_DeO87ZcoQmTgFUE7adpL7', + md5='71d5555f4f6772a9022768fe5dabf813', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_GarbageCollectionArea_inside_counterclockwise.walk.tar.gz', + url='https://drive.google.com/uc?id=1pIrpc_q9GVCtH4YO2-E2rXSEf1yTfP4U', + md5='d0f279bc94b194600bccf3a1c5d98c35', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_GarbageCollectionArea_to_1FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=1rUCVbhbSmUd4OEA6W35quKs-6CJIJW7b', + md5='2fdfc99d640bc9c796c36e3127078fcf', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_73b2kitchen.walk.tar.gz', + url='https://drive.google.com/uc?id=1JrDRIQpGpB6sqofIyk9eR03KsE2EJ5Ev', + md5='8666d1c8b21ae7ffb88978ed0b1bfb33', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_73b2trashcans.walk.tar.gz', + url='https://drive.google.com/uc?id=1w19wmLsQHMM9ErjK4mN-zstWkvrHITLO', + md5='264c2e561a0c128a26b490059e10e30c', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_MechOffice_inside.walk.tar.gz', + url='https://drive.google.com/uc?id=1dSbU0tADrxwGLEBywMt312NXUgFHtsCy', + md5='5460bc55c34824ce75722881a4e0b77d', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_MechOffice_to_MainEntrance.walk.tar.gz', + url='https://drive.google.com/uc?id=1aepZnkxV8SLo9a8DdCEU9KnNoyjG7sZF', + md5='4aba2398e8eae87509820180b2e89cb0', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_MechOffice_to_3felevator_subwayside.walk.tar.gz', + url='https://drive.google.com/uc?id=1twc6Ui5z7nqqZHMHPeynNAyE19PKyihN', + md5='6d720c0c6f5692a205b386c9248d69f6', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_1fentrance_to_1FElevator.walk.tar.gz', + url='https://drive.google.com/uc?id=1Q7gX8Cg58dUtV0he7R8fxYciinYnAmd9', + md5='b82831bd317014367af5f28614368bd9', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_2fentrance_to_1fentrance.walk.tar.gz', + url='https://drive.google.com/uc?id=1eKdUYOyWEO2lz07uUlpayb6StFC_hCco', + md5='0b7c8e960825014a3d3b4637265e966c', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_73B2_to_83A2.walk.tar.gz', + url='https://drive.google.com/uc?id=1GwOD53C7I8dpi3YcrrMcHlHbN3k_Auj1', + md5='aa063402ab604b012ae7c7a4123fb5ac', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_7FElevator_to_73B2_trash.walk.tar.gz', + url='https://drive.google.com/uc?id=1gmkQa5oTSIJAKST8SBLev_nui_XN40dE', + md5='d8b0711270fb2ae67159d28d50bb5231', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_7f_around.walk.tar.gz', + url='https://drive.google.com/uc?id=1lKELjOiUU02RnrK2V7AWos8GkJqylEKu', + md5='daae9c4c7555780b148baad7906b6710', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_outside.walk.tar.gz', + url='https://drive.google.com/uc?id=1E9QJJYz7Puk72KL2lFRTKfPtRiCHZYYw', + md5='f44de8d70da1bd1a512b5e1002335c81', + extract=True + ) + + download_data( + pkg_name=PKG, + path='autowalk/eng2_8F.walk.tar.gz', + url='https://drive.google.com/uc?id=1LPluVprB4FCZ8L7ePpeQM8qFzijHQ3Ic', + md5='1032a79f9ac5201caddcefcb867637a5', + extract=True + ) + +if __name__ == '__main__': + main() diff --git a/jsk_spot_robot/spot_autowalk_data/scripts/view_map.py b/jsk_spot_robot/spot_autowalk_data/scripts/view_map.py new file mode 100755 index 0000000000..16891ea280 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/scripts/view_map.py @@ -0,0 +1,405 @@ +#!/usr/bin/env python3 +# Copyright (c) 2021 Boston Dynamics, Inc. All rights reserved. +# +# Downloading, reproducing, distributing or otherwise using the SDK Software +# is subject to the terms and conditions of the Boston Dynamics Software +# Development Kit License (20191101-BDSDK-SL). + +import argparse + +from vtk.util import numpy_support +import google.protobuf.timestamp_pb2 +import math +import numpy as np +import numpy.linalg +import os +import sys +import time +import vtk + +from bosdyn.api.graph_nav import map_pb2 +from bosdyn.api import geometry_pb2 +from bosdyn.client.frame_helpers import * +from bosdyn.client.math_helpers import * +""" +This example shows how to load and view a graph nav map. + +""" + + +def numpy_to_poly_data(pts): + """ + Converts numpy array data into vtk poly data. + :param pts: the numpy array to convert (3 x N). + :return: a vtkPolyData. + """ + pd = vtk.vtkPolyData() + pd.SetPoints(vtk.vtkPoints()) + # Makes a deep copy + pd.GetPoints().SetData(numpy_support.numpy_to_vtk(pts.copy())) + + f = vtk.vtkVertexGlyphFilter() + f.SetInputData(pd) + f.Update() + pd = f.GetOutput() + + return pd + + +def mat_to_vtk(mat): + """ + Converts a 4x4 homogenous transform into a vtk transform object. + :param mat: A 4x4 homogenous transform (numpy array). + :return: A VTK transform object representing the transform. + """ + t = vtk.vtkTransform() + t.SetMatrix(mat.flatten()) + return t + + +def vtk_to_mat(transform): + """ + Converts a VTK transform object to 4x4 homogenous numpy matrix. + :param transform: an object of type vtkTransform + : return: a numpy array with a 4x4 matrix representation of the transform. + """ + tf_matrix = transform.GetMatrix() + out = np.array(np.eye(4)) + for r in range(4): + for c in range(4): + out[r, c] = tf_matrix.GetElement(r, c) + return out + + +def api_to_vtk_se3_pose(se3_pose): + """ + Convert a bosdyn SDK SE3Pose into a VTK pose. + :param se3_pose: the bosdyn SDK SE3 Pose. + :return: A VTK pose representing the bosdyn SDK SE3 Pose. + """ + return mat_to_vtk(se3_pose.to_matrix()) + + +def create_fiducial_object(world_object, waypoint, renderer): + """ + Creates a VTK object representing a fiducial. + :param world_object: A WorldObject representing a fiducial. + :param waypoint: The waypoint the AprilTag is associated with. + :param renderer: The VTK renderer + :return: a tuple of (vtkActor, 4x4 homogenous transform) representing the vtk actor for the fiducial, and its + transform w.r.t the waypoint. + """ + fiducial_object = world_object.apriltag_properties + odom_tform_fiducial_filtered = get_a_tform_b( + world_object.transforms_snapshot, ODOM_FRAME_NAME, + world_object.apriltag_properties.frame_name_fiducial_filtered) + waypoint_tform_odom = SE3Pose.from_obj(waypoint.waypoint_tform_ko) + waypoint_tform_fiducial_filtered = api_to_vtk_se3_pose( + waypoint_tform_odom * odom_tform_fiducial_filtered) + plane_source = vtk.vtkPlaneSource() + plane_source.SetCenter(0.0, 0.0, 0.0) + plane_source.SetNormal(0.0, 0.0, 1.0) + plane_source.Update() + plane = plane_source.GetOutput() + mapper = vtk.vtkPolyDataMapper() + mapper.SetInputData(plane) + + actor = vtk.vtkActor() + actor.SetMapper(mapper) + actor.GetProperty().SetColor(0.5, 0.7, 0.9) + actor.SetScale(fiducial_object.dimensions.x, fiducial_object.dimensions.y, 1.0) + renderer.AddActor(actor) + return actor, waypoint_tform_fiducial_filtered + + +def create_point_cloud_object(waypoints, snapshots, waypoint_id): + """ + Create a VTK object representing the point cloud in a snapshot. Note that in graph_nav, "point cloud" refers to the + feature cloud of a waypoint -- that is, a collection of visual features observed by all five cameras at a particular + point in time. The visual features are associated with points that are rigidly attached to a waypoint. + :param waypoints: dict of waypoint ID to waypoint. + :param snapshots: dict of waypoint snapshot ID to waypoint snapshot. + :param waypoint_id: the waypoint ID of the waypoint whose point cloud we want to render. + :return: a vtkActor containing the point cloud data. + """ + wp = waypoints[waypoint_id] + snapshot = snapshots[wp.snapshot_id] + cloud = snapshot.point_cloud + odom_tform_cloud = get_a_tform_b(cloud.source.transforms_snapshot, ODOM_FRAME_NAME, + cloud.source.frame_name_sensor) + waypoint_tform_odom = SE3Pose.from_obj(wp.waypoint_tform_ko) + waypoint_tform_cloud = api_to_vtk_se3_pose(waypoint_tform_odom * odom_tform_cloud) + + point_cloud_data = np.frombuffer(cloud.data, dtype=np.float32).reshape(int(cloud.num_points), 3) + poly_data = numpy_to_poly_data(point_cloud_data) + arr = vtk.vtkFloatArray() + for i in range(cloud.num_points): + arr.InsertNextValue(point_cloud_data[i, 2]) + arr.SetName("z_coord") + poly_data.GetPointData().AddArray(arr) + poly_data.GetPointData().SetActiveScalars("z_coord") + actor = vtk.vtkActor() + mapper = vtk.vtkPolyDataMapper() + mapper.SetInputData(poly_data) + mapper.ScalarVisibilityOn() + actor.SetMapper(mapper) + actor.GetProperty().SetPointSize(2) + actor.SetUserTransform(waypoint_tform_cloud) + return actor + + +def create_waypoint_object(renderer, waypoints, snapshots, waypoint_id): + """ + Creates a VTK object representing a waypoint and its point cloud. + :param renderer: The VTK renderer. + :param waypoints: dict of waypoint ID to waypoint. + :param snapshots: dict of snapshot ID to snapshot. + :param waypoint_id: the waypoint id of the waypoint object we wish to create. + :return: A vtkAssembly representing the waypoint (an axis) and its point cloud. + """ + assembly = vtk.vtkAssembly() + actor = vtk.vtkAxesActor() + actor.SetXAxisLabelText("") + actor.SetYAxisLabelText("") + actor.SetZAxisLabelText("") + actor.SetTotalLength(0.2, 0.2, 0.2) + point_cloud_actor = create_point_cloud_object(waypoints, snapshots, waypoint_id) + assembly.AddPart(actor) + assembly.AddPart(point_cloud_actor) + renderer.AddActor(assembly) + return assembly + + +def make_line(pt_A, pt_B, renderer): + """ + Creates a VTK object which is a white line between two points. + :param pt_A: starting point of the line. + :param pt_B: ending point of the line. + :param renderer: the VTK renderer. + :return: A VTK object that is a while line between pt_A and pt_B. + """ + line_source = vtk.vtkLineSource() + line_source.SetPoint1(pt_A[0], pt_A[1], pt_A[2]) + line_source.SetPoint2(pt_B[0], pt_B[1], pt_B[2]) + mapper = vtk.vtkPolyDataMapper() + mapper.SetInputConnection(line_source.GetOutputPort()) + + actor = vtk.vtkActor() + actor.SetMapper(mapper) + actor.GetProperty().SetLineWidth(2) + actor.GetProperty().SetColor(0.7, 0.7, 0.7) + renderer.AddActor(actor) + return actor + + +def make_text(name, pt, renderer): + """ + Creates white text on a black background at a particular point. + :param name: The text to display. + :param pt: The point in the world where the text will be displayed. + :param renderer: The VTK renderer + :return: the vtkActor representing the text. + """ + actor = vtk.vtkTextActor() + actor.SetInput(name) + prop = actor.GetTextProperty() + prop.SetBackgroundColor(0.0, 0.0, 0.0) + prop.SetBackgroundOpacity(0.5) + prop.SetFontSize(16) + coord = actor.GetPositionCoordinate() + coord.SetCoordinateSystemToWorld() + coord.SetValue((pt[0], pt[1], pt[2])) + + renderer.AddActor(actor) + return actor + + +def create_edge_object(curr_wp_tform_to_wp, world_tform_curr_wp, renderer): + # Concatenate the edge transform. + world_tform_to_wp = np.dot(world_tform_curr_wp, curr_wp_tform_to_wp) + # Make a line between the current waypoint and the neighbor. + make_line(world_tform_curr_wp[:3, 3], world_tform_to_wp[:3, 3], renderer) + return world_tform_to_wp + + +def load_map(path): + """ + Load a map from the given file path. + :param path: Path to the root directory of the map. + :return: the graph, waypoints, waypoint snapshots and edge snapshots. + """ + with open(os.path.join(path, "graph"), "rb") as graph_file: + # Load the graph file and deserialize it. The graph file is a protobuf containing only the waypoints and the + # edges between them. + data = graph_file.read() + current_graph = map_pb2.Graph() + current_graph.ParseFromString(data) + + # Set up maps from waypoint ID to waypoints, edges, snapshots, etc. + current_waypoints = {} + current_waypoint_snapshots = {} + current_edge_snapshots = {} + + # For each waypoint, load any snapshot associated with it. + for waypoint in current_graph.waypoints: + current_waypoints[waypoint.id] = waypoint + + # Load the snapshot. Note that snapshots contain all of the raw data in a waypoint and may be large. + file_name = os.path.join(path, "waypoint_snapshots", waypoint.snapshot_id) + if not os.path.exists(file_name): + continue + with open(file_name, "rb") as snapshot_file: + waypoint_snapshot = map_pb2.WaypointSnapshot() + waypoint_snapshot.ParseFromString(snapshot_file.read()) + current_waypoint_snapshots[waypoint_snapshot.id] = waypoint_snapshot + # Similarly, edges have snapshot data. + for edge in current_graph.edges: + + if not edge.snapshot_id: + continue # Rare case that snapshot id is empty + + file_name = os.path.join(path, "edge_snapshots", edge.snapshot_id) + if not os.path.exists(file_name): + continue + with open(file_name, "rb") as snapshot_file: + edge_snapshot = map_pb2.EdgeSnapshot() + edge_snapshot.ParseFromString(snapshot_file.read()) + current_edge_snapshots[edge_snapshot.id] = edge_snapshot + print("Loaded graph with {} waypoints and {} edges".format( + len(current_graph.waypoints), len(current_graph.edges))) + return (current_graph, current_waypoints, current_waypoint_snapshots, + current_edge_snapshots) + + +def create_graph_objects(current_graph, current_waypoint_snapshots, current_waypoints, renderer, draw_id=True): + """ + Creates all the VTK objects associated with the graph. + :param current_graph: the graph to use. + :param current_waypoint_snapshots: dict from snapshot id to snapshot. + :param current_waypoints: dict from waypoint id to waypoint. + :param renderer: The VTK renderer + :return: the average position in world space of all the waypoints. + """ + waypoint_objects = {} + # Create VTK objects associated with each waypoint. + for waypoint in current_graph.waypoints: + waypoint_objects[waypoint.id] = create_waypoint_object(renderer, current_waypoints, + current_waypoint_snapshots, + waypoint.id) + # Now, perform a breadth first search of the graph starting from an arbitrary waypoint. Graph nav graphs + # have no global reference frame. The only thing we can say about waypoints is that they have relative + # transformations to their neighbors via edges. So the goal is to get the whole graph into a global reference + # frame centered on some waypoint as the origin. + queue = [] + queue.append((current_graph.waypoints[0], np.eye(4))) + visited = {} + # Get the camera in the ballpark of the right position by centering it on the average position of a waypoint. + avg_pos = np.array([0.0, 0.0, 0.0]) + + # Breadth first search. + while len(queue) > 0: + # Visit a waypoint. + curr_element = queue[0] + queue.pop(0) + curr_waypoint = curr_element[0] + visited[curr_waypoint.id] = True + + # We now know the global pose of this waypoint, so set the pose. + waypoint_objects[curr_waypoint.id].SetUserTransform(mat_to_vtk(curr_element[1])) + world_tform_current_waypoint = curr_element[1] + # Add text to the waypoint. + print('{}: {}'.format(curr_waypoint.annotations.name, curr_waypoint.id)) + if draw_id: + make_text( + '{}: {}'.format(curr_waypoint.annotations.name, curr_waypoint.id), + world_tform_current_waypoint[:3, 3], + renderer) + + # For each fiducial in the waypoint's snapshot, add an object at the world pose of that fiducial. + if (curr_waypoint.snapshot_id in current_waypoint_snapshots): + snapshot = current_waypoint_snapshots[curr_waypoint.snapshot_id] + for fiducial in snapshot.objects: + if fiducial.HasField("apriltag_properties"): + (fiducial_object, + curr_wp_tform_fiducial) = create_fiducial_object(fiducial, curr_waypoint, + renderer) + world_tform_fiducial = np.dot(world_tform_current_waypoint, + vtk_to_mat(curr_wp_tform_fiducial)) + fiducial_object.SetUserTransform(mat_to_vtk(world_tform_fiducial)) + make_text( + 'fid: {}'.format(fiducial.apriltag_properties.tag_id), world_tform_fiducial[:3, 3], + renderer) + + # Now, for each edge, walk along the edge and concatenate the transform to the neighbor. + for edge in current_graph.edges: + # If the edge is directed away from us... + if edge.id.from_waypoint == curr_waypoint.id and edge.id.to_waypoint not in visited: + current_waypoint_tform_to_waypoint = SE3Pose.from_obj( + edge.from_tform_to).to_matrix() + world_tform_to_wp = create_edge_object(current_waypoint_tform_to_waypoint, + world_tform_current_waypoint, renderer) + # Add the neighbor to the queue. + queue.append((current_waypoints[edge.id.to_waypoint], world_tform_to_wp)) + avg_pos += world_tform_to_wp[:3, 3] + # If the edge is directed toward us... + elif edge.id.to_waypoint == curr_waypoint.id and edge.id.from_waypoint not in visited: + current_waypoint_tform_from_waypoint = (SE3Pose.from_obj( + edge.from_tform_to).inverse()).to_matrix() + world_tform_from_wp = create_edge_object(current_waypoint_tform_from_waypoint, + world_tform_current_waypoint, renderer) + # Add the neighbor to the queue. + queue.append((current_waypoints[edge.id.from_waypoint], world_tform_from_wp)) + avg_pos += world_tform_from_wp[:3, 3] + return avg_pos + + +def main( path, draw_id ): + # Load the map from the given file. + (current_graph, current_waypoints, current_waypoint_snapshots, + current_edge_snapshots) = load_map(path) + + # Create the renderer. + renderer = vtk.vtkRenderer() + renderer.SetBackground(0.05, 0.1, 0.15) + + avg_pos = create_graph_objects(current_graph, current_waypoint_snapshots, current_waypoints, + renderer, draw_id) + + # Compute the average waypoint position to place the camera appropriately. + avg_pos /= len(current_waypoints) + camera_pos = avg_pos + np.array([-1, 0, 5]) + + camera = renderer.GetActiveCamera() + camera.SetViewUp(0, 0, 1) + camera.SetPosition(camera_pos[0], camera_pos[1], camera_pos[2]) + + # Create the VTK renderer and interactor. + renderWindow = vtk.vtkRenderWindow() + renderWindow.SetWindowName(path) + renderWindow.AddRenderer(renderer) + renderWindowInteractor = vtk.vtkRenderWindowInteractor() + renderWindowInteractor.SetRenderWindow(renderWindow) + renderWindow.SetSize(1280, 720) + style = vtk.vtkInteractorStyleTerrain() + renderWindowInteractor.SetInteractorStyle(style) + renderer.ResetCamera() + + # Start rendering. + renderWindow.Render() + renderWindow.Start() + renderWindowInteractor.Start() + + +if __name__ == '__main__': + + parser = argparse.ArgumentParser() + + parser.add_argument('map_directory') + parser.add_argument('--draw-id', help='Draw ID of each waypoints', action='store_true') + + args = parser.parse_args() + + if args.draw_id: + main( args.map_directory, True ) + else: + main( args.map_directory, False ) diff --git a/jsk_spot_robot/spot_ros_client/CMakeLists.txt b/jsk_spot_robot/spot_ros_client/CMakeLists.txt new file mode 100644 index 0000000000..4c7961bfb2 --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 2.8.3) +project(spot_ros_client) + +find_package(catkin REQUIRED) + +catkin_python_setup() + +catkin_package() + +catkin_install_python(PROGRAMS scripts/demo.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/jsk_spot_robot/spot_ros_client/README.md b/jsk_spot_robot/spot_ros_client/README.md new file mode 100644 index 0000000000..f6918bd87c --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/README.md @@ -0,0 +1,9 @@ +# spot_ros_client + +This package provide a python client library for spot_ros. + +## Demo + +```python +rosrun spot_ros_client demo.py +``` diff --git a/jsk_spot_robot/spot_ros_client/package.xml b/jsk_spot_robot/spot_ros_client/package.xml new file mode 100644 index 0000000000..8fb8c91515 --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/package.xml @@ -0,0 +1,24 @@ + + + spot_ros_client + 1.1.0 + The spot_ros_client package + + Koki Shinjo + + Koki Shinjo + + BSD + + catkin + + rospy + + actionlib + spot_msgs + std_srvs + geometry_msgs + + + + diff --git a/jsk_spot_robot/spot_ros_client/scripts/demo.py b/jsk_spot_robot/spot_ros_client/scripts/demo.py new file mode 100755 index 0000000000..955588af34 --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/scripts/demo.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python + +import rospy +import math +from spot_ros_client.libspotros import SpotRosClient +from geometry_msgs.msg import Quaternion # for body pose + +rospy.init_node('spot_ros_client_demo') +client = SpotRosClient() + +# claim +rospy.loginfo('claim') +client.claim() +rospy.sleep(2) + +# power on +rospy.loginfo('power on') +client.power_on() +rospy.sleep(2) + +# stand +rospy.loginfo('stand') +client.stand() +rospy.sleep(2) + +# sit +rospy.loginfo('sit') +client.sit() +rospy.sleep(2) + +# stand +rospy.loginfo('stand') +client.stand() +rospy.sleep(2) + +# send velocity commands +rospy.loginfo('sending cmd_vel') +rate = rospy.Rate(10) +start_time = rospy.Time.now() +while not rospy.is_shutdown() and rospy.Time.now() < start_time + rospy.Duration(10): + rate.sleep() + client.pub_cmd_vel(0, 0, 0.5) + +# change body pose +quaternion_rotated = Quaternion(x=0,y=0,z=math.sin(0.2),w=math.cos(0.2)) +quaternion_equal = Quaternion(x=0,y=0,z=0,w=1) + +rospy.loginfo('rotating body pose') +client.pub_body_pose(0,quaternion_rotated) +client.stand() +rospy.sleep(5) + +client.pub_body_pose(0,quaternion_equal) +client.stand() +rospy.sleep(5) + +rospy.loginfo('changing body height') +client.pub_body_pose(0.2,quaternion_equal) +client.stand() +rospy.sleep(5) + +client.pub_body_pose(0,quaternion_equal) +client.stand() +rospy.sleep(5) + +# switch stair mode +rospy.loginfo('switching stair mode') +client.stair_mode(True) +rospy.sleep(5) +client.stair_mode(False) +rospy.sleep(5) + +# send a trajectory command +rospy.loginfo('sending trajectory command') +client.trajectory(1.0,1.0,1.57,5,blocking=True) +rospy.sleep(5) + +# use graphnav navigation +## assuming spot is at initial place of example.walk +rospy.loginfo('sending navigate to command') +import rospkg +upload_filepath = rospkg.RosPack().get_path('spot_autowalk_data')+'/autowalk/eng2_73b2_to_81c1_night.walk' +rospy.loginfo('upload map') +client.upload_graph(upload_filepath) +rospy.sleep(1) +rospy.loginfo('localization') +client.set_localization_fiducial() +rospy.sleep(1) +rospy.loginfo('list graph') +waypoint_ids = client.list_graph() +rospy.sleep(1) +rospy.loginfo('send command') +client.navigate_to(waypoint_ids[-1],blocking=False) +rospy.sleep(1) +rospy.loginfo('wait for result') +client.wait_navigate_to_result(rospy.Duration(30)) +rospy.sleep(1) +rospy.loginfo('get a result') +result = client.get_navigate_to_result() diff --git a/jsk_spot_robot/spot_ros_client/setup.py b/jsk_spot_robot/spot_ros_client/setup.py new file mode 100644 index 0000000000..fa9d4a812b --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/setup.py @@ -0,0 +1,9 @@ +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['spot_ros_client'], + package_dir={'': 'src'} + ) + +setup(**d) diff --git a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/__init__.py b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/__init__.py new file mode 100644 index 0000000000..a45c5f4930 --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/__init__.py @@ -0,0 +1 @@ +import spot_ros_client.libspotros as libspotros diff --git a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py new file mode 100644 index 0000000000..a81d5feb52 --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py @@ -0,0 +1,523 @@ +import PyKDL +import rospy +import actionlib + +import math + +# msg +from std_msgs.msg import Bool +from std_msgs.msg import Float32 +from std_msgs.msg import String +from spot_msgs.msg import Feedback +from spot_msgs.msg import PowerState +from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseArray +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import Quaternion +from geometry_msgs.msg import Twist +# services +from std_srvs.srv import Trigger +from std_srvs.srv import TriggerRequest +from std_srvs.srv import SetBool +from std_srvs.srv import SetBoolRequest +from spot_msgs.srv import ListGraph +from spot_msgs.srv import ListGraphRequest +from spot_msgs.srv import SetLocalizationFiducial +from spot_msgs.srv import SetLocalizationFiducialRequest +from spot_msgs.srv import SetLocalizationWaypoint +from spot_msgs.srv import SetLocalizationWaypointRequest +from spot_msgs.srv import SetLocomotion +from spot_msgs.srv import SetLocomotionRequest +from spot_msgs.srv import UploadGraph +from spot_msgs.srv import UploadGraphRequest +from spot_msgs.srv import Dock +from spot_msgs.srv import DockRequest +# actions +from spot_behavior_manager_msgs.msg import LeadPersonAction +from spot_behavior_manager_msgs.msg import LeadPersonGoal +from spot_msgs.msg import NavigateToAction +from spot_msgs.msg import NavigateToGoal +from spot_msgs.msg import TrajectoryAction +from spot_msgs.msg import TrajectoryGoal + + +def calc_distance_to_pose(pose): + if isinstance(pose, Pose): + return math.sqrt(pose.position.x ** 2 + pose.position.y ** 2 + pose.position.z ** 2) + elif isinstance(pose, PoseStamped): + pose = pose.pose + return math.sqrt(pose.position.x ** 2 + pose.position.y ** 2 + pose.position.z ** 2) + else: + raise TypeError( + 'pose must be geometry_msgs.msg.Pose or geometry_msgs.msg.PoseStamped') + + +def convert_msg_point_to_kdl_vector(point): + return PyKDL.Vector(point.x, point.y, point.z) + + +def get_nearest_person_pose(topicname='/spot_recognition_person_tracker/people_pose_array'): + try: + msg = rospy.wait_for_message( + topicname, + PoseArray, + timeout=rospy.Duration(5)) + except rospy.ROSException as e: + rospy.logwarn('Timeout exceede: {}'.format(e)) + return None + + if len(msg.poses) == 0: + rospy.logwarn('No person visible') + return None + + distance = calc_distance_to_pose(msg.poses[0]) + target_pose = msg.poses[0] + for pose in msg.poses: + if calc_distance_to_pose(pose) < distance: + distance = calc_distance_to_pose(pose) + target_pose = pose + + pose_stamped = PoseStamped() + pose_stamped.header = msg.header + pose_stamped.pose = target_pose + + return pose_stamped + + +def get_diff_for_person(pose): + + if isinstance(pose, Pose): + x = pose.position.x + y = pose.position.y + z = pose.position.z + elif isinstance(pose, PoseStamped): + x = pose.pose.position.x + y = pose.pose.position.y + z = pose.pose.position.z + else: + raise TypeError( + 'pose must be geometry_msgs.msg.Pose or geometry_msgs.msg.PoseStamped') + + yaw = math.atan2(y, x) + try: + pitch = math.acos(z / math.sqrt(x**2 + y**2)) + except ValueError: + pitch = 0 + return pitch, yaw + + +class SpotRosClient: + + def __init__(self, + topicname_cmd_vel='/spot/cmd_vel', + topicname_body_pose='/spot/body_pose', + topicname_cable_connected='/spot/status/cable_connected', + topicname_current_node_id='/spot_behavior_manager_server/current_node_id', + topicname_laptop_percentage='/spot/status/laptop_battery_percentage', + topicname_battery_percentage='/spot/status/battery_percentage', + topicname_status_feedback='/spot/status/feedback', + topicname_status_power_state='/spot/status/power_state', + servicename_claim='/spot/claim', + servicename_release='/spot/release', + servicename_stop='/spot/stop', + servicename_self_right='/spot/self_right', + servicename_sit='/spot/sit', + servicename_stand='/spot/stand', + servicename_power_on='/spot/power_on', + servicename_power_off='/spot/power_off', + servicename_estop_hard='/spot/estop/hard', + servicename_estop_gentle='/spot/estop/gentle', + servicename_stair_mode='/spot/stair_mode', + servicename_locomotion_mode='/spot/locomotion_mode', + servicename_upload_graph='/spot/upload_graph', + servicename_list_graph='/spot/list_graph', + servicename_set_localization_fiducial='/spot/set_localization_fiducial', + servicename_set_localization_waypoint='/spot/set_localization_waypoint', + servicename_dock='/spot/dock', + servicename_undock='/spot/undock', + actionname_navigate_to='/spot/navigate_to', + actionname_trajectory='/spot/trajectory', + actionname_execute_behaviors='/spot_behavior_manager_server/execute_behaviors', + duration_timeout=0.05): + + self.topicname_cable_connected = topicname_cable_connected + self.topicname_current_node_id = topicname_current_node_id + self.topicname_laptop_percentage = topicname_laptop_percentage + self.topicname_battery_percentage = topicname_battery_percentage + self.topicname_status_feedback = topicname_status_feedback + self.topicname_status_power_state = topicname_status_power_state + + # Publishers + self._pub_cmd_vel = rospy.Publisher( + topicname_cmd_vel, + Twist, + queue_size=1 + ) + self._pub_body_pose = rospy.Publisher( + topicname_body_pose, + Pose, + queue_size=1 + ) + + # wait for services + try: + rospy.wait_for_service(servicename_claim, rospy.Duration(5)) + rospy.wait_for_service(servicename_release, rospy.Duration(5)) + rospy.wait_for_service(servicename_stop, rospy.Duration(5)) + rospy.wait_for_service(servicename_self_right, rospy.Duration(5)) + rospy.wait_for_service(servicename_sit, rospy.Duration(5)) + rospy.wait_for_service(servicename_stand, rospy.Duration(5)) + rospy.wait_for_service(servicename_power_on, rospy.Duration(5)) + rospy.wait_for_service(servicename_power_off, rospy.Duration(5)) + rospy.wait_for_service(servicename_estop_hard, rospy.Duration(5)) + rospy.wait_for_service(servicename_estop_gentle, rospy.Duration(5)) + rospy.wait_for_service(servicename_stair_mode, rospy.Duration(5)) + rospy.wait_for_service( + servicename_locomotion_mode, rospy.Duration(5)) + rospy.wait_for_service(servicename_upload_graph, rospy.Duration(5)) + rospy.wait_for_service(servicename_list_graph, rospy.Duration(5)) + rospy.wait_for_service( + servicename_set_localization_fiducial, rospy.Duration(5)) + rospy.wait_for_service( + servicename_set_localization_waypoint, rospy.Duration(5)) + rospy.wait_for_service(servicename_dock, rospy.Duration(5)) + rospy.wait_for_service(servicename_undock, rospy.Duration(5)) + except rospy.ROSException as e: + rospy.logerr('Service unavaliable: {}'.format(e)) + + # Service Clients + self._srv_client_claim = rospy.ServiceProxy( + servicename_claim, + Trigger + ) + self._srv_client_release = rospy.ServiceProxy( + servicename_release, + Trigger + ) + self._srv_client_stop = rospy.ServiceProxy( + servicename_stop, + Trigger + ) + self._srv_client_self_right = rospy.ServiceProxy( + servicename_self_right, + Trigger + ) + self._srv_client_sit = rospy.ServiceProxy( + servicename_sit, + Trigger + ) + self._srv_client_stand = rospy.ServiceProxy( + servicename_stand, + Trigger + ) + self._srv_client_power_on = rospy.ServiceProxy( + servicename_power_on, + Trigger + ) + self._srv_client_power_off = rospy.ServiceProxy( + servicename_power_off, + Trigger + ) + self._srv_client_estop_hard = rospy.ServiceProxy( + servicename_estop_hard, + Trigger + ) + self._srv_client_estop_gentle = rospy.ServiceProxy( + servicename_estop_gentle, + Trigger + ) + self._srv_client_stair_mode = rospy.ServiceProxy( + servicename_stair_mode, + SetBool + ) + self._srv_client_locomotion_mode = rospy.ServiceProxy( + servicename_locomotion_mode, + SetLocomotion + ) + self._srv_client_upload_graph = rospy.ServiceProxy( + servicename_upload_graph, + UploadGraph + ) + self._srv_client_list_graph = rospy.ServiceProxy( + servicename_list_graph, + ListGraph + ) + self._srv_client_set_localization_fiducial = rospy.ServiceProxy( + servicename_set_localization_fiducial, + SetLocalizationFiducial + ) + self._srv_client_set_localization_waypoint = rospy.ServiceProxy( + servicename_set_localization_waypoint, + SetLocalizationWaypoint + ) + self._srv_client_dock = rospy.ServiceProxy( + servicename_dock, + Dock + ) + self._srv_client_undock = rospy.ServiceProxy( + servicename_undock, + Trigger + ) + + # Action Clients + self._actionclient_navigate_to = actionlib.SimpleActionClient( + actionname_navigate_to, + NavigateToAction + ) + self._actionclient_trajectory = actionlib.SimpleActionClient( + actionname_trajectory, + TrajectoryAction + ) + self._actionclient_execute_behaviors = actionlib.SimpleActionClient( + actionname_execute_behaviors, + LeadPersonAction + ) + + # wait for action + try: + self._actionclient_navigate_to.wait_for_server(rospy.Duration(5)) + self._actionclient_trajectory.wait_for_server(rospy.Duration(5)) + self._actionclient_execute_behaviors.wait_for_server( + rospy.Duration(5)) + except rospy.ROSException as e: + rospy.logerr('Action unavaliable: {}'.format(e)) + + def get_laptop_percepntage(self): + try: + msg = rospy.wait_for_message( + self.topicname_laptop_percentage, Float32, timeout=rospy.Duration(5)) + return msg.data + except rospy.ROSException as e: + rospy.logwarn('{}'.format(e)) + return None + + def get_battery_percepntage(self): + try: + msg = rospy.wait_for_message( + self.topicname_battery_percentage, Float32, timeout=rospy.Duration(5)) + return msg.data + except rospy.ROSException as e: + rospy.logwarn('{}'.format(e)) + return None + + def is_connected(self): + try: + msg = rospy.wait_for_message( + self.topicname_cable_connected, Bool, timeout=rospy.Duration(5)) + return msg.data + except rospy.ROSException as e: + rospy.logwarn('{}'.format(e)) + return None + + def is_sitting(self): + try: + msg = rospy.wait_for_message( + self.topicname_status_feedback, Feedback, timeout=rospy.Duration(5)) + return msg.sitting + except rospy.ROSException as e: + rospy.logwarn('{}'.format(e)) + return None + + def is_standing(self): + try: + msg = rospy.wait_for_message( + self.topicname_status_feedback, Feedback, timeout=rospy.Duration(5)) + return msg.standing + except rospy.ROSException as e: + rospy.logwarn('{}'.format(e)) + return None + + def is_powered_on(self): + try: + msg = rospy.wait_for_message( + self.topicname_status_power_state, PowerState, timeout=rospy.Duration(5)) + if msg.motor_power_state == PowerState.STATE_ON: + return True + else: + return False + except rospy.ROSException as e: + rospy.logwarn('{}'.format(e)) + return None + + def pub_cmd_vel(self, vx, vy, vtheta): + msg = Twist() + msg.linear.x = vx + msg.linear.y = vy + msg.angular.z = vtheta + self._pub_cmd_vel.publish(msg) + + def pub_body_pose(self, height, orientation): + msg = Pose() + msg.position.z = height + msg.orientation = orientation + self._pub_body_pose.publish(msg) + + def claim(self): + res = self._srv_client_claim(TriggerRequest()) + return res.success, res.message + + def release(self): + res = self._srv_client_release(TriggerRequest()) + return res.success, res.message + + def stop(self): + res = self._srv_client_stop(TriggerRequest()) + return res.success, res.message + + def self_right(self): + res = self._srv_client_self_right(TriggerRequest()) + return res.success, res.message + + def sit(self): + res = self._srv_client_sit(TriggerRequest()) + return res.success, res.message + + def stand(self): + res = self._srv_client_stand(TriggerRequest()) + return res.success, res.message + + def power_on(self): + res = self._srv_client_power_on(TriggerRequest()) + return res.success, res.message + + def power_off(self): + res = self._srv_client_power_off(TriggerRequest()) + return res.success, res.message + + def estop_hard(self): + res = self._srv_client_estop_hard(TriggerRequest()) + return res.success, res.message + + def estop_gentle(self): + res = self._srv_client_estop_gentle(TriggerRequest()) + return res.success, res.message + + def stair_mode(self, stair_mode): + res = self._srv_client_stair_mode(SetBoolRequest(data=stair_mode)) + return res.success, res.message + + def locomotion_mode(self, locomotion_mode): + res = self._srv_client_locomotion_mode( + SetLocomotionRequest(locomotion_mode=locomotion_mode)) + return res.success, res.message + + def upload_graph(self, upload_filepath): + res = self._srv_client_upload_graph( + UploadGraphRequest(upload_filepath=upload_filepath)) + return res.success, res.message + + def list_graph(self): + res = self._srv_client_list_graph(ListGraphRequest()) + return res.waypoint_ids + + def set_localization_fiducial(self): + res = self._srv_client_set_localization_fiducial( + SetLocalizationFiducialRequest()) + return res.success, res.message + + def set_localization_waypoint(self, waypoint_id): + res = self._srv_client_set_localization_waypoint( + SetLocalizationWaypointRequest(waypoint_id=waypoint_id)) + return res.success, res.message + + def dock(self, dock_id): + req = DockRequest() + req.dock_id = dock_id + self.pub_body_pose(0, Quaternion(0, 0, 0, 1.0)) + self.sit() + self.stand() + res = self._srv_client_dock(req) + return res.success, res.message + + def undock(self): + self.power_on() + res = self._srv_client_undock(TriggerRequest()) + if not self.is_powered_on(): + self.power_on() + if not self.is_standing(): + self.stand() + return res.success, res.message + + def auto_dock(self, dock_id, home_node_id='eng2_73B2'): + res = self.execute_behaviors(home_node_id) + if not res.success: + return res.success, res.message + success, message = self.dock(dock_id) + return success, message + + def auto_undock(self): + self.sit() + self.power_off() + self.release() + self.claim() + self.power_on() + if self.is_connected(): + self.undock() + else: + self.stand() + + def navigate_to(self, id_navigate_to, blocking=False): + goal = NavigateToGoal() + goal.id_navigate_to = id_navigate_to + self._actionclient_navigate_to.send_goal(goal) + if blocking: + self._actionclient_navigate_to.wait_for_result() + return self._actionclient_navigate_to.get_result() + + def wait_for_navigate_to_result(self, duration=rospy.Duration(0)): + return self._actionclient_navigate_to.wait_for_result(duration) + + def get_navigate_to_result(self): + return self._actionclient_navigate_to.get_result() + + def cancel_navigate_to(self): + self._actionclient_navigate_to.cancel_all_goals() + + def get_current_node(self): + try: + msg = rospy.wait_for_message( + self.topicname_current_node_id, String, timeout=rospy.Duration(5)) + return msg.data + except rospy.ROSException as e: + rospy.logwarn('{}'.format(e)) + return None + + def execute_behaviors(self, target_node_id, blocking=True): + goal = LeadPersonGoal() + goal.target_node_id = target_node_id + self._actionclient_execute_behaviors.send_goal(goal) + if blocking: + self._actionclient_execute_behaviors.wait_for_result() + return self._actionclient_execute_behaviors.get_result() + + def wait_execute_behaviors_result(self, duration=None): + if duration is None: + return self._actionclient_execute_behaviors.wait_for_result() + else: + return self._actionclient_execute_behaviors.wait_for_result(duration=duration) + + def get_execute_behaviors_result(self): + return self._actionclient_execute_behaviors.get_result() + + # \brief call trajectory service + # \param x x value of the target position [m] + # \param x y value of the target position [m] + # \param theta theta value of the target position [rad] + # \param duration duration of trajectory command [secs] + def trajectory(self, x, y, theta, duration=None, blocking=False): + rotation = PyKDL.Rotation.RotZ(theta) + goal = TrajectoryGoal() + goal.target_pose.header.frame_id = 'body' + goal.target_pose.pose.position.x = x + goal.target_pose.pose.position.y = y + goal.target_pose.pose.orientation.x = rotation.GetQuaternion()[0] + goal.target_pose.pose.orientation.y = rotation.GetQuaternion()[1] + goal.target_pose.pose.orientation.z = rotation.GetQuaternion()[2] + goal.target_pose.pose.orientation.w = rotation.GetQuaternion()[3] + if duration is None: + goal.duration.data = rospy.Duration(math.sqrt(x**2 + y**2)*10) + else: + goal.duration.data = rospy.Duration(duration) + self._actionclient_trajectory.send_goal(goal) + if blocking: + self._actionclient_trajectory.wait_for_result() From ad00e066daa6615f7ebabeed6d0df0320e979a81 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 6 Jul 2023 16:16:35 +0900 Subject: [PATCH 02/40] [spot_autowalk_data] update metadata for python dependency --- jsk_spot_robot/spot_autowalk_data/CMakeLists.txt | 10 +++++++++- jsk_spot_robot/spot_autowalk_data/package.xml | 5 ++++- jsk_spot_robot/spot_autowalk_data/requirements.txt | 4 ++++ jsk_spot_robot/spot_autowalk_data/scripts/view_map.py | 0 4 files changed, 17 insertions(+), 2 deletions(-) create mode 100644 jsk_spot_robot/spot_autowalk_data/requirements.txt mode change 100755 => 100644 jsk_spot_robot/spot_autowalk_data/scripts/view_map.py diff --git a/jsk_spot_robot/spot_autowalk_data/CMakeLists.txt b/jsk_spot_robot/spot_autowalk_data/CMakeLists.txt index 056cb9480f..7dea41d2c2 100644 --- a/jsk_spot_robot/spot_autowalk_data/CMakeLists.txt +++ b/jsk_spot_robot/spot_autowalk_data/CMakeLists.txt @@ -1,7 +1,15 @@ cmake_minimum_required(VERSION 2.8.3) project(spot_autowalk_data) -find_package(catkin REQUIRED) +find_package(catkin REQUIRED COMPONENTS + catkin_virtualenv +) + +catkin_generate_virtualenv( + PYTHON_INTERPRETER python3 + USE_SYSTEM_PACKAGES FALSE + CHECK_VENV FALSE +) add_custom_target(${PROJECT_NAME}_download_autowalk_data ALL COMMAND python$ENV{ROS_PYTHON_VERSION} ${PROJECT_SOURCE_DIR}/scripts/download_autowalk_data.py) diff --git a/jsk_spot_robot/spot_autowalk_data/package.xml b/jsk_spot_robot/spot_autowalk_data/package.xml index 715f494d26..a77a1f0987 100644 --- a/jsk_spot_robot/spot_autowalk_data/package.xml +++ b/jsk_spot_robot/spot_autowalk_data/package.xml @@ -11,8 +11,11 @@ BSD catkin - jsk_data + jsk_data + catkin_virtualenv + python-gdown-pip + requirements.txt diff --git a/jsk_spot_robot/spot_autowalk_data/requirements.txt b/jsk_spot_robot/spot_autowalk_data/requirements.txt new file mode 100644 index 0000000000..934b434976 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/requirements.txt @@ -0,0 +1,4 @@ +bosdyn-client +bosdyn-mission +bosdyn-choreography-client +vtk diff --git a/jsk_spot_robot/spot_autowalk_data/scripts/view_map.py b/jsk_spot_robot/spot_autowalk_data/scripts/view_map.py old mode 100755 new mode 100644 From 380cf6838c20fb1db94b4fb6b7df8061aed125a4 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 6 Jul 2023 16:56:45 +0900 Subject: [PATCH 03/40] [jsk_spot_behavior_msgs] rename behavior message package --- .../CMakeLists.txt | 6 +++--- .../action/Navigation.action} | 0 .../package.xml | 5 ++--- .../srv/ResetCurrentNode.srv | 0 4 files changed, 5 insertions(+), 6 deletions(-) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_manager_msgs => jsk_spot_behavior_msgs}/CMakeLists.txt (75%) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_manager_msgs/action/LeadPerson.action => jsk_spot_behavior_msgs/action/Navigation.action} (100%) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_manager_msgs => jsk_spot_behavior_msgs}/package.xml (76%) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_manager_msgs => jsk_spot_behavior_msgs}/srv/ResetCurrentNode.srv (100%) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behavior_msgs/CMakeLists.txt similarity index 75% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/CMakeLists.txt rename to jsk_spot_robot/jsk_spot_behavior_msgs/CMakeLists.txt index f3eb72c71c..30e82ce82f 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/CMakeLists.txt +++ b/jsk_spot_robot/jsk_spot_behavior_msgs/CMakeLists.txt @@ -1,11 +1,11 @@ cmake_minimum_required(VERSION 2.8.3) -project(spot_behavior_manager_msgs) +project(jsk_spot_behavior_msgs) find_package(catkin REQUIRED COMPONENTS genmsg actionlib actionlib_msgs) add_action_files( DIRECTORY action - FILES LeadPerson.action + FILES Navigation.action ) add_service_files( @@ -16,6 +16,6 @@ add_service_files( generate_messages(DEPENDENCIES actionlib_msgs) catkin_package( - LIBRARIES spot_behavior_manager_msgs + LIBRARIES jsk_spot_behavior_msgs CATKIN_DEPENDS message_runtime ) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/action/LeadPerson.action b/jsk_spot_robot/jsk_spot_behavior_msgs/action/Navigation.action similarity index 100% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/action/LeadPerson.action rename to jsk_spot_robot/jsk_spot_behavior_msgs/action/Navigation.action diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/package.xml b/jsk_spot_robot/jsk_spot_behavior_msgs/package.xml similarity index 76% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/package.xml rename to jsk_spot_robot/jsk_spot_behavior_msgs/package.xml index 98f45f148c..be66e24a55 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/package.xml +++ b/jsk_spot_robot/jsk_spot_behavior_msgs/package.xml @@ -1,10 +1,9 @@ - spot_behavior_manager_msgs + jsk_spot_behavior_msgs 1.1.0 - The spot_behavior_manager_msgs package + The jsk_spot_behavior_msgs package - Kei Okada Koki Shinjo Koki Shinjo diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/srv/ResetCurrentNode.srv b/jsk_spot_robot/jsk_spot_behavior_msgs/srv/ResetCurrentNode.srv similarity index 100% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_msgs/srv/ResetCurrentNode.srv rename to jsk_spot_robot/jsk_spot_behavior_msgs/srv/ResetCurrentNode.srv From 03e7a3b07182fd5b0316daa335ec5db6fc15304e Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 6 Jul 2023 16:59:47 +0900 Subject: [PATCH 04/40] [jsk_spot_behavior_graph] rename graph package --- .../CMakeLists.txt | 2 +- .../README.md | 0 .../config/edges.yaml | 0 .../config/edges_person_lead.yaml | 0 .../config/nodes.yaml | 0 .../package.xml | 8 +------- .../scripts/visualize_map.py | 0 7 files changed, 2 insertions(+), 8 deletions(-) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_graph => jsk_spot_behavior_graph}/CMakeLists.txt (89%) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_graph => jsk_spot_behavior_graph}/README.md (100%) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_graph => jsk_spot_behavior_graph}/config/edges.yaml (100%) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_graph => jsk_spot_behavior_graph}/config/edges_person_lead.yaml (100%) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_graph => jsk_spot_behavior_graph}/config/nodes.yaml (100%) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_graph => jsk_spot_behavior_graph}/package.xml (61%) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_graph => jsk_spot_behavior_graph}/scripts/visualize_map.py (100%) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behavior_graph/CMakeLists.txt similarity index 89% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/CMakeLists.txt rename to jsk_spot_robot/jsk_spot_behavior_graph/CMakeLists.txt index f28f925292..1e8b0a290f 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/CMakeLists.txt +++ b/jsk_spot_robot/jsk_spot_behavior_graph/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(spot_behavior_graph) +project(jsk_spot_behavior_graph) find_package(catkin REQUIRED) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/README.md b/jsk_spot_robot/jsk_spot_behavior_graph/README.md similarity index 100% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/README.md rename to jsk_spot_robot/jsk_spot_behavior_graph/README.md diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/edges.yaml b/jsk_spot_robot/jsk_spot_behavior_graph/config/edges.yaml similarity index 100% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/edges.yaml rename to jsk_spot_robot/jsk_spot_behavior_graph/config/edges.yaml diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/edges_person_lead.yaml b/jsk_spot_robot/jsk_spot_behavior_graph/config/edges_person_lead.yaml similarity index 100% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/edges_person_lead.yaml rename to jsk_spot_robot/jsk_spot_behavior_graph/config/edges_person_lead.yaml diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/nodes.yaml b/jsk_spot_robot/jsk_spot_behavior_graph/config/nodes.yaml similarity index 100% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/config/nodes.yaml rename to jsk_spot_robot/jsk_spot_behavior_graph/config/nodes.yaml diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/package.xml b/jsk_spot_robot/jsk_spot_behavior_graph/package.xml similarity index 61% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/package.xml rename to jsk_spot_robot/jsk_spot_behavior_graph/package.xml index 8dda04d6cd..5111c213e4 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/package.xml +++ b/jsk_spot_robot/jsk_spot_behavior_graph/package.xml @@ -1,10 +1,9 @@ - spot_behavior_graph + jsk_spot_behavior_graph 1.1.0 The spot_behavior_graph package - Kei Okada Koki Shinjo Koki Shinjo @@ -12,11 +11,6 @@ catkin - rospy - spot_basic_behaviors - spot_person_lead_behaviors - spot_autowalk_data - python-graphviz-pip python-xdot diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/scripts/visualize_map.py b/jsk_spot_robot/jsk_spot_behavior_graph/scripts/visualize_map.py similarity index 100% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_graph/scripts/visualize_map.py rename to jsk_spot_robot/jsk_spot_behavior_graph/scripts/visualize_map.py From 062377c3bb2a7abb12e9e083c8bbfbb6c744fcc2 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 6 Jul 2023 17:05:49 +0900 Subject: [PATCH 05/40] [jsk_spot_behavior_manager] rename to jsk_spot_behavior_manageR --- .../CMakeLists.txt | 2 +- .../launch/demo.launch | 0 .../node_scripts/behavior_manager_server.py | 2 +- .../node_scripts/interactive_behavior_executor.py | 0 .../package.xml | 6 +++--- .../setup.py | 2 +- .../src/jsk_spot_behavior_manager/__init__.py | 2 ++ .../src/jsk_spot_behavior_manager}/base_behavior.py | 0 .../jsk_spot_behavior_manager}/behavior_manager_node.py | 8 ++++---- .../jsk_spot_behavior_manager}/support_behavior_graph.py | 0 .../src/spot_behavior_manager/__init__.py | 2 -- 11 files changed, 12 insertions(+), 12 deletions(-) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_manager => jsk_spot_behavior_manager}/CMakeLists.txt (76%) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_manager_server => jsk_spot_behavior_manager}/launch/demo.launch (100%) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_manager_server => jsk_spot_behavior_manager}/node_scripts/behavior_manager_server.py (72%) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_manager_server => jsk_spot_behavior_manager}/node_scripts/interactive_behavior_executor.py (100%) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_manager => jsk_spot_behavior_manager}/package.xml (96%) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_manager => jsk_spot_behavior_manager}/setup.py (78%) create mode 100644 jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/__init__.py rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager => jsk_spot_behavior_manager/src/jsk_spot_behavior_manager}/base_behavior.py (100%) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager => jsk_spot_behavior_manager/src/jsk_spot_behavior_manager}/behavior_manager_node.py (95%) rename jsk_spot_robot/{jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager => jsk_spot_behavior_manager/src/jsk_spot_behavior_manager}/support_behavior_graph.py (100%) delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/__init__.py diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behavior_manager/CMakeLists.txt similarity index 76% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/CMakeLists.txt rename to jsk_spot_robot/jsk_spot_behavior_manager/CMakeLists.txt index ec7345df41..46b773ac6b 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/CMakeLists.txt +++ b/jsk_spot_robot/jsk_spot_behavior_manager/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(spot_behavior_manager) +project(jsk_spot_behavior_manager) find_package(catkin REQUIRED) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/launch/demo.launch b/jsk_spot_robot/jsk_spot_behavior_manager/launch/demo.launch similarity index 100% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/launch/demo.launch rename to jsk_spot_robot/jsk_spot_behavior_manager/launch/demo.launch diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/node_scripts/behavior_manager_server.py b/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/behavior_manager_server.py similarity index 72% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/node_scripts/behavior_manager_server.py rename to jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/behavior_manager_server.py index cb1a90adc1..c56a7fd62a 100755 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/node_scripts/behavior_manager_server.py +++ b/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/behavior_manager_server.py @@ -2,7 +2,7 @@ # -*- coding: utf-8 -*- import rospy -from spot_behavior_manager.behavior_manager_node import BehaviorManagerNode +from jsk_spot_behavior_manager.behavior_manager_node import BehaviorManagerNode def main(): diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/node_scripts/interactive_behavior_executor.py b/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/interactive_behavior_executor.py similarity index 100% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/node_scripts/interactive_behavior_executor.py rename to jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/interactive_behavior_executor.py diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/package.xml b/jsk_spot_robot/jsk_spot_behavior_manager/package.xml similarity index 96% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/package.xml rename to jsk_spot_robot/jsk_spot_behavior_manager/package.xml index 02ed8c9256..c2bd50a324 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/package.xml +++ b/jsk_spot_robot/jsk_spot_behavior_manager/package.xml @@ -1,6 +1,6 @@ - spot_behavior_manager + jsk_spot_behavior_manager 1.1.0 The spot_behavior_manager package @@ -16,9 +16,9 @@ actionlib networkx + roslaunch rospkg rospy - roslaunch sound_play spot_behavior_manager_msgs spot_ros_client @@ -35,4 +35,4 @@ std_msgs - \ No newline at end of file + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/setup.py b/jsk_spot_robot/jsk_spot_behavior_manager/setup.py similarity index 78% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/setup.py rename to jsk_spot_robot/jsk_spot_behavior_manager/setup.py index 41e9829c62..799b412b34 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/setup.py +++ b/jsk_spot_robot/jsk_spot_behavior_manager/setup.py @@ -2,7 +2,7 @@ from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( - packages=['spot_behavior_manager'], + packages=['jsk_spot_behavior_manager'], package_dir={'': 'src'} ) diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/__init__.py b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/__init__.py new file mode 100644 index 0000000000..3eb6bfca1a --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/__init__.py @@ -0,0 +1,2 @@ +import jsk_spot_behavior_manager.support_behavior_graph as support_behavior_graph +import jsk_spot_behavior_manager.base_behavior as base_behavior diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/base_behavior.py b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/base_behavior.py similarity index 100% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/base_behavior.py rename to jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/base_behavior.py diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/behavior_manager_node.py b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py similarity index 95% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/behavior_manager_node.py rename to jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py index d3e09497c4..8061111c6c 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/behavior_manager_node.py +++ b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py @@ -12,12 +12,12 @@ from sound_play.libsoundplay import SoundClient from spot_ros_client.libspotros import SpotRosClient -from spot_behavior_manager.support_behavior_graph import SupportBehaviorGraph -from spot_behavior_manager.base_behavior import BaseBehavior, load_behavior_class +from jsk_spot_behavior_manager.support_behavior_graph import SupportBehaviorGraph +from jsk_spot_behavior_manager.base_behavior import BaseBehavior, load_behavior_class from std_msgs.msg import String -from spot_behavior_manager_msgs.msg import LeadPersonAction, LeadPersonFeedback, LeadPersonResult, LeadPersonActionFeedback -from spot_behavior_manager_msgs.srv import ResetCurrentNode, ResetCurrentNodeResponse +from jsk_spot_behavior_manager_msgs.msg import LeadPersonAction, LeadPersonFeedback, LeadPersonResult, LeadPersonActionFeedback +from jsk_spot_behavior_manager_msgs.srv import ResetCurrentNode, ResetCurrentNodeResponse class BehaviorManagerNode(object): diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/support_behavior_graph.py b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/support_behavior_graph.py similarity index 100% rename from jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/support_behavior_graph.py rename to jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/support_behavior_graph.py diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/__init__.py b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/__init__.py deleted file mode 100644 index defc635b8b..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager/src/spot_behavior_manager/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -import spot_behavior_manager.support_behavior_graph as support_behavior_graph -import spot_behavior_manager.base_behavior as base_behavior From 37fa6d198c6c241eebac0492bae2a5626fbc091c Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 25 Jul 2023 12:43:53 +0900 Subject: [PATCH 06/40] [spot_ros_client] fix --- .../spot_ros_client/src/spot_ros_client/libspotros.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py index a81d5feb52..5eff13baed 100644 --- a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py +++ b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py @@ -33,8 +33,8 @@ from spot_msgs.srv import Dock from spot_msgs.srv import DockRequest # actions -from spot_behavior_manager_msgs.msg import LeadPersonAction -from spot_behavior_manager_msgs.msg import LeadPersonGoal +from jsk_spot_behavior_msgs.msg import NavigationAction +from jsk_spot_behavior_msgs.msg import NavigationGoal from spot_msgs.msg import NavigateToAction from spot_msgs.msg import NavigateToGoal from spot_msgs.msg import TrajectoryAction @@ -270,7 +270,7 @@ def __init__(self, ) self._actionclient_execute_behaviors = actionlib.SimpleActionClient( actionname_execute_behaviors, - LeadPersonAction + NavigationAction ) # wait for action @@ -483,7 +483,7 @@ def get_current_node(self): return None def execute_behaviors(self, target_node_id, blocking=True): - goal = LeadPersonGoal() + goal = NavigationGoal() goal.target_node_id = target_node_id self._actionclient_execute_behaviors.send_goal(goal) if blocking: From 6960734164803304b48b0648dd5e233fe4756c0f Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 25 Jul 2023 12:45:50 +0900 Subject: [PATCH 07/40] [jsk_spot_behavior_graph][jsk_spot_behavior_manager] update --- .../behavior_graph}/edges.yaml | 0 .../behavior_graph}/edges_person_lead.yaml | 0 .../behavior_graph}/nodes.yaml | 0 .../behavior_graph/simple_behavior_graph.yaml | 17 +++++++++++++++ .../launch/simple_demo.launch | 21 +++++++++++++++++++ .../scripts/visualize_map.py | 0 .../behavior_manager_node.py | 18 ++++++++-------- 7 files changed, 47 insertions(+), 9 deletions(-) rename jsk_spot_robot/{jsk_spot_behavior_graph/config => jsk_spot_behavior_manager/behavior_graph}/edges.yaml (100%) rename jsk_spot_robot/{jsk_spot_behavior_graph/config => jsk_spot_behavior_manager/behavior_graph}/edges_person_lead.yaml (100%) rename jsk_spot_robot/{jsk_spot_behavior_graph/config => jsk_spot_behavior_manager/behavior_graph}/nodes.yaml (100%) create mode 100644 jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/simple_behavior_graph.yaml create mode 100644 jsk_spot_robot/jsk_spot_behavior_manager/launch/simple_demo.launch rename jsk_spot_robot/{jsk_spot_behavior_graph => jsk_spot_behavior_manager}/scripts/visualize_map.py (100%) diff --git a/jsk_spot_robot/jsk_spot_behavior_graph/config/edges.yaml b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/edges.yaml similarity index 100% rename from jsk_spot_robot/jsk_spot_behavior_graph/config/edges.yaml rename to jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/edges.yaml diff --git a/jsk_spot_robot/jsk_spot_behavior_graph/config/edges_person_lead.yaml b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/edges_person_lead.yaml similarity index 100% rename from jsk_spot_robot/jsk_spot_behavior_graph/config/edges_person_lead.yaml rename to jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/edges_person_lead.yaml diff --git a/jsk_spot_robot/jsk_spot_behavior_graph/config/nodes.yaml b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/nodes.yaml similarity index 100% rename from jsk_spot_robot/jsk_spot_behavior_graph/config/nodes.yaml rename to jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/nodes.yaml diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/simple_behavior_graph.yaml b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/simple_behavior_graph.yaml new file mode 100644 index 0000000000..cc3d893a95 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/simple_behavior_graph.yaml @@ -0,0 +1,17 @@ +edges: + - from: 'nodeA' + to: 'nodeB' + behavior_type: 'jsk_spot_behavior_manager.base_behavior.SimpleBehavior' + cost: 10 + - from: 'nodeB' + to: 'nodeC' + behavior_type: 'jsk_spot_behavior_manager.base_behavior.SimpleBehavior' + cost: 10 + - from: 'nodeC' + to: 'nodeB' + behavior_type: 'jsk_spot_behavior_manager.base_behavior.SimpleBehavior' + cost: 10 + - from: 'nodeB' + to: 'nodeA' + behavior_type: 'jsk_spot_behavior_manager.base_behavior.SimpleBehavior' + cost: 10 diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/launch/simple_demo.launch b/jsk_spot_robot/jsk_spot_behavior_manager/launch/simple_demo.launch new file mode 100644 index 0000000000..d54bab4484 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/launch/simple_demo.launch @@ -0,0 +1,21 @@ + + + + + + + + initial_node_id: 'nodeA' + silent_mode: false + + + diff --git a/jsk_spot_robot/jsk_spot_behavior_graph/scripts/visualize_map.py b/jsk_spot_robot/jsk_spot_behavior_manager/scripts/visualize_map.py similarity index 100% rename from jsk_spot_robot/jsk_spot_behavior_graph/scripts/visualize_map.py rename to jsk_spot_robot/jsk_spot_behavior_manager/scripts/visualize_map.py diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py index 8061111c6c..558f29875e 100644 --- a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py +++ b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py @@ -16,8 +16,8 @@ from jsk_spot_behavior_manager.base_behavior import BaseBehavior, load_behavior_class from std_msgs.msg import String -from jsk_spot_behavior_manager_msgs.msg import LeadPersonAction, LeadPersonFeedback, LeadPersonResult, LeadPersonActionFeedback -from jsk_spot_behavior_manager_msgs.srv import ResetCurrentNode, ResetCurrentNodeResponse +from jsk_spot_behavior_msgs.msg import NavigationAction, NavigationFeedback, NavigationResult, NavigationActionFeedback +from jsk_spot_behavior_msgs.srv import ResetCurrentNode, ResetCurrentNodeResponse class BehaviorManagerNode(object): @@ -72,13 +72,13 @@ def __init__(self): self.list_behaviors_execution_actions = [] for action_name in self.list_action_name_synchronizer: self.list_behaviors_execution_actions.append( - rospy.Subscriber('{}/feedback'.format(action_name), LeadPersonActionFeedback, self.callback_synchronizer) + rospy.Subscriber('{}/feedback'.format(action_name), NavigationActionFeedback, self.callback_synchronizer) ) # action server self.server_execute_behaviors = actionlib.SimpleActionServer( '~execute_behaviors', - LeadPersonAction, + NavigationAction, execute_cb=self.handler_execute_behaviors, auto_start=False ) @@ -154,7 +154,7 @@ def handler_execute_behaviors(self, goal): self.current_node_id, goal.target_node_id)) if not self.silent_mode: self.sound_client.say('パスが見つかりませんでした') - result = LeadPersonResult( + result = NavigationResult( success=False, message='No path found') self.server_execute_behaviors.set_aborted(result) return @@ -170,7 +170,7 @@ def handler_execute_behaviors(self, goal): if self.navigate_edge(edge): rospy.loginfo('Edge {} succeeded.'.format(edge)) self.current_node_id = edge.node_id_to - self.server_execute_behaviors.publish_feedback(LeadPersonFeedback(current_node_id=self.current_node_id)) + self.server_execute_behaviors.publish_feedback(NavigationFeedback(current_node_id=self.current_node_id)) self.pre_edge = edge self.set_anchor_pose() else: @@ -178,7 +178,7 @@ def handler_execute_behaviors(self, goal): if not self.silent_mode: self.sound_client.say( '移動に失敗しました。経路を探索し直します。', blocking=True) - self.server_execute_behaviors.publish_feedback(LeadPersonFeedback(current_node_id=self.current_node_id)) + self.server_execute_behaviors.publish_feedback(NavigationFeedback(current_node_id=self.current_node_id)) current_graph.remove_edge( edge.node_id_from, edge.node_id_to) success_navigation = False @@ -189,7 +189,7 @@ def handler_execute_behaviors(self, goal): if not self.silent_mode: self.sound_client.say('エラーが発生しました', blocking=True) self.pre_edge = None - result = LeadPersonResult( + result = NavigationResult( success=False, message='Got an error while navigating edge {}: {}'.format(edge, e)) self.server_execute_behaviors.set_aborted(result) @@ -202,7 +202,7 @@ def handler_execute_behaviors(self, goal): rospy.loginfo('Goal Reached!') if not self.silent_mode: self.sound_client.say('目的地に到着しました.', blocking=True) - result = LeadPersonResult(success=True, message='Goal Reached.') + result = NavigationResult(success=True, message='Goal Reached.') self.server_execute_behaviors.set_succeeded(result) self.set_anchor_pose() return From 8efed6ec3703377cb95cdc731b4d1bd5dcbb8e22 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 25 Jul 2023 12:51:18 +0900 Subject: [PATCH 08/40] [jsk_spot_behavior_manager] Fix behavior_manager --- .../behavior_graph/simple_behavior_graph.yaml | 11 ++++++++ .../behavior_manager_node.py | 26 +++++++++---------- 2 files changed, 24 insertions(+), 13 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/simple_behavior_graph.yaml b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/simple_behavior_graph.yaml index cc3d893a95..cba2b070a0 100644 --- a/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/simple_behavior_graph.yaml +++ b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/simple_behavior_graph.yaml @@ -3,15 +3,26 @@ edges: to: 'nodeB' behavior_type: 'jsk_spot_behavior_manager.base_behavior.SimpleBehavior' cost: 10 + args: {} - from: 'nodeB' to: 'nodeC' behavior_type: 'jsk_spot_behavior_manager.base_behavior.SimpleBehavior' cost: 10 + args: {} - from: 'nodeC' to: 'nodeB' behavior_type: 'jsk_spot_behavior_manager.base_behavior.SimpleBehavior' cost: 10 + args: {} - from: 'nodeB' to: 'nodeA' behavior_type: 'jsk_spot_behavior_manager.base_behavior.SimpleBehavior' cost: 10 + args: {} +nodes: + 'nodeA': + name_en: 'nodeA' + 'nodeB': + name_en: 'nodeB' + 'nodeC': + name_en: 'nodeC' diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py index 558f29875e..3c83b63c03 100644 --- a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py +++ b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py @@ -44,7 +44,7 @@ def __init__(self): '~action_names_synchronizer', []) # action clients - self.spot_client = SpotRosClient() + #self.spot_client = SpotRosClient() self.sound_client = SoundClient( blocking=False, sound_action='/robotsound_jp', @@ -102,21 +102,21 @@ def get_robot_pose(self): def set_anchor_pose(self): rospy.loginfo('Set new anchor pose') - self.anchor_pose = self.get_robot_pose() + #self.anchor_pose = self.get_robot_pose() def go_back_to_anchor_pose(self): rospy.loginfo('Going back to an anchor pose') - current_pose = self.get_robot_pose() - if current_pose is None or self.anchor_pose is None: - return False - frame_current_to_anchor = current_pose.Inverse() * self.anchor_pose - self.spot_client.trajectory( - frame_current_to_anchor.p[0], - frame_current_to_anchor.p[1], - frame_current_to_anchor.M.GetRPY()[2], - 10, - blocking=True - ) + #current_pose = self.get_robot_pose() + #if current_pose is None or self.anchor_pose is None: + # return False + #frame_current_to_anchor = current_pose.Inverse() * self.anchor_pose + #self.spot_client.trajectory( + # frame_current_to_anchor.p[0], + # frame_current_to_anchor.p[1], + # frame_current_to_anchor.M.GetRPY()[2], + # 10, + # blocking=True + # ) def callback_synchronizer(self, msg): From 161a319c9689cf236f56f1c1f389c2aed951c677 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 25 Jul 2023 13:02:35 +0900 Subject: [PATCH 09/40] [jsk_spot_bheavior_manager] udpate --- .../launch/simple_demo.launch | 2 +- .../interactive_behavior_executor.py | 6 ++--- .../src/jsk_spot_behavior_manager/__init__.py | 2 -- ...rt_behavior_graph.py => behavior_graph.py} | 4 ++-- .../behavior_manager_node.py | 22 +++++++++++-------- 5 files changed, 19 insertions(+), 17 deletions(-) rename jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/{support_behavior_graph.py => behavior_graph.py} (97%) diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/launch/simple_demo.launch b/jsk_spot_robot/jsk_spot_behavior_manager/launch/simple_demo.launch index d54bab4484..354212284e 100644 --- a/jsk_spot_robot/jsk_spot_behavior_manager/launch/simple_demo.launch +++ b/jsk_spot_robot/jsk_spot_behavior_manager/launch/simple_demo.launch @@ -15,7 +15,7 @@ initial_node_id: 'nodeA' - silent_mode: false + silent_mode: true diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/interactive_behavior_executor.py b/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/interactive_behavior_executor.py index a06d2a75e4..85333b6ba6 100755 --- a/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/interactive_behavior_executor.py +++ b/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/interactive_behavior_executor.py @@ -4,7 +4,7 @@ import actionlib import rospy -from spot_behavior_manager_msgs.msg import LeadPersonAction, LeadPersonGoal +from jsk_spot_behavior_msgs.msg import NavigationAction, NavigationGoal from sound_play.libsoundplay import SoundClient from ros_speech_recognition import SpeechRecognitionClient @@ -16,7 +16,7 @@ def main(): speech_recognition_client = SpeechRecognitionClient() sound_client = SoundClient(sound_action='/robotsound_jp', sound_topic='/robotsound_jp') - action_server_lead_to = actionlib.SimpleActionClient('~execute_behaviors', LeadPersonAction) + action_server_lead_to = actionlib.SimpleActionClient('~execute_behaviors', NavigationAction) node_list = rospy.get_param('~nodes', {}) @@ -59,7 +59,7 @@ def main(): sound_client.say('{} へ移動します'.format(target_node_name_jp)) rospy.loginfo('executing behaviors to {}'.format(target_node_name_jp)) - action_server_lead_to.send_goal_and_wait(LeadPersonGoal(target_node_id=target_node_id)) + action_server_lead_to.send_goal_and_wait(NavigationGoal(target_node_id=target_node_id)) if __name__ == '__main__': diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/__init__.py b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/__init__.py index 3eb6bfca1a..e69de29bb2 100644 --- a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/__init__.py +++ b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/__init__.py @@ -1,2 +0,0 @@ -import jsk_spot_behavior_manager.support_behavior_graph as support_behavior_graph -import jsk_spot_behavior_manager.base_behavior as base_behavior diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/support_behavior_graph.py b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_graph.py similarity index 97% rename from jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/support_behavior_graph.py rename to jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_graph.py index 7c61102556..861dcf3c00 100644 --- a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/support_behavior_graph.py +++ b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_graph.py @@ -29,9 +29,9 @@ def __init__(self, self.properties = properties -class SupportBehaviorGraph: +class BehaviorGraph: - # 現在の SupportBehaviorGraph の仕様 + # 現在の BehaviorGraph の仕様 # 重み付きの有向グラフ # あるノードからあるノードまでのエッジの数は 0 or 1 diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py index 3c83b63c03..40e7e541ab 100644 --- a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py +++ b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py @@ -12,7 +12,7 @@ from sound_play.libsoundplay import SoundClient from spot_ros_client.libspotros import SpotRosClient -from jsk_spot_behavior_manager.support_behavior_graph import SupportBehaviorGraph +from jsk_spot_behavior_manager.behavior_graph import BehaviorGraph from jsk_spot_behavior_manager.base_behavior import BaseBehavior, load_behavior_class from std_msgs.msg import String @@ -27,7 +27,7 @@ def __init__(self): # navigation dictonary raw_edges = rospy.get_param('~map/edges') raw_nodes = rospy.get_param('~map/nodes') - self.graph = SupportBehaviorGraph(raw_edges, raw_nodes) + self.graph = BehaviorGraph(raw_edges, raw_nodes) self.current_node_id = rospy.get_param('~initial_node_id') self.pre_edge = None self.anchor_pose = None @@ -86,6 +86,10 @@ def __init__(self): rospy.loginfo('Initialized!') + def say(self, message, blocking=True): + + self.sound_client.say(message, blocking=blocking) + def get_robot_pose(self): try: @@ -142,7 +146,7 @@ def handler_reset_current_node_id(self, req): def handler_execute_behaviors(self, goal): - rospy.loginfo('Lead Action started. goal: {}'.format(goal)) + rospy.loginfo('Behavior Action started. goal: {}'.format(goal)) current_graph = copy.deepcopy(self.graph) while True: @@ -153,7 +157,7 @@ def handler_execute_behaviors(self, goal): rospy.logerr('No path from {} to {}'.format( self.current_node_id, goal.target_node_id)) if not self.silent_mode: - self.sound_client.say('パスが見つかりませんでした') + self.say('パスが見つかりませんでした') result = NavigationResult( success=False, message='No path found') self.server_execute_behaviors.set_aborted(result) @@ -161,7 +165,7 @@ def handler_execute_behaviors(self, goal): else: # navigation of edges in the path if not self.silent_mode: - self.sound_client.say('目的地に向かいます', blocking=True) + self.say('目的地に向かいます', blocking=True) self.go_back_to_anchor_pose() success_navigation = True for edge in path: @@ -176,7 +180,7 @@ def handler_execute_behaviors(self, goal): else: rospy.logwarn('Edge {} failed'.format(edge)) if not self.silent_mode: - self.sound_client.say( + self.say( '移動に失敗しました。経路を探索し直します。', blocking=True) self.server_execute_behaviors.publish_feedback(NavigationFeedback(current_node_id=self.current_node_id)) current_graph.remove_edge( @@ -187,7 +191,7 @@ def handler_execute_behaviors(self, goal): rospy.logerr( 'Got an error while navigating edge {}: {}'.format(edge, e)) if not self.silent_mode: - self.sound_client.say('エラーが発生しました', blocking=True) + self.say('エラーが発生しました', blocking=True) self.pre_edge = None result = NavigationResult( success=False, @@ -201,7 +205,7 @@ def handler_execute_behaviors(self, goal): rospy.loginfo('Goal Reached!') if not self.silent_mode: - self.sound_client.say('目的地に到着しました.', blocking=True) + self.say('目的地に到着しました.', blocking=True) result = NavigationResult(success=True, message='Goal Reached.') self.server_execute_behaviors.set_succeeded(result) self.set_anchor_pose() @@ -229,7 +233,7 @@ def navigate_edge(self, edge): rospy.logerr( 'Failed to load and initialize behavior class: {}'.format(e)) if not self.silent_mode: - self.sound_client.say('行動クラスを読み込めませんでした', blocking=True) + self.say('行動クラスを読み込めませんでした', blocking=True) self.pre_edge = None return False From 927bf18f8b275720815c9d949e12a929a354e26e Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 25 Jul 2023 13:28:27 +0900 Subject: [PATCH 10/40] [spot_ros_client] update --- .../src/spot_ros_client/libspotros.py | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py index 5eff13baed..120c64f7e3 100644 --- a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py +++ b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py @@ -1,6 +1,8 @@ import PyKDL import rospy import actionlib +import tf2_ros +import tf2_geometry_msgs import math @@ -273,6 +275,10 @@ def __init__(self, NavigationAction ) + # + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) + # wait for action try: self._actionclient_navigate_to.wait_for_server(rospy.Duration(5)) @@ -300,6 +306,28 @@ def get_battery_percepntage(self): rospy.logwarn('{}'.format(e)) return None + def get_robot_pose(self): + try: + frame_odom_to_base = tf2_geometry_msgs.transform_to_kdl( + self.tf_buffer.lookup_transform( + 'odom', + 'base_link', + rospy.Time() + ) + ) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + return None + return frame_odom_to_base + + def go_pose(self, target_frame): + self.trajectory( + target_frame.p[0], + target_frame.p[1], + target_frame.M.GetRPY()[2], + 10, + blocking=True + ) + def is_connected(self): try: msg = rospy.wait_for_message( From 2ea8d3d70701e3df6217776cacd32daf2f927048 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 25 Jul 2023 13:28:46 +0900 Subject: [PATCH 11/40] [jsk_spot_bheavior_manager --- .../launch/simple_demo.launch | 1 + .../behavior_manager_node.py | 83 +++++++------------ 2 files changed, 32 insertions(+), 52 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/launch/simple_demo.launch b/jsk_spot_robot/jsk_spot_behavior_manager/launch/simple_demo.launch index 354212284e..4cc59161ec 100644 --- a/jsk_spot_robot/jsk_spot_behavior_manager/launch/simple_demo.launch +++ b/jsk_spot_robot/jsk_spot_behavior_manager/launch/simple_demo.launch @@ -16,6 +16,7 @@ initial_node_id: 'nodeA' silent_mode: true + dry_mode: true diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py index 40e7e541ab..48c6cd1777 100644 --- a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py +++ b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py @@ -5,8 +5,6 @@ import actionlib import rospy import roslaunch -import tf2_ros -import tf2_geometry_msgs import PyKDL from sound_play.libsoundplay import SoundClient @@ -32,24 +30,25 @@ def __init__(self): self.pre_edge = None self.anchor_pose = None - # silent mode + # silent mode / dry mode self.silent_mode = rospy.get_param('~silent_mode', False) - - # - self.tf_buffer = tf2_ros.Buffer() - self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) + self.dry_mode = rospy.get_param('~dry_mode', False) # get target action name list for synchronizer self.list_action_name_synchronizer = rospy.get_param( '~action_names_synchronizer', []) # action clients - #self.spot_client = SpotRosClient() - self.sound_client = SoundClient( - blocking=False, - sound_action='/robotsound_jp', - sound_topic='/robotsound_jp' - ) + if self.dry_mode: + self.spot_client = None + self.sound_client = None + else: + self.spot_client = SpotRosClient() + self.sound_client = SoundClient( + blocking=False, + sound_action='/robotsound_jp', + sound_topic='/robotsound_jp' + ) # publisher self.pub_current_node_id = rospy.Publisher( @@ -88,39 +87,25 @@ def __init__(self): def say(self, message, blocking=True): - self.sound_client.say(message, blocking=blocking) - - def get_robot_pose(self): - - try: - frame_odom_to_base = tf2_geometry_msgs.transform_to_kdl( - self.tf_buffer.lookup_transform( - 'odom', - 'base_link', - rospy.Time() - ) - ) - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: - return None - return frame_odom_to_base + rospy.loginfo('Saying: {}'.format(message)) + if not self.silent_mode and not self.dry_mode: + self.sound_client.say(message, blocking=blocking) def set_anchor_pose(self): + rospy.loginfo('Set new anchor pose') - #self.anchor_pose = self.get_robot_pose() + if not self.dry_mode: + self.anchor_pose = self.spot_client.get_robot_pose() def go_back_to_anchor_pose(self): + rospy.loginfo('Going back to an anchor pose') - #current_pose = self.get_robot_pose() - #if current_pose is None or self.anchor_pose is None: - # return False - #frame_current_to_anchor = current_pose.Inverse() * self.anchor_pose - #self.spot_client.trajectory( - # frame_current_to_anchor.p[0], - # frame_current_to_anchor.p[1], - # frame_current_to_anchor.M.GetRPY()[2], - # 10, - # blocking=True - # ) + if not self.dry_mode: + current_pose = self.spot_client.get_robot_pose() + if current_pose is None or self.anchor_pose is None: + return False + frame_current_to_anchor = current_pose.Inverse() * self.anchor_pose + self.spot_client.go_pose(frame_current_to_anchor) def callback_synchronizer(self, msg): @@ -156,16 +141,14 @@ def handler_execute_behaviors(self, goal): if path is None: rospy.logerr('No path from {} to {}'.format( self.current_node_id, goal.target_node_id)) - if not self.silent_mode: - self.say('パスが見つかりませんでした') + self.say('パスが見つかりませんでした') result = NavigationResult( success=False, message='No path found') self.server_execute_behaviors.set_aborted(result) return else: # navigation of edges in the path - if not self.silent_mode: - self.say('目的地に向かいます', blocking=True) + self.say('目的地に向かいます', blocking=True) self.go_back_to_anchor_pose() success_navigation = True for edge in path: @@ -179,8 +162,7 @@ def handler_execute_behaviors(self, goal): self.set_anchor_pose() else: rospy.logwarn('Edge {} failed'.format(edge)) - if not self.silent_mode: - self.say( + self.say( '移動に失敗しました。経路を探索し直します。', blocking=True) self.server_execute_behaviors.publish_feedback(NavigationFeedback(current_node_id=self.current_node_id)) current_graph.remove_edge( @@ -190,8 +172,7 @@ def handler_execute_behaviors(self, goal): except Exception as e: rospy.logerr( 'Got an error while navigating edge {}: {}'.format(edge, e)) - if not self.silent_mode: - self.say('エラーが発生しました', blocking=True) + self.say('エラーが発生しました', blocking=True) self.pre_edge = None result = NavigationResult( success=False, @@ -204,8 +185,7 @@ def handler_execute_behaviors(self, goal): break rospy.loginfo('Goal Reached!') - if not self.silent_mode: - self.say('目的地に到着しました.', blocking=True) + self.say('目的地に到着しました.', blocking=True) result = NavigationResult(success=True, message='Goal Reached.') self.server_execute_behaviors.set_succeeded(result) self.set_anchor_pose() @@ -232,8 +212,7 @@ def navigate_edge(self, edge): except Exception as e: rospy.logerr( 'Failed to load and initialize behavior class: {}'.format(e)) - if not self.silent_mode: - self.say('行動クラスを読み込めませんでした', blocking=True) + self.say('行動クラスを読み込めませんでした', blocking=True) self.pre_edge = None return False From e171854ebb2d000e1437bb3ee58fa51de91fe1ec Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 25 Jul 2023 14:00:36 +0900 Subject: [PATCH 12/40] [jsk_spot_behavior_manager] update README --- .../jsk_spot_behavior_graph/CMakeLists.txt | 15 ------- .../jsk_spot_behavior_graph/README.md | 25 ----------- .../jsk_spot_behavior_graph/package.xml | 19 -------- .../README.md | 44 +++++++++++++++++++ .../CMakeLists.txt | 15 ------- .../spot_behavior_manager_server/README.md | 16 ------- .../spot_behavior_manager_server/package.xml | 24 ---------- 7 files changed, 44 insertions(+), 114 deletions(-) delete mode 100644 jsk_spot_robot/jsk_spot_behavior_graph/CMakeLists.txt delete mode 100644 jsk_spot_robot/jsk_spot_behavior_graph/README.md delete mode 100644 jsk_spot_robot/jsk_spot_behavior_graph/package.xml rename jsk_spot_robot/{jsk_spot_behaviors => jsk_spot_behavior_manager}/README.md (70%) delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/CMakeLists.txt delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/README.md delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/package.xml diff --git a/jsk_spot_robot/jsk_spot_behavior_graph/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behavior_graph/CMakeLists.txt deleted file mode 100644 index 1e8b0a290f..0000000000 --- a/jsk_spot_robot/jsk_spot_behavior_graph/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(jsk_spot_behavior_graph) - -find_package(catkin REQUIRED) - -catkin_package( -) - -install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -catkin_install_python(PROGRAMS scripts/visualize_map.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/jsk_spot_robot/jsk_spot_behavior_graph/README.md b/jsk_spot_robot/jsk_spot_behavior_graph/README.md deleted file mode 100644 index ffc335bf96..0000000000 --- a/jsk_spot_robot/jsk_spot_behavior_graph/README.md +++ /dev/null @@ -1,25 +0,0 @@ -# spot_behavior_graph - -This package provides graph and node data for spot_behavior_manager. - -## scripts - -### visualize_map.py - -Visualizer script of map file of spot_behavior_manager - -Befor using this script, you need to install - -``` -pip3 install graphviz xdot -``` - -#### Usage - -``` -rosrun spot_behavior_graph visualize_map.py --filename -``` - -#### Example Output - -![map](https://user-images.githubusercontent.com/9410362/132942120-4a4e652b-3d25-43df-a678-fd3c09782284.png) diff --git a/jsk_spot_robot/jsk_spot_behavior_graph/package.xml b/jsk_spot_robot/jsk_spot_behavior_graph/package.xml deleted file mode 100644 index 5111c213e4..0000000000 --- a/jsk_spot_robot/jsk_spot_behavior_graph/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - jsk_spot_behavior_graph - 1.1.0 - The spot_behavior_graph package - - Koki Shinjo - Koki Shinjo - - BSD - - catkin - - python-graphviz-pip - python-xdot - - - - diff --git a/jsk_spot_robot/jsk_spot_behaviors/README.md b/jsk_spot_robot/jsk_spot_behavior_manager/README.md similarity index 70% rename from jsk_spot_robot/jsk_spot_behaviors/README.md rename to jsk_spot_robot/jsk_spot_behavior_manager/README.md index 648d8bfc09..96d7c3e0fd 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/README.md +++ b/jsk_spot_robot/jsk_spot_behavior_manager/README.md @@ -47,3 +47,47 @@ Then Spot will go to 2FElevator of eng2 by walk behavior and elevator behavior i https://user-images.githubusercontent.com/9410362/124338016-aad25380-dbe0-11eb-962f-b9a27e1e08cb.mp4 For more details, please see [spot_behavior_manager](./spot_behavior_manager), [spot_behavior_manager_server](./spot_behavior_manager_server) and each behavior documentation. (e.g. [spot_basic_behaviors](./spot_basic_behaviors) ) + +## Behavior Graph + + +This package provides graph and node data for spot_behavior_manager. + +### scripts + +#### visualize_map.py + +Visualizer script of map file of spot_behavior_manager + +Befor using this script, you need to install + +``` +pip3 install graphviz xdot +``` + +##### Usage + +``` +rosrun spot_behavior_graph visualize_map.py --filename +``` + +##### Example Output + +![map](https://user-images.githubusercontent.com/9410362/132942120-4a4e652b-3d25-43df-a678-fd3c09782284.png) + +## behavior manager server + +This package provides ros nodes for spot_behavior_manager. + +### ROS Node + +#### behavior_manager_server.py + +ROS Node script for spot_behavior_manager. + +See [the source of behavior_manager_node](../spot_behavior_manager/src/spot_behavior_manager/behavior_manager_node.py). + +#### interactive_behavior_executor.py + +ROS Node script to execute action of spot_behavior_manager by language. + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/CMakeLists.txt deleted file mode 100644 index e592051190..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(spot_behavior_manager_server) - -find_package(catkin REQUIRED) - -catkin_package( -) - -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -catkin_install_python(PROGRAMS node_scripts/behavior_manager_server.py node_scripts/interactive_behavior_executor.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/README.md b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/README.md deleted file mode 100644 index 21fbfa82c8..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/README.md +++ /dev/null @@ -1,16 +0,0 @@ -# spot_behavior_manager_server - -This package provides ros nodes for spot_behavior_manager. - -## ROS Node - -### behavior_manager_server.py - -ROS Node script for spot_behavior_manager. - -See [the source of behavior_manager_node](../spot_behavior_manager/src/spot_behavior_manager/behavior_manager_node.py). - -### interactive_behavior_executor.py - -ROS Node script to execute action of spot_behavior_manager by language. - diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/package.xml b/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/package.xml deleted file mode 100644 index 084333f92e..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_behavior_manager_server/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - spot_behavior_manager_server - 1.1.0 - The spot_behavior_manager_server package - - Kei Okada - Koki Shinjo - Koki Shinjo - - BSD - - catkin - - rospy - spot_behavior_manager - spot_behavior_map - spot_basic_behaviors - spot_person_lead_behaviors - spot_autowalk_data - - - - From 71b102204a11ee6d4b40064b7649bdb845e6d606 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 3 Aug 2023 18:17:34 +0900 Subject: [PATCH 13/40] [spot_ros_client] update dependency --- jsk_spot_robot/spot_ros_client/package.xml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/jsk_spot_robot/spot_ros_client/package.xml b/jsk_spot_robot/spot_ros_client/package.xml index 8fb8c91515..a0e71c116f 100644 --- a/jsk_spot_robot/spot_ros_client/package.xml +++ b/jsk_spot_robot/spot_ros_client/package.xml @@ -18,6 +18,13 @@ spot_msgs std_srvs geometry_msgs + jsk_spot_behavior_msgs + + actionlib + spot_msgs + std_srvs + geometry_msgs + jsk_spot_behavior_msgs From 4568a1ebbfd761978c60039a9e27526617f72a9e Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 3 Aug 2023 18:17:33 +0900 Subject: [PATCH 14/40] [spot_autowalk_data] update view_map.py --- jsk_spot_robot/spot_autowalk_data/scripts/view_map.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 jsk_spot_robot/spot_autowalk_data/scripts/view_map.py diff --git a/jsk_spot_robot/spot_autowalk_data/scripts/view_map.py b/jsk_spot_robot/spot_autowalk_data/scripts/view_map.py old mode 100644 new mode 100755 From 90968ed12bc3acd368696f67d2038760832faf33 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 3 Aug 2023 18:19:51 +0900 Subject: [PATCH 15/40] [jsk_spot_behaviors] update behvaiors --- .../launch/crosswalk_detection.launch | 24 -- .../node_scripts/car_tracker.py | 233 -------------- .../crosswalk_behavior.py | 161 ---------- .../spot_basic_behaviors/elevator_behavior.py | 2 +- .../src/spot_basic_behaviors/walk_behavior.py | 2 +- .../spot_person_lead_behaviors/CMakeLists.txt | 17 -- .../launch/crosswalk_detection.launch | 47 --- .../launch/elevator_detection.launch | 213 ------------- .../launch/narrow_detection.launch | 96 ------ .../launch/stair_detection.launch | 19 -- .../launch/walk_detection.launch | 19 -- .../node_scripts/car_tracker.py | 219 ------------- .../node_scripts/person_tracker.py | 94 ------ .../spot_person_lead_behaviors/package.xml | 18 -- .../spot_person_lead_behaviors/setup.py | 9 - .../spot_person_lead_behaviors/__init__.py | 5 - .../crosswalk_behavior.py | 201 ------------ .../elevator_behavior.py | 288 ------------------ .../narrow_behavior.py | 231 -------------- .../stair_behavior.py | 194 ------------ .../walk_behavior.py | 157 ---------- 21 files changed, 2 insertions(+), 2247 deletions(-) delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/crosswalk_detection.launch delete mode 100755 jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/node_scripts/car_tracker.py delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/crosswalk_behavior.py delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/CMakeLists.txt delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/crosswalk_detection.launch delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/elevator_detection.launch delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/narrow_detection.launch delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/stair_detection.launch delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/walk_detection.launch delete mode 100755 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/node_scripts/car_tracker.py delete mode 100755 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/node_scripts/person_tracker.py delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/package.xml delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/setup.py delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/__init__.py delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/crosswalk_behavior.py delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/elevator_behavior.py delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/narrow_behavior.py delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/stair_behavior.py delete mode 100644 jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/walk_behavior.py diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/crosswalk_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/crosswalk_detection.launch deleted file mode 100644 index 69499d7307..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/crosswalk_detection.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - frame_fixed: 'odom' - frame_robot: 'body' - timeout_transform: 0.05 - num_max_track: 10 - thresholds_distance: - '2': 50 - '5': 50 - '7': 50 - threshold_move_velocity: 2 - threshold_lost_duration: 1.0 - - - - diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/node_scripts/car_tracker.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/node_scripts/car_tracker.py deleted file mode 100755 index 01ab2037c8..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/node_scripts/car_tracker.py +++ /dev/null @@ -1,233 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import threading - -import PyKDL -import rospy -import message_filters -import tf2_ros -import tf2_geometry_msgs - -from std_msgs.msg import Bool -from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray -from jsk_recognition_msgs.msg import Label, LabelArray - - -def convert_msg_point_to_kdl_vector(point): - return PyKDL.Vector(point.x, point.y, point.z) - - -class TrackedObject(object): - - UNKNOWN = 0 - FOUND = 1 - MOVING = 2 - LOST = 4 - - def __init__(self, - label, - position, - observed_time - ): - - self.label = label - self.state = self.FOUND - self.position = position - self.velocity = PyKDL.Vector() - self.last_observed = observed_time - self.lock = threading.Lock() - - def checkLost(self, - observed_time, - threshold_lost): - - with self.lock: - if (observed_time - self.last_observed).to_sec() > threshold_lost: - self.state = self.LOST - return True - else: - return False - - def update(self, - observed_position, - observed_time, - robot_position_fixedbased, - threshold_velocity, - threshold_distance): - - with self.lock: - if observed_time < self.last_observed: - rospy.logerr( - 'observed time is earlier than last observed time, dropped.') - return - elif observed_time == self.last_observed: - rospy.loginfo('this object is already observed') - return - - # Update position and velocity - if self.state == self.FOUND or self.state == self.MOVING or self.state == self.APPROCHING: - self.velocity = (observed_position - self.position) / \ - (observed_time - self.last_observed).to_sec() - else: - self.velocity = PyKDL.Vector() - self.position = observed_position - self.last_observed = observed_time - - # Update State - if (self.state == self.LOST or self.state == self.UNKNOWN) or \ - (self.velocity.Norm() < threshold_velocity) or \ - ((robot_position_fixedbased - self.position).Norm() > threshold_distance): - self.state = self.FOUND - else: - self.state = self.MOVING - - def getDistance(self, robot_position): - return (self.position - robot_position).Norm() - - -class MultiObjectTracker(object): - - def __init__(self): - - self._frame_fixed = rospy.get_param('~frame_fixed', 'fixed_frame') - self._frame_robot = rospy.get_param('~frame_robot', 'base_link') - self._timeout_transform = rospy.get_param('~timeout_transform', 0.05) - slop = rospy.get_param('~slop', 0.1) - - # parameters for multi object trackers - # maximum number of tracking objects - self._num_max_track = rospy.get_param('~num_max_track', 10) - # threshold for MOT state update - self._thresholds_distance = rospy.get_param('~thresholds_distance', {}) - self._threshold_velocity = rospy.get_param( - '~threshold_move_velocity', 1.0) - self._threshold_lost = rospy.get_param('~threshold_lost_duration', 1.0) - - # members - self._lock_for_dict_objects = threading.RLock() - self._dict_objects = {} - - # TF - self._tf_buffer = tf2_ros.Buffer() - self._tf_listener = tf2_ros.TransformListener(self._tf_buffer) - - # Publisher - # publsher for if there is a moving object or not - self._pub_visible = rospy.Publisher( - '~visible', - Bool, - queue_size=1 - ) - - # Subscriber - rate = rospy.Rate(1) - while not rospy.is_shutdown(): - try: - rospy.wait_for_message( - '~input_bbox_array', BoundingBoxArray, 3) - rospy.wait_for_message('~input_tracking_labels', LabelArray, 3) - break - except (rospy.ROSException, rospy.ROSInterruptException) as e: - rospy.logerr('subscribing topic seems not to be published.') - rospy.logerr('Error: {}'.format(e)) - rate.sleep() - mf_sub_bbbox_array = message_filters.Subscriber( - '~input_bbox_array', BoundingBoxArray) - mf_sub_tracking_labels = message_filters.Subscriber( - '~input_tracking_labels', LabelArray) - ts = message_filters.ApproximateTimeSynchronizer( - [mf_sub_bbbox_array, mf_sub_tracking_labels], 10, slop) - ts.registerCallback(self._cb_object) - - def _cb_object(self, - msg_bbox_array, - msg_tracking_labels): - - if msg_bbox_array.header.frame_id != self._frame_fixed: - rospy.logerr('frame_id of bbox (which is {}) array must be the same `~frame_fixed` which is {}'.format( - msg_bbox_array.header.frame_id, - self._frame_fixed)) - return - - time_observed = msg_tracking_labels.header.stamp - - try: - pykdl_transform_fixed_to_robot = tf2_geometry_msgs.transform_to_kdl( - self._tf_buffer.lookup_transform( - self._frame_fixed, - self._frame_robot, - time_observed, - timeout=rospy.Duration(self._timeout_transform) - ) - ) - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: - rospy.logerr('lookup transform failed. {}'.format(e)) - return - - with self._lock_for_dict_objects: - # add new object and update existing object state - for bbox, tracking_label in zip(msg_bbox_array.boxes, msg_tracking_labels.labels): - - if bbox.label not in self._thresholds_distance.keys(): - continue - - if tracking_label.id not in self._dict_objects: - if len(self._dict_objects) < self._num_max_track: - self._dict_objects[tracking_label.id] = \ - TrackedObject( - bbox.label, - convert_msg_point_to_kdl_vector( - bbox.pose.position), - time_observed - ) - else: - rospy.logwarn( - 'number of objects exceeds max track. dropped.') - else: - self._dict_objects[tracking_label.id].update( - convert_msg_point_to_kdl_vector(bbox.pose.position), - time_observed, - pykdl_transform_fixed_to_robot.p, - self._threshold_velocity, - self._thresholds_distance[str(bbox.label)] - ) - # check if there is lost object - to_be_removed = [] - for key in self._dict_objects: - is_lost = self._dict_objects[key].checkLost( - time_observed, - self._threshold_lost - ) - if is_lost: - to_be_removed.append(key) - # remove lost object from dict - for key in to_be_removed: - self._dict_objects.pop(key) - - def spin(self): - - rate = rospy.Rate(2) - while not rospy.is_shutdown(): - - rate.sleep() - - with self._lock_for_dict_objects: - - exist_moving_object = False - for key in self._dict_objects: - if self._dict_objects[key].state == TrackedObject.MOVING: - exist_moving_object = True - - self._pub_visible.publish(Bool(exist_moving_object)) - - -def main(): - - rospy.init_node('person_tracker') - tracker = MultiObjectTracker() - tracker.spin() - - -if __name__ == '__main__': - main() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/crosswalk_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/crosswalk_behavior.py deleted file mode 100644 index 6a6444929b..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/crosswalk_behavior.py +++ /dev/null @@ -1,161 +0,0 @@ -# -*- coding: utf-8 -*- - -from spot_behavior_manager.base_behavior import BaseBehavior - -import roslaunch -import rospkg -import rospy - -import threading - -from std_msgs.msg import Bool - - -class CrosswalkBehavior(BaseBehavior): - - def callback_car_visible(self, msg): - - if self.car_state_visible != msg.data: - self.car_starttime_visibility = rospy.Time.now() - self.car_duration_visibility = rospy.Duration() - self.car_state_visible = msg.data - else: - self.car_duration_visibility = rospy.Time.now() - self.car_starttime_visibility - - def run_initial(self, start_node, end_node, edge, pre_edge): - - rospy.logdebug('run_initial() called') - - self.silent_mode = rospy.get_param('~silent_mode', True) - - # launch recognition launch - uuid = roslaunch.rlutil.get_or_generate_uuid(None, True) - roslaunch_path = rospkg.RosPack().get_path('spot_basic_behaviors') +\ - '/launch/crosswalk_detection.launch' - roslaunch_cli_args = [roslaunch_path] - roslaunch_file = roslaunch.rlutil.resolve_launch_arguments( - roslaunch_cli_args) - self.roslaunch_parent = roslaunch.parent.ROSLaunchParent( - uuid, - roslaunch_file - ) - self.roslaunch_parent.start() - - # value for car checker - self.subscriber_car_visible = None - self.car_state_visible = False - self.car_starttime_visibility = rospy.Time.now() - self.car_duration_visibility = rospy.Duration(10) - - # start subscribers - try: - self.subscriber_car_visible = rospy.Subscriber( - '/crosswalk_detection_car_tracker/visible', - Bool, - self.callback_car_visible - ) - except Exception as e: - rospy.logerr('{}'.format(e)) - return False - - return True - - def run_main(self, start_node, end_node, edge, pre_edge): - - rospy.logdebug('run_main() called') - - graph_name = edge.properties['graph'] - start_id = filter( - lambda x: x['graph'] == graph_name, - start_node.properties['waypoints_on_graph'] - )[0]['id'] - end_id = filter( - lambda x: x['graph'] == graph_name, - end_node.properties['waypoints_on_graph'] - )[0]['id'] - localization_method = filter( - lambda x: x['graph'] == graph_name, - start_node.properties['waypoints_on_graph'] - )[0]['localization_method'] - - # graph uploading and localization - if pre_edge is not None and \ - graph_name == pre_edge.properties['graph']: - rospy.loginfo('graph upload and localization skipped.') - else: - # Upload - ret = self.spot_client.upload_graph(graph_name) - if ret[0]: - rospy.loginfo('graph {} uploaded.'.format(graph_name)) - else: - rospy.logerr('graph uploading failed: {}'.format(ret[1])) - return False - # Localization - if localization_method == 'fiducial': - ret = self.spot_client.set_localization_fiducial() - elif localization_method == 'waypoint': - ret = self.spot_client.set_localization_waypoint(start_id) - else: - ret = (False, 'Unknown localization method') - if ret[0]: - rospy.loginfo('robot is localized on the graph.') - else: - rospy.logwarn('Localization failed: {}'.format(ret[1])) - return False - - # checking if there is a moving car visible or not - if not self.silent_mode: - self.sound_client.say('車が通るかみています', blocking=True) - safety_count = 0 - rate = rospy.Rate(1) - while not rospy.is_shutdown(): - rate.sleep() - rospy.loginfo('safety_count: {}'.format(safety_count)) - if safety_count > 10: - break - try: - rospy.loginfo('car visible: {}'.format(self.car_state_visible)) - if self.car_state_visible == True: - safety_count = 0 - if not self.silent_mode: - self.sound_client.say('車が通ります', blocking=True) - else: - safety_count += 1 - except Exception as e: - rospy.logwarn('{}'.format(e)) - safety_count = 0 - - # start leading - success = False - rate = rospy.Rate(10) - if not self.silent_mode: - self.sound_client.say('移動します', blocking=True) - self.spot_client.navigate_to(end_id, blocking=False) - while not rospy.is_shutdown(): - rate.sleep() - if self.spot_client.wait_for_navigate_to_result(rospy.Duration(0.1)): - result = self.spot_client.get_navigate_to_result() - success = result.success - rospy.loginfo('result: {}'.format(result)) - break - - # recovery on failure - if not success: - if not self.silent_mode: - self.sound_client.say('失敗したので元に戻ります', blocking=True) - self.spot_client.navigate_to(start_id, blocking=True) - self.spot_client.wait_for_navigate_to_result() - - return success - - def run_final(self, start_node, end_node, edge, pre_edge): - - rospy.logdebug('run_finalize() called') - - self.spot_client.cancel_navigate_to() - - if self.subscriber_car_visible != None: - self.subscriber_car_visible.unregister() - self.subscriber_car_visible = None - - self.roslaunch_parent.shutdown() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py index 9823177b03..a11c31a8e9 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py @@ -1,6 +1,6 @@ # -*- coding: utf-8 -*- -from spot_behavior_manager.base_behavior import BaseBehavior +from jsk_spot_behavior_manager.base_behavior import BaseBehavior import actionlib import roslaunch diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py index aaac0c3e5f..d8fa01e3b6 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py @@ -1,6 +1,6 @@ # -*- coding: utf-8 -*- -from spot_behavior_manager.base_behavior import BaseBehavior +from jsk_spot_behavior_manager.base_behavior import BaseBehavior import rospy diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/CMakeLists.txt deleted file mode 100644 index a01c3a362d..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/CMakeLists.txt +++ /dev/null @@ -1,17 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(spot_person_lead_behaviors) - -find_package(catkin REQUIRED) - -catkin_python_setup() - -catkin_package( -) - -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -catkin_install_python(PROGRAMS node_scripts/car_tracker.py node_scripts/person_tracker.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/crosswalk_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/crosswalk_detection.launch deleted file mode 100644 index 5ac7cceff2..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/crosswalk_detection.launch +++ /dev/null @@ -1,47 +0,0 @@ - - - - - - - - frame_id_robot: 'body' - label_person: 0 - dist_visible: 5.0 - timeout_observation: 5.0 - - - - - - - - - - - - - frame_fixed: 'odom' - frame_robot: 'body' - timeout_transform: 0.05 - num_max_track: 10 - thresholds_distance: - '2': 50 - '5': 50 - '7': 50 - threshold_move_velocity: 2 - threshold_lost_duration: 1.0 - - - - diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/elevator_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/elevator_detection.launch deleted file mode 100644 index 4693e60ca2..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/elevator_detection.launch +++ /dev/null @@ -1,213 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - max_duration: 5.0 - timeout_duration: 0.05 - timer_duration: 0.1 - reference_frame_id: elevator_door_frame_raw - output_frame_id: elevator_door_frame - fixed_frame_id: odom - - - - - - - - approximate_sync: true - output_frame: body - input_topics: - - $(arg TOPIC_DEPTH_FRONT_RIGHT)/points - - $(arg TOPIC_DEPTH_FRONT_LEFT)/points - - $(arg TOPIC_DEPTH_RIGHT)/points - - $(arg TOPIC_DEPTH_LEFT)/points - - $(arg TOPIC_DEPTH_BACK)/points - - - - - - - - initial_pos: [0, 0, 0.5] - initial_rot: [0, 0, 0] - dimension_x: 0.5 - dimension_y: 0.5 - dimension_z: 0.8 - frame_id: elevator_door_frame - - - - - - - - - - - - - - diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/narrow_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/narrow_detection.launch deleted file mode 100644 index fc9644a5b6..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/narrow_detection.launch +++ /dev/null @@ -1,96 +0,0 @@ - - - - - - - - - - - frame_id_robot: 'body' - label_person: 0 - dist_visible: 5.0 - timeout_observation: 5.0 - - - - - - - - - - - - - initial_pos: [1.0, 0, 0] - initial_rot: [0, 0, 0] - dimension_x: 1.0 - dimension_y: 1.5 - dimension_z: 0.5 - frame_id: "body" - - - - - - - - - - - - - - - - - - initial_pos: [1.0, 0, 0] - initial_rot: [0, 0, 0] - dimension_x: 1.0 - dimension_y: 1.5 - dimension_z: 0.5 - frame_id: "body" - - - - - - - - diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/stair_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/stair_detection.launch deleted file mode 100644 index bea1639b05..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/stair_detection.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - frame_id_robot: 'body' - label_person: 0 - dist_visible: 5.0 - timeout_observation: 5.0 - - - diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/walk_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/walk_detection.launch deleted file mode 100644 index 6b7f8850c0..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/launch/walk_detection.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - frame_id_robot: 'body' - label_person: 0 - dist_visible: 5.0 - timeout_observation: 5.0 - - - diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/node_scripts/car_tracker.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/node_scripts/car_tracker.py deleted file mode 100755 index 2763078a42..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/node_scripts/car_tracker.py +++ /dev/null @@ -1,219 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import threading - -import PyKDL -import rospy -import message_filters -import tf2_ros -import tf2_geometry_msgs - -from std_msgs.msg import Bool -from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray -from jsk_recognition_msgs.msg import Label, LabelArray - -def convert_msg_point_to_kdl_vector(point): - return PyKDL.Vector(point.x,point.y,point.z) - -class TrackedObject(object): - - UNKNOWN = 0 - FOUND = 1 - MOVING = 2 - LOST = 4 - - def __init__(self, - label, - position, - observed_time - ): - - self.label = label - self.state = self.FOUND - self.position = position - self.velocity = PyKDL.Vector() - self.last_observed = observed_time - self.lock = threading.Lock() - - def checkLost(self, - observed_time, - threshold_lost): - - with self.lock: - if (observed_time - self.last_observed).to_sec() > threshold_lost: - self.state = self.LOST - return True - else: - return False - - def update(self, - observed_position, - observed_time, - robot_position_fixedbased, - threshold_velocity, - threshold_distance): - - with self.lock: - if observed_time < self.last_observed: - rospy.logerr('observed time is earlier than last observed time, dropped.') - return - elif observed_time == self.last_observed: - rospy.loginfo('this object is already observed') - return - - # Update position and velocity - if self.state == self.FOUND or self.state == self.MOVING or self.state == self.APPROCHING: - self.velocity = (observed_position - self.position) / (observed_time - self.last_observed).to_sec() - else: - self.velocity = PyKDL.Vector() - self.position = observed_position - self.last_observed = observed_time - - # Update State - if (self.state == self.LOST or self.state == self.UNKNOWN) or \ - (self.velocity.Norm() < threshold_velocity) or \ - ((robot_position_fixedbased - self.position).Norm() > threshold_distance): - self.state = self.FOUND - else: - self.state = self.MOVING - - def getDistance(self, robot_position): - return (self.position - robot_position).Norm() - -class MultiObjectTracker(object): - - def __init__(self): - - self._frame_fixed = rospy.get_param('~frame_fixed', 'fixed_frame') - self._frame_robot = rospy.get_param('~frame_robot', 'base_link') - self._timeout_transform = rospy.get_param('~timeout_transform',0.05) - slop = rospy.get_param('~slop', 0.1) - - # parameters for multi object trackers - ## maximum number of tracking objects - self._num_max_track = rospy.get_param('~num_max_track', 10) - ## threshold for MOT state update - self._thresholds_distance = rospy.get_param('~thresholds_distance', {}) - self._threshold_velocity = rospy.get_param('~threshold_move_velocity', 1.0) - self._threshold_lost = rospy.get_param('~threshold_lost_duration', 1.0) - - # members - self._lock_for_dict_objects = threading.RLock() - self._dict_objects = {} - - # TF - self._tf_buffer = tf2_ros.Buffer() - self._tf_listener = tf2_ros.TransformListener(self._tf_buffer) - - # Publisher - ## publsher for if there is a moving object or not - self._pub_visible = rospy.Publisher( - '~visible', - Bool, - queue_size=1 - ) - - # Subscriber - rate = rospy.Rate(1) - while not rospy.is_shutdown(): - try: - rospy.wait_for_message('~input_bbox_array', BoundingBoxArray, 3) - rospy.wait_for_message('~input_tracking_labels', LabelArray, 3) - break - except (rospy.ROSException, rospy.ROSInterruptException) as e: - rospy.logerr('subscribing topic seems not to be published.') - rospy.logerr('Error: {}'.format(e)) - rate.sleep() - mf_sub_bbbox_array = message_filters.Subscriber('~input_bbox_array', BoundingBoxArray) - mf_sub_tracking_labels = message_filters.Subscriber('~input_tracking_labels', LabelArray) - ts = message_filters.ApproximateTimeSynchronizer([mf_sub_bbbox_array, mf_sub_tracking_labels], 10, slop) - ts.registerCallback(self._cb_object) - - def _cb_object(self, - msg_bbox_array, - msg_tracking_labels): - - if msg_bbox_array.header.frame_id != self._frame_fixed: - rospy.logerr('frame_id of bbox (which is {}) array must be the same `~frame_fixed` which is {}'.format( - msg_bbox_array.header.frame_id, - self._frame_fixed)) - return - - time_observed = msg_tracking_labels.header.stamp - - try: - pykdl_transform_fixed_to_robot = tf2_geometry_msgs.transform_to_kdl( - self._tf_buffer.lookup_transform( - self._frame_fixed, - self._frame_robot, - time_observed, - timeout=rospy.Duration(self._timeout_transform) - ) - ) - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: - rospy.logerr('lookup transform failed. {}'.format(e)) - return - - with self._lock_for_dict_objects: - # add new object and update existing object state - for bbox, tracking_label in zip(msg_bbox_array.boxes, msg_tracking_labels.labels): - - if bbox.label not in self._thresholds_distance.keys(): - continue - - if tracking_label.id not in self._dict_objects: - if len(self._dict_objects) < self._num_max_track: - self._dict_objects[tracking_label.id] = \ - TrackedObject( - bbox.label, - convert_msg_point_to_kdl_vector(bbox.pose.position), - time_observed - ) - else: - rospy.logwarn('number of objects exceeds max track. dropped.') - else: - self._dict_objects[tracking_label.id].update( - convert_msg_point_to_kdl_vector(bbox.pose.position), - time_observed, - pykdl_transform_fixed_to_robot.p, - self._threshold_velocity, - self._thresholds_distance[str(bbox.label)] - ) - # check if there is lost object - to_be_removed = [] - for key in self._dict_objects: - is_lost = self._dict_objects[key].checkLost( - time_observed, - self._threshold_lost - ) - if is_lost: - to_be_removed.append(key) - # remove lost object from dict - for key in to_be_removed: - self._dict_objects.pop(key) - - def spin(self): - - rate = rospy.Rate(2) - while not rospy.is_shutdown(): - - rate.sleep() - - with self._lock_for_dict_objects: - - exist_moving_object = False - for key in self._dict_objects: - if self._dict_objects[key].state == TrackedObject.MOVING: - exist_moving_object = True - - self._pub_visible.publish(Bool(exist_moving_object)) - -def main(): - - rospy.init_node('person_tracker') - tracker = MultiObjectTracker() - tracker.spin() - -if __name__=='__main__': - main() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/node_scripts/person_tracker.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/node_scripts/person_tracker.py deleted file mode 100755 index 7ecdc56219..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/node_scripts/person_tracker.py +++ /dev/null @@ -1,94 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import PyKDL -import actionlib -import rospy -import tf2_ros -import tf2_geometry_msgs - -from jsk_recognition_msgs.msg import BoundingBoxArray -from geometry_msgs.msg import PoseArray, Pose -from std_msgs.msg import Bool - -def convert_msg_point_to_kdl_vector(point): - return PyKDL.Vector(point.x,point.y,point.z) - -class PersonTracker(object): - - def __init__(self): - - self._frame_id_robot = rospy.get_param('~frame_id_robot','body') - self._label_person = rospy.get_param('~label_person',0) - self._dist_visible = rospy.get_param('~dist_visible',1.0) - self._timeout_transform = rospy.get_param('~timeout_transform',0.05) - - self._tf_buffer = tf2_ros.Buffer() - self._tf_listener = tf2_ros.TransformListener(self._tf_buffer) - - self._sub_bbox_array = rospy.Subscriber( - '~bbox_array', - BoundingBoxArray, - self._cb_bbox_array - ) - self._pub_visible = rospy.Publisher( - '~visible', - Bool, - queue_size=1 - ) - self._pub_people_pose_array = rospy.Publisher( - '~people_pose_array', - PoseArray, - queue_size=1 - ) - - def _cb_bbox_array(self,msg): - - frame_id_msg = msg.header.frame_id - frame_id_robot = self._frame_id_robot - - try: - frame_fixed_to_robot = tf2_geometry_msgs.transform_to_kdl( - self._tf_buffer.lookup_transform( - frame_id_robot, - frame_id_msg, - msg.header.stamp, - timeout=rospy.Duration(self._timeout_transform) - ) - ) - except (tf2_ros.LookupException, - tf2_ros.ConnectivityException, - tf2_ros.ExtrapolationException) as e: - rospy.logerr('lookup transform failed. {}'.format(e)) - return - - # check if there is a person - is_person_visible = False - people_pose_array = PoseArray() - people_pose_array.header.stamp = msg.header.stamp - people_pose_array.header.frame_id = frame_id_robot - for bbox in msg.boxes: - if bbox.label == self._label_person: - vector_bbox = convert_msg_point_to_kdl_vector(bbox.pose.position) - vector_bbox_robotbased = frame_fixed_to_robot * vector_bbox - rospy.logdebug('Person found at the distance {}'.format(vector_bbox_robotbased.Norm())) - if vector_bbox_robotbased.Norm() < self._dist_visible: - pose = Pose() - pose.position.x = vector_bbox_robotbased[0] - pose.position.y = vector_bbox_robotbased[1] - pose.position.z = vector_bbox_robotbased[2] - pose.orientation.w = 1.0 - people_pose_array.poses.append(pose) - is_person_visible = True - break - self._pub_people_pose_array.publish(people_pose_array) - self._pub_visible.publish(Bool(is_person_visible)) - -def main(): - - rospy.init_node('person_tracker') - person_tracker = PersonTracker() - rospy.spin() - -if __name__=='__main__': - main() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/package.xml b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/package.xml deleted file mode 100644 index b2fdca62b2..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - spot_person_lead_behaviors - 1.1.0 - The spot_person_lead_behaviors package - - Kei Okada - Koki Shinjo - Koki Shinjo - BSD - - catkin - - spot_behavior_manager - - - - diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/setup.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/setup.py deleted file mode 100644 index ed21b775e2..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/setup.py +++ /dev/null @@ -1,9 +0,0 @@ -from setuptools import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['spot_person_lead_behaviors'], - package_dir={'': 'src'} - ) - -setup(**d) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/__init__.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/__init__.py deleted file mode 100644 index cc1bd31158..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/__init__.py +++ /dev/null @@ -1,5 +0,0 @@ -import spot_person_lead_behaviors.walk_behavior as walk_behavior -import spot_person_lead_behaviors.narrow_behavior as narrow_behavior -import spot_person_lead_behaviors.crosswalk_behavior as crosswalk_behavior -import spot_person_lead_behaviors.stair_behavior as stair_behavior -import spot_person_lead_behaviors.elevator_behavior as elevator_behavior diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/crosswalk_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/crosswalk_behavior.py deleted file mode 100644 index 2ed96d7ab2..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/crosswalk_behavior.py +++ /dev/null @@ -1,201 +0,0 @@ -# -*- coding: utf-8 -*- - -from spot_behavior_manager.base_behavior import BaseBehavior - -import roslaunch -import rospkg -import rospy - -import threading - -from std_msgs.msg import Bool - -class CrosswalkBehavior(BaseBehavior): - - def callback_person_visible(self, msg): - - if self.person_state_visible != msg.data: - self.person_starttime_visibility = rospy.Time.now() - self.person_duration_visibility = rospy.Duration() - self.person_state_visible = msg.data - else: - self.person_duration_visibility = rospy.Time.now() - self.person_starttime_visibility - - def callback_car_visible(self, msg): - - if self.car_state_visible != msg.data: - self.car_starttime_visibility = rospy.Time.now() - self.car_duration_visibility = rospy.Duration() - self.car_state_visible = msg.data - else: - self.car_duration_visibility = rospy.Time.now() - self.car_starttime_visibility - - def run_initial(self, start_node, end_node, edge, pre_edge ): - - rospy.logdebug('run_initial() called') - - # launch recognition launch - uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) - roslaunch_path = rospkg.RosPack().get_path('spot_person_lead_behaviors') +\ - '/launch/crosswalk_detection.launch' - roslaunch_cli_args = [roslaunch_path] - roslaunch_file = roslaunch.rlutil.resolve_launch_arguments(roslaunch_cli_args) - self.roslaunch_parent = roslaunch.parent.ROSLaunchParent( - uuid, - roslaunch_file - ) - self.roslaunch_parent.start() - - # value for person checker - self.subscriber_person_visible = None - self.person_state_visible = False - self.person_starttime_visibility = rospy.Time.now() - self.person_duration_visibility = rospy.Duration(10) - - # value for car checker - self.subscriber_car_visible = None - self.car_state_visible = False - self.car_starttime_visibility = rospy.Time.now() - self.car_duration_visibility = rospy.Duration(10) - - # start subscribers - try: - self.subscriber_person_visible = rospy.Subscriber( - '/crosswalk_detection_person_tracker/visible', - Bool, - self.callback_person_visible - ) - self.subscriber_car_visible = rospy.Subscriber( - '/crosswalk_detection_car_tracker/visible', - Bool, - self.callback_car_visible - ) - except Exception as e: - rospy.logerr('{}'.format(e)) - return False - - return True - - def run_main(self, start_node, end_node, edge, pre_edge ): - - rospy.logdebug('run_main() called') - - graph_name = edge.properties['graph'] - start_id = filter( - lambda x: x['graph'] == graph_name, - start_node.properties['waypoints_on_graph'] - )[0]['id'] - end_id = filter( - lambda x: x['graph'] == graph_name, - end_node.properties['waypoints_on_graph'] - )[0]['id'] - localization_method = filter( - lambda x: x['graph'] == graph_name, - start_node.properties['waypoints_on_graph'] - )[0]['localization_method'] - - # graph uploading and localization - if pre_edge is not None and \ - graph_name == pre_edge.properties['graph']: - rospy.loginfo('graph upload and localization skipped.') - else: - # Upload - ret = self.spot_client.upload_graph(graph_name) - if ret[0]: - rospy.loginfo('graph {} uploaded.'.format(graph_name)) - else: - rospy.logerr('graph uploading failed: {}'.format(ret[1])) - return False - # Localization - if localization_method == 'fiducial': - ret = self.spot_client.set_localization_fiducial() - elif localization_method == 'waypoint': - ret = self.spot_client.set_localization_waypoint(start_id) - else: - ret = (False,'Unknown localization method') - if ret[0]: - rospy.loginfo('robot is localized on the graph.') - else: - rospy.logwarn('Localization failed: {}'.format(ret[1])) - return False - - # checking if there is a moving car visible or not - self.sound_client.say('車が通るかみています',blocking=True) - safety_count = 0 - rate = rospy.Rate(1) - while not rospy.is_shutdown(): - rate.sleep() - rospy.loginfo('safety_count: {}'.format(safety_count)) - if safety_count > 10: - break; - try: - rospy.loginfo('car visible: {}'.format(self.car_state_visible)) - if self.car_state_visible == True: - safety_count = 0 - self.sound_client.say('車が通ります',blocking=True) - else: - safety_count += 1 - except Exception as e: - rospy.logwarn('{}'.format(e)) - safety_count = 0 - - # start leading - success = False - rate = rospy.Rate(10) - self.sound_client.say('ついてきてください',blocking=True) - self.spot_client.navigate_to( end_id, blocking=False) - while not rospy.is_shutdown(): - rate.sleep() - - if self.spot_client.wait_for_navigate_to_result(rospy.Duration(0.1)): - result = self.spot_client.get_navigate_to_result() - success = result.success - rospy.loginfo('result: {}'.format(result)) - break - - if not self.person_state_visible and self.person_duration_visibility > rospy.Duration(0.5): - flag_speech = False - def notify_visibility(): - self.sound_client.say( - '近くに人が見えません', - volume=1.0, - blocking=True - ) - flag_speech = False - speech_thread = threading.Thread(target=notify_visibility) - speech_thread.start() - while not self.person_state_visible and self.person_duration_visibility > rospy.Duration(0.5): - rate.sleep() - self.spot_client.pub_cmd_vel(0,0,0) - if not flag_speech: - flag_speech = True - speech_thread = threading.Thread(target=notify_visibility) - speech_thread.start() - if not self.person_state_visible and self.person_duration_visibility > rospy.Duration(5.0): - self.spot_client.cancel_navigate_to() - if self.person_state_visible: - self.spot_client.navigate_to( end_id, blocking=False) - - # recovery on failure - if not success: - self.sound_client.say('失敗したので元に戻ります', blocking=True) - self.spot_client.navigate_to( start_id, blocking=True) - self.spot_client.wait_for_navigate_to_result() - - return success - - def run_final(self, start_node, end_node, edge, pre_edge ): - - rospy.logdebug('run_finalize() called') - - self.spot_client.cancel_navigate_to() - - if self.subscriber_person_visible != None: - self.subscriber_person_visible.unregister() - self.subscriber_person_visible = None - - if self.subscriber_car_visible != None: - self.subscriber_car_visible.unregister() - self.subscriber_car_visible = None - - self.roslaunch_parent.shutdown() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/elevator_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/elevator_behavior.py deleted file mode 100644 index fe68d2d10b..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/elevator_behavior.py +++ /dev/null @@ -1,288 +0,0 @@ -# -*- coding: utf-8 -*- - -from spot_behavior_manager.base_behavior import BaseBehavior - -import actionlib -import roslaunch -import rospkg -import rospy - -import math - -from switchbot_ros.msg import SwitchBotCommandGoal, SwitchBotCommandAction -from sensor_msgs.msg import PointCloud2 -from spinal.msg import Barometer, Imu -from geometry_msgs.msg import Quaternion - -class ElevatorBehavior(BaseBehavior): - - def door_point_cloud_callback(self, msg): - if len(msg.data) == 0: - self.door_is_open = True - else: - self.door_is_open = False - - def barometer_callback(self, msg): - - rospy.logdebug('altitude: {}'.format(msg.altitude)) - if math.fabs(msg.altitude - self.target_altitude) < self.threshold_altitude: - self.is_target_floor = True - else: - self.is_target_floor = False - - def imu_callback(self, msg): - - rospy.logdebug('acc z: {}'.format(msg.acc_data[2])) - if math.fabs(msg.acc_data[2] - self.stable_acc_z) < self.threshold_acc: - self.elevator_stop_acc = True - else: - self.elevator_stop_acc = False - - def run_initial(self, start_node, end_node, edge, pre_edge ): - - rospy.logdebug('run_initial() called') - - # launch recognition launch - uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) - roslaunch_path = rospkg.RosPack().get_path('spot_person_lead_behaviors') +\ - '/launch/elevator_detection.launch' - roslaunch_cli_args = [roslaunch_path] - roslaunch_file = roslaunch.rlutil.resolve_launch_arguments(roslaunch_cli_args) - self.roslaunch_parent = roslaunch.parent.ROSLaunchParent( - uuid, - roslaunch_file - ) - self.roslaunch_parent.start() - - # value for door openring checker - self.door_is_open = False - self.subscriber_door_check = None - - # value for floor detection - self.threshold_altitude = rospy.get_param('/elevator_behavior/threshold_altitude', 2) - self.is_target_floor = False - self.target_altitude = 0 - self.subscriber_floor_detection = None - - # - self.threshold_acc = rospy.get_param('/elevator_behavior/threshold_acc', 0.2) - self.stable_acc_z = 0 - self.elevator_stop_acc = False - self.subscriber_imu = None - - # value for switchbot - self.action_client_switchbot = actionlib.SimpleActionClient( - '/switchbot_ros/switch', - SwitchBotCommandAction - ) - - return True - - def run_main(self, start_node, end_node, edge, pre_edge ): - - rospy.logdebug('run_main() called') - - graph_name = edge.properties['graph'] - start_id = filter( - lambda x: x['graph'] == graph_name, - start_node.properties['waypoints_on_graph'] - )[0]['id'] - end_id = edge.properties['goal_waypoint_id'] - rest_waypoint_id = edge.properties['rest_waypoint_id'] - localization_method = filter( - lambda x: x['graph'] == graph_name, - start_node.properties['waypoints_on_graph'] - )[0]['localization_method'] - - start_altitude = start_node.properties['floor_height'] - end_altitude = end_node.properties['floor_height'] - end_floor = end_node.properties['floor'] - - # graph uploading and localization - if pre_edge is not None and \ - graph_name == pre_edge.properties['graph']: - rospy.loginfo('graph upload and localization skipped.') - else: - # Upload - ret = self.spot_client.upload_graph(graph_name) - if ret[0]: - rospy.loginfo('graph {} uploaded.'.format(graph_name)) - else: - rospy.logerr('graph uploading failed: {}'.format(ret[1])) - return False - # Localization - if localization_method == 'fiducial': - ret = self.spot_client.set_localization_fiducial() - elif localization_method == 'waypoint': - ret = self.spot_client.set_localization_waypoint(start_id) - else: - ret = (False,'Unknown localization method') - if ret[0]: - rospy.loginfo('robot is localized on the graph.') - else: - rospy.logwarn('Localization failed: {}'.format(ret[1])) - return False - - # start floor detection - try: - current_altitude = rospy.wait_for_message( '/spinal/baro', Barometer, rospy.Duration(5)).altitude - self.target_altitude = current_altitude - ( start_altitude - end_altitude ) - self.subscriber_floor_detection = rospy.Subscriber( - '/spinal/baro', - Barometer, - self.barometer_callback) - except Exception as e: - rospy.logerr('{}'.format(e)) - return False - - # start imu - try: - self.stable_acc_z = rospy.wait_for_message( '/spinal/imu', Imu, rospy.Duration(5)).acc_data[2] - self.subscriber_imu = rospy.Subscriber( - '/spinal/imu', - Imu, - self.imu_callback) - except Exception as e: - rospy.logerr('{}'.format(e)) - return False - - # start door opening check from outside - self.subscriber_door_check = rospy.Subscriber( - '/spot_recognition/elevator_door_points', - PointCloud2, - self.door_point_cloud_callback) - - self.sound_client.say('エレベーターに乗ります', blocking=True) - - # push button with switchbot - rospy.loginfo('calling elevator when riding...') - self.sound_client.say('エレベーターを呼んでいます', blocking=True) - success_calling = False - if not self.action_client_switchbot.wait_for_server(rospy.Duration(10)): - rospy.logerr('switchbot server seems to fail.') - return False - else: - switchbot_goal = SwitchBotCommandGoal() - switchbot_goal.device_name = start_node.properties['switchbot_device'] - switchbot_goal.command = 'press' - count = 0 - while True: - self.action_client_switchbot.send_goal(switchbot_goal) - if self.action_client_switchbot.wait_for_result(rospy.Duration(10)): - break - count += 1 - if count >= 3: - rospy.logerr('switchbot calling failed.') - return False - result = self.action_client_switchbot.get_result() - rospy.loginfo('switchbot result: {}'.format(result)) - if not result.done: - rospy.logerr('switchbot calling failed.') - return False - rospy.loginfo('elevator calling when riding on has succeeded') - - # wait for elevator - rate = rospy.Rate(1) - door_open_count = 0 - while not rospy.is_shutdown(): - rate.sleep() - if self.door_is_open: - door_open_count += 1 - else: - door_open_count = 0 - if door_open_count >= 2: - break - rospy.loginfo('elevator door opened.') - self.subscriber_door_check.unregister() - self.subscriber_door_check = None - - # start navigation to rest point - rate = rospy.Rate(10) - self.sound_client.say('エレベータに乗り込みます。ご注意ください。', blocking=False) - self.spot_client.navigate_to( rest_waypoint_id, blocking=False) - ## call elevator from destination floor while - rospy.loginfo('calling elevator when getting off...') - switchbot_goal = SwitchBotCommandGoal() - switchbot_goal.device_name = end_node.properties['switchbot_device'] - switchbot_goal.command = 'press' - self.action_client_switchbot.send_goal(switchbot_goal) - ## - if not self.action_client_switchbot.wait_for_result(timeout=rospy.Duration(20)): - rospy.logerr('Switchbot timeout') - self.spot_client.wait_for_navigate_to_result() - self.spot_client.navigate_to( start_id, blocking=True) - self.spot_client.wait_for_navigate_to_result() - return False - result_switchbot = self.action_client_switchbot.get_result() - self.spot_client.wait_for_navigate_to_result() - result_navigation = self.spot_client.get_navigate_to_result() - ## recovery when riding on - if not result_navigation.success or not result_switchbot.done: - rospy.logerr('Failed to ride on a elevator. result_navigation: {}, result_switchbot: {}'.format(result_navigation,result_switchbot)) - self.spot_client.navigate_to( start_id, blocking=True) - self.spot_client.wait_for_navigate_to_result() - return False - else: - rospy.loginfo('Riding on succeded.') - - self.sound_client.say('{}階に行きます'.format(end_floor), blocking=False) - - # start door openning check from inside - self.subscriber_door_check = rospy.Subscriber( - '/spot_recognition/elevator_door_points', - PointCloud2, - self.door_point_cloud_callback) - - # check if the door is closed - rate = rospy.Rate(2) - while not rospy.is_shutdown(): - rate.sleep() - if not self.door_is_open: - break - rospy.loginfo('elevator door closed') - - # check if the door is open and at the target floor - rate = rospy.Rate(1) - while not rospy.is_shutdown(): - rate.sleep() - rospy.loginfo('door_is_open: {}, is_target_floor: {}, stop from acc: {}'.format( - self.door_is_open, self.is_target_floor, self.elevator_stop_acc)) - if self.door_is_open and self.is_target_floor and self.elevator_stop_acc: - break - rospy.loginfo('elevator door opened and at the target_floor') - self.sound_client.say('エレベーターが{}階に到着しました'.format(end_floor), blocking=False) - - # dance before starting to move - self.spot_client.pub_body_pose(0.0,Quaternion(w=1)) - self.spot_client.stand() - rospy.sleep(0.5) - self.spot_client.pub_body_pose(-0.2,Quaternion(w=1)) - self.spot_client.stand() - rospy.sleep(0.5) - self.spot_client.pub_body_pose(0.0,Quaternion(w=1)) - self.spot_client.stand() - - # get off the elevator - self.sound_client.say('エレベーターからおります', blocking=False) - self.spot_client.navigate_to(end_id, blocking=True) - result = self.spot_client.get_navigate_to_result() - - return result.success - - def run_final(self, start_node, end_node, edge, pre_edge ): - - rospy.logdebug('run_finalize() called') - - if self.subscriber_door_check != None: - self.subscriber_door_check.unregister() - self.subscriber_door_check = None - - if self.subscriber_floor_detection != None: - self.subscriber_floor_detection.unregister() - self.subscriber_floor_detection = None - - if self.subscriber_imu != None: - self.subscriber_imu.unregister() - self.subscriber_imu = None - - self.roslaunch_parent.shutdown() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/narrow_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/narrow_behavior.py deleted file mode 100644 index b7a90ecf26..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/narrow_behavior.py +++ /dev/null @@ -1,231 +0,0 @@ -# -*- coding: utf-8 -*- - -from spot_behavior_manager.base_behavior import BaseBehavior - -import roslaunch -import rospkg -import rospy - -import threading - -from std_msgs.msg import Bool -from sensor_msgs.msg import PointCloud2 - -class NarrowBehavior(BaseBehavior): - - def callback_visible(self, msg): - - if self.state_visible != msg.data: - self.starttime_visibility = rospy.Time.now() - self.duration_visibility = rospy.Duration() - self.state_visible = msg.data - else: - self.duration_visibility = rospy.Time.now() - self.starttime_visibility - - def callback_obstacle_right(self, msg): - - if len(msg.data) > 0: - self.last_obstacle_right = msg.header.stamp - - def callback_obstacle_left(self, msg): - - if len(msg.data) > 0: - self.last_obstacle_left = msg.header.stamp - - def run_initial(self, start_node, end_node, edge, pre_edge ): - - rospy.logdebug('run_initial() called') - - # get parameters - self.use_person_detection = rospy.get_param('/narrow_behavior/use_person_detection', True) - self.use_obstacle_detection = rospy.get_param('/narrow_behavior/use_obstacle_detection', True) - - # launch recognition launch - uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) - roslaunch_path = rospkg.RosPack().get_path('spot_person_lead_behaviors') +\ - '/launch/narrow_detection.launch' - roslaunch_cli_args = [roslaunch_path] - roslaunch_file = roslaunch.rlutil.resolve_launch_arguments(roslaunch_cli_args) - self.roslaunch_parent = roslaunch.parent.ROSLaunchParent( - uuid, - roslaunch_file - ) - self.roslaunch_parent.start() - - # value for person checker - self.subscriber_visible = None - self.state_visible = False - self.starttime_visibility = rospy.Time.now() - self.duration_visibility = rospy.Duration(10) - - # value for obstacle detection - self.subscriber_obstacle_right = None - self.subscriber_obstacle_left = None - self.last_obstacle_right = rospy.Time() - self.last_obstacle_left = rospy.Time() - - # start subscribers - try: - self.subscriber_obstacle_right = rospy.Subscriber( - '/spot_recognition/right_obstacle', - PointCloud2, - self.callback_obstacle_right - ) - self.subscriber_obstacle_left = rospy.Subscriber( - '/spot_recognition/left_obstacle', - PointCloud2, - self.callback_obstacle_left - ) - self.subscriber_visible = rospy.Subscriber( - '/narrow_detection_person_tracker/visible', - Bool, - self.callback_visible - ) - except Exception as e: - rospy.logerr('{}'.format(e)) - return False - - return True - - def run_main(self, start_node, end_node, edge, pre_edge ): - - rospy.logdebug('run_main() called') - - graph_name = edge.properties['graph'] - start_id = filter( - lambda x: x['graph'] == graph_name, - start_node.properties['waypoints_on_graph'] - )[0]['id'] - end_id = filter( - lambda x: x['graph'] == graph_name, - end_node.properties['waypoints_on_graph'] - )[0]['id'] - localization_method = filter( - lambda x: x['graph'] == graph_name, - start_node.properties['waypoints_on_graph'] - )[0]['localization_method'] - - # graph uploading and localization - if pre_edge is not None and \ - graph_name == pre_edge.properties['graph']: - rospy.loginfo('graph upload and localization skipped.') - else: - # Upload - ret = self.spot_client.upload_graph(graph_name) - if ret[0]: - rospy.loginfo('graph {} uploaded.'.format(graph_name)) - else: - rospy.logerr('graph uploading failed: {}'.format(ret[1])) - return False - # Localization - if localization_method == 'fiducial': - ret = self.spot_client.set_localization_fiducial() - elif localization_method == 'waypoint': - ret = self.spot_client.set_localization_waypoint(start_id) - else: - ret = (False,'Unknown localization method') - if ret[0]: - rospy.loginfo('robot is localized on the graph.') - else: - rospy.logwarn('Localization failed: {}'.format(ret[1])) - return False - - # thread for obstacle notification - flag_valid_obstacle_notification = True - def notify_obstacle(): - rate = rospy.Rate(2) - while flag_valid_obstacle_notification and not rospy.is_shutdown(): - current_time = rospy.Time.now() - if current_time - self.last_obstacle_right < rospy.Duration(1.0) \ - and current_time - self.last_obstacle_left < rospy.Duration(1.0): - self.sound_client.say( - '周囲にご注意ください', - blocking=True - ) - elif current_time - self.last_obstacle_right < rospy.Duration(1.0): - self.sound_client.say( - '右にご注意ください', - blocking=True - ) - elif current_time - self.last_obstacle_left < rospy.Duration(1.0): - self.sound_client.say( - '左にご注意ください', - blocking=True - ) - rospy.logwarn('notify_obstacle finished.') - if self.use_obstacle_detection: - thread_notify_obstacle = threading.Thread(target=notify_obstacle) - else: - thread_notify_obstacle = None - - # start leading - success = False - rate = rospy.Rate(10) - self.sound_client.say('ついてきてください',blocking=True) - self.spot_client.navigate_to( end_id, blocking=False) - if thread_notify_obstacle is not None: - thread_notify_obstacle.start() - while not rospy.is_shutdown(): - rate.sleep() - - if self.spot_client.wait_for_navigate_to_result(rospy.Duration(0.1)): - result = self.spot_client.get_navigate_to_result() - success = result.success - rospy.loginfo('result: {}'.format(result)) - break - - if self.use_person_detection and not self.state_visible and self.duration_visibility > rospy.Duration(0.5): - flag_speech = False - def notify_visibility(): - self.sound_client.say( - '近くに人が見えません', - volume=1.0, - blocking=True - ) - flag_speech = False - speech_thread = threading.Thread(target=notify_visibility) - speech_thread.start() - while not self.state_visible and self.duration_visibility > rospy.Duration(0.5): - rate.sleep() - self.spot_client.pub_cmd_vel(0,0,0) - if not flag_speech: - flag_speech = True - speech_thread = threading.Thread(target=notify_visibility) - speech_thread.start() - if not self.state_visible and self.duration_visibility > rospy.Duration(5.0): - self.spot_client.cancel_navigate_to() - if self.state_visible: - self.spot_client.navigate_to( end_id, blocking=False) - - # stop notification thread - flag_valid_obstacle_notification = False - if thread_notify_obstacle is not None: - thread_notify_obstacle.join() - - # recovery on failure - if not success: - self.sound_client.say('失敗したので元に戻ります', blocking=True) - self.spot_client.navigate_to( start_id, blocking=True) - self.spot_client.wait_for_navigate_to_result() - - return success - - def run_final(self, start_node, end_node, edge, pre_edge ): - - rospy.logdebug('run_finalize() called') - - self.spot_client.cancel_navigate_to() - - if self.subscriber_obstacle_right != None: - self.subscriber_obstacle_right.unregister() - self.subscriber_obstacle_right = None - - if self.subscriber_obstacle_left != None: - self.subscriber_obstacle_left.unregister() - self.subscriber_obstacle_left = None - - if self.subscriber_visible != None: - self.subscriber_visible.unregister() - self.subscriber_visible = None - - self.roslaunch_parent.shutdown() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/stair_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/stair_behavior.py deleted file mode 100644 index 83d0613980..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/stair_behavior.py +++ /dev/null @@ -1,194 +0,0 @@ -# -*- coding: utf-8 -*- - -from spot_behavior_manager.base_behavior import BaseBehavior - -import roslaunch -import rospkg -import rospy - -from std_msgs.msg import Bool -from geometry_msgs.msg import PoseArray - -import threading - -class StairBehavior(BaseBehavior): - - def callback_visible(self, msg): - - if self.state_visible != msg.data: - self.starttime_visibility = rospy.Time.now() - self.duration_visibility = rospy.Duration() - self.state_visible = msg.data - else: - self.duration_visibility = rospy.Time.now() - self.starttime_visibility - - def callback_people_pose_array(self, msg): - - self.exist_person_down = False - for pose in msg.poses: - if pose.position.z < -0.5: - self.exist_person_down = True - - def run_initial(self, start_node, end_node, edge, pre_edge ): - - rospy.logdebug('run_initial() called') - - # launch recognition launch - uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) - roslaunch_path = rospkg.RosPack().get_path('spot_person_lead_behaviors') +\ - '/launch/stair_detection.launch' - roslaunch_cli_args = [roslaunch_path] - roslaunch_file = roslaunch.rlutil.resolve_launch_arguments(roslaunch_cli_args) - self.roslaunch_parent = roslaunch.parent.ROSLaunchParent( - uuid, - roslaunch_file - ) - self.roslaunch_parent.start() - - # value for person checker - self.subscriber_visible = None - self.state_visible = False - self.starttime_visibility = rospy.Time.now() - self.duration_visibility = rospy.Duration(10) - - # value for people pose array - self.subscriber_people_pose_array = None - self.msg_people_pose_array = None - self.exist_person_down = False - - # start subscribers - try: - self.subscriber_visible = rospy.Subscriber( - '/stair_detection_person_tracker/visible', - Bool, - self.callback_visible - ) - self.subscriber_people_pose_array = rospy.Subscriber( - '/stair_detection_person_tracker/people_pose_array', - PoseArray, - self.callback_people_pose_array - ) - except Exception as e: - rospy.logerr('{}'.format(e)) - return False - - return True - - def run_main(self, start_node, end_node, edge, pre_edge ): - - rospy.logdebug('run_main() called') - - graph_name = edge.properties['graph'] - start_id = filter( - lambda x: x['graph'] == graph_name, - start_node.properties['waypoints_on_graph'] - )[0]['id'] - end_id = filter( - lambda x: x['graph'] == graph_name, - end_node.properties['waypoints_on_graph'] - )[0]['id'] - localization_method = filter( - lambda x: x['graph'] == graph_name, - start_node.properties['waypoints_on_graph'] - )[0]['localization_method'] - stair_nums = edge.properties['stair_nums'] - - # graph uploading and localization - if pre_edge is not None and \ - graph_name == pre_edge.properties['graph']: - rospy.loginfo('graph upload and localization skipped.') - else: - # Upload - ret = self.spot_client.upload_graph(graph_name) - if ret[0]: - rospy.loginfo('graph {} uploaded.'.format(graph_name)) - else: - rospy.logerr('graph uploading failed: {}'.format(ret[1])) - return False - # Localization - if localization_method == 'fiducial': - ret = self.spot_client.set_localization_fiducial() - elif localization_method == 'waypoint': - ret = self.spot_client.set_localization_waypoint(start_id) - else: - ret = (False,'Unknown localization method') - if ret[0]: - rospy.loginfo('robot is localized on the graph.') - else: - rospy.logwarn('Localization failed: {}'.format(ret[1])) - return False - - self.sound_client.say('階段を移動します。',blocking=True) - for index, num in enumerate(stair_nums): - if num < 0: - self.sound_client.say('{}段の下り階段があります'.format(-num),blocking=True) - else: - self.sound_client.say('{}段の登り階段があります'.format(num),blocking=True) - - if index != len(stair_nums) - 1: - self.sound_client.say('その後踊り場があった後',blocking=True) - - # start leading - success = False - rate = rospy.Rate(10) - self.sound_client.say('ついてきてください',blocking=True) - self.spot_client.navigate_to( end_id, blocking=False) - while not rospy.is_shutdown(): - rate.sleep() - - if self.spot_client.wait_for_navigate_to_result(rospy.Duration(0.1)): - result = self.spot_client.get_navigate_to_result() - success = result.success - rospy.loginfo('result: {}'.format(result)) - break - - if not self.state_visible and self.duration_visibility > rospy.Duration(0.5): - flag_speech = False - def notify_visibility(): - self.sound_client.say( - '近くに人が見えません', - blocking=True - ) - flag_speech = False - speech_thread = threading.Thread(target=notify_visibility) - speech_thread.start() - while not self.state_visible and self.duration_visibility > rospy.Duration(0.5): - rate.sleep() - self.spot_client.pub_cmd_vel(0,0,0) - if not flag_speech: - flag_speech = True - speech_thread = threading.Thread(target=notify_visibility) - speech_thread.start() - if not self.state_visible and self.duration_visibility > rospy.Duration(5.0): - self.spot_client.cancel_navigate_to() - if self.state_visible: - self.spot_client.navigate_to( end_id, blocking=False) - - # recovery on failure - if not result.success: - self.sound_client.say('失敗したので元に戻ります', blocking=True) - self.spot_client.navigate_to( start_id, blocking=True) - self.spot_client.wait_for_navigate_to_result() - return result.success - else: - rate = rospy.Rate(1) - while not self.state_visible: - self.sound_client.say('ひとが見えるまでお待ちしています。',blocking=True) - rate.sleep() - return result.success - - def run_final(self, start_node, end_node, edge, pre_edge ): - - rospy.logdebug('run_finalize() called') - - self.spot_client.cancel_navigate_to() - - if self.subscriber_visible != None: - self.subscriber_visible.unregister() - self.subscriber_visible = None - - if self.subscriber_people_pose_array != None: - self.subscriber_people_pose_array.unregister() - self.subscriber_people_pose_array = None - - self.roslaunch_parent.shutdown() diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/walk_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/walk_behavior.py deleted file mode 100644 index e825a4b9d9..0000000000 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_person_lead_behaviors/src/spot_person_lead_behaviors/walk_behavior.py +++ /dev/null @@ -1,157 +0,0 @@ -# -*- coding: utf-8 -*- - -from spot_behavior_manager.base_behavior import BaseBehavior - -import roslaunch -import rospkg -import rospy - -import threading - -from std_msgs.msg import Bool - -class WalkBehavior(BaseBehavior): - - def callback_visible(self, msg): - - if self.state_visible != msg.data: - self.starttime_visibility = rospy.Time.now() - self.duration_visibility = rospy.Duration() - self.state_visible = msg.data - else: - self.duration_visibility = rospy.Time.now() - self.starttime_visibility - - def run_initial(self, start_node, end_node, edge, pre_edge ): - - rospy.logdebug('run_initial() called') - - # launch recognition launch - uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) - roslaunch_path = rospkg.RosPack().get_path('spot_person_lead_behaviors') +\ - '/launch/walk_detection.launch' - roslaunch_cli_args = [roslaunch_path] - roslaunch_file = roslaunch.rlutil.resolve_launch_arguments(roslaunch_cli_args) - self.roslaunch_parent = roslaunch.parent.ROSLaunchParent( - uuid, - roslaunch_file - ) - self.roslaunch_parent.start() - - # value for person checker - self.subscriber_visible = None - self.state_visible = False - self.starttime_visibility = rospy.Time.now() - self.duration_visibility = rospy.Duration(10) - - # start subscriber - try: - self.subscriber_visible = rospy.Subscriber( - '/walk_detection_person_tracker/visible', - Bool, - self.callback_visible - ) - except Exception as e: - rospy.logerr('{}'.format(e)) - return False - - return True - - def run_main(self, start_node, end_node, edge, pre_edge ): - - rospy.logdebug('run_main() called') - - graph_name = edge.properties['graph'] - start_id = filter( - lambda x: x['graph'] == graph_name, - start_node.properties['waypoints_on_graph'] - )[0]['id'] - end_id = filter( - lambda x: x['graph'] == graph_name, - end_node.properties['waypoints_on_graph'] - )[0]['id'] - localization_method = filter( - lambda x: x['graph'] == graph_name, - start_node.properties['waypoints_on_graph'] - )[0]['localization_method'] - - # graph uploading and localization - if pre_edge is not None and \ - graph_name == pre_edge.properties['graph']: - rospy.loginfo('graph upload and localization skipped.') - else: - # Upload - ret = self.spot_client.upload_graph(graph_name) - if ret[0]: - rospy.loginfo('graph {} uploaded.'.format(graph_name)) - else: - rospy.logerr('graph uploading failed: {}'.format(ret[1])) - return False - # Localization - if localization_method == 'fiducial': - ret = self.spot_client.set_localization_fiducial() - elif localization_method == 'waypoint': - ret = self.spot_client.set_localization_waypoint(start_id) - else: - ret = (False,'Unknown localization method') - if ret[0]: - rospy.loginfo('robot is localized on the graph.') - else: - rospy.logwarn('Localization failed: {}'.format(ret[1])) - return False - - # start leading - success = False - rate = rospy.Rate(10) - self.sound_client.say('ついてきてください',blocking=True) - self.spot_client.navigate_to( end_id, blocking=False) - while not rospy.is_shutdown(): - rate.sleep() - - if self.spot_client.wait_for_navigate_to_result(rospy.Duration(0.1)): - result = self.spot_client.get_navigate_to_result() - success = result.success - rospy.loginfo('result: {}'.format(result)) - break - - if not self.state_visible and self.duration_visibility > rospy.Duration(0.5): - flag_speech = False - def notify_visibility(): - self.sound_client.say( - '近くに人が見えません', - volume=1.0, - blocking=True - ) - flag_speech = False - speech_thread = threading.Thread(target=notify_visibility) - speech_thread.start() - while not self.state_visible and self.duration_visibility > rospy.Duration(0.5): - rate.sleep() - self.spot_client.pub_cmd_vel(0,0,0) - if not flag_speech: - flag_speech = True - speech_thread = threading.Thread(target=notify_visibility) - speech_thread.start() - if not self.state_visible and self.duration_visibility > rospy.Duration(5.0): - self.spot_client.cancel_navigate_to() - if self.state_visible: - self.spot_client.navigate_to( end_id, blocking=False) - - # recovery on failure - if not success: - self.sound_client.say('失敗したので元に戻ります', blocking=True) - self.spot_client.navigate_to( start_id, blocking=True) - self.spot_client.wait_for_navigate_to_result() - - return success - - def run_final(self, start_node, end_node, edge, pre_edge ): - - rospy.logdebug('run_finalize() called') - - self.spot_client.cancel_navigate_to() - - if self.subscriber_visible != None: - self.subscriber_visible.unregister() - self.subscriber_visible = None - - self.roslaunch_parent.shutdown() From 9ca0b7a83e35f5dd4fa2351e67ca62e10002a257 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 3 Aug 2023 19:32:40 +0900 Subject: [PATCH 16/40] [spot_autowalk_data] update --- jsk_spot_robot/spot_autowalk_data/autowalk/.gitignore | 1 + jsk_spot_robot/spot_autowalk_data/scripts/view_map.py | 0 2 files changed, 1 insertion(+) create mode 100644 jsk_spot_robot/spot_autowalk_data/autowalk/.gitignore mode change 100755 => 100644 jsk_spot_robot/spot_autowalk_data/scripts/view_map.py diff --git a/jsk_spot_robot/spot_autowalk_data/autowalk/.gitignore b/jsk_spot_robot/spot_autowalk_data/autowalk/.gitignore new file mode 100644 index 0000000000..72e8ffc0db --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/autowalk/.gitignore @@ -0,0 +1 @@ +* diff --git a/jsk_spot_robot/spot_autowalk_data/scripts/view_map.py b/jsk_spot_robot/spot_autowalk_data/scripts/view_map.py old mode 100755 new mode 100644 From dd256a17d907a3b065340d9070a8a4002cff13b0 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 3 Aug 2023 19:33:04 +0900 Subject: [PATCH 17/40] [jsk_spot_behaviors][jsk_spot_behavior_manager] update --- .../launch/demo.launch | 48 ++----------------- .../node_scripts/behavior_manager_server.py | 2 +- .../jsk_spot_behavior_manager/package.xml | 1 + .../behavior_manager_node.py | 8 +++- .../spot_basic_behaviors/CMakeLists.txt | 4 -- .../src/spot_basic_behaviors/__init__.py | 3 +- .../src/spot_basic_behaviors/walk_behavior.py | 12 ++--- 7 files changed, 18 insertions(+), 60 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/launch/demo.launch b/jsk_spot_robot/jsk_spot_behavior_manager/launch/demo.launch index 57bb98f92b..8986335870 100644 --- a/jsk_spot_robot/jsk_spot_behavior_manager/launch/demo.launch +++ b/jsk_spot_robot/jsk_spot_behavior_manager/launch/demo.launch @@ -1,17 +1,9 @@ - - - - - - - /narrow_behavior/use_person_detection: $(arg use_person_detection) - /narrow_behavior/use_obstacle_detection: $(arg use_obstacle_detection) - + - - - - initial_node_id: '$(arg initial_node_id)' - action_names_synchronizer: - - '/spot_person_lead_server/execute_behaviors' - silent_mode: true - - - - - - - - initial_node_id: '$(arg initial_node_id)' - action_names_synchronizer: - - '/spot_behavior_manager_server/execute_behaviors' silent_mode: true diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/behavior_manager_server.py b/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/behavior_manager_server.py index c56a7fd62a..af70e08639 100755 --- a/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/behavior_manager_server.py +++ b/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/behavior_manager_server.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # -*- coding: utf-8 -*- import rospy diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/package.xml b/jsk_spot_robot/jsk_spot_behavior_manager/package.xml index c2bd50a324..bc941c558b 100644 --- a/jsk_spot_robot/jsk_spot_behavior_manager/package.xml +++ b/jsk_spot_robot/jsk_spot_behavior_manager/package.xml @@ -23,6 +23,7 @@ spot_behavior_manager_msgs spot_ros_client std_msgs + spot_autowalk_data actionlib networkx diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py index 48c6cd1777..abb4049ef8 100644 --- a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py +++ b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py @@ -1,6 +1,8 @@ # -*- coding: utf-8 -*- import copy +import sys +import traceback import actionlib import rospy @@ -171,7 +173,8 @@ def handler_execute_behaviors(self, goal): break except Exception as e: rospy.logerr( - 'Got an error while navigating edge {}: {}'.format(edge, e)) + 'Got an error while navigating edge {}: {}'.format(edge, sys.exc_info())) + traceback.print_exc() self.say('エラーが発生しました', blocking=True) self.pre_edge = None result = NavigationResult( @@ -211,7 +214,8 @@ def navigate_edge(self, edge): ) except Exception as e: rospy.logerr( - 'Failed to load and initialize behavior class: {}'.format(e)) + 'Failed to load and initialize behavior class: {}'.format(sys.exc_info())) + traceback.print_exc() self.say('行動クラスを読み込めませんでした', blocking=True) self.pre_edge = None return False diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt index d0b53fd0b6..fd9652fb99 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt @@ -8,10 +8,6 @@ catkin_python_setup() catkin_package( ) -catkin_install_python(PROGRAMS node_scripts/car_tracker.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - install(DIRECTORY config launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py index 2e6a9eefe6..7b122d2e77 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py @@ -1,3 +1,2 @@ import spot_basic_behaviors.walk_behavior as walk_behavior -import spot_basic_behaviors.crosswalk_behavior as crosswalk_behavior -import spot_basic_behaviors.elevator_behavior as elevator_behavior +#import spot_basic_behaviors.elevator_behavior as elevator_behavior diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py index d8fa01e3b6..8361264e4d 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py @@ -18,18 +18,18 @@ def run_main(self, start_node, end_node, edge, pre_edge): rospy.logdebug('run_main() called') graph_name = edge.properties['graph'] - start_id = filter( + start_id = list(filter( lambda x: x['graph'] == graph_name, start_node.properties['waypoints_on_graph'] - )[0]['id'] - end_id = filter( + ))[0]['id'] + end_id = list(filter( lambda x: x['graph'] == graph_name, end_node.properties['waypoints_on_graph'] - )[0]['id'] - localization_method = filter( + ))[0]['id'] + localization_method = list(filter( lambda x: x['graph'] == graph_name, start_node.properties['waypoints_on_graph'] - )[0]['localization_method'] + ))[0]['localization_method'] # graph uploading and localization if pre_edge is not None and \ From 2bcf72c7cd624408ca1e892b88678e17ec0ff299 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 3 Aug 2023 19:48:54 +0900 Subject: [PATCH 18/40] [spot_basic_behaviors] update elevator_behaviors --- .../spot_basic_behaviors/elevator_behavior.py | 83 +++++-------------- 1 file changed, 19 insertions(+), 64 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py index a11c31a8e9..8e5544dd5d 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py @@ -11,8 +11,8 @@ from switchbot_ros.msg import SwitchBotCommandGoal, SwitchBotCommandAction from sensor_msgs.msg import PointCloud2 -from spinal.msg import Barometer, Imu from geometry_msgs.msg import Quaternion +from std_msgs.msg import Float32, Bool, Int16 class ElevatorBehavior(BaseBehavior): @@ -23,21 +23,11 @@ def door_point_cloud_callback(self, msg): else: self.door_is_open = False - def barometer_callback(self, msg): + def current_floor_callback(self, msg): + self.current_floor = msg.data - rospy.logdebug('altitude: {}'.format(msg.altitude)) - if math.fabs(msg.altitude - self.target_altitude) < self.threshold_altitude: - self.is_target_floor = True - else: - self.is_target_floor = False - - def imu_callback(self, msg): - - rospy.logdebug('acc z: {}'.format(msg.acc_data[2])) - if math.fabs(msg.acc_data[2] - self.stable_acc_z) < self.threshold_acc: - self.elevator_stop_acc = True - else: - self.elevator_stop_acc = False + def rest_elevator_callback(self, msg): + self.rest_elevator = msg.data def run_initial(self, start_node, end_node, edge, pre_edge): @@ -62,19 +52,12 @@ def run_initial(self, start_node, end_node, edge, pre_edge): self.door_is_open = False self.subscriber_door_check = None - # value for floor detection - self.threshold_altitude = rospy.get_param( - '/elevator_behavior/threshold_altitude', 2) - self.is_target_floor = False - self.target_altitude = 0 - self.subscriber_floor_detection = None + # elevator state + self.current_floor = None + self.rest_elevator = None - # - self.threshold_acc = rospy.get_param( - '/elevator_behavior/threshold_acc', 0.2) - self.stable_acc_z = 0 - self.elevator_stop_acc = False - self.subscriber_imu = None + self.subscriber_current_floor = rospy.Subscriber("/elevator_state_publisher/current_floor", Int16, self.current_floor_callback) + self.subscriber_rest_elevator = rospy.Subscriber("/elevator_state_publisher/rest_elevator", Bool, self.rest_elevator_callback) # value for switchbot self.action_client_switchbot = actionlib.SimpleActionClient( @@ -100,8 +83,6 @@ def run_main(self, start_node, end_node, edge, pre_edge): start_node.properties['waypoints_on_graph'] )[0]['localization_method'] - start_altitude = start_node.properties['floor_height'] - end_altitude = end_node.properties['floor_height'] end_floor = end_node.properties['floor'] # graph uploading and localization @@ -129,32 +110,6 @@ def run_main(self, start_node, end_node, edge, pre_edge): rospy.logwarn('Localization failed: {}'.format(ret[1])) return False - # start floor detection - try: - current_altitude = rospy.wait_for_message( - '/spinal/baro', Barometer, rospy.Duration(5)).altitude - self.target_altitude = current_altitude - \ - (start_altitude - end_altitude) - self.subscriber_floor_detection = rospy.Subscriber( - '/spinal/baro', - Barometer, - self.barometer_callback) - except Exception as e: - rospy.logerr('{}'.format(e)) - return False - - # start imu - try: - self.stable_acc_z = rospy.wait_for_message( - '/spinal/imu', Imu, rospy.Duration(5)).acc_data[2] - self.subscriber_imu = rospy.Subscriber( - '/spinal/imu', - Imu, - self.imu_callback) - except Exception as e: - rospy.logerr('{}'.format(e)) - return False - # start door opening check from outside self.subscriber_door_check = rospy.Subscriber( '/spot_recognition/elevator_door_points', @@ -254,9 +209,9 @@ def run_main(self, start_node, end_node, edge, pre_edge): rate = rospy.Rate(1) while not rospy.is_shutdown(): rate.sleep() - rospy.loginfo('door_is_open: {}, is_target_floor: {}, stop from acc: {}'.format( - self.door_is_open, self.is_target_floor, self.elevator_stop_acc)) - if self.door_is_open and self.is_target_floor and self.elevator_stop_acc: + rospy.loginfo('door_is_open: {}, is_target_floor: {}, elevator stop: {}'.format( + self.door_is_open, end_floor == self.current_floor, self.rest_elevator)) + if self.door_is_open and end_floor == self.current_floor and self.rest_elevator: break rospy.loginfo('elevator door opened and at the target_floor') @@ -284,12 +239,12 @@ def run_final(self, start_node, end_node, edge, pre_edge): self.subscriber_door_check.unregister() self.subscriber_door_check = None - if self.subscriber_floor_detection != None: - self.subscriber_floor_detection.unregister() - self.subscriber_floor_detection = None + if self.subscriber_current_floor != None: + self.subscriber_current_floor.unregister() + self.subscriber_current_floor = None - if self.subscriber_imu != None: - self.subscriber_imu.unregister() - self.subscriber_imu = None + if self.subscriber_rest_elevator != None: + self.subscriber_rest_elevator.unregister() + self.subscriber_rest_elevator = None self.roslaunch_parent.shutdown() From 86805901424e5f32a08ad394c68aa363124a1660 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 7 Aug 2023 20:37:21 +0900 Subject: [PATCH 19/40] [jsk_spot_behaviors] fix bugs --- .../src/spot_basic_behaviors/__init__.py | 2 +- .../src/spot_basic_behaviors/elevator_behavior.py | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py index 7b122d2e77..077d19e4ad 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py @@ -1,2 +1,2 @@ import spot_basic_behaviors.walk_behavior as walk_behavior -#import spot_basic_behaviors.elevator_behavior as elevator_behavior +import spot_basic_behaviors.elevator_behavior as elevator_behavior diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py index 8e5544dd5d..d4b4ebdb69 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py @@ -72,16 +72,16 @@ def run_main(self, start_node, end_node, edge, pre_edge): rospy.logdebug('run_main() called') graph_name = edge.properties['graph'] - start_id = filter( + start_id = list(filter( lambda x: x['graph'] == graph_name, start_node.properties['waypoints_on_graph'] - )[0]['id'] + ))[0]['id'] end_id = edge.properties['goal_waypoint_id'] rest_waypoint_id = edge.properties['rest_waypoint_id'] - localization_method = filter( + localization_method = list(filter( lambda x: x['graph'] == graph_name, start_node.properties['waypoints_on_graph'] - )[0]['localization_method'] + ))[0]['localization_method'] end_floor = end_node.properties['floor'] From a7bb8c325381c2c22599ef1a8daf68ecb453ce9a Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 7 Aug 2023 23:59:57 +0900 Subject: [PATCH 20/40] [spot_basic_behaviors] update elevator behaviors --- .../launch/elevator_detection.launch | 11 ++++ .../spot_basic_behaviors/elevator_behavior.py | 52 +++++++++++-------- 2 files changed, 42 insertions(+), 21 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch index 529eb1538e..681be2c7a3 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch @@ -12,6 +12,8 @@ + + @@ -212,4 +214,13 @@ /> + + + + + + + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py index d4b4ebdb69..707ad18c25 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py @@ -35,11 +35,13 @@ def run_initial(self, start_node, end_node, edge, pre_edge): self.silent_mode = rospy.get_param('~silent_mode', True) + start_floor = start_node.properties['floor'] + # launch recognition launch uuid = roslaunch.rlutil.get_or_generate_uuid(None, True) roslaunch_path = rospkg.RosPack().get_path('spot_basic_behaviors') +\ '/launch/elevator_detection.launch' - roslaunch_cli_args = [roslaunch_path] + roslaunch_cli_args = [roslaunch_path, "initial_floor:={}".format(start_floor)] roslaunch_file = roslaunch.rlutil.resolve_launch_arguments( roslaunch_cli_args) self.roslaunch_parent = roslaunch.parent.ROSLaunchParent( @@ -65,6 +67,18 @@ def run_initial(self, start_node, end_node, edge, pre_edge): SwitchBotCommandAction ) + if not self.action_client_switchbot.wait_for_server(rospy.Duration(30)): + rospy.logerr('switchbot server seems to fail.') + return False + + try: + rospy.wait_for_message('/spot_recognition/elevator_door_points', timeout=rospy.Duration(30)) + rospy.wait_for_message("/elevator_state_publisher/current_floor", timeout=rospy.Duration(30)) + rospy.wait_for_message("/elevator_state_publisher/rest_elevator", timeout=rospy.Duration(30)) + except rospy.ROSException: + rospy.logerr("Some topics are not published.") + return False + return True def run_main(self, start_node, end_node, edge, pre_edge): @@ -119,27 +133,23 @@ def run_main(self, start_node, end_node, edge, pre_edge): # push button with switchbot rospy.loginfo('calling elevator when riding...') success_calling = False - if not self.action_client_switchbot.wait_for_server(rospy.Duration(10)): - rospy.logerr('switchbot server seems to fail.') + switchbot_goal = SwitchBotCommandGoal() + switchbot_goal.device_name = start_node.properties['switchbot_device'] + switchbot_goal.command = 'press' + count = 0 + while True: + self.action_client_switchbot.send_goal(switchbot_goal) + if self.action_client_switchbot.wait_for_result(rospy.Duration(10)): + break + count += 1 + if count >= 3: + rospy.logerr('switchbot calling failed.') + return False + result = self.action_client_switchbot.get_result() + rospy.loginfo('switchbot result: {}'.format(result)) + if not result.done: + rospy.logerr('switchbot calling failed.') return False - else: - switchbot_goal = SwitchBotCommandGoal() - switchbot_goal.device_name = start_node.properties['switchbot_device'] - switchbot_goal.command = 'press' - count = 0 - while True: - self.action_client_switchbot.send_goal(switchbot_goal) - if self.action_client_switchbot.wait_for_result(rospy.Duration(10)): - break - count += 1 - if count >= 3: - rospy.logerr('switchbot calling failed.') - return False - result = self.action_client_switchbot.get_result() - rospy.loginfo('switchbot result: {}'.format(result)) - if not result.done: - rospy.logerr('switchbot calling failed.') - return False rospy.loginfo('elevator calling when riding on has succeeded') # wait for elevator From 47cf82999b550c8efe1966b3b3bcb179491a8010 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 8 Aug 2023 00:54:04 +0900 Subject: [PATCH 21/40] [spot_basic_behaviors] update elevator behavior --- .../launch/elevator_detection.launch | 6 +++--- .../src/spot_basic_behaviors/elevator_behavior.py | 13 +++++++------ 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch index 681be2c7a3..2a4e2886a1 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch @@ -13,7 +13,7 @@ - + @@ -217,8 +217,8 @@ - - + + diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py index 707ad18c25..ee139b8d9c 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py @@ -36,14 +36,15 @@ def run_initial(self, start_node, end_node, edge, pre_edge): self.silent_mode = rospy.get_param('~silent_mode', True) start_floor = start_node.properties['floor'] + rospy.loginfo("Start floor: {}".format(start_floor)) # launch recognition launch uuid = roslaunch.rlutil.get_or_generate_uuid(None, True) roslaunch_path = rospkg.RosPack().get_path('spot_basic_behaviors') +\ '/launch/elevator_detection.launch' - roslaunch_cli_args = [roslaunch_path, "initial_floor:={}".format(start_floor)] - roslaunch_file = roslaunch.rlutil.resolve_launch_arguments( - roslaunch_cli_args) + cli_args = [roslaunch_path, "initial_floor:={}".format(start_floor)] + roslaunch_args = cli_args[1:] + roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0], roslaunch_args)] self.roslaunch_parent = roslaunch.parent.ROSLaunchParent( uuid, roslaunch_file @@ -72,9 +73,9 @@ def run_initial(self, start_node, end_node, edge, pre_edge): return False try: - rospy.wait_for_message('/spot_recognition/elevator_door_points', timeout=rospy.Duration(30)) - rospy.wait_for_message("/elevator_state_publisher/current_floor", timeout=rospy.Duration(30)) - rospy.wait_for_message("/elevator_state_publisher/rest_elevator", timeout=rospy.Duration(30)) + rospy.wait_for_message('/spot_recognition/elevator_door_points', PointCloud2, timeout=rospy.Duration(30)) + rospy.wait_for_message("/elevator_state_publisher/current_floor", Int16, timeout=rospy.Duration(30)) + rospy.wait_for_message("/elevator_state_publisher/rest_elevator", Bool, timeout=rospy.Duration(30)) except rospy.ROSException: rospy.logerr("Some topics are not published.") return False From 78e4dcb8876e93395b3c4ce0dd52491f540d1c03 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 12 Aug 2023 19:12:28 +0900 Subject: [PATCH 22/40] [spot_basic_behaviors] udpate elevator behaviors --- .../src/spot_basic_behaviors/elevator_behavior.py | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py index ee139b8d9c..059c68aac8 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py @@ -73,11 +73,21 @@ def run_initial(self, start_node, end_node, edge, pre_edge): return False try: - rospy.wait_for_message('/spot_recognition/elevator_door_points', PointCloud2, timeout=rospy.Duration(30)) + end_time = rospy.Time.now() + rospy.Duration(30) + except rospy.ROSException: + rospy.logerr("Door points topics are not published.") + return False + + try: rospy.wait_for_message("/elevator_state_publisher/current_floor", Int16, timeout=rospy.Duration(30)) + except rospy.ROSException: + rospy.logerr("Current floor topics are not published.") + return False + + try: rospy.wait_for_message("/elevator_state_publisher/rest_elevator", Bool, timeout=rospy.Duration(30)) except rospy.ROSException: - rospy.logerr("Some topics are not published.") + rospy.logerr("Elevator state topic are not published.") return False return True From dfb2767889406ec1cd2ddbbf7650bb7fa070b70b Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 12 Aug 2023 19:13:16 +0900 Subject: [PATCH 23/40] [spot_ros_client] update default duration of trajectory --- .../spot_ros_client/src/spot_ros_client/libspotros.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py index 120c64f7e3..3ce80dba80 100644 --- a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py +++ b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py @@ -543,7 +543,7 @@ def trajectory(self, x, y, theta, duration=None, blocking=False): goal.target_pose.pose.orientation.z = rotation.GetQuaternion()[2] goal.target_pose.pose.orientation.w = rotation.GetQuaternion()[3] if duration is None: - goal.duration.data = rospy.Duration(math.sqrt(x**2 + y**2)*10) + goal.duration.data = rospy.Duration(math.sqrt(x**2 + y**2)/0.5) else: goal.duration.data = rospy.Duration(duration) self._actionclient_trajectory.send_goal(goal) From 2f17b60c59d6b2411a4f71a11bc030d05963cd86 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 16 Aug 2023 11:45:40 +0900 Subject: [PATCH 24/40] [spot_basic_behaiviors] keep pressing while riding on a elevator --- .../src/spot_basic_behaviors/elevator_behavior.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py index 059c68aac8..f986616fbf 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py @@ -197,7 +197,13 @@ def run_main(self, start_node, end_node, edge, pre_edge): self.spot_client.wait_for_navigate_to_result() return False result_switchbot = self.action_client_switchbot.get_result() - self.spot_client.wait_for_navigate_to_result() + while not rospy.is_shutdown(): + switchbot_goal = SwitchBotCommandGoal() + switchbot_goal.device_name = start_node.properties['switchbot_device'] + switchbot_goal.command = 'press' + self.action_client_switchbot.send_goal_and_wait(switchbot_goal, execute_timeout=rospy.Duration(10)) + if self.spot_client.wait_for_navigate_to_result(rospy.Duration(5)): + break result_navigation = self.spot_client.get_navigate_to_result() # recovery when riding on if not result_navigation.success or not result_switchbot.done: From 3c888e0587736a8a53f0bbb2448b3d10e1f2b279 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 16 Oct 2023 19:18:14 +0900 Subject: [PATCH 25/40] [jsk_spot_startup] update launch --- jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch | 3 +++ 1 file changed, 3 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch index 91643dc584..70cc431b96 100644 --- a/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch +++ b/jsk_spot_robot/jsk_spot_startup/launch/jsk_spot_bringup.launch @@ -27,4 +27,7 @@ + + + From dd5ea42676385ea0a445d46818acc920daa6ada7 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 18 Oct 2023 14:12:30 +0900 Subject: [PATCH 26/40] Add reset_current_node method --- .../src/spot_ros_client/libspotros.py | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py index 3ce80dba80..d39fe2142e 100644 --- a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py +++ b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py @@ -34,6 +34,7 @@ from spot_msgs.srv import UploadGraphRequest from spot_msgs.srv import Dock from spot_msgs.srv import DockRequest +from jsk_spot_behavior_msgs.srv import ResetCurrentNode, ResetCurrentNodeRequest # actions from jsk_spot_behavior_msgs.msg import NavigationAction from jsk_spot_behavior_msgs.msg import NavigationGoal @@ -137,6 +138,7 @@ def __init__(self, servicename_set_localization_waypoint='/spot/set_localization_waypoint', servicename_dock='/spot/dock', servicename_undock='/spot/undock', + servicename_reset_current_node='/spot_behavior_manager_server/reset_current_node_id', actionname_navigate_to='/spot/navigate_to', actionname_trajectory='/spot/trajectory', actionname_execute_behaviors='/spot_behavior_manager_server/execute_behaviors', @@ -184,6 +186,7 @@ def __init__(self, servicename_set_localization_waypoint, rospy.Duration(5)) rospy.wait_for_service(servicename_dock, rospy.Duration(5)) rospy.wait_for_service(servicename_undock, rospy.Duration(5)) + rospy.wait_for_service(servicename_reset_current_node, rospy.Duration(5)) except rospy.ROSException as e: rospy.logerr('Service unavaliable: {}'.format(e)) @@ -260,6 +263,10 @@ def __init__(self, servicename_undock, Trigger ) + self._srv_client_reset_current_node = rospy.ServiceProxy( + servicename_reset_current_node, + ResetCurrentNode + ) # Action Clients self._actionclient_navigate_to = actionlib.SimpleActionClient( @@ -466,7 +473,7 @@ def undock(self): self.stand() return res.success, res.message - def auto_dock(self, dock_id, home_node_id='eng2_73B2'): + def auto_dock(self, dock_id, home_node_id='eng2_73B2_dock'): res = self.execute_behaviors(home_node_id) if not res.success: return res.success, res.message @@ -526,6 +533,12 @@ def wait_execute_behaviors_result(self, duration=None): def get_execute_behaviors_result(self): return self._actionclient_execute_behaviors.get_result() + + def reset_current_node(self, node): + req = ResetCurrentNodeRequest() + req.current_node_id = node + res = self._srv_client_reset_current_node(req) + return res.success # \brief call trajectory service # \param x x value of the target position [m] From 7ee2f81c5929ca059f150ccf9f0c3d3a4a15d1d5 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 21 Oct 2023 20:08:46 +0900 Subject: [PATCH 27/40] [spot_ros_client] add list_Node for jsk_spot_behavior_server --- .../spot_ros_client/src/spot_ros_client/libspotros.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py index d39fe2142e..f84de0a868 100644 --- a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py +++ b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py @@ -540,6 +540,9 @@ def reset_current_node(self, node): res = self._srv_client_reset_current_node(req) return res.success + def list_nodes(self): + return list(rospy.get_param("/spot_behavior_manager_server/map/nodes", {}).keys()) + # \brief call trajectory service # \param x x value of the target position [m] # \param x y value of the target position [m] From 0bb7f5429c7d47b4ffb0ebfd8fe81879e77b6d1e Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 22 Nov 2023 12:14:56 +0900 Subject: [PATCH 28/40] [jsk_spot_startup] add jsk_spot_behavior_maanger to package.xml --- jsk_spot_robot/jsk_spot_startup/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_spot_robot/jsk_spot_startup/package.xml b/jsk_spot_robot/jsk_spot_startup/package.xml index fefef06d3d..452e07ea5a 100644 --- a/jsk_spot_robot/jsk_spot_startup/package.xml +++ b/jsk_spot_robot/jsk_spot_startup/package.xml @@ -47,6 +47,8 @@ xacro xterm + jsk_spot_behavior_manager + rostest From dcdde6674908cb84c310c761aa9d1e9c79169700 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 21 Oct 2023 20:45:28 +0900 Subject: [PATCH 29/40] [spot_ros_client] add velocity_limit to SpotRosCLient --- .../spot_ros_client/src/spot_ros_client/libspotros.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py index f84de0a868..b699cddc04 100644 --- a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py +++ b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py @@ -491,9 +491,15 @@ def auto_undock(self): else: self.stand() - def navigate_to(self, id_navigate_to, blocking=False): + def navigate_to(self, + id_navigate_to, + velocity_limit=(0.0, 0.0, 0.0), + blocking=False): goal = NavigateToGoal() goal.id_navigate_to = id_navigate_to + goal.velocity_limit.linear.x = velocity_limit[0] + goal.velocity_limit.linear.y = velocity_limit[1] + goal.velocity_limit.angular.z = velocity_limit[2] self._actionclient_navigate_to.send_goal(goal) if blocking: self._actionclient_navigate_to.wait_for_result() From c7f7eb51a3b729cdab0e5e5cd1d4aeb40bade9cf Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 21 Oct 2023 21:39:20 +0900 Subject: [PATCH 30/40] [spot_ros_client] update go_pos timeout time --- .../spot_ros_client/src/spot_ros_client/libspotros.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py index b699cddc04..3041e822d0 100644 --- a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py +++ b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py @@ -326,12 +326,12 @@ def get_robot_pose(self): return None return frame_odom_to_base - def go_pose(self, target_frame): + def go_pose(self, target_frame, timeout = None): self.trajectory( target_frame.p[0], target_frame.p[1], target_frame.M.GetRPY()[2], - 10, + duration=timeout, blocking=True ) From c0a5b4a215fbec9375c65d0e9236cbcbeb524b41 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 21 Oct 2023 21:43:22 +0900 Subject: [PATCH 31/40] [jsk_spot_startup][spot_basic_behaviors] update dependency --- .../jsk_spot_behaviors/spot_basic_behaviors/package.xml | 6 ------ jsk_spot_robot/jsk_spot_startup/package.xml | 1 + 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml index 48b7ae10f5..0f41e0c325 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml @@ -13,19 +13,13 @@ spot_behavior_manager - PyKDL apriltag_ros depth_image_proc geometry_msgs jsk_pcl_ros jsk_recognition_msgs - jsk_spot_startup message_filters - pcl - roslaunch - rospy sensor_msgs - spinal std_msgs switchbot_ros tf2_geometry_msgs diff --git a/jsk_spot_robot/jsk_spot_startup/package.xml b/jsk_spot_robot/jsk_spot_startup/package.xml index 452e07ea5a..7637fd59e5 100644 --- a/jsk_spot_robot/jsk_spot_startup/package.xml +++ b/jsk_spot_robot/jsk_spot_startup/package.xml @@ -48,6 +48,7 @@ xterm jsk_spot_behavior_manager + spot_basic_behaviors rostest From 42f895f5c78d5c7c4b38a03bb77f12e731ba6831 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 21 Oct 2023 21:52:06 +0900 Subject: [PATCH 32/40] [spot_basic_behaviors] set velocity_limit --- .../src/spot_basic_behaviors/walk_behavior.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py index 8361264e4d..9769c85447 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py @@ -61,7 +61,7 @@ def run_main(self, start_node, end_node, edge, pre_edge): rate = rospy.Rate(10) if not self.silent_mode: self.sound_client.say('移動します', blocking=True) - self.spot_client.navigate_to(end_id, blocking=False) + self.spot_client.navigate_to(end_id, velocity_limit=(0.2, 0.2, 0.1), blocking=False) while not rospy.is_shutdown(): rate.sleep() if self.spot_client.wait_for_navigate_to_result(rospy.Duration(0.1)): From 6f80bd4a94c74a21fbc140274091c5a42a887dfc Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sun, 22 Oct 2023 18:06:20 +0900 Subject: [PATCH 33/40] [spot_basic_behaviors] set limit to rosparam --- .../src/spot_basic_behaviors/walk_behavior.py | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py index 9769c85447..ae46067075 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py @@ -59,9 +59,19 @@ def run_main(self, start_node, end_node, edge, pre_edge): # start navigation success = False rate = rospy.Rate(10) + velocity_limit_linear_x = rospy.get_param('/spot_basic_behaviors/walk_behaviors/velocity_limit_linear_x', None) + velocity_limit_linear_y = rospy.get_param('/spot_basic_behaviors/walk_behaviors/velocity_limit_linear_y', None) + velocity_limit_angular_z = rospy.get_param('/spot_basic_behaviors/walk_behaviors/velocity_limit_angular_z', None) if not self.silent_mode: self.sound_client.say('移動します', blocking=True) - self.spot_client.navigate_to(end_id, velocity_limit=(0.2, 0.2, 0.1), blocking=False) + self.spot_client.navigate_to( + end_id, + velocity_limit=( + velocity_limit_linear_x, + velocity_limit_linear_y, + velocity_limit_angular_z + ), + blocking=False) while not rospy.is_shutdown(): rate.sleep() if self.spot_client.wait_for_navigate_to_result(rospy.Duration(0.1)): From f4719ff9f3d1b3169bffc8f26ad75b3b0bb6375f Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 10 Nov 2023 13:35:37 +0900 Subject: [PATCH 34/40] [spot_basicZ_behaviors] add pu/down to switchbot device name ntry --- .../src/spot_basic_behaviors/elevator_behavior.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py index f986616fbf..68d212fcbe 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py @@ -96,6 +96,8 @@ def run_main(self, start_node, end_node, edge, pre_edge): rospy.logdebug('run_main() called') + start_floor = start_node.properties['floor'] + graph_name = edge.properties['graph'] start_id = list(filter( lambda x: x['graph'] == graph_name, @@ -145,7 +147,7 @@ def run_main(self, start_node, end_node, edge, pre_edge): rospy.loginfo('calling elevator when riding...') success_calling = False switchbot_goal = SwitchBotCommandGoal() - switchbot_goal.device_name = start_node.properties['switchbot_device'] + switchbot_goal.device_name = start_node.properties['switchbot_device_up'] if end_floor > start_floor else start_node.properties['switchbot_device_down'] switchbot_goal.command = 'press' count = 0 while True: @@ -186,7 +188,7 @@ def run_main(self, start_node, end_node, edge, pre_edge): # call elevator from destination floor while rospy.loginfo('calling elevator when getting off...') switchbot_goal = SwitchBotCommandGoal() - switchbot_goal.device_name = end_node.properties['switchbot_device'] + switchbot_goal.device_name = end_node.properties['switchbot_device_up'] if end_floor > start_floor else end_node.properties['switchbot_device_down'] switchbot_goal.command = 'press' self.action_client_switchbot.send_goal(switchbot_goal) ## @@ -199,7 +201,7 @@ def run_main(self, start_node, end_node, edge, pre_edge): result_switchbot = self.action_client_switchbot.get_result() while not rospy.is_shutdown(): switchbot_goal = SwitchBotCommandGoal() - switchbot_goal.device_name = start_node.properties['switchbot_device'] + switchbot_goal.device_name = start_node.properties['switchbot_device_up'] if end_floor > start_floor else end_node.properties['switchbot_device_down'] switchbot_goal.command = 'press' self.action_client_switchbot.send_goal_and_wait(switchbot_goal, execute_timeout=rospy.Duration(10)) if self.spot_client.wait_for_navigate_to_result(rospy.Duration(5)): From 47ace0e87f3b5aaaedc0d6c4ad2ae55e8e9a5cfa Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 10 Nov 2023 15:33:29 +0900 Subject: [PATCH 35/40] [spot_ros_client] add default limit to navigate_to --- .../spot_ros_client/src/spot_ros_client/libspotros.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py index 3041e822d0..3daaf9e6dc 100644 --- a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py +++ b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py @@ -493,13 +493,13 @@ def auto_undock(self): def navigate_to(self, id_navigate_to, - velocity_limit=(0.0, 0.0, 0.0), + velocity_limit=(1.0, 1.0, 1.0), blocking=False): goal = NavigateToGoal() goal.id_navigate_to = id_navigate_to - goal.velocity_limit.linear.x = velocity_limit[0] - goal.velocity_limit.linear.y = velocity_limit[1] - goal.velocity_limit.angular.z = velocity_limit[2] + goal.velocity_limit.linear.x = float(velocity_limit[0]) + goal.velocity_limit.linear.y = float(velocity_limit[1]) + goal.velocity_limit.angular.z = float(velocity_limit[2]) self._actionclient_navigate_to.send_goal(goal) if blocking: self._actionclient_navigate_to.wait_for_result() From e078090c8193d25359c159be94598a564a48da49 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 10 Nov 2023 15:34:04 +0900 Subject: [PATCH 36/40] [spot_basic_behaviors] add velocity limit to walk_behaviors --- .../src/spot_basic_behaviors/walk_behavior.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py index ae46067075..9f595d0255 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py @@ -59,9 +59,9 @@ def run_main(self, start_node, end_node, edge, pre_edge): # start navigation success = False rate = rospy.Rate(10) - velocity_limit_linear_x = rospy.get_param('/spot_basic_behaviors/walk_behaviors/velocity_limit_linear_x', None) - velocity_limit_linear_y = rospy.get_param('/spot_basic_behaviors/walk_behaviors/velocity_limit_linear_y', None) - velocity_limit_angular_z = rospy.get_param('/spot_basic_behaviors/walk_behaviors/velocity_limit_angular_z', None) + velocity_limit_linear_x = rospy.get_param('/spot_basic_behaviors/walk_behaviors/velocity_limit_linear_x', 1.0) + velocity_limit_linear_y = rospy.get_param('/spot_basic_behaviors/walk_behaviors/velocity_limit_linear_y', 1.0) + velocity_limit_angular_z = rospy.get_param('/spot_basic_behaviors/walk_behaviors/velocity_limit_angular_z', 1.0) if not self.silent_mode: self.sound_client.say('移動します', blocking=True) self.spot_client.navigate_to( From 4fe579c645216cbc5ee61f6a85a5b229fffebe40 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 8 Aug 2023 00:00:21 +0900 Subject: [PATCH 37/40] [spot_basic_behaviors] update elevator behaviors --- .../jsk_spot_behaviors/spot_basic_behaviors/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml index 0f41e0c325..d11b5b7d7f 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml @@ -25,6 +25,7 @@ tf2_geometry_msgs tf2_ros tf_relay + elevator_operation From da01860045fc4e0d764dfe715392d3df6620068d Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 22 Nov 2023 15:22:46 +0900 Subject: [PATCH 38/40] [spot_basic_behaviors] update elevator detection launch --- .../launch/elevator_detection.launch | 16 ++-------------- 1 file changed, 2 insertions(+), 14 deletions(-) diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch index 2a4e2886a1..e5c9f2d921 100644 --- a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch @@ -11,7 +11,7 @@ - + @@ -202,23 +202,11 @@ - - - - - - + From e3c930e76166d7241ccd1dccc716b9f3f2cc143d Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Wed, 22 Nov 2023 15:22:46 +0900 Subject: [PATCH 39/40] [spot_basic_behaviors] update elevator detection launch From f2c9b3ae44e1e366a262860283f0c114479d155d Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 28 Nov 2023 20:39:46 +0900 Subject: [PATCH 40/40] [spot_ros_client] add print --- .../.vscode/c_cpp_properties.json | 30 +++++++++++++++++++ .../spot_ros_client/.vscode/settings.json | 10 +++++++ .../src/spot_ros_client/libspotros.py | 17 ++++++++--- 3 files changed, 53 insertions(+), 4 deletions(-) create mode 100644 jsk_spot_robot/spot_ros_client/.vscode/c_cpp_properties.json create mode 100644 jsk_spot_robot/spot_ros_client/.vscode/settings.json diff --git a/jsk_spot_robot/spot_ros_client/.vscode/c_cpp_properties.json b/jsk_spot_robot/spot_ros_client/.vscode/c_cpp_properties.json new file mode 100644 index 0000000000..2efcb04f38 --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/.vscode/c_cpp_properties.json @@ -0,0 +1,30 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/home/sktometometo/ros/ws_jsk_spot/devel/include/**", + "/opt/ros/noetic/include/**", + "/home/sktometometo/ros/ws_jsk_spot/src/jsk-ros-pkg/jsk_robot/jsk_robot_common/jsk_robot_startup/include/**", + "/opt/ros/noetic/share/julius/include/**", + "/home/sktometometo/ros/ws_jsk_spot/src/jsk-ros-pkg/jsk_robot/jsk_pr2_robot/pr2_base_trajectory_action/include/**", + "/home/sktometometo/ros/ws_jsk_spot/src/jsk-ros-pkg/jsk_robot/jsk_robot_common/speak_and_wait_recovery/include/**", + "/home/sktometometo/ros/ws_jsk_spot/src/ublox/ublox_gps/include/**", + "/home/sktometometo/ros/ws_jsk_spot/src/ublox/ublox_msg_filters/include/**", + "/home/sktometometo/ros/ws_jsk_spot/src/ublox/ublox_msgs/include/**", + "/home/sktometometo/ros/ws_jsk_spot/src/ublox/ublox_serialization/include/**", + "/home/sktometometo/ros/ws_jsk_spot/src/jsk-ros-pkg/jsk_robot/jsk_robot_common/update_move_base_parameter_recovery/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/jsk_spot_robot/spot_ros_client/.vscode/settings.json b/jsk_spot_robot/spot_ros_client/.vscode/settings.json new file mode 100644 index 0000000000..97568205e8 --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/.vscode/settings.json @@ -0,0 +1,10 @@ +{ + "python.autoComplete.extraPaths": [ + "/home/sktometometo/ros/ws_jsk_spot/devel/lib/python3/dist-packages", + "/opt/ros/noetic/lib/python3/dist-packages" + ], + "python.analysis.extraPaths": [ + "/home/sktometometo/ros/ws_jsk_spot/devel/lib/python3/dist-packages", + "/opt/ros/noetic/lib/python3/dist-packages" + ] +} diff --git a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py index 3daaf9e6dc..86279a1661 100644 --- a/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py +++ b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py @@ -162,8 +162,10 @@ def __init__(self, Pose, queue_size=1 ) + rospy.loginfo("Publisher initialization done.") # wait for services + rospy.loginfo("Waiting all service available...") try: rospy.wait_for_service(servicename_claim, rospy.Duration(5)) rospy.wait_for_service(servicename_release, rospy.Duration(5)) @@ -268,6 +270,8 @@ def __init__(self, ResetCurrentNode ) + rospy.loginfo("Service initialization done.") + # Action Clients self._actionclient_navigate_to = actionlib.SimpleActionClient( actionname_navigate_to, @@ -282,10 +286,7 @@ def __init__(self, NavigationAction ) - # - self.tf_buffer = tf2_ros.Buffer() - self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) - + rospy.loginfo("Waiting actions available") # wait for action try: self._actionclient_navigate_to.wait_for_server(rospy.Duration(5)) @@ -295,6 +296,14 @@ def __init__(self, except rospy.ROSException as e: rospy.logerr('Action unavaliable: {}'.format(e)) + # + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) + + rospy.loginfo("Action initialization done.") + + rospy.loginfo("Initialization done") + def get_laptop_percepntage(self): try: msg = rospy.wait_for_message(