0 引言

机器人中经常使用多种传感器来做定位和建图，而多个传感器数据融合的话，就会进行多传感器标定，一般标定的外参结果都是存放在yaml文件中，了解到ROS中有TF关系树，那就创建一个完整的ROS工程（可参考以下链接），目的是从多传感器外参标定结果的yaml文件中读取旋转矩阵R和平移矩阵t，然后发布静态TF的Topic。

ROS学习之基础包创建的详细流程：包括rosnode, rostopic, rosrun,roslaunch等使用

默认已在Ubuntu系统中安装ROS机器人系统，比如Ubuntu18.04-melodic
并且安装了OpenCV，Eigen

工程目录：

# catkin_ws/src目录下
.
└── tfsensors
    ├── CMakeLists.txt
    ├── config
    │   └── config.yaml
    ├── include
    │   └── config.h
    ├── launch
    │   └── tfsensors.launch
    ├── main
    │   └── tf_broadcaster.cpp
    ├── package.xml
    └── src
        └── config.cpp

👉本文代码GitHub：https://github.com/MRZHUGH/CSDN/tree/main/ROS/catkin_tf/src/tfsensors

1 工作空间创建并初始化

# 新开一个终端
# 创建一个名为catkin_ws工作空间，及src子目录
mkdir -p ~/catkin_ws/src
# 进入到src文件夹
cd ~/catkin_ws/src
# 初始化工作空间
catkin_init_workspace
# 回到工作空间根目录 
cd .. 
# 编译工作空间 
catkin_make

2 创建ROS程序包

# 切换到src文件夹
cd ~/catkin_ws/src
# 创建名为tfsensors的ROS程序包，用到了tf geometry_msgs
catkin_create_pkg tfsensors roscpp tf geometry_msgs

执行后，src目录下新建了一个 tfsensors文件夹及子目录和子文件

.
└── tfsensors
    ├── CMakeLists.txt
    ├── include
    │   └── tfsensors
    ├── package.xml
    └── src

然后根据个人习惯修改成了如下的工作目录

# 切换到src文件夹
cd ~/catkin_ws/src/tfsensors
rm -rf include/tfsensors
mkdir main config

调整后，main文件夹放main函数的cpp文件，include文件夹放头文件，src文件夹放其他cpp文件，config文件夹放配置文件

.
├── CMakeLists.txt
├── config
├── include
├── main
├── package.xml
└── src

3 创建yaml读取函数

3.1 yaml文件

首先把多传感器外参的标定yaml文件放到config下，以下以lidar和imu外参的标定config.yaml文件为例

%YAML:1.0

lidar_to_imu_rotation: !!opencv-matrix
  rows: 3
  cols: 3
  dt: d
  data: [ 7.533745000000e-03, -9.999714000000e-01, -6.166020000000e-04,
          1.480249000000e-02, 7.280733000000e-04, -9.998902000000e-01,
          9.998621000000e-01, 7.523790000000e-03, 1.480755000000e-02]

lidar_to_imu_translation: !!opencv-matrix
  rows: 3
  cols: 1
  dt: d
  data: [ -4.069766000000e-03, -7.631618000000e-02, -2.717806000000e-01 ]

3.2 读取函数

首先在include文件夹中创建 config.h 文件，声明 readParameters 读取函数

#ifndef PROJECT_CONFIG_H
#define PROJECT_CONFIG_H

#include <iostream>

#include <eigen3/Eigen/Dense>  // 小tips：须在 #include <opencv2/core/eigen.hpp> 顺序之前

#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>

// #include <vector>

void readParameters(std::string config_file, cv::Mat& R, cv::Mat& t);
#endif

其次在src文件夹中创建对应的 config.cpp 文件，进而定义 readParameters 函数

#include "config.h"

// config_file是输入，R和t是返回值
void readParameters(std::string config_file, cv::Mat& R, cv::Mat& t)
{
    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        std::cerr << "ERROR: Wrong path to settings" << std::endl;
    }

    cv::Mat cv_R, cv_t;
    fsSettings["lidar_to_imu_rotation"] >> cv_R;
    fsSettings["lidar_to_imu_translation"] >> cv_t;

    R = cv_R;
    t = cv_t;

    fsSettings.release();
}

4 创建静态TF关系的发布主函数

在main文件夹中创建 tf_broadcaster.cpp 文件，并写main函数，主要就是通过一个模板类获取config.yaml的绝对路径，然后调用config.cpp中的读取函数，返回lidar和imu之间的旋转矩阵和平移矩阵，最后借助TransformBroadcaster循环发布静态TF关系

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include "math.h"
#include "config.h"

// #include <opencv2/opencv.hpp> //config.h已经包含了

