五、ROS常用组件
1.6 TF坐标变换实操
案例分析：乌龟跟随实现的核心—乌龟A和乌龟B发布相对世界坐标系的坐标信息，订阅到该信息，对该信息进行转换，获取A相对于B的坐标系信息，生成速度信息，控制乌龟B跟随运动；
实现分析：
1、启动乌龟GUI
2、spawn生成new乌龟
3、编写两只乌龟发布坐标信息的节点
4、编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息
实现流程：
1、创建功能包，通过调用服务客户端，生成新乌龟
2、发布方，发布两个乌龟的坐标信息
3、订阅方，订阅两只乌龟的坐标信息，控制乌龟二跟随速度信息并发布
4、执行
实现：
1、创建功能包，服务客户端
#include <ros/ros.h>
#include "turtlesim/Spawn.h"
/**
* @brief
* 需求：想服务端发送请求，生产一只新乌龟
* 话题：/spawn
* 消息：turtlesim/Spawn
* 1、包含头文件
* 2、初始化ros节点
* 3、创建句柄
* 4、创建客户端对象
* 5、组织数据并发送
* 6、处理响应
*/
int main(int argc, char *argv[])
{
setlocale(LC_ALL, "");
ros::init(argc, argv, "service_call");
ros::NodeHandle nh;
ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
turtlesim::Spawn spawn;
spawn.request.x = 1.5;
spawn.request.y = 1.5;
spawn.request.theta = 1.57;
spawn.request.name = "turtle2";
//判断服务器状态（挂起）
ros::service::waitForService("/spawn");
bool flag = client.call(spawn); // flag接收响应状态，响应结果也会被设进spawn对象
if (flag)
{
ROS_INFO("乌龟生成成功，新乌龟名称为%s", spawn.response.name.c_str());
}
else
{
ROS_INFO("乌龟生成失败！");
}
return 0;
}
2、发布方，发布两只乌龟的坐标信息
订阅乌龟的位资信息，转换成坐标信息，两只乌龟实现逻辑相同，只是订阅话题不同，因此采用args动态传参的方法将差异部分传入：
#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()
*/
//声明一个变量用于接受传递的参数
std::string turtle_name;
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();
//关键点2：动态传入
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 没有翻滚和府仰角 所以可以得出乌龟的欧拉角为：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;
/*
解析： launch文件 通过args传入的参数
*/
if(argc!=2)//文件本身也是一个参数 地址 因此需要判断是否为2
{
ROS_INFO("请传入一个参数！");
return 1;//下面是状态码0 ，此处应该是1
}else{
turtle_name = argv[1]; //文件本身也是一个参数 地址 因此需要判断是否为2
}
// 3、创建订阅对象、订阅/turtle1/pose
//关键点1：动态传入
ros::Subscriber sub = nh.subscribe(turtle_name + "/pose", 100, doPose);
// 4、回调函数处理订阅的消息。将位资信息转换成坐标相对关系并发布
// 5、spin()
ros::spin();
return 0;
}
注意该节点要启动两次，第一次args传入turtle1，第二次args传入turtle2
注意args传入数据应该是两个，因为main本就占一个内存，所以应该传入参数是第二个
3、订阅方，控制turtle2跟随turtle1（解析坐标信息，并且生成速度信息）
订阅turtle1和turtle2的tf广播信息，查找并将时间节点最近的tf信息转换，将turtle1转换成相对于turtle2的坐标信息，并且根据数学公式计算出相对应的距离和角速度
#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"
/**
* 需求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、创建订阅对象，
tf2_ros::Buffer buffer;
tf2_ros::TransformListener sub(buffer);
// A、创建发布对象
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",100);
// * 4、编写解析逻辑
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("turtle2", "turtle1", ros::Time(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、根据相对计算并组织速度消息
geometry_msgs::Twist twist;
/*组织速度，只需要设置线速度的x，以及角速度的z
x=系数*开根号(y^2+x^2)
z=系数*arctan(对边/临边)
*/
twist.linear.x = 0.5 * sqrt(pow(son1ToSon2.transform.translation.x, 2) + pow(son1ToSon2.transform.translation.y, 2));
twist.angular.z = 4 * atan2(son1ToSon2.transform.translation.y,son1ToSon2.transform.translation.x);
// C、发布
pub.publish(twist);
}
catch (const std::exception &e)
{
// std::cerr << e.what() << '\n';
}
// * 5、spinOnce
rate.sleep();
ros::spinOnce();
}
return 0;
}
4、运行
新建launch文件，编写launch文件：
<?xml version="1.0"?>
<launch>
<!-- 1、启动乌龟GUI节点 -->
<node name="turtle1" pkg="turtlesim" type="turtlesim_node" output="screen">
</node>
<node name="key" pkg="turtlesim" type="turtle_teleop_key" output="screen"/>
<!-- 2、生成新乌龟GUI节点 -->
<node name="turtle2" pkg="tf04_test" type="test01_new_turtle" output="screen"/>
<!-- 3、需要启动两个乌龟想对于世界的坐标关系的发布 -->
<!--
基本实现思路：
1、节点只编写一个
2、该节点启动两次
3、节点启动时候动态传参数 第一次启动传递turtle1 第二次启动传入turtle2
-->
<node name="pub1" pkg="tf04_test" type="test02_pub_turtle" args="turtle1" output="screen"/>
<node name="pub2" pkg="tf04_test" type="test02_pub_turtle" args="turtle2" output="screen"/>
<!--
4、需要订阅turtle1 与turtle2相对于世界坐标系的坐标消息，
并且转换成turtle1相对于turtle2的坐标关系
再生成速度消息
-->
<node name="control" pkg="tf04_test" type="test03_control_turtle2" output="screen"/>
</launch>
可以采用rviz来验证；