文章目录
- 0 引言
- 1 工作空间创建并初始化
- 2 创建ROS程序包
- 3 创建yaml读取函数
- 3.1 yaml文件
- 3.2 读取函数
- 4 创建静态TF关系的发布主函数
- 5 创建launch文件
- 6 编辑CMakeLists.txt
- 7 编译运行
- 7.1 编译
- 7.2 运行
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
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(Eigen REQUIRED)
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关系等操作。
