使用apriltag_ros及tf的一些笔记

---

一. 下载源码及编译

1. 相关网站

https://github.com/ManiiXu/Apriltags2_VO
https://blog.csdn.net/qq_42879437/article/details/108850578

2. 编译问题解决

(1) 问题：

src/apriltag/apriltags2/include/common/matd.h:280:60: warning: comparison between signed and unsigned integer expressions [-Wsign-compare]
     return (a->ncols == 1 && a->nrows == len) || (a->ncols == len && a->nrows ==1）

解决：
将matd.h中的

static inline int matd_is_vector_len(const matd_t *a, int len)

修改为：

static inline int matd_is_vector_len(const matd_t *a, unsigned int len)

二. 向/tf话题中发布"base_link"到"camera_link"的转换

1.定义相机坐标系的名称

在apriltag_ros的launch文件中，添加：

<param name="sensor_frame_id" type="str" value="camera_link" />

其中sensor_frame_id参数在apriltag_detector.cpp文件中读取。
如果是新版的apriltag_ros，则应添加：

<param name="camera_frame" type="str" value="camera_link" />

其中camera_frame在common_functions.cpp文件中读取。

2.发布坐标系转换

同样，找到apriltag_ros的launch文件，先让其发布tf：

<param name="publish_tf" type="bool" value="true" />

如果是最新版本的apriltag_ros，也可在config/settings.yaml中选择：

publish_tf:        true       # default: false

下一步，便是在launch文件中定义发布节点：

<node pkg="tf" type="static_transform_publisher" name="base_link_to_camera_link" args="0 0 0 1.5707963 3.1415927 0 base_link camera_link 100" />

• node标签的解释可参照网站：https://blog.csdn.net/zlb_zlb/article/details/102475754。其中pkg="tf"表示包名，name="base_link_to_camera_link"表示文件名，type表示可执行文件的名字。
• static_transform_publisher有两种发布方式，一种是发布yaw pitch roll，一种是发布四元数。这个命令是发布静态坐标变换的，只能发布两个静止的坐标系间的坐标变换，且它是将坐标变换发布到tf中，剩下的由tf进行操作。（参考：https://blog.csdn.net/tiancailx/article/details/78910317）
• args 参数中的前三个数“x y z”分别代表着相应轴的平移，单位是米；后三个数“yaw pitch roll”分别代表着绕三个轴的转动，单位是弧度(yaw pitch roll 分别对应着 Z，Y，X轴的旋转)。
• args 参数中的“base_link”为frame_id，表示坐标系变换中的父坐标系；“camera_link”为child_frame_id，表示坐标系变换中的子坐标系；最后一个参数100为发布频率，单位为毫秒。
  
  

三. 利用tf监听坐标系间的关系

注意: 在利用tf进行监听时，需要在CMakeLists.txt的find_package()里面添加tf，否则编译时会报下面错误：

undefined reference to `tf::Transformer::DEFAULT_CACHE_TIME'
undefined reference to `tf::TransformListener::TransformListener(ros::Duration, bool)'
undefined reference to `tf::TransformListener::~TransformListener()'

1. 方法一

(1) 在头文件中添加：

#include <tf/transform_listener.h>

(2) 创建TransformListener类监听对象:

tf::TransformListener tf_listener;

(3) 读取坐标系之间的转换关系：

tf::StampedTransform transform;
try
{
    // 通过tf监听获得无人机机身坐标系"base_link"与世界坐标系"map"之间的相对位姿
    tf_listener.lookupTransform("map", "base_link", ros::Time(0), transform);
    qua_.x = transform.getRotation().getX();
    qua_.y = transform.getRotation().getY();
    qua_.z = transform.getRotation().getZ();
    qua_.w = transform.getRotation().getW();
    pose_.x = transform.getOrigin().x();
    pose_.y = transform.getOrigin().y();
    pose_.z = transform.getOrigin().z();
}
catch(const std::exception& e)
{
    ROS_ERROR("%s", e.what());
}          

注意1: try与catch要一起用，没有catch时会报错：

error: expected catch

注意2: transform需要是tf::StampedTransform格式，否则会报错：

error: no matching member function for call to 'lookupTransform'

2. 方法二

参考网站：
https://blog.csdn.net/CAIYUNFREEDOM/article/details/90747310
https://www.cnblogs.com/dlonng/p/13394131.html
(1) 在头文件中添加：

#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>

(2) 创建TransformListener类监听对象:

tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);

(3) 读取坐标系之间的转换关系：

geometry_msgs::TransformStamped transformStamped;
try
{
    // 通过tf监听获得无人机机身坐标系"base_link"与世界坐标系"map"之间的相对位姿
    transformStamped = tfBuffer.lookupTransform("map", "base_link", ros::Time(0));
    qua_.x = transformStamped.transform.rotation.x;
    qua_.y = transformStamped.transform.rotation.y;
    qua_.z = transformStamped.transform.rotation.z;
    qua_.w = transformStamped.transform.rotation.w;
    pose_.x = transformStamped.transform.translation.x;
    pose_.y = transformStamped.transform.translation.y;
    pose_.z = transformStamped.transform.translation.z;
}
catch(const std::exception& e)
{
    ROS_ERROR("%s", e.what());
}          

四. 向tf中发布坐标系间的位姿变换

1. 添加头文件及定义

#include <tf/transform_broadcaster.h>
tf::TransformBroadcaster tf_pub_;

2. 定义存储相对位姿的变量

geometry_msgs::PoseStamped tag_pose;
tag_pose.pose.position.x = transform(0, 3);
tag_pose.pose.position.y = transform(1, 3);
tag_pose.pose.position.z = transform(2, 3);
tag_pose.pose.orientation.x = rot_quaternion.x();
tag_pose.pose.orientation.y = rot_quaternion.y();
tag_pose.pose.orientation.z = rot_quaternion.z();
tag_pose.pose.orientation.w = rot_quaternion.w();
tag_pose.header = cv_ptr->header;

3.将变量转成tf格式

tf::Stamped<tf::Transform> tag_transform;
tf::poseStampedMsgToTF(tag_pose, tag_transform);

4.向tf话题中发布上述tf格式的变量

tf_pub_.sendTransform(tf::StampedTransform(tag_transform, tag_transform.stamp_, tag_transform.frame_id_, description.frame_name()));

• stamp_: 表示tag_transform的时间戳
• frame_id_: 表示父坐标系对应的名称(比如camera_link)
• frame_name(): 表示子坐标系对应的名称(比如tag_0)

五. frame_id_从launch文件中读取的过程

1. 在launch文件中定义参数名

<param name="sensor_frame_id" type="str" value="camera_link" />

• 新版的apriltag_ros中该参数名为"camera_frame”。

2. 在apriltag_detector.cpp文件中读取

(1) 读取sensor_frame_id参数

if(!pnh.getParam("sensor_frame_id", sensor_frame_id_)){
    sensor_frame_id_ = "";
  }

cv_bridge::CvImagePtr cv_ptr;
if(!sensor_frame_id_.empty())
    cv_ptr->header.frame_id = sensor_frame_id_;

(2) 保存到tag_transform的frame_id_中

geometry_msgs::PoseStamped tag_pose;

tag_pose.header = cv_ptr->header;

tf::poseStampedMsgToTF(tag_pose, tag_transform);