ARTag

Artag 是一种AR标签，可以用于在三维场景中标记物体位置或者帮助机器人定位和导航。通过在环境中放置二维码标记，机器人可以扫描这些标记，从而获取关于它所在位置的信息，并通过这些信息来规划路径、执行任务等

ar_track_alvar 功能包

AR标签识别的功能通过开源的ar_track_alvar包实现，参考官方文档。通过该功能包，可以实现单个或者多个artag的识别与定位。
功能包路径
- ~/robot_ros_application/catkin_ws/src/ar_tag_detect/ar_track_alvar

节点启动

ar_track_alvar节点运行需要传入artag码的大小、相机图像的topic、相机内参的topic、跟踪误差等众多参数，因此通常使用launch文件运行。

通过以下命令分别启动下巴摄像头和头部rgbd摄像头识别节点

头部摄像头

source ~/robot_ros_application/catkin_ws/devel/setup.bash
roslaunch ar_track_alvar roban_head_camera.launch

下巴摄像头

source ~/robot_ros_application/catkin_ws/devel/setup.bash
roslaunch ar_track_alvar roban_chin_camera.launch

launch 文件路径
- 头部摄像头
  - /home/lemon/robot_ros_application/catkin_ws/src/ar_tag_detect/ar_track_alvar/ar_track_alvar/launch/roban_head_camera.launch
- 下巴摄像头
  - /home/lemon/robot_ros_application/catkin_ws/src/ar_tag_detect/ar_track_alvar/ar_track_alvar/launch/roban_chin_camera.launch

单个标签检测的launch文件配置

只需要单个标签检测时，常见的配置文件如下（以下巴摄像头的 launch 文件为例说明）：

```xml
<launch>
<group ns="chin">
        <arg name="marker_size" default="4.4" />
        <arg name="max_new_marker_error" default="0.08" />
        <arg name="max_track_error" default="0.2" />
        <arg name="cam_image_topic" default="/chin_camera/image" />
        <arg name="cam_info_topic" default="/chin_camera/camera_info" />
        <arg name="output_frame" default="/chin_camera" />

        <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen">
            <param name="marker_size"           type="double" value="$(arg marker_size)" />
            <param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
            <param name="max_track_error"       type="double" value="$(arg max_track_error)" />
            <param name="output_frame"          type="string" value="$(arg output_frame)" />

            <remap from="camera_image"  to="$(arg cam_image_topic)" />
            <remap from="camera_info"   to="$(arg cam_info_topic)" />
            <remap from="visualization_marker"   to="visualization_marker_chin" /> 
            <!-- 通过remap将原有的结果输出的topic名字visualization_marker重命名为visualization_marker_chin, 结果发布的topic为chin组下的/chin/visualization_marker_chin -->
        </node>
</group>
</launch>
```
- marker_size -- artag码的宽度（以厘米为单位）

- max_new_marker_error -- 检测新标签的误差阈值

- max_track_error -- 跟踪误差量阈值

- camera_image -- 输入图像的topic，可以是单色或彩色，但应该是未校正的图像，因为校正在此包中进行

- camera_info -- 提供相机校准参数以便可以校正图像的主题的名称

- output_frame -- 可以指定tf坐标系名称，AR 标签发布的坐标将相对于该坐标系

多个标签组合检测

ar_track_alvar 允许将多个 artag 码贴于同一个物体中，在识别时将其看作一个整体。要将多个标签进行“捆绑”，需要在 launch 文件中传入一个bundle_files参数，参数的值应该是一个定义了这个标签集中每个标签的四个角的坐标的xml格式文件，一个示例为：

<?xml version="1.0" encoding="utf-8"?>
<multimarker markers="11">
    <marker status="1" index="10">
        <corner z="0" x="-62.5" y="-32.5"></corner>
        <corner z="0" x="-57.5" y="-32.5"></corner>
        <corner z="0" x="-57.5" y="-27.5"></corner>
        <corner z="0" x="-62.5" y="-27.5"></corner>
    </marker>
    <marker status="1" index="1">
        <corner z="0" x="27.5" y="-2.5"></corner>
        <corner z="0" x="32.5" y="-2.5"></corner>
        <corner z="0" x="32.5" y="2.5"></corner>
        <corner z="0" x="27.5" y="2.5"></corner>
    </marker>
    .
    .
    .
    <marker status="1" index="8">
        <corner z="0" x="-2.5" y="27.5"></corner>
        <corner z="0" x="2.5" y="27.5"></corner>
        <corner z="0" x="2.5" y="32.5"></corner>
        <corner z="0" x="-2.5" y="32.5"></corner>
    </marker>
</multimarker>

这个示例中，<multimarker markers="11"> 标签定义了该 bundle 总共的 artag 码数目为11个
在multimarker标签下，使用marker标签列出每个artag码的四个角点坐标的x,y,z值，属性 index 中填入单个artag码的 id 值

编写好bundle_files后，在上述单个标签检测的launch文件中新增一个bundle_files，指定为定义好的 bundle_files 文件路径，运行即可进行多个标签联合定位
一个下巴摄像头多标签检测的示例在
- launch文件路径： /home/lemon/robot_ros_application/catkin_ws/src/ar_tag_detect/ar_track_alvar/ar_track_alvar/launch/roban_chin_camera_multi.launch
- 使用的bundle_files文件路径： /home/lemon/robot_ros_application/catkin_ws/src/ar_tag_detect/ar_track_alvar/ar_track_alvar/launch/MultiMarkerMap.xml
- 该bundle_files文件对应各个标签的位置如下图所示：
- 运行以下指令执行多标签识别：
```
source ~/robot_ros_application/catkin_ws/devel/setup.bash
roslaunch ar_track_alvar roban_chin_camera_multi.launch 
```

标定 artag 码尺寸

artag 的尺寸应为所打印出来ARtag码（正方形）的实际边长
Tips:可以在 word 中编辑图片的宽和高然后通过"实际大小"或"100%大小"进行打印,即可获得指定大小的码
在对应的launch文件配置中修改参数 marker_size 为实际打印的 artag 尺寸大小即可

节点信息

节点名称
- 头部摄像头
  - /head/ar_track_alvar
- 下巴摄像头
  - /chin/ar_track_alvar
功能包路径
- ~/robot_ros_application/catkin_ws/src/ar_tag_detect/ar_track_alvar
消息名称
- 头部摄像头
  - /head/visualization_marker_head
- 下巴摄像头
  - /chin/visualization_marker_chin (单标签和多标签检测的结果都通过同一个topic发布)
消息类型
- visualization_msgs/Marker

消息类型定义

uint8 ARROW=0
uint8 CUBE=1
uint8 SPHERE=2
uint8 CYLINDER=3
uint8 LINE_STRIP=4
uint8 LINE_LIST=5
uint8 CUBE_LIST=6
uint8 SPHERE_LIST=7
uint8 POINTS=8
uint8 TEXT_VIEW_FACING=9
uint8 MESH_RESOURCE=10
uint8 TRIANGLE_LIST=11
uint8 ADD=0
uint8 MODIFY=0
uint8 DELETE=2
uint8 DELETEALL=3
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string ns
int32 id
int32 type
int32 action
geometry_msgs/Pose pose
geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w
geometry_msgs/Vector3 scale
float64 x
float64 y
float64 z
std_msgs/ColorRGBA color
float32 r
float32 g
float32 b
float32 a
duration lifetime
bool frame_locked
geometry_msgs/Point[] points
float64 x
float64 y
float64 z
std_msgs/ColorRGBA[] colors
float32 r
float32 g
float32 b
float32 a
string text
string mesh_resource
bool mesh_use_embedded_materials

结果输出与解析

运行下巴摄像头检测的launch文件后，我们可以在终端使用命令 rostopic echo /chin/visualization_marker_chin 实时查看 artag 识别的结果，取其中一条数据如下进行解析：

header: 
seq: 809
stamp: 
    secs: 1671697367
    nsecs: 476323108
frame_id: "chin_camera"         # 输出结果的坐标系
ns: "basic_shapes"              # 结果类型：basic_shapes为单个artag结果，main_shapes为多个artag码联合检测的结果
id: 6                           # artag 码编号
type: 1
action: 0
pose: 
position: 
    x: -0.00860899694651        # 二维码垂直于相机，正方向向右
    y: -0.0696511716506         # 二维码垂直于相机，正方向向上
    z: 0.271939015273           # 二维码垂直于相机，正方向向前
orientation:                    # 四元数
    x: 0.986290075697
    y: 0.026093318553
    z: -0.0374334556082
    w: 0.158586763981
scale: 
x: 0.044
y: 0.044
z: 0.0088
color: 
r: 0.5
g: 0.0
b: 0.5
a: 1.0
lifetime: 
secs: 1
nsecs:         0
frame_locked: False
points: []
colors: []
text: ''
mesh_resource: ''
mesh_use_embedded_materials: False

在进行多标签检测时，也会同时输出单标签检测的结果，需要根据获取到消息的ns属性去区分和处理。

订阅消息（Python 实现）

```python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from visualization_msgs.msg import Marker
import math
import numpy as np
import scipy.linalg as linalg

ARTAG_CHIN_TOPIC = "/chin/visualization_marker_chin"        # 订阅下巴摄像头

class ArtagData():
    def __init__(self):
        self.topic_name = ARTAG_CHIN_TOPIC
        rospy.Subscriber("/chin/visualization_marker_chin", Marker, self.chin_marker_callback)

    def quart_to_rpy(self, w, x, y, z):                     # 四元数换算欧拉角
        r = math.atan2(2*(w*x+y*z), 1-2*(x*x+y*y))
        p = math.asin(2*(w*y-z*x))
        y = math.atan2(2*(w*z+x*y), 1-2*(z*z+y*y))
        return [r, p, y]

    def rotate_mat(self, axis, radian):                     # 校正图像旋转，为了更准确得出 x、y、z 值
        rot_matrix = linalg.expm(np.cross(np.eye(3), axis / linalg.norm(axis) * radian))
        return rot_matrix

    def chin_marker_callback(self,msg):
        rpy = self.quart_to_rpy(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z)
        cam_rot = self.rotate_mat([0, 1, 0], 10 * math.pi / 180.0)  # 绕机器人坐标 y 轴旋转
        pos_in_torso = np.dot(cam_rot, np.array([[-msg.pose.position.y], [-msg.pose.position.x], [msg.pose.position.z]])).tolist()

        rospy.loginfo("\nresult type: {}\ny: {} x: {} z: {}\nroll: {} pitch: {} yaw: {}\n======".format(msg.ns if msg.ns == "basic_shapes" else "\033[92m"+msg.ns + "\033[00m" , pos_in_torso[0][0], pos_in_torso[1][0], pos_in_torso[2][0], rpy[0], rpy[1], rpy[2]))

if __name__ == "__main__":
    rospy.init_node("artag_data")
    ad = ArtagData()
    rospy.spin()

```
运行下巴摄像头检测的launch文件后，再运行上述示例程序，将artag码到机器人下巴摄像头处，可以看到终端打印出来当前artag码相对于摄像头的位置和姿态角。
![](image/result.png)

使用tf2进行坐标系转换

默认情况下，通过 "/chin/visualization_marker_chin" 或 "/head/visualization_marker_head" 返回的位姿信息都是相机坐标系下的，即获取到的位置是标签相对于机器人的位置。如果需要获取机器人相对于标签的位置，可以使用 tf2_ros 进行机器人坐标系到标签坐标系的转换。
实际上，ar_track_alvar 包已经为我们构建好了相机和标签之间的 tf 树分支
- 通过命令行执行rosrun tf tf_monitor即可打印出当前所有tf树节点以及关系。
- 也可以通过 rviz 可视化工具或 rosrun tf view_frames 命令生成tf树连接图上图右侧显示的是当前 chin_camera 和两个artag码(ar_marker_n)坐标的转换关系
  在使用多标签检测时，标签集的tf坐标系名字是bundle_files中定义的第一个artag码的名称(ar_marker_n)。

要获取到机器人在标签坐标系下位置，一个实例如下：

#! /usr/bin/env python
# coding=UTF-8

import rospy
import tf2_ros
import tf
from tf2_geometry_msgs import tf2_geometry_msgs

if __name__ == '__main__':
    rospy.init_node('tf_listener')
    buffer = tf2_ros.Buffer()  # 创建缓存对象
    sub = tf2_ros.TransformListener(buffer)  # 创建订阅对象，将缓存传入
    
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            now = rospy.Time.now()
            ps_out = buffer.lookup_transform("ar_marker_2", "chin_camera",rospy.Time())#获取tf数转换关系
            print("{}\n{}".format("==="*10,ps_out))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException)as e:
            continue
        rate.sleep()

通过 lookup_transform 函数可以方便地获取到各个坐标之间的转换关系，如示例中获取到了chin_camera相对于ar_marker_2的位置。

ARTag

ar_track_alvar 功能包​

节点启动​

单个标签检测的launch文件配置​

多个标签组合检测​

标定 artag 码尺寸​

节点信息​

结果输出与解析​

订阅消息（Python 实现）​

使用tf2进行坐标系转换​