template <typename T>
T readParam(ros::NodeHandle &n, std::string name)
{
    // std::cout << name <<std::endl;
    T ans;
    if (n.getParam(name, ans))
    {
    ROS_INFO_STREAM("Loaded " << name << ": " << ans);
    }
    else
    {
    ROS_ERROR_STREAM("Failed to load " << name);
    n.shutdown();
    }
    return ans;
}


int main(int argc, char** argv)
{

    ros::init(argc, argv, "tf_publisher");//初始化ROS节点与节点名称
    ros::NodeHandle n;                    //创建节点的句柄
    
    ros::NodeHandle pnh("~");

    std::string config_file;
    config_file = readParam<std::string>(pnh, "config_file");
    cv::Mat R, t;
    readParameters(config_file, R, t);

    Eigen::Matrix3d eigen_R;
    Eigen::Vector3d eigen_t;
    cv::cv2eigen(R, eigen_R);
    cv::cv2eigen(t, eigen_t);
    Eigen::Vector3d rpy = eigen_R.matrix().eulerAngles(2,1,0);

    ros::Rate loop_rate(100);             //设定节点运行的频率,与loop.sleep共同使用

    tf::TransformBroadcaster broadCaster; //创建TransformBroadcaster对象(tf广播器)，是用来发布tf变换树;
    
    tf::Transform lidar2base;             //激光雷达坐标系lidar_link;创建一个Transform对象,用于描述两个坐标系之间的转化关系;
    tf::Quaternion q;
    // M_PI = π; M_PI/2 = π/2;
    q.setRPY(M_PI*rpy[0], M_PI*rpy[1], M_PI*rpy[2]);              //RPY欧拉角(zyx),外参的旋转坐标
                                       
    lidar2base.setRotation(q);
    lidar2base.setOrigin(tf::Vector3(eigen_t[0], eigen_t[1], eigen_t[2])); // 平移坐标, lidar在base的xyz位置

    while (n.ok())
    {
        // 循环发布坐标变换
        broadCaster.sendTransform(tf::StampedTransform(lidar2base,ros::Time::now(),"base_link","lidar_link"));

        loop_rate.sleep();
    }
    return 0;
}

5 创建launch文件

在launch文件夹下，创建tfsensors.launch文件，添加如下xml格式的代码，其中arg对应的是config.yaml路径，作为main函数的参数传入

<launch>
	<arg name="config_path" default = "$(find tfsensors)/config/config.yaml" />
	<node name="tf_publisher" pkg="tfsensors" type="tf_broadcaster" output="screen">

    	<param name="config_file" type="string" value="$(arg config_path)" />

	</node>
</launch>

6 编辑CMakeLists.txt

执行完第二步中后tfsensors文件目录下生成了CMakeLists.txt文件，由于该工程用到了OpenCV，Eigen，并且把主函数放到了新建的main文件夹中，也用到了头文件和对应的cpp文件，所以可按如下来修改CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(tfsensors)

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  tf
)

find_package(OpenCV REQUIRED)

catkin_package(
  INCLUDE_DIRS include
#  LIBRARIES tfsensors
#  CATKIN_DEPENDS geometry_msgs roscpp tf
#  DEPENDS system_lib
)

include_directories(include

)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
  # ${Eigen_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
)

include_directories("/usr/include/eigen3")

file(GLOB_RECURSE cpp_files
        ${PROJECT_SOURCE_DIR}/src/*.cpp
        )
add_library(tfsensorscpp ${cpp_files})


add_executable(tf_broadcaster main/tf_broadcaster.cpp)
target_link_libraries(tf_broadcaster
        ${catkin_LIBRARIES}
        ${OpenCV_LIBRARIES}
        tfsensorscpp
        )

7 编译运行

7.1 编译

# 回到catkin_ws工作空间
cd ~/catkin_ws/src
# 执行catkin_make编译
catkin_make

7.2 运行

# 编译后，重新source
source ~/catkin_ws/devel/setup.bash
# 运行talker_listener.launch
roslaunch tfsensors tfsensors.launch

运行后，新开终端运行rviz查看

# 运行rviz
rviz

运行rviz后，依次 Add --> By display type --> rviz --> TF，然后点击OK，添加TF，并且下拉选择或直接修改 Fixed Frame 为lidar_link 或 base_link

rviz坐标系符合右手坐标系原则
X轴—红色
Y轴—绿色
Z轴—蓝色
Roll(滚转角)绕X轴旋转
Pitch(俯仰角)绕Y轴旋转
Yaw(偏航角)绕Z轴旋转

至此，成功创建一个发布静态TF关系的ROS工程，并包含从yaml中读取旋转矩阵和平移矩阵，可视化该TF关系等操作。

须知少时凌云志，曾许人间第一流。

⭐️👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍🌔