ROS学习笔记12——tf坐标变换

以ros自带的小乌龟作为案例的载体，完成通信方式、坐标变换等的练习，实现程序启动之初产生两只乌龟，中间的乌龟(turtle1) 和左下乌龟(turtle2), turtle2 会自动运行至turtle1的位置，并且键盘控制时，只是控制 turtle1 的运动，但是 turtle2 可以跟随 turtle1 运行。

1、创建功能包

创建项目功能包依赖于roscpp、rospy、std_msgs、 tf2、tf2_ros、tf2_geometry_msgs、geometry_msgs、turtlesim

2、服务客户端（生成新的小乌龟turtle2）

#include "ros/ros.h"
#include "turtlesim/Spawn.h"
/*需求：向服务端发送请求，生成一只新乌龟话题：/spawn -> 通过rosservice list看到消息类型：turtlesim/Spawn -> 通过rosservice info /spawn看到消息的具体内容：。。。 -> 通过rossrv info turtlesim/Spawn看到
*/int main(int argc, char *argv[])
{/* code */setlocale(LC_ALL, "");ros::init(argc, argv,"create_new_turtle");ros::NodeHandle nh;//创建服务对象ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");//组织数据turtlesim::Spawn spawn;spawn.request.name = "turtle2";spawn.request.x = 1.0;spawn.request.y = 2.0;spawn.request.theta = 1.57;client.waitForExistence();//判断服务器状态//发送请求bool flag = client.call(spawn);//flag用来接收响应状态的，响应结果也会被设置进spawn对象if(flag){ROS_INFO("新乌龟 %s 成功生成", spawn.response.name.c_str());}else{ROS_INFO("新乌龟生成失败");}ros::spin();return 0;
}

3、发布方（发布两只小乌龟的坐标信息）

可以订阅乌龟的位姿信息，然后转换成坐标系关系，两只乌龟的实现逻辑是相同的，修改订阅话题和消息类型之后，新建一个cpp重复实现一遍逻辑当然是可以的，但是加大了代码量，并且降低了代码的复用率。这些细微的差异可以通过args设置参数来传入：

该节点需要启动两次
每次启动时都需要传入乌龟节点名称(第一次是 turtle1 第二次是 turtle2)

#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、/turtle2/pose消息：turtlesim/Pose流程：1、包含头文件2、设置编码、初始化、NodeHandle等3、创建订阅对象、订阅话题：/turtle1/pose、/turtle2/pose4、**回调函数，处理订阅的消息：把订阅的位姿信息转换成坐标系相对关系，并发布5、spin()
*///声明接收传递参数的变量
std::string turtle_name;void doPose(const turtlesim::Pose::ConstPtr &pose)
{//获取位姿信息，转换成坐标系相对关系，并发布//5.1 创建发布对象static tf2_ros::TransformBroadcaster pub;   //设为static静态变量，每次回调使用同一个pub对象//5.2 组织被发布的数据  geometry_msgs::TransformStamped ts;ts.header.frame_id = "world";ts.header.stamp = ros::Time::now();//**子级坐标系名称动态传入ts.child_frame_id = turtle_name;//坐标系偏移量ts.transform.translation.x = pose->x;ts.transform.translation.y = pose->y;ts.transform.translation.z = 0;tf2::Quaternion qnt;qnt.setRPY(0,0,pose->theta);ts.transform.rotation.w = qnt.getW();ts.transform.rotation.x = qnt.getX();ts.transform.rotation.y = qnt.getY();ts.transform.rotation.z = qnt.getZ();//5.3 发布pub.sendTransform(ts);
}int main(int argc, char *argv[])
{/* code *//*解析 launch 文件通过 args 传入的参数*/if(argc != 4){ROS_INFO("请传入一个参数");return 1;}else{turtle_name = argv[1];}setlocale(LC_ALL,"");// 2、设置编码、初始化、NodeHandle等ros::init(argc,argv,"dynamic_pub_turtle");ros::NodeHandle nh;// **3、创建订阅对象、订阅话题：/turtle1 或者 turtle2 是动态传入的ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtle_name + "/pose",100,doPose);// 4、**回调函数，处理订阅的消息：把订阅的位姿信息转换成坐标相对关系，并发布// 5、spin()    ros::spin();return 0;
}

4、订阅方（解析坐标信息并且生成速度信息）

#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[])
{/* code */setlocale(LC_ALL, "");// 2、编码、初始化、NodeHandleros::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::Duration(2).sleep();ros::Rate rate(10);while(ros::ok()){try{// １、计算 turtle1 和 turtle2 之间的相对关系/* lookupTransform(参数1：B，参数2：A，参数3)## A 相对于 B 的坐标系关系参数1："目标坐标系 B"参数2："源坐标系 A"参数3：ros::Time(0) 取间隔最短的两个坐标系帧计算相对关系返回值：geometry_msgs::TransformStamped*/geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));ROS_INFO("turtle1 相对于 turtle2 的信息：父级名称:%s,子级名称:%s,偏移量:(%.2f,%.2f,%.2f)",tfs.header.frame_id.c_str(), //turtle2tfs.child_frame_id.c_str(),   //turtle1tfs.transform.translation.x,tfs.transform.translation.y,tfs.transform.translation.z); //B、根据相对位置计算并组织速度消息geometry_msgs::Twist twist;/*linear.x = 系数 * 开放(y^2 + x^2)angular.z = 反正切(对边，临边)*/twist.linear.x = 0.5 * sqrt(pow(tfs.transform.translation.x,2) + pow(tfs.transform.translation.y,2));twist.angular.z = 4 * atan2(tfs.transform.translation.y,tfs.transform.translation.x);//C、发布pub.publish(twist);}catch(const std::exception& e){//std::cerr << e.what() << '\n';ROS_INFO("异常消息：%s", e.what());}rate.sleep();// 5、spinOnce()ros::spinOnce();}return 0;
}

5、运行

使用 launch 文件组织需要运行的节点

<launch><!-- 启动小乌龟GUI --><node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" /><!-- 键盘控制节点 --><node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" /><!-- 创建新的小乌龟 --><node pkg="tfs_test" type="create_new_turtle" name="turtle2" output="screen" /><!-- 需要启动两个乌龟相对于世界坐标系的坐标关系的发布节点 --><!-- 实现思路：1、只编写一个节点2、这个节点需要启动两次3、节点启动时，动态传参：第一次：turtle1第二次：turtle2--><node pkg="tfs_test" type="pub_turtle" name="pub_turtle1" args="turtle1" output="screen" /><node pkg="tfs_test" type="pub_turtle" name="pub_turtle2" args="turtle2" output="screen" /><!-- 订阅turtle1与 turtle2 相对于世界坐标系的坐标关系并生成 turtle1 相对于 turtle2 的坐标关系再生成速度消息 --><node pkg="tfs_test" type="control_turtle2" name="control" output="screen" />
</launch>