Article directory
Five, ROS common components
1.6 Practical operation of TF coordinate transformation
Case study: The core of the implementation of the turtle following - turtle A and turtle B publish coordinate information relative to the world coordinate system, subscribe to the information, convert the information, obtain the coordinate system information of A relative to B, generate speed information, and control the turtle B follows the movement;
Realize analysis:
1. Start the turtle GUI
2. Spawn generates a new turtle
3. Write a node for two turtles to publish coordinate information
4. Write a subscription node to subscribe to coordinate information and generate a new relative relationship to generate speed information
Implementation process:
1. Create a function package and generate a new turtle by calling the service client
2. The publisher publishes the coordinate information of the two turtles
3. Subscriber, subscribe to the coordinate information of the two turtles, control the second turtle to follow the speed information and publish it
4. Execution
accomplish:
1. Create a function package and serve the client
#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. The publisher publishes the coordinate information of the two turtles
Subscribe to the location information of the turtle and convert it into coordinate information. The logic of the two turtles is the same, but the subscription topic is different. Therefore, the args dynamic parameter transfer method is used to pass in the difference:
#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;
}
Note that the node needs to be started twice, the first args is passed to turtle1, and the second args is passed to turtle2
Note that there should be two args incoming data, because main already occupies one memory, so the incoming parameter should be the second
3. Subscriber, control turtle2 to follow turtle1 (analyze coordinate information and generate speed information)
Subscribe to the tf broadcast information of turtle1 and turtle2, find and convert the nearest tf information of the time node, convert turtle1 into coordinate information relative to turtle2, and calculate the corresponding distance and angular velocity according to the mathematical formula
#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. Run
Create a new launch file and write a launch file:
<?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>
You can use rviz to verify;