diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behavior_manager/CMakeLists.txt new file mode 100644 index 0000000000..46b773ac6b --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_spot_behavior_manager) + +find_package(catkin REQUIRED) + +catkin_python_setup() + +catkin_package( +) diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/README.md b/jsk_spot_robot/jsk_spot_behavior_manager/README.md new file mode 100644 index 0000000000..96d7c3e0fd --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/README.md @@ -0,0 +1,93 @@ +# 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) ) + +## 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_behavior_manager/behavior_graph/edges.yaml b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/edges.yaml new file mode 100644 index 0000000000..13455ad03f --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/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_behavior_manager/behavior_graph/edges_person_lead.yaml b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/edges_person_lead.yaml new file mode 100644 index 0000000000..64632e982f --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/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_behavior_manager/behavior_graph/nodes.yaml b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/nodes.yaml new file mode 100644 index 0000000000..49006a7fde --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/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_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..cba2b070a0 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/behavior_graph/simple_behavior_graph.yaml @@ -0,0 +1,28 @@ +edges: + - from: 'nodeA' + 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/launch/demo.launch b/jsk_spot_robot/jsk_spot_behavior_manager/launch/demo.launch new file mode 100644 index 0000000000..8986335870 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/launch/demo.launch @@ -0,0 +1,23 @@ + + + + + + + + + + initial_node_id: '$(arg initial_node_id)' + silent_mode: true + + + 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..4cc59161ec --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/launch/simple_demo.launch @@ -0,0 +1,22 @@ + + + + + + + + initial_node_id: 'nodeA' + silent_mode: true + dry_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 new file mode 100755 index 0000000000..af70e08639 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/behavior_manager_server.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import rospy +from jsk_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_behavior_manager/node_scripts/interactive_behavior_executor.py b/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/interactive_behavior_executor.py new file mode 100755 index 0000000000..85333b6ba6 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/node_scripts/interactive_behavior_executor.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python +# -*- encoding: utf-8 -*- + +import actionlib +import rospy + +from jsk_spot_behavior_msgs.msg import NavigationAction, NavigationGoal + +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', NavigationAction) + + 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(NavigationGoal(target_node_id=target_node_id)) + + +if __name__ == '__main__': + main() diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/package.xml b/jsk_spot_robot/jsk_spot_behavior_manager/package.xml new file mode 100644 index 0000000000..bc941c558b --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/package.xml @@ -0,0 +1,39 @@ + + + jsk_spot_behavior_manager + 1.1.0 + The spot_behavior_manager package + + Kei Okada + Koki Shinjo + Koki Shinjo + + BSD + + catkin + + rospy + + actionlib + networkx + roslaunch + rospkg + rospy + sound_play + spot_behavior_manager_msgs + spot_ros_client + std_msgs + spot_autowalk_data + + actionlib + networkx + rospkg + rospy + roslaunch + sound_play + spot_behavior_manager_msgs + spot_ros_client + std_msgs + + + diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/scripts/visualize_map.py b/jsk_spot_robot/jsk_spot_behavior_manager/scripts/visualize_map.py new file mode 100755 index 0000000000..f26b7742da --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/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_behavior_manager/setup.py b/jsk_spot_robot/jsk_spot_behavior_manager/setup.py new file mode 100644 index 0000000000..799b412b34 --- /dev/null +++ b/jsk_spot_robot/jsk_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=['jsk_spot_behavior_manager'], + package_dir={'': 'src'} +) + +setup(**d) 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..e69de29bb2 diff --git a/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/base_behavior.py b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/base_behavior.py new file mode 100644 index 0000000000..5c3fa4266f --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_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_behavior_manager/src/jsk_spot_behavior_manager/behavior_graph.py b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_graph.py new file mode 100644 index 0000000000..861dcf3c00 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/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 BehaviorGraph: + + # 現在の BehaviorGraph の仕様 + # 重み付きの有向グラフ + # あるノードからあるノードまでのエッジの数は 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_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 new file mode 100644 index 0000000000..abb4049ef8 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_manager/src/jsk_spot_behavior_manager/behavior_manager_node.py @@ -0,0 +1,236 @@ +# -*- coding: utf-8 -*- + +import copy +import sys +import traceback + +import actionlib +import rospy +import roslaunch +import PyKDL + +from sound_play.libsoundplay import SoundClient +from spot_ros_client.libspotros import SpotRosClient + +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 +from jsk_spot_behavior_msgs.msg import NavigationAction, NavigationFeedback, NavigationResult, NavigationActionFeedback +from jsk_spot_behavior_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 = BehaviorGraph(raw_edges, raw_nodes) + self.current_node_id = rospy.get_param('~initial_node_id') + self.pre_edge = None + self.anchor_pose = None + + # silent mode / dry mode + self.silent_mode = rospy.get_param('~silent_mode', False) + 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 + 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( + '~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), NavigationActionFeedback, self.callback_synchronizer) + ) + + # action server + self.server_execute_behaviors = actionlib.SimpleActionServer( + '~execute_behaviors', + NavigationAction, + execute_cb=self.handler_execute_behaviors, + auto_start=False + ) + self.server_execute_behaviors.start() + + rospy.loginfo('Initialized!') + + def say(self, message, blocking=True): + + 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') + 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') + 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): + + 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('Behavior 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)) + 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 + self.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(NavigationFeedback(current_node_id=self.current_node_id)) + self.pre_edge = edge + self.set_anchor_pose() + else: + rospy.logwarn('Edge {} failed'.format(edge)) + self.say( + '移動に失敗しました。経路を探索し直します。', blocking=True) + 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 + break + except Exception as e: + rospy.logerr( + 'Got an error while navigating edge {}: {}'.format(edge, sys.exc_info())) + traceback.print_exc() + self.say('エラーが発生しました', blocking=True) + self.pre_edge = None + result = NavigationResult( + 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!') + self.say('目的地に到着しました.', blocking=True) + result = NavigationResult(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(sys.exc_info())) + traceback.print_exc() + self.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_behavior_msgs/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behavior_msgs/CMakeLists.txt new file mode 100644 index 0000000000..30e82ce82f --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_msgs/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_spot_behavior_msgs) + +find_package(catkin REQUIRED COMPONENTS genmsg actionlib actionlib_msgs) + +add_action_files( + DIRECTORY action + FILES Navigation.action +) + +add_service_files( + DIRECTORY srv + FILES ResetCurrentNode.srv +) + +generate_messages(DEPENDENCIES actionlib_msgs) + +catkin_package( + LIBRARIES jsk_spot_behavior_msgs + CATKIN_DEPENDS message_runtime +) diff --git a/jsk_spot_robot/jsk_spot_behavior_msgs/action/Navigation.action b/jsk_spot_robot/jsk_spot_behavior_msgs/action/Navigation.action new file mode 100644 index 0000000000..8f41dfdaf9 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_msgs/action/Navigation.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_behavior_msgs/package.xml b/jsk_spot_robot/jsk_spot_behavior_msgs/package.xml new file mode 100644 index 0000000000..be66e24a55 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_msgs/package.xml @@ -0,0 +1,24 @@ + + + jsk_spot_behavior_msgs + 1.1.0 + The jsk_spot_behavior_msgs package + + Koki Shinjo + Koki Shinjo + + BSD + + catkin + + actionlib + actionlib_msgs + message_generation + + actionlib + actionlib_msgs + message_runtime + + + + diff --git a/jsk_spot_robot/jsk_spot_behavior_msgs/srv/ResetCurrentNode.srv b/jsk_spot_robot/jsk_spot_behavior_msgs/srv/ResetCurrentNode.srv new file mode 100644 index 0000000000..8fa01bd4c0 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behavior_msgs/srv/ResetCurrentNode.srv @@ -0,0 +1,3 @@ +string current_node_id +--- +bool success 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..fd9652fb99 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 2.8.3) +project(spot_basic_behaviors) + +find_package(catkin REQUIRED) + +catkin_python_setup() + +catkin_package( +) + +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/elevator_detection.launch b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch new file mode 100644 index 0000000000..e5c9f2d921 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/launch/elevator_detection.launch @@ -0,0 +1,214 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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/package.xml b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml new file mode 100644 index 0000000000..d11b5b7d7f --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/package.xml @@ -0,0 +1,32 @@ + + + spot_basic_behaviors + 1.1.0 + The spot_basic_behaviors package + + Kei Okada + Koki Shinjo + Koki Shinjo + BSD + + catkin + + spot_behavior_manager + + apriltag_ros + depth_image_proc + geometry_msgs + jsk_pcl_ros + jsk_recognition_msgs + message_filters + sensor_msgs + std_msgs + switchbot_ros + tf2_geometry_msgs + tf2_ros + tf_relay + elevator_operation + + + + 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..077d19e4ad --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/__init__.py @@ -0,0 +1,2 @@ +import spot_basic_behaviors.walk_behavior as walk_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 new file mode 100644 index 0000000000..68d212fcbe --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/elevator_behavior.py @@ -0,0 +1,279 @@ +# -*- coding: utf-8 -*- + +from jsk_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 geometry_msgs.msg import Quaternion +from std_msgs.msg import Float32, Bool, Int16 + + +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 current_floor_callback(self, msg): + self.current_floor = msg.data + + def rest_elevator_callback(self, msg): + self.rest_elevator = msg.data + + 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) + + 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' + 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 + ) + self.roslaunch_parent.start() + + # value for door openring checker + self.door_is_open = False + self.subscriber_door_check = None + + # elevator state + self.current_floor = None + self.rest_elevator = 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( + '/switchbot_ros/switch', + SwitchBotCommandAction + ) + + if not self.action_client_switchbot.wait_for_server(rospy.Duration(30)): + rospy.logerr('switchbot server seems to fail.') + return False + + try: + 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("Elevator state topic are not published.") + return False + + return True + + 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, + 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 = list(filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + ))[0]['localization_method'] + + 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 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 + switchbot_goal = SwitchBotCommandGoal() + 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: + 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_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) + ## + 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() + while not rospy.is_shutdown(): + switchbot_goal = SwitchBotCommandGoal() + 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)): + break + 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: {}, 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') + + # 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_current_floor != None: + self.subscriber_current_floor.unregister() + self.subscriber_current_floor = None + + if self.subscriber_rest_elevator != None: + self.subscriber_rest_elevator.unregister() + self.subscriber_rest_elevator = 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..9f595d0255 --- /dev/null +++ b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/src/spot_basic_behaviors/walk_behavior.py @@ -0,0 +1,94 @@ +# -*- coding: utf-8 -*- + +from jsk_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 = list(filter( + lambda x: x['graph'] == graph_name, + start_node.properties['waypoints_on_graph'] + ))[0]['id'] + end_id = list(filter( + lambda x: x['graph'] == graph_name, + end_node.properties['waypoints_on_graph'] + ))[0]['id'] + localization_method = list(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) + 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( + 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)): + 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_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 @@ + + + diff --git a/jsk_spot_robot/jsk_spot_startup/package.xml b/jsk_spot_robot/jsk_spot_startup/package.xml index fefef06d3d..7637fd59e5 100644 --- a/jsk_spot_robot/jsk_spot_startup/package.xml +++ b/jsk_spot_robot/jsk_spot_startup/package.xml @@ -47,6 +47,9 @@ xacro xterm + jsk_spot_behavior_manager + spot_basic_behaviors + rostest 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..7dea41d2c2 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 2.8.3) +project(spot_autowalk_data) + +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) + +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/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/package.xml b/jsk_spot_robot/spot_autowalk_data/package.xml new file mode 100644 index 0000000000..a77a1f0987 --- /dev/null +++ b/jsk_spot_robot/spot_autowalk_data/package.xml @@ -0,0 +1,21 @@ + + + spot_autowalk_data + 1.1.0 + The spot_autowalk_data package + + Kei Okada + Koki Shinjo + Koki Shinjo + + BSD + + catkin + 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/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 100644 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/.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/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..a0e71c116f --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/package.xml @@ -0,0 +1,31 @@ + + + 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 + jsk_spot_behavior_msgs + + actionlib + spot_msgs + std_srvs + geometry_msgs + jsk_spot_behavior_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..86279a1661 --- /dev/null +++ b/jsk_spot_robot/spot_ros_client/src/spot_ros_client/libspotros.py @@ -0,0 +1,582 @@ +import PyKDL +import rospy +import actionlib +import tf2_ros +import tf2_geometry_msgs + +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 +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 +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', + 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', + 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 + ) + 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)) + 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)) + rospy.wait_for_service(servicename_reset_current_node, 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 + ) + self._srv_client_reset_current_node = rospy.ServiceProxy( + servicename_reset_current_node, + ResetCurrentNode + ) + + rospy.loginfo("Service initialization done.") + + # 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, + NavigationAction + ) + + rospy.loginfo("Waiting actions available") + # 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)) + + # + 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( + 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 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, timeout = None): + self.trajectory( + target_frame.p[0], + target_frame.p[1], + target_frame.M.GetRPY()[2], + duration=timeout, + blocking=True + ) + + 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_dock'): + 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, + velocity_limit=(1.0, 1.0, 1.0), + blocking=False): + goal = NavigateToGoal() + goal.id_navigate_to = id_navigate_to + 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() + 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 = NavigationGoal() + 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() + + def reset_current_node(self, node): + req = ResetCurrentNodeRequest() + req.current_node_id = 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] + # \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)/0.5) + else: + goal.duration.data = rospy.Duration(duration) + self._actionclient_trajectory.send_goal(goal) + if blocking: + self._actionclient_trajectory.wait_for_result()