Article directory
Five, ROS common components
Some relatively useful tools are built into ROS; this chapter mainly introduces the following built-in components of ROS:
TF coordinate transformation, realize the conversion between different types of coordinate systems;
rosbag records the execution process of ROS nodes and realizes the effect of replaying the process;
rqt toolbox, a graphical debugging tool;
1. TF coordinate transformation
The robot system has multiple sensors to perceive the surrounding environment information, but it is wrong to directly equate the coordinate information of the object relative to the sensor with the coordinate orientation information between the robot system and the object, and it needs to go through a conversion process.
The conversion between different coordinates is relatively complicated and cumbersome, and related modules are directly encapsulated in ROS: Coordinate Transformation (TF);
**Concept: **tf: coordinate transformation;
**Coordinate system: **ROS calibrates the object through the coordinate system, and calibrates through the right-hand coordinate system;
[External link image transfer failed, the source site may have an anti-leeching mechanism, it is recommended to save the image and upload it directly (img-v0jRw5FA-1668434092634) (C:\Users\Haotian\AppData\Roaming\Typora\typora-user-images \1668314087211.png)]
**Function: **Used in ROS to realize the conversion of points or vectors between different coordinate systems;
**Description:**tf is deprecated, and now tf2 is used, which is simple and efficient, and the corresponding function packages are:
tf2_geometry_msgs: ROS message is converted to TF2 message;
tf2: encapsulates common messages of coordinate changes;
tf2_ros: provides roscpp and rospy bindings for tf2, and encapsulates common APIs for coordinate transformation;
1.1 Coordinate msg message
The commonly used data transmission carrier msg in coordinate transformation is:
geometry_msgs/TransformStamped
geometry_msgs/PointStamped
The former is used to transmit the position information related to the coordinate system, and the latter is used to transmit the information of coordinate points in a certain coordinate system;
- geometry_msgs/TransformStamped
Terminal input:
rosmsg info geometry_msgs/TransformStamped
std_msgs/Header header
uint32 seq
time stamp
string frame_id #坐标id
string child_frame_id #子坐标系的id
geometry_msgs/Transform transform #坐标信息
geometry_msgs/Vector3 translation #偏移量
float64 x
float64 y
float64 z
geometry_msgs/Quaternion rotation #四元数
float64 x
float64 y
float64 z
float64 w
explain:
- A and B two coordinate systems, discuss the relationship between the A coordinate system and the B coordinate system, at this time A is child_frame_id, and B is frame_id.
- Quaternion is another form of Euler angle (flip, pitch, rotation), but there is a singularity in Euler angle, and there will be bugs when writing code, so choose a quaternion that looks like a complex number instead.
- geometry_msgs/PointStamped
Terminal input:
rosmsg info geometry_msgs/PointStamped
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Point point
float64 x
float64 y
float64 z
1.2 Static coordinate transformation
Static coordinate transformation, the relative position between two coordinate systems is fixed.
Basic implementation logic:
- The relative relationship of the coordinate system, sent by the publisher
- According to the coordinate system relationship of the subscription publisher, the subscriber passes in the coordinate point information (can be hard-coded), realizes the coordinate transformation with the help of tf, and outputs the result.
Basic implementation process:
- Function package, add tf dependency
- Write the publisher
- Write the subscriber
Option A: C++
experiment:
1. Create a new function package demo04_ws, and create a new .cpp file under its src file that should contain the following dependent packages:
roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs
2. Configure the new code to quickly compile the tasks.json file:
{
// 有关 tasks.json 格式的文档，请参见
// https://go.microsoft.com/fwlink/?LinkId=733558
"version": "2.0.0",
"tasks": [
{
"label": "catkin_make:debug", //代表提示的描述性信息
"type": "shell", //可以选择shell或者process,如果是shell代码是在shell里面运行一个命令，如果是process代表作为一个进程来运行
"command": "catkin_make",//这个是我们需要运行的命令
"args": [],//如果需要在命令后面加一些后缀，可以写在这里，比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”
"group": {
"kind":"build","isDefault":true},
"presentation": {
"reveal": "always"//可选always或者silence，代表是否输出信息
},
"problemMatcher": "$msCompile"
}
]
}
- Publisher
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TwistWithCovarianceStamped.h"
#include "tf2/LinearMath/Quaternion.h"//欧拉角
/**
* @brief 需求：该节点需要发布两个坐标系之间的关系
*
* 流程：
* 1、包含头文件
* 2、设置编码 节点初始化 nodehandle
* 3、创建发布对象
* 4、组织被发布的消息
* 5、发布数据
* 6、spin（）
*
*/
int main(int argc, char* argv[])
{
// * 2、设置编码 节点初始化 nodehandle
setlocale(LC_ALL,"");
ros::init(argc, argv, "static_pub");
ros::NodeHandle nh;
// * 3、创建发布对象
tf2_ros::StaticTransformBroadcaster pub;
// * 4、组织被发布的消息
geometry_msgs::TransformStamped tfs;
tfs.header.stamp = ros::Time::now();
tfs.header.frame_id = "base_link";//相对关系之中被参考的那个 参考坐标系
tfs.child_frame_id = "laser";
tfs.transform.translation.x = 0.2;
tfs.transform.translation.y = 0.0;
tfs.transform.translation.z = 0.5;
//需要参考欧拉角转换
tf2::Quaternion qtn;//创建 四元数 对象
//向该对象设置欧拉角 这个对象可以将欧拉角转化为四元数
qtn.setRPY(0, 0, 0); //目前研究对象雷达相对于小车是固定的，所以没有翻滚，俯仰，偏航 等因此设为0,0,0 欧拉角的单位是弧度
tfs.transform.rotation.x = qtn.getX();
tfs.transform.rotation.y = qtn.getY();
tfs.transform.rotation.z = qtn.getZ();
tfs.transform.rotation.w = qtn.getW();
// * 5、发布数据
pub.sendTransform(tfs);
// * 6、spin（）
ros::spin();
return 0;
}
rostopic list
/rosout
/rosout_agg
/tf_static
rostopic echo /tf_static
transforms:
-
header:
seq: 0
stamp:
secs: 1668322909
nsecs: 686407571
frame_id: "base_link"
child_frame_id: "laser"
transform:
translation:
x: 0.2
y: 0.0
z: 0.5
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
---
Use rviz to display:
rviz
[External link image transfer failed, the source site may have an anti-leeching mechanism, it is recommended to save the image and upload it directly (img-YxcJHdjc-1668434092636) (C:\Users\Haotian\AppData\Roaming\Typora\typora-user-images \1668324095841.png)]
- Subscriber
#include <ros/ros.h>
#include "tf2_ros/transform_listener.h"//订阅数据
#include "tf2_ros/buffer.h"//缓存
#include "geometry_msgs/PointStamped.h"//坐标点信息
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
/**
* @brief 订阅方
* 需求：
*订阅发布的坐标系相对关系，传入一个坐标点，调用tf实现转换
头发》订阅坐标系相对关系
* 4、组织一个坐标点的数据
* 5、转换算法实现 调用tf内置实现
* 6、输出转换结果
*/
int main(int argc, char* argv[])
{
// * 2、编码 初始化 nodehandle 必须有nodehandle的
setlocale(LC_ALL,"");
ros::init(argc, argv, "static_sub");
ros::NodeHandle nh;
// * 3、创建一个订阅对象 ----》订阅坐标系相对关系
//3-1 创建一个buffer缓存
tf2_ros::Buffer buffer;
// 3-2 创建一个监听对象 （监听对象可以将被订阅的的数据存入缓存buffer之中）
tf2_ros::TransformListener listener(buffer);
// * 4、组织一个坐标点的数据
geometry_msgs::PointStamped ps;
ps.header.frame_id = "laser";
ps.header.stamp = ros::Time::now();
ps.point.x = 2.0;
ps.point.y = 3.0;
ps.point.z = 5.0;
//添加休眠 等待发布方
// ros::Duration(2).sleep();
// * 5、转换算法实现 调用tf内置实现
ros::Rate rate(10);
while(ros::ok())
{
//核心代码 ----将 ps 转换成相对于 base——link的坐标点
geometry_msgs::PointStamped ps_out;
/*
调用了buffer的转换函数 transform
参数1：被转换的坐标点
参数2：目标坐标系
返回值：输出的坐标点
注意：调用是必须包含头文件 tf2_geometry_msgs/
注意：运行时候出现报错，提示base_link不存在
原因分析：是因为发布方可能还没有发布 ，订阅数据是一个耗时操作，可能调用转换时，坐标系的相对关系还没有订阅
解决办法：方案1：调用转换函数的前面加一个休眠
方案2：异常处理 (建议使用)
*/
try
{
ps_out = buffer.transform(ps, "base_link");
// * 6、输出转换结果
ROS_INFO(
"转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
ps_out.point.x,
ps_out.point.y,
ps_out.point.z,
ps_out.header.frame_id.c_str());
}
catch(const std::exception& e)
{
//std::cerr << e.what() << '\n';
ROS_INFO("异常消息:%s", e.what());
}
// ps_out = buffer.transform(ps, "base_link");
// // * 6、输出转换结果
// ROS_INFO(
// "转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
// ps_out.point.x,
// ps_out.point.y,
// ps_out.point.z,
// ps_out.header.frame_id.c_str());
rate.sleep();
ros::spinOnce();
}
return 0;
}
Results of the:
[ INFO] [1668329057.735620630]: 异常消息:"base_link" passed to lookupTransform argument target_frame does not exist.
[ INFO] [1668329057.835439384]: 转换后的坐标值:(2.20,3.00,5.50),参考的坐标系:base_link
Replenish:
When the relative position between coordinate systems is fixed, the required parameters are also fixed: parent coordinate name, child coordinate system name, x offset, y offset, z offset, x roll angle, y pitch Angle, z yaw angle, the implementation logic is the same, but the parameters are different, then the ROS system has already packaged a special node, and the usage method is as follows:
rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系
Example:
rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /base_link /camera
rviz
rosrun tf2_ros static_transform_publisher 0.1 0.0 0.3 1.0 0 0 /base_link /camera
The conversion effect can be seen in rviz, and it is also recommended to use this method to directly realize the relative information release of the static coordinate system.
1.3 Dynamic coordinate transformation
Requires the coordinate transformation release from the previous section
and dynamic coordinate subscription
- Publisher implements
#include <ros/ros.h>
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
发布方：需要订阅乌龟的位资信息，转换成想对于窗体的一个坐标关系，并发布
准备：
话题：/turtle1/pose
消息：turtlesim/Pose
liucheng流程：
1、包含头文件
2、设置编码、初始化、nodehandle
3、创建订阅对象、订阅/turtle1/pose
4、回调函数处理订阅的消息。将位资信息转换成坐标相对关系并发布
5、spin()
*/
void doPose(const turtlesim::Pose::ConstPtr& pose)//const 防止函数修改引用内容 传引用提高效率
{
//获取位资信息，转换成坐标系想对关系（核心）并发布
//创建TF发布对象
static tf2_ros::TransformBroadcaster pub; // static修饰 使得每次回调函数是哟你pub这个函数;锁定pub
//组织被发布的数据
geometry_msgs::TransformStamped ts;
ts.header.frame_id = "world";
ts.header.stamp = ros::Time::now();
ts.child_frame_id = "turtle1";
//坐标系偏移量
ts.transform.translation.x = pose->x;
ts.transform.translation.y = pose->y;
ts.transform.translation.z = 0;
//四元数
//位资信息之中 没有四元数 有个z轴的偏航角度 而乌龟是2D 没有翻滚和府仰角 所以可以得出乌龟的欧拉角为：0 0 theta
tf2::Quaternion qtn;
qtn.setRPY(0,0,pose->theta);
ts.transform.rotation.x = qtn.getX();
ts.transform.rotation.y = qtn.getY();
ts.transform.rotation.z = qtn.getZ();
ts.transform.rotation.w = qtn.getW();
//发布
pub.sendTransform(ts);
}
int main(int argc, char* argv[])
{
// 2、设置编码、初始化、nodehandle
setlocale(LC_ALL,"");
ros::init(argc, argv, "dynamic_pub");
ros::NodeHandle nh;
// 3、创建订阅对象、订阅/turtle1/pose
ros::Subscriber sub = nh.subscribe("turtle1/pose",100,doPose);
// 4、回调函数处理订阅的消息。将位资信息转换成坐标相对关系并发布
// 5、spin()
ros::spin();
return 0;
}
Show results:
- Subscriber implementation
#include <ros/ros.h>
#include "tf2_ros/transform_listener.h" //订阅数据
#include "tf2_ros/buffer.h" //缓存
#include "geometry_msgs/PointStamped.h" //坐标点信息
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
/**
* @brief 订阅方
* 需求：
*订阅发布的坐标系相对关系，传入一个坐标点，调用tf实现转换
头发》订阅坐标系相对关系
* 4、组织一个坐标点的数据
* 5、转换算法实现 调用tf内置实现
* 6、输出转换结果
*/
int main(int argc, char *argv[])
{
// * 2、编码 初始化 nodehandle 必须有nodehandle的
setlocale(LC_ALL, "");
ros::init(argc, argv, "static_sub");
ros::NodeHandle nh;
// * 3、创建一个订阅对象 ----》订阅坐标系相对关系
// 3-1 创建一个buffer缓存
tf2_ros::Buffer buffer;
// 3-2 创建一个监听对象 （监听对象可以将被订阅的的数据存入缓存buffer之中）
tf2_ros::TransformListener listener(buffer);
// * 4、组织一个坐标点的数据
geometry_msgs::PointStamped ps;//被转换的坐标点
//参考坐标系
ps.header.frame_id = "turtle1";
//时间戳
ps.header.stamp = ros::Time(0.0);//动态坐标转换的时候buffer生成会有时间戳，时间戳不对代表坐标变化，因此报错
// ps.header.stamp = ros::Time::now();
ps.point.x = 2.0;
ps.point.y = 3.0;
ps.point.z = 0.0;
//添加休眠 等待发布方
// ros::Duration(2).sleep();
// * 5、转换算法实现 调用tf内置实现
ros::Rate rate(10);
while (ros::ok())
{
//核心代码 ----将 ps 转换成相对于 base——link的坐标点
geometry_msgs::PointStamped ps_out;
/*
调用了buffer的转换函数 transform
参数1：被转换的坐标点
参数2：目标坐标系
返回值：输出的坐标点
注意：调用是必须包含头文件 tf2_geometry_msgs/
注意：运行时候出现报错，提示base_link不存在
原因分析：是因为发布方可能还没有发布 ，订阅数据是一个耗时操作，可能调用转换时，坐标系的相对关系还没有订阅
解决办法：方案1：调用转换函数的前面加一个休眠
方案2：异常处理 (建议使用)
*/
try
{
ps_out = buffer.transform(ps, "world");
// * 6、输出转换结果
ROS_INFO(
"转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
ps_out.point.x,
ps_out.point.y,
ps_out.point.z,
ps_out.header.frame_id.c_str());
}
catch (const std::exception &e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("异常消息:%s", e.what());
}
// ps_out = buffer.transform(ps, "base_link");
// // * 6、输出转换结果
// ROS_INFO(
// "转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
// ps_out.point.x,
// ps_out.point.y,
// ps_out.point.z,
// ps_out.header.frame_id.c_str());
rate.sleep();
ros::spinOnce();
}
return 0;
}
- Show results
[External link image transfer failed, the source site may have an anti-leeching mechanism, it is recommended to save the image and upload it directly (img-qymlplVl-1668434092640) (C:\Users\Haotian\AppData\Roaming\Typora\typora-user-images \1668426497090.png)]
Summary: The implementation process of the subscriber is roughly the same as that of the static subscriber in the previous section. You only need to change the corresponding coordinate name and note that the timestamp of the buffer is not the current now, but should be changed to 0.0;
1.4 Multi-coordinate transformation
**Requirement: **Transformation between multiple coordinates, the previous two sections are transformations of relative positions between two coordinates. This section will introduce the transformation between multiple coordinates; the parent coordinate world, two child coordinates son1, son2, son1 relative to the world and son2 relative to the world are known, find the coordinates of the origin of son1 in son2, or already Knowing the coordinates of a point in son1, find the coordinates of its reference coordinate son2
analyze:
1. Publish son1 relative to world and son2 relative to world
2. Subscribe and release information, and use tf2 to convert the coordinate relationship between son1 and son2
3. Realize the conversion between coordinate points
process:
1. Create a new function package and add related dependencies
2. Create a coordinate relative relationship publisher
3. Create a coordinate relative relationship subscriber
- The publisher uses static coordinates to transform the launch file
<?xml version="1.0"?>
<launch>
<!-- 发布son1 相对于world 以及 son2相对于world 的坐标关系 -->
<node name="son1" pkg="tf2_ros" type="static_transform_publisher" args="5 0 0 0 0 0 /world /son1" output="screen"/>
<node name="son2" pkg="tf2_ros" type="static_transform_publisher" args="3 0 0 0 0 0 /world /son2" output="screen"/>
</launch>
- Subscriber
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
/**
* @brief
* 订阅方实现：1.计算son1与2之间的相对关系2.计算son1的某个坐标点对于son2的坐标值
* 流程：
* 1、包含头文件
* 2、编码初始化nodehandle
* 3、创建订阅对象，
* 4、编写解析逻辑
* 5、spinOnce
*
*/
int main(int argc, char *argv[])
{
// * 2、编码初始化nodehandle
setlocale(LC_ALL,"");
ros::init(argc, argv, "tfs_sub");
ros::NodeHandle nh;
// * 3、创建订阅对象，
tf2_ros::Buffer buffer;
tf2_ros::TransformListener sub(buffer);
// * 4、编写解析逻辑
//创建坐标点
geometry_msgs::PointStamped psAtson1;
psAtson1.header.stamp = ros::Time::now();
psAtson1.header.frame_id = "son1";
psAtson1.point.x = 1.0;
psAtson1.point.y = 2.0;
psAtson1.point.z = 3.0;
ros::Rate rate(10);
while (ros::ok())
{
//核心
try
{
//1.计算son1与son2的相对关系
/*
A 相对 B 坐标关系
参数1：目标坐标系 B
参数2：源坐标系 A
参数3：ros::Time(0.0) 取时间间隔最短的两个坐标关系计算相对关系
返回值：geometry_msgs::TransformStamped 源坐标系相对于目标坐标系的相对关系
*/
geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("son2","son1",ros::Time(0));
ROS_INFO("son1相对son2相对关系信息:父级：%s,子级：%s 偏移量(%.2f,%.2f,%.2f)",
son1ToSon2.header.frame_id.c_str(), // son2
son1ToSon2.child_frame_id.c_str(), // son1
son1ToSon2.transform.translation.x,
son1ToSon2.transform.translation.y,
son1ToSon2.transform.translation.z
);
// 2.计算son1坐标点在son2的坐标值
geometry_msgs::PointStamped psAtson2= buffer.transform(psAtson1,"son2");
ROS_INFO(
"坐标点在son2中的值为(%.2f,%.2f,%.2f)",psAtson2.point.x,psAtson2.point.y,psAtson2.point.z
);
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
}
// * 5、spinOnce
rate.sleep();
ros::spinOnce();
}
return 0;
}
Review the args parameter in the node tag under the launch file again, and generate a coordinate point and a new function buffer.lookupTraansform("", "", ros::Time(0.0))
1.5 Check the coordinate system relationship
- Check if tf2_tools is installed:' rospack find tf2_tools'
- Installation command: 'sudo apt install ros-noetic-tf2-tools'
- roslaunch tf03_tfs tfs_c.launch
- rosrun tf2_tools view_frames.py generates a frame.pdf file
- evince frame.pdf
- Effect:
