第 5 章 ROS 常用组件 3 —— TF 坐标变换_TF坐标变换实操 tf04_test(重难点)
文章目录
- 0 需求 - 分析 - 流程
- 1 准备工作
- 2 新建功能包，添加依赖
- 3 C++ 实现
-
- 3.1 服务通信生成新的乌龟节点 —— test01_new_turtle.cpp
-
- 3.1.1 建立乌龟GUI节点 —— test.launch
- 3.1.2 C++ test01_new_turtle.cpp(参考 plumbing_server_client)
- 3.1.3 配置 CMakeLists.txt
- 3.1.4 编译运行
- 3.2 发布方(发布两只乌龟相对于世界坐标系关系) —— test02_pub_turtle.cpp
-
- 3.2.1 两只乌龟相对于世界坐标系 —— test.launch
- 3.2.2 C++ test02_pub_turtle.cpp(参考 demo02_dynamic_pub.cpp 动态坐标变换)
- 3.2.3 配置 CMakeLists.txt
- 3.2.4 编译运行
- 3.3 订阅方(解析坐标信息并生成速度信息)
-
- 3.3.1 turtle1 坐标转换成相对 turtle2 坐标 —— test.launch
- 3.3.2 C++实现坐标转换 test03_control_turtle2(参考 demo01_tfs.cpp 多坐标变换)
- 3.4 计算线速度和角速度并发布
-
- 3.4.1 实现乌龟的跟踪 —— test.launch
- 3.4.2 C++实现 test03_control_turtle2.cpp
0 需求 - 分析 - 流程
需求描述
- 程序启动之初：产生两只乌龟，中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是B 可以跟随 A 运行。
实现分析
乌龟跟随实现的核心，是乌龟A和B都要发布相对世界坐标系的坐标信息，然后，订阅到该信息需要转换获取A相对于B坐标系的信息，最后，再生成速度信息，并控制B运动。
- 启动乌龟显示节点
- 在乌龟显示窗体中生成一只新的乌龟(需要使用服务)
- 编写两只乌龟发布坐标信息的节点
- 编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息
实现流程：C++ 与 Python 实现流程一致
- 新建功能包，添加依赖
- 编写服务客户端，用于生成一只新的乌龟
- 编写发布方，发布两只乌龟的坐标信息
- 编写订阅方，订阅两只乌龟信息，生成速度信息并发布
- 编译运行
1 准备工作
- 了解如何创建第二只乌龟，且不受键盘控制
创建第二只乌龟需要使用rosservice，话题使用的是 spawn
rosservice call /spawn "x: 1.0
y: 1.0
theta: 1.0
name: 'turtle_flow'"
name: "turtle_flow"
键盘是无法控制第二只乌龟运动的，因为使用的话题： /第二只乌龟名称/cmd_vel,对应的要控制乌龟运动必须发布对应的话题消息。
- 了解如何获取两只乌龟的坐标
是通过话题 /乌龟名称/pose 来获取的
x: 1.0 //x坐标
y: 1.0 //y坐标
theta: -1.21437060833 //角度
linear_velocity: 0.0 //线速度
angular_velocity: 1.0 //角速度
2 新建功能包，添加依赖
- 新建功能包
tf04_test
- 添加依赖
roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim
3 C++ 实现
3.1 服务通信生成新的乌龟节点 —— test01_new_turtle.cpp
3.1.1 建立乌龟GUI节点 —— test.launch
<launch>
<!-- 1. 启动乌龟GUI节点 -->
<node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen"/>
<!-- 键盘控制-->
<node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" />
<!-- 2. 生成新的乌龟节点 --> type 表示是cpp文件
<node pkg = "tf04_test" type = "test01_new_turtle" name = "turtle2" output = "screen"/>
</launch>
服务调用
3.1.2 C++ test01_new_turtle.cpp(参考 plumbing_server_client)
#include"ros/ros.h"
#include"turtlesim/Spawn.h"// 消息类型
/*
需求：向服务器发送请求，生成一个新的乌龟
话题：/spawn
消息：turtlesim/Spawn
1.包含头文件
2.初始化ROS节点
3.创建节点句柄
4.创建客户端对象
5.组织数据并发送
6.处理响应
*/
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化ROS节点
ros::init(argc,argv,"service_call");
// 3.创建节点句柄
ros::NodeHandle nh;
// 4.创建客户端对象
ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn"); // /spawn是话题名称
// 5.组织数据并发送
// 5.1 组织请求数据
turtlesim::Spawn spawn;
spawn.request.x = 1.0;
spawn.request.y = 4.0;
spawn.request.theta = 1.57;
spawn.request.name = "turtle2";
// 5.2 发送请求
// 判断服务器状态
// ros::service::waitForService("/spawn");
client.waitForExistence();
bool flag = client.call(spawn);//flag 接受响应状态，响应结果也会被设置进spawn对象
// 6.处理响应
if(flag) // 响应成功
{
ROS_INFO("乌龟生成成功，新乌龟叫：%s",spawn.response.name.c_str());
}
else
{
ROS_INFO("请求失败!!!");
}
return 0;
}
3.1.3 配置 CMakeLists.txt
add_executable(test01_new_turtle src/test01_new_turtle.cpp)
add_dependencies(test01_new_turtle ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(test01_new_turtle
${catkin_LIBRARIES}
)
3.1.4 编译运行
- Ctrl + Shift + B
- 启动窗口1：启动launch文件生成一个新的乌龟
cd demo02_ws/
source ./devel/setup.bash
roslaunch tf04_test test.launch
- 启动窗口2
rostopic list
/rosout
/rosout_agg
/tf_static
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose
/turtle2/cmd_vel
/turtle2/color_sensor
/turtle2/pose
- 如何控制第2个乌龟运动
- rostopic pub -r 10 /turtle2/cmd_vel geometry_msgs/Twist(参考 通信机制实操)
“linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0”
3.2 发布方(发布两只乌龟相对于世界坐标系关系) —— test02_pub_turtle.cpp
3.2.1 两只乌龟相对于世界坐标系 —— test.launch
<launch>
<!-- 1. 启动乌龟GUI节点 -->
<node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen"/>
<!-- 键盘控制-->
<node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" />
<!-- 2. 生成新的乌龟节点 -->
<node pkg = "tf04_test" type = "test01_new_turtle" name = "turtle2" output = "screen"/>
<!-- 3. 需要设置两个乌龟相对于世界的坐标系的发布 -->
<!--
基本实现思路:
1. 节点只编写一个
2. 这个节点需要启动两次
3. 节点启动时动态传参: 第1次启动传递turtle1，第2次传turtle2
-->
<node pkg = "tf04_test" type = "test02_pub_turtle" name = "pub1" args = "turtle1" output = "screen"/>
<node pkg = "tf04_test" type = "test02_pub_turtle" name = "pub2" args = "turtle2" output = "screen"/>
<!-- 由type类型确定和test02_pub_turtle.cpp建立联系 -->
</launch>
3.2.2 C++ test02_pub_turtle.cpp(参考 demo02_dynamic_pub.cpp 动态坐标变换)
- argc中的c指count，代表参数数量
- *argv中的v是vector，代表参数数组
- argv数组内容存放：程序名字 参数A 参数B …
- 程序名字也算一个入口参数，以后使用参数的时候一定要从第argv下标1开始取才行
#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
流程:
1. 包含头文件
2. 设置编码，初始化， NodeHandle
3. 创建订阅对象，订阅/turtle1/pose
4. 回调函数处理订阅信息: 将位姿信息转换成坐标相对关系并发布(关注)
5. spin()
*/
// 声明变量接受传递的参数
std::string turtle_name;
// 发布turtle1相对于世界坐标系的位置
// 给传进回调函数的数据定义范型 头文件2
void doPose(const turtlesim::Pose::ConstPtr& pose){
// 获取位姿信息，转换成坐标系相对关系(核心)，并发布
// a. 创建发布对象 头文件3
static tf2_ros::TransformBroadcaster pub;
// b. 组织被发布的数据 头文件4
geometry_msgs::TransformStamped ts;
ts.header.frame_id = "world"; // 相对于世界坐标系
ts.header.stamp = ros::Time::now();
// 关键点2: 动态传入
// ts.child_frame_id = "turtle1";
ts.child_frame_id = turtle_name;
// 坐标系偏移量
ts.transform.translation.x = pose->x;
ts.transform.translation.y = pose->y;
ts.transform.translation.z = 0; // z一直是零（2D）
// 坐标系四元数 头文件5
/*
位姿信息中没有四元数，但是有偏航角度，又已知乌龟时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();
// c. 发布
pub.sendTransform(ts);
}
int main(int argc, char *argv[])
{
// 2. 设置编码，初始化， NodeHandle
setlocale(LC_ALL,"");
ros::init(argc,argv,"pub_turtle");
ros::NodeHandle nh;
/*
解析launch文件，通过args传入的参数，第1次传入turtle1,传入第2次是turtle2
*/
if (argc != 2)// 成立时应该是两个
{
ROS_INFO("请传入一个参数");
return 1;
}else{
turtle_name = argv[1];// argv[0]表示程序名称，argv[1]表示第一个参数就是 turtle1或者turtle2
ROS_INFO("乌龟 %s 坐标发送启动",turtle_name.c_str());
}
// 3. 创建订阅对象，订阅/turtle1/pose
// 关键点1: 订阅的话题名称，turtle1 或 turtle2 动态传入
ros::Subscriber sub = nh.subscribe(turtle_name + "/pose",100,doPose);
// 4. 回调函数处理订阅信息: 将位姿信息转换成坐标相对关系并发布(关注)
// 5. spin()
ros::spin();
return 0;
}
3.2.3 配置 CMakeLists.txt
add_executable(test02_pub_turtle src/test02_pub_turtle.cpp)
add_dependencies(test02_pub_turtle ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(test02_pub_turtle
${catkin_LIBRARIES}
)
3.2.4 编译运行
- Ctrl + Shift + B
- 启动窗口1：发布两个乌龟相对于世界坐标系的位置关系
cd demo02_ws/
source ./devel/setup.bash
roslaunch tf04_test test.launch
- 因为有两个节点，所以该函数会启动两次
3.3 订阅方(解析坐标信息并生成速度信息)
3.3.1 turtle1 坐标转换成相对 turtle2 坐标 —— test.launch
<launch>
<!-- 1. 启动乌龟GUI节点 -->
<node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen"/>
<!-- 键盘控制-->
<node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" />
<!-- 2. 生成新的乌龟节点 -->
<node pkg = "tf04_test" type = "test01_new_turtle" name = "turtle2" output = "screen"/>
<!-- 3. 需要设置两个乌龟相对于世界的坐标系的发布 -->
<!--
基本实现思路:
1. 节点只编写一个
2. 这个节点需要启动两次
3. 节点启动时动态传参: 第1次启动传递turtle1，第2次传turtle2
-->
<node pkg = "tf04_test" type = "test02_pub_turtle" name = "pub1" args = "turtle1" output = "screen"/>
<node pkg = "tf04_test" type = "test02_pub_turtle" name = "pub2" args = "turtle2" output = "screen"/>
<!-- 由type类型确定和test02_pub_turtle.cpp建立联系 -->
<!-- 4. 需要订阅 turtle1 与 urtle2相对于世界坐标系的坐标消息，并转换成 turtle1 相对于 turtle2 的坐标关系
再生成速度消息
-->
<node pkg = "tf04_test" type = "test03_control_turtle2" name = "control" output = "screen"/>
</launch>
3.3.2 C++实现坐标转换 test03_control_turtle2(参考 demo01_tfs.cpp 多坐标变换)
#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"
// 求turtle1在turtle2中的坐标，查看二者的坐标关系
/*
需求1: 换算出 turtle1 相对于 turtle2 的关系
需求2: 计算角速度和线速度并发布
*/
int main(int argc, char *argv[])
{
// 2. 编码，初始化 NodeHandle
setlocale(LC_ALL,"");
ros::init(argc,argv,"tfs_sub");
ros::NodeHandle nh;
// 3. 创建订阅对象 头文件2/3
tf2_ros::Buffer buffer;
tf2_ros::TransformListener sub(buffer);
// 4. 编写解析逻辑
ros::Rate rate(10);
while(ros::ok())
{
// 核心
try
{
// 1. 计算 turtle1 与 turtle2 的相对关系
/*
A相对于B的坐标系关系
参数1: 目标坐标系 B
参数2: 源坐标系 A
参数3: ros::Time(0)取间隔最短的两个坐标关系帧来计算相对关系
返回值: geometry_msgs::TransformStamped 源相对于目标坐标系的相对关系
*/
geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
// 目标坐标系，源坐标系，时间0表示查找最近两个坐标系消息
ROS_INFO("turtle1 相对于 turtle2 的信息: 父级: %s,子级: %s 偏移量(%.2f,%.2f,%.2f)",
son1ToSon2.header.frame_id.c_str(), // 父坐标系 turtle2
son1ToSon2.child_frame_id.c_str(), // 子坐标系 turtle1
son1ToSon2.transform.translation.x, // 子坐标系与父坐标系的相对位置
son1ToSon2.transform.translation.y,
son1ToSon2.transform.translation.z
);
}
catch(const std::exception& e)
{
ROS_INFO("错误提示: %s",e.what());
}
rate.sleep();
// 5. spinOnce()
ros::spinOnce();
}
return 0;
}
订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息
roslaunch tf04_test test.launch
3.4 计算线速度和角速度并发布
3.4.1 实现乌龟的跟踪 —— test.launch
<launch>
<!-- 1. 启动乌龟GUI节点 -->
<node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen"/>
<!-- 键盘控制-->
<node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" />
<!-- 2. 生成新的乌龟节点 -->
<node pkg = "tf04_test" type = "test01_new_turtle" name = "turtle2" output = "screen"/>
<!-- 3. 需要设置两个乌龟相对于世界的坐标系的发布 -->
<!--
基本实现思路:
1. 节点只编写一个
2. 这个节点需要启动两次
3. 节点启动时动态传参: 第1次启动传递turtle1，第2次传turtle2
-->
<node pkg = "tf04_test" type = "test02_pub_turtle" name = "pub1" args = "turtle1" output = "screen"/>
<node pkg = "tf04_test" type = "test02_pub_turtle" name = "pub2" args = "turtle2" output = "screen"/>
<!-- 由type类型确定和test02_pub_turtle.cpp建立联系 -->
<!-- 4. 需要订阅 turtle1 与 urtle2相对于世界坐标系的坐标消息，并转换成 turtle1 相对于 turtle2 的坐标关系
再生成速度消息
-->
<node pkg = "tf04_test" type = "test03_control_turtle2" name = "control" output = "screen"/>
</launch>
3.4.2 C++实现 test03_control_turtle2.cpp
#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"
#include"geometry_msgs/Twist.h"
// 求turtle1在turtle2中的坐标，查看二者的坐标关系
/*
需求1: 换算出 turtle1 相对于 turtle2 的关系
需求2: 计算角速度和线速度并发布
*/
int main(int argc, char *argv[])
{
// 2. 编码，初始化 NodeHandle
setlocale(LC_ALL,"");
ros::init(argc,argv,"tfs_sub");
ros::NodeHandle nh;
// 3. 创建订阅对象 头文件2/3
tf2_ros::Buffer buffer;
tf2_ros::TransformListener sub(buffer);
// A. 创建速度发布对象 头文件7 (有关于速度消息发布)
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",100);
// 4. 编写解析逻辑
ros::Rate rate(10);
while(ros::ok())
{
// 核心
try
{
// 1. 计算 turtle1 与 turtle2 的相对关系
/*
A相对于B的坐标系关系
参数1: 目标坐标系 B
参数2: 源坐标系 A
参数3: ros::Time(0)取间隔最短的两个坐标关系帧来计算相对关系
返回值: geometry_msgs::TransformStamped 源相对于目标坐标系的相对关系
*/
geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
// 目标坐标系，源坐标系，时间0表示查找最近两个坐标系消息
// ROS_INFO("turtle1 相对于 turtle2 的信息: 父级: %s,子级: %s 偏移量(%.2f,%.2f,%.2f)",
// son1ToSon2.header.frame_id.c_str(), // 父坐标系 turtle2
// son1ToSon2.child_frame_id.c_str(), // 子坐标系 turtle1
// son1ToSon2.transform.translation.x, // 子坐标系与父坐标系的相对位置
// son1ToSon2.transform.translation.y,
// son1ToSon2.transform.translation.z
// );
// B. 根据相对计算并组织速度消息 头文件8
geometry_msgs::Twist twist;
/*
组织速度,只需要设置线速度的 x 与 角速度的 z
x = 系数 * sqrt((y^2+x^2))
z = 系数 * atan2(对边/邻边) 反正切，以弧度为单位
*/
// 乌龟只会前后走 所以线速度只有x方向上有
twist.linear.x = 0.5 * sqrt(pow(son1ToSon2.transform.translation.x,2) + pow(son1ToSon2.transform.translation.y,2)); // 2 表示平方
twist.angular.z = 4 * atan2(son1ToSon2.transform.translation.y,son1ToSon2.transform.translation.x);
// C. 发布
pub.publish(twist);
}
catch(const std::exception& e)
{
ROS_INFO("错误提示: %s",e.what());
}
rate.sleep();
// 5. spinOnce()
ros::spinOnce();
}
return 0;
}