You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
Updated align_to_aruco.md to include more background information on ArUco tags, how to create them, and special considerations when running this tutorial (#27)
* Updated align_to_aruco.md to include more background information on ArUco tags, how to create them, and special considerations when running this tutorial
Added more information to help new users become familiar with ArUco tags and how to use them with Stretch.
Copy file name to clipboardExpand all lines: ros2/align_to_aruco.md
+119-7Lines changed: 119 additions & 7 deletions
Original file line number
Diff line number
Diff line change
@@ -1,13 +1,103 @@
1
1
# Align to ArUco
2
2
ArUco markers are a type of fiducials that are used extensively in robotics for identification and pose estimation. In this tutorial we will learn how to identify ArUco markers with the ArUco detection node and enable Stretch to navigate and align itself with respect to the marker.
3
3
4
+
There are three main parameters to creating an ArUco tag:
5
+
- Dictionary Size (Use 6x6_250 when using Stretch tutorials or ROS2 nodes, this is explained more [below](#aruco-detection))
6
+
- ID (A unique identifier for the tag, see [Create a New ArUco Marker](#create-a-new-aruco-marker) for reserved ID's)
7
+
- Marker Size (The physical dimensions of the tag in mm, this can be customized freely.)
8
+
9
+
## The ArUco Marker Dictionary
10
+
When defining the ArUco markers on Stretch, hello robot utilizes a YAML file, [stretch_marker_dict.yaml](https://github.com/hello-robot/stretch_ros/blob/master/stretch_core/config/stretch_marker_dict.yaml), that holds the information about the markers.
11
+
12
+
If [detect_aruco_markers](https://github.com/hello-robot/stretch_ros/blob/master/stretch_core/nodes/detect_aruco_markers) node doesn’t find an entry in [stretch_marker_dict.yaml](https://github.com/hello-robot/stretch_ros/blob/master/stretch_core/config/stretch_marker_dict.yaml) for a particular ArUco marker ID number, it uses the default entry. For example, most robots have shipped with the following default entry:
13
+
14
+
```yaml
15
+
'default':
16
+
'length_mm': 24
17
+
'use_rgb_only': False
18
+
'name': 'unknown'
19
+
'link': None
20
+
```
21
+
22
+
and the following entry for the ArUco marker on the top of the wrist
23
+
24
+
```yaml
25
+
'133':
26
+
'length_mm': 23.5
27
+
'use_rgb_only': False
28
+
'name': 'wrist_top'
29
+
'link': 'link_aruco_top_wrist'
30
+
```
31
+
32
+
33
+
**Dictionary Breakdown**
34
+
35
+
```yaml
36
+
'133':
37
+
```
38
+
39
+
The dictionary key for each entry is the ArUco marker’s ID number or `default`. For example, the entry shown above for the ArUco marker on the top of the wrist assumes that the marker’s ID number is `133`.
40
+
41
+
```yaml
42
+
'length_mm': 23.5
43
+
```
44
+
45
+
The `length_mm` value used by [detect_aruco_markers](https://github.com/hello-robot/stretch_ros/blob/master/stretch_core/nodes/detect_aruco_markers) is important for estimating the pose of an ArUco marker.
46
+
47
+
!!! note
48
+
If the actual width and height of the marker do not match this value, then pose estimation will be poor. Thus, carefully measure custom Aruco markers.
49
+
50
+
```yaml
51
+
'use_rgb_only': False
52
+
```
53
+
54
+
If `use_rgb_only` is `True`, [detect_aruco_markers](https://github.com/hello-robot/stretch_ros/blob/master/stretch_core/nodes/detect_aruco_markers) will ignore depth images from the [Intel RealSense D435i depth camera](https://www.intelrealsense.com/depth-camera-d435i/) when estimating the pose of the marker and will instead only use RGB images from the D435i.
55
+
56
+
```yaml
57
+
'name': 'wrist_top'
58
+
```
59
+
60
+
`name`is used for the text string of the ArUco marker’s [ROS Marker](http://docs.ros.org/en/melodic/api/visualization_msgs/html/msg/Marker.html) in the [ROS MarkerArray](http://docs.ros.org/en/melodic/api/visualization_msgs/html/msg/MarkerArray.html) Message published by the [detect_aruco_markers](https://github.com/hello-robot/stretch_ros/blob/master/stretch_core/nodes/detect_aruco_markers) ROS node.
61
+
62
+
```yaml
63
+
'link': 'link_aruco_top_wrist'
64
+
```
65
+
66
+
`link`is currently used by [stretch_calibration](https://github.com/hello-robot/stretch_ros/blob/master/stretch_calibration/nodes/collect_head_calibration_data). It is the name of the link associated with a body-mounted ArUco marker in the [robot’s URDF](https://github.com/hello-robot/stretch_ros/blob/master/stretch_description/urdf/stretch_aruco.xacro).
67
+
68
+
It’s good practice to add an entry to [stretch_marker_dict.yaml](https://github.com/hello-robot/stretch_ros/blob/master/stretch_core/config/stretch_marker_dict.yaml) for each ArUco marker you use.
69
+
70
+
### Create a New ArUco Marker
71
+
At Hello Robot, we’ve used the following guide when generating new ArUco markers.
72
+
73
+
We generate ArUco markers using a 6x6-bit grid (36 bits) with 250 unique codes. This corresponds with[ DICT_6X6_250 defined in OpenCV](https://docs.opencv.org/3.4/d9/d6a/group__aruco.html). We generate markers using this [online ArUco marker generator](https://chev.me/arucogen/) by setting the Dictionary entry to 6x6 and then setting the Marker ID and Marker size, mm as appropriate for the specific application. We strongly recommend measuring the actual marker by hand before adding an entry for it to [stretch_marker_dict.yaml](https://github.com/hello-robot/stretch_ros/blob/master/stretch_core/config/stretch_marker_dict.yaml).
74
+
75
+
We select marker ID numbers using the following ranges.
76
+
77
+
* 0 - 99: reserved for users
78
+
* 100 - 249: reserved for official use by Hello Robot Inc.
79
+
* 100 - 199: reserved for robots with distinct sets of body-mounted markers
80
+
* Allows different robots near each other to use distinct sets of body-mounted markers to avoid confusion. This could be valuable for various uses of body-mounted markers, including calibration, visual servoing, visual motion capture, and multi-robot tasks.
81
+
* 5 markers per robot = 2 on the mobile base + 2 on the wrist + 1 on the shoulder
82
+
* 20 distinct sets = 100 available ID numbers / 5 ID numbers per robot
83
+
* 200 - 249: reserved for official accessories
84
+
* 245 for the prototype docking station
85
+
* 246-249 for large floor markers
86
+
87
+
When coming up with this guide, we expected the following:
88
+
89
+
* Body-mounted accessories with the same ID numbers mounted to different robots could be disambiguated using the expected range of 3D locations of the ArUco markers on the calibrated body.
90
+
* Accessories in the environment with the same ID numbers could be disambiguated using a map or nearby observable features of the environment.
91
+
92
+
> Note: This section of the tutorial is borrowed from its [ROS1 predecessor](https://github.com/hello-robot/stretch_tutorials/blob/master/ros1/aruco_marker_detection.md).
93
+
4
94
## ArUco Detection
5
95
Stretch uses the OpenCV ArUco detection library and is configured to identify a specific set of ArUco markers belonging to the 6x6, 250 dictionary. To understand why this is important, please refer to [this](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html) handy guide provided by OpenCV.
6
96
7
-
Stretch comes preconfigured to identify ArUco markers. The ROS node that enables this is the detect_aruco_markers [node](https://github.com/hello-robot/stretch_ros2/blob/humble/stretch_core/stretch_core/detect_aruco_markers.py) in the stretch_core package. Thanks to this node, identifying and estimating the pose of a marker is as easy as pointing the camera at the marker and running the detection node. It is also possible and quite convenient to visualize the detections with RViz.
97
+
Stretch comes preconfigured to identify ArUco markers. The ROS node that enables this is the [detect_aruco_markers node](https://github.com/hello-robot/stretch_ros2/blob/humble/stretch_core/stretch_core/detect_aruco_markers.py) in the stretch_core package. Thanks to this node, identifying and estimating the pose of a marker is as easy as pointing the camera at the marker and running the detection node. It is also possible and quite convenient to visualize the detections with RViz.
8
98
9
99
## Computing Transformations
10
-
If you have not already done so, now might be a good time to review the [tf listener](https://docs.hello-robot.com/0.2/stretch-tutorials/ros2/example_10/) tutorial. Go on, we can wait…
100
+
If you have not already done so, now might be a good time to review the [tf listener](https://docs.hello-robot.com/latest/ros2/example_10/) tutorial. Go on, we can wait…
11
101
Now that we know how to program stretch to return the transform between known reference frames, we can use this knowledge to compute the transform between the detected marker and the robot base_link. From its current pose, for Stretch to align itself in front of the marker, we need to command it to reach there. But even before that, we need to program Stretch to know the goal pose. We define the goal pose to be 0.5 metre outward from the marker in the marker negative y-axis (Green axis). This is easier to visualize through the figure below.
12
102
13
103
<p align="center">
@@ -18,17 +108,21 @@ Now that we know how to program stretch to return the transform between known re
18
108
19
109
By monitoring the /aruco/marker_array and /aruco/axes topics, we can visualize the markers in RViz. The detection node also publishes the tf pose of the detected markers. This can be visualized by using the TF plugin and selecting the detected marker to inspect the pose. Next, we will use exactly that to compute the transform between the detected marker and the base_link of the robot.
20
110
21
-
Now, we can compute the transformation from the robot base_link frame to the goal pose and pass this as an SE2 pose to the mobile base.
111
+
Now, we can compute the transformation from the robot `base_link` frame to the `goal` pose and pass this as a 2D special Euclidian (SE2) pose to the mobile base.
112
+
113
+
Since we want Stretch to stop at a fixed distance with respect to the marker, we define a 0.75m offset in the marker y-axis where Stretch would come to a stop.
22
114
23
-
Since we want Stretch to stop at a fixed distance with respect to the marker, we define a 0.5m offset in the marker y-axis where Stretch would come to a stop. At the same time, we also want Stretch to align its orientation to point its arm towards the marker so as to make the subsequent manipulation tasks easier to accomplish. This would result in the end pose of the base_link as shown in the above figure. Sweet! The next task is to generate a simple motion plan for the mobile base to reach this end pose. We do this in three steps:
115
+
At the same time, we also want Stretch to align its orientation to point its arm towards the marker so as to make the subsequent manipulation tasks easier to accomplish. This would result in the end pose of the base_link as shown in the above figure. Sweet!
116
+
117
+
The next task is to generate a simple motion plan for the mobile base to reach this end pose. We do this in three steps:
24
118
1. Turn theta degrees towards the goal position. This would be the angle formed between the robot x-axis and the line connecting the start and the goal positions.
25
119
2. Travel straight to the goal position. This would be the euclidean distance between the start and the goal positions.
26
120
3. Turn phi degrees to attain the goal orientation. This would be the correction angle necessary to align the robot y-axis with the marker x-axis.
27
121
28
122
Luckily, we know how to command Stretch to execute a trajectory using the joint trajectory server. If you are just starting, have a look at the [Follow Joint Trajectory Commands](https://docs.hello-robot.com/0.2/stretch-tutorials/ros2/follow_joint_trajectory/) tutorial to know how to command Stretch using the Joint trajectory Server.
29
123
30
124
!!! warning
31
-
Since we won't be using the arm for this demo, it's safer to stow Stretch's arm in.
125
+
Since we won't be using the arm for this demo, it's safer to stow Stretch's arm in. Run `stretch_robot_stow.py` from anywhere on a terminal.
32
126
33
127
```{.bash .shell-prompt}
34
128
stretch_robot_stow.py
@@ -44,11 +138,29 @@ When you are ready, execute the following command:
1. Since we won't be using the arm for this demo, it's safer to stow Stretch's arm in. Run `stretch_robot_stow.py` from anywhere on a terminal.
148
+
2. the ArUco tag needs to be laid flat and rotated to match the image in [Computing Transformations](https://docs.hello-robot.com/latest/ros2/align_to_aruco/#computing-transformations) above.
149
+
3. The robot will move 0.75m away from the marker. This means that if the robot is within 0.75m, then it will rotate and move forward until it is at 0.75m. If the robot is too close while rotating, it may collide with its surroundings.
150
+
4. If you wish to use a custom ArUco tag, 1. you should [create a tag](#create-a-new-aruco-marker). 2. print it so you can place it in your environment. 3. Duplicate the `131` `base_right` marker in [stretch_marker_dict.yml](https://github.com/hello-robot/stretch_ros/blob/master/stretch_core/config/stretch_marker_dict.yaml) and replace `131` with your marker's ID, `base_right` with a new name, and the `marker_size`, if it is different. 4. Lastly, you will need to pass the `aruco_tag_name` rosparam to the `stretch_aruco.py` node. You can do this either by editting the `align_to_aruco.launch.py` file or following the [Manual Launch](#manual-launch) steps below.
151
+
152
+
153
+
### Manual Launch
154
+
155
+
Instead of using the launch file, you can also run the following commands. This allows you to specify a custom ArUco tag name to the `align_to_aruco.py` node. The custom tag name must be defined in [stretch_marker_dict.yml](https://github.com/hello-robot/stretch_ros/blob/master/stretch_core/config/stretch_marker_dict.yaml):
Let's jump into the code to see how things work under the hood. Follow along [here](https://github.com/hello-robot/stretch_ros2/blob/humble/stretch_core/stretch_core/align_to_aruco.py) to have a look at the entire script.
54
166
@@ -118,4 +230,4 @@ The align_to_marker() method is where we command Stretch to the pose goal in thr
118
230
def align_to_marker(self):
119
231
```
120
232
121
-
If you want to work with a different ArUco marker than the one we used in this tutorial, you can do so by changing line 44 in the [code](https://github.com/hello-robot/stretch_ros2/blob/humble/stretch_core/stretch_core/align_to_aruco.py#L44) to the one you wish to detect. Also, don't forget to add the marker in the [stretch_marker_dict.yaml](https://github.com/hello-robot/stretch_ros2/blob/humble/stretch_core/config/stretch_marker_dict.yaml) ArUco marker dictionary.
233
+
If you want to work with a different ArUco marker than the one we used in this tutorial, you can do so by changing line 44 in the [code](https://github.com/hello-robot/stretch_ros2/blob/humble/stretch_core/stretch_core/align_to_aruco.py#L130) to the one you wish to detect or pass in that rosparam. Also, don't forget to add the marker in the [stretch_marker_dict.yaml](https://github.com/hello-robot/stretch_ros2/blob/humble/stretch_core/config/stretch_marker_dict.yaml) ArUco marker dictionary.
0 commit comments