Add spot_autowalk_data package and jsk_spot_behaviors packages (#26)

* [spot_autowalk_data] add spot_autowalk_data package * [jsk_spot_behaviors] add jsk_spot_behaviors packages * [spot_autowalk_data] add .gitignore file to autowalk directory * [spot_autowalk_data] add README.md * [jsk_spot_behaviors] add README.md * [jsk_spot_behaviors] Update README.md * [spot_basic_behaviors] add README.md * [spot_basic_behaviors] Update README.md * [spot_behavior_manager_demo] add README.md * [jsk_spot_behaviors] Update README.md * [spot_basic_behaviors][spot_behavior_manager][spot_behavior_manager_demo] fix bugs * [spot_basic_behaviors] update elevator detections * [jsk_spot_robot] update package.xml * [jsk_spot_robot] update rosinstall to support tf_relay
knorth55 · Sep 28, 2021 · d46c69e · d46c69e
1 parent 56d8db0
commit d46c69e
Show file tree

Hide file tree

Showing 37 changed files with 1,905 additions and 3 deletions.
diff --git a/jsk_spot_robot/jsk_spot.rosinstall b/jsk_spot_robot/jsk_spot.rosinstall
@@ -24,12 +24,12 @@
     local-name: jsk-ros-pkg/jsk_3rdparty
     uri: https://github.com/jsk-ros-pkg/jsk_3rdparty.git
     version: master
-# Until https://github.com/jsk-ros-pkg/jsk_common/pull/1685 is merged, this patch is
-# required to build spoteus_demo
+# Until sktometometo/feature/add-tf-relay-package is merged, this patch is
+# required for spot_basic_behaviors
 - git:
     local-name: jsk-ros-pkg/jsk_common
     uri: https://github.com/sktometometo/jsk_common.git
-    version: PR/update-download-data
+    version: feature/add-tf-relay-package
 # This is a develop branch for jsk version.
 # We need to use it until it is merged to master
 - git:

diff --git a/jsk_spot_robot/jsk_spot_behaviors/README.md b/jsk_spot_robot/jsk_spot_behaviors/README.md
@@ -0,0 +1,49 @@
+# jsk_spot_behaviors
+
+These packages enable for Spot to execute locomotoion behaviors to reach a desired position.
+
+## Concept
+
+In this framework, knowledge about positions and transtion behaviors between them are represented as a digraph like below.
+
+Each node represents specified positions and each edge represents a behavior to transition between them.
+
+![example_graph](https://user-images.githubusercontent.com/9410362/124147589-cc8ce700-dac9-11eb-930f-1c00c2a4777e.png)
+
+A graph is defined by a yaml file ( e.g. [map.yaml in spot_behavior_manager_demo](./spot_behavior_manager_demo/config/map.yaml) )
+Please see nodes and edges format section below.
+
+Knowledge representation and execution process of behaviors are separated from actual behavior implementation.
+The iplementation of former is in spot_behavior_manager and spot_behavior_manager_demo, but actual behaviors like walk and elevator are in spot_basic_behaviors.
+behavior_manager_demo 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_manager_demo demo.launch
+```
+
+```bash
+$ rostopic pub -1 /spot_behavior_manager_demo/lead_person/goal 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_demo](./spot_behavior_manager_demo) and each behavior documentation. (e.g. [spot_basic_behaviors](./spot_basic_behaviors) )
diff --git a/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt b/jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt
@@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(spot_basic_behaviors)
+
+find_package(catkin REQUIRED)
+
+catkin_python_setup()
+
+catkin_package(
+)
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
@@ -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
@@ -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
@@ -0,0 +1,54 @@
+# # 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, },
+        ]
+    }
+  ]
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
@@ -0,0 +1 @@
+token: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1 @@
		token: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX