Skip to content

Commit c42cf7c

Browse files
sktometometosktometometo
committed
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
1 parent 38cfec5 commit c42cf7c

File tree

37 files changed

+1905
-3
lines changed

37 files changed

+1905
-3
lines changed

‎jsk_spot_robot/jsk_spot.rosinstall‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -24,12 +24,12 @@
2424
local-name: jsk-ros-pkg/jsk_3rdparty
2525
uri: https://github.com/jsk-ros-pkg/jsk_3rdparty.git
2626
version: master
27-
# Until https://github.com/jsk-ros-pkg/jsk_common/pull/1685 is merged, this patch is
28-
# required to build spoteus_demo
27+
# Until sktometometo/feature/add-tf-relay-package is merged, this patch is
28+
# required for spot_basic_behaviors
2929
- git:
3030
local-name: jsk-ros-pkg/jsk_common
3131
uri: https://github.com/sktometometo/jsk_common.git
32-
version: PR/update-download-data
32+
version: feature/add-tf-relay-package
3333
# This is a develop branch for jsk version.
3434
# We need to use it until it is merged to master
3535
- git:

‎jsk_spot_robot/jsk_spot_behaviors/README.md‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
# jsk_spot_behaviors
2+
3+
These packages enable for Spot to execute locomotoion behaviors to reach a desired position.
4+
5+
## Concept
6+
7+
In this framework, knowledge about positions and transtion behaviors between them are represented as a digraph like below.
8+
9+
Each node represents specified positions and each edge represents a behavior to transition between them.
10+
11+
![example_graph](https://user-images.githubusercontent.com/9410362/124147589-cc8ce700-dac9-11eb-930f-1c00c2a4777e.png)
12+
13+
A graph is defined by a yaml file ( e.g. [map.yaml in spot_behavior_manager_demo](./spot_behavior_manager_demo/config/map.yaml) )
14+
Please see nodes and edges format section below.
15+
16+
Knowledge representation and execution process of behaviors are separated from actual behavior implementation.
17+
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.
18+
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.
19+
Please see spot_basic_behaviors package for behavior examples.
20+
21+
## How to use it
22+
23+
To run demo, please make Spot stand in front of the fiducial in 73B2 and run
24+
25+
```bash
26+
roslaunch spot_behavior_manager_demo demo.launch
27+
```
28+
29+
```bash
30+
$ rostopic pub -1 /spot_behavior_manager_demo/lead_person/goal spot_behavior_manager_msgs/LeadPersonActionGoal "header:
31+
seq: 0
32+
stamp:
33+
secs: 0
34+
nsecs: 0
35+
frame_id: ''
36+
goal_id:
37+
stamp:
38+
secs: 0
39+
nsecs: 0
40+
id: ''
41+
goal:
42+
target_node_id: 'eng2_2FElevator'"
43+
```
44+
45+
Then Spot will go to 2FElevator of eng2 by walk behavior and elevator behavior implemented in spot_basic_behaviors packages.
46+
47+
https://user-images.githubusercontent.com/9410362/124338016-aad25380-dbe0-11eb-962f-b9a27e1e08cb.mp4
48+
49+
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) )

‎jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/CMakeLists.txt‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
cmake_minimum_required(VERSION 3.0.2)
2+
project(spot_basic_behaviors)
3+
4+
find_package(catkin REQUIRED)
5+
6+
catkin_python_setup()
7+
8+
catkin_package(
9+
)

‎jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/README.md‎

Lines changed: 124 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,124 @@
1+
# spot_basic_behaviors
2+
3+
This package includes exaple implementation of base_behavior for spot_behavior_manager.
4+
5+
## walk behavior
6+
7+
This behavior enables Spot to walk a specified route in a autowalk data from a given waypoint to another waypoint.
8+
9+
behavior name: `spot_basic_behaviors.walk_behavior.WalkBehavior`
10+
11+
https://user-images.githubusercontent.com/9410362/124337233-896f6880-dbdc-11eb-9588-30d4a5193bbd.mp4
12+
13+
### Required Configuration
14+
15+
Before using this behavior, belows are required.
16+
17+
- autowalk data containing a route for the behavior
18+
- edge and nodes configuration for the behavior
19+
20+
#### Edge
21+
22+
Fields below are required in args of a edge
23+
24+
- `graph`: path to autowalk data
25+
26+
e.g.
27+
28+
```yaml
29+
- from: 'eng2_73B2'
30+
to: 'eng2_73A2'
31+
behavior_type: 'spot_basic_behavior.walk_behavior.WalkBehavior'
32+
cost: 10
33+
args:
34+
graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk'
35+
```
36+
37+
#### Start and End Node
38+
39+
Fields below are required for nodes
40+
41+
- `waypoints_on_graph`: list of dict. each dict has `graph`, `id`, `localization_method`
42+
+ `graph`: path to a autowalk data
43+
+ `id`: waypoint id of the start node in the graph
44+
+ `localization_method`: localization_method to be used when starting autowalk, this must be 'fiducial' or 'waypoint'
45+
46+
e.g.
47+
48+
```yaml
49+
'eng2_73B2':
50+
waypoints_on_graph:
51+
- graph: '$(find spot_autowalk_data)/autowalk/eng2_73B2_to_7FElevator.walk'
52+
id: 'dyed-bat-t00VKo5XixLihCvpsZPRqw=='
53+
localization_method: 'fiducial'
54+
```
55+
56+
## elevator behavior
57+
58+
This behavior enables Spot to use an elevator to move to anothor floor.
59+
60+
behavior name: `spot_basic_behaviors.elevator_behavior.ElevatorBehavior`
61+
62+
https://user-images.githubusercontent.com/9410362/124337679-cd636d00-dbde-11eb-8db9-c5fedfda4ad9.mp4
63+
64+
### Required Configuration
65+
66+
Before using this behavior, belows are required.
67+
68+
- autowalk data containing a route for the behavior.
69+
- apriltag pose information for elevator door frame detection.
70+
- edge and nodes configuration for the behavior
71+
72+
#### autowalk data
73+
74+
To use this behavior, you need to record autowalk data while Spot riding on the elevator and getting off.
75+
76+
Here is an example.
77+
78+
![124134162-86318b00-dabd-11eb-86e3-092fcc5e8719](https://user-images.githubusercontent.com/9410362/124337765-364ae500-dbdf-11eb-83d3-a99e4025102e.png)
79+
80+
#### apriltag pose information
81+
82+
To use this behavior, an Fiducial (april tag) must be placed on the wall near the elevator doors.
83+
And for elevator door opening and closing detection, transform from elevator foor frame ( center point on the ground ) to the fiducial is required.
84+
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
85+
86+
#### Edge
87+
88+
Fields below are required in args of a edge
89+
90+
- `graph`: path to autowalk data
91+
- `rest_waypoint_id`: waypoint of rest position in a elevator
92+
93+
e.g.
94+
95+
```yaml
96+
- from: 'eng2_7FElevator'
97+
to: 'eng2_2FElevator'
98+
behavior_type: 'spot_basic_behavior.elevator_behavior.ElevatorBehavior'
99+
cost: 10
100+
args:
101+
graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk'
102+
rest_waypoint_id: 'unsaid-collie-jvatvS.7KX9jpzQz61GL4A=='
103+
```
104+
105+
#### Start and End Node
106+
107+
Fields below are required for nodes
108+
109+
- `waypoints_on_graph`: list of dict. each dict has `graph`, `id`, `localization_method`
110+
+ `graph`: path to a autowalk data
111+
+ `id`: waypoint id of the start node in the graph
112+
+ `localization_method`: localization_method to be used when starting autowalk, this must be 'fiducial' or 'waypoint'
113+
- `switchbot_device`: switchbot device name for elevator button
114+
115+
e.g.
116+
117+
```yaml
118+
'eng2_7FElevator':
119+
waypoints_on_graph:
120+
- graph: '$(find spot_autowalk_data)/autowalk/eng2_elevator_7FElevator_to_2FElevator.walk'
121+
id: 'sly-chetah-IZ4pVY7vrqO36OoKCYk9Zg=='
122+
localization_method: 'fiducial'
123+
switchbot_device: '/eng2/7f/elevator/down/button'
124+
```

‎jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/settings.yaml‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
tag_family: 'tag36h11' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12
2+
tag_threads: 2 # default: 2
3+
tag_decimate: 1.0 # default: 1.0
4+
tag_blur: 0.0 # default: 0.0
5+
tag_refine_edges: 1 # default: 1
6+
tag_debug: 0 # default: 0
7+
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.)
8+
# Other parameters
9+
publish_tf: true # default: false

‎jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/apriltag/tags.yaml‎

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
# # Definitions of tags to detect
2+
#
3+
# ## General remarks
4+
#
5+
# - All length in meters
6+
# - Ellipsis (...) signifies that the previous element can be repeated multiple times.
7+
#
8+
# ## Standalone tag definitions
9+
# ### Remarks
10+
#
11+
# - name is optional
12+
#
13+
# ### Syntax
14+
#
15+
# standalone_tags:
16+
# [
17+
# {id: ID, size: SIZE, name: NAME},
18+
# ...
19+
# ]
20+
standalone_tags:
21+
[
22+
]
23+
# ## Tag bundle definitions
24+
# ### Remarks
25+
#
26+
# - name is optional
27+
# - x, y, z have default values of 0 thus they are optional
28+
# - qw has default value of 1 and qx, qy, qz have default values of 0 thus they are optional
29+
#
30+
# ### Syntax
31+
#
32+
# tag_bundles:
33+
# [
34+
# {
35+
# name: 'CUSTOM_BUNDLE_NAME',
36+
# layout:
37+
# [
38+
# {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},
39+
# ...
40+
# ]
41+
# },
42+
# ...
43+
# ]
44+
tag_bundles:
45+
[
46+
{
47+
name: 'elevator_door_frame_raw',
48+
layout:
49+
[
50+
{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, },
51+
{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, },
52+
]
53+
}
54+
]

‎jsk_spot_robot/jsk_spot_behaviors/spot_basic_behaviors/config/switchbot_ros/token.yaml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
token: XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

0 commit comments

Comments
 (0)