文章目录
一、turtle_tf_broadcaster.cpp
#include <tf/transform_broadcaster.h>
//使用 TransformBroadcaster，我们需要包含头文件：tf/transform_broadcaster.h
#include <turtlesim/Pose.h>
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg)
//ConstPtr& 为常量指针的引用
{
static tf::TransformBroadcaster br;
//创建一个 TransformBroadcaster ，我们需要它来广播转换
tf::Transform transform;
transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
//创建一个transform对象，我们将turtle的2D位置信息转变为3D的transform中
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
//创建一个四元数，把turtle中的欧拉角数据转换为四元数q
transform.setRotation(q);
//这里我们设定了旋转
br.sendTransform(tf::StampedTransform(transform, ros::Time::now( ), "world", turtle_name));
//这里才是执行部分，TransformBroadcaster 发布转换需要四个参数
//1.发布转换本身
//2.给发布的转换一个时间戳，需要用现在的时间标记它，使用ros::Time::now()获取现在的时间
//3.发布父坐标系，例子中是“world”
//4.发布子坐标系，例子中是“turtle_name”
}
int main(int argc, char** argv) //argc表示接收的命令个数，argv表示传入的命令内容
//例如：已经编译好的程序为my.exe，在命令执行my.exe，则：此时argc就是1，接收的参数是1个，即参数argv[0]是“my.exe”
//在执行命令my.exe 1 2 3 则：此时argc就是4个，即参数argv[0]是“my.exe”,参数argv[1]是“1”，参数argv[2]是“2”，参数argv[3]是“3”
{
ros::init(argc, argv, "my_tf_broadcaster");
if (argc != 2){
ROS_ERROR("need turtle name as argument"); return -1;};
//return 如果什么都不接的话，其实就是void类型函数的返回，一般不再执行return后面的语句；
//如果函数执行成功返回0，不成功返回非零，一般情况下非零值用-1表示；
//return 0；一般用在主函数结束时，表示程序正常终止，即告诉系统程序正常。
//return 1或者-1；表示返回一个代数值，一般用在子函数的结尾，表示程序异常终止
turtle_name = argv[1];
//传进来的参数命名前面定义的turtle_name
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
};
二、turtle_tf_listener.cpp
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "my_tf_listener");
ros::NodeHandle node;
ros::service::waitForService("spawn");
ros::ServiceClient add_turtle =
node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);
//创建一个serviceClient即请求服务的客户端
//add_turtle.call(srv),客户端请求spawn服务，生成另一只turtlebot
ros::Publisher turtle_vel =
node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
tf::TransformListener listener;
//创建一个TransformListener 对象
ros::Rate rate(10.0);
while (node.ok())
{
tf::StampedTransform transform;
try
{
listener.lookupTransform("/turtle2", "/turtle1",
ros::Time(0), transform);
//我们使用listener接收特定的转换，其中包含四个参数：
// 1. /turtle1到/turtle2的坐标系转换
// 2. 使用ros::Time(0)得到最新可用的变换
// 3. transform用来存储变换结果
}
catch(tf::TransformException ex)
{
ROS_ERROR("%s", ex.what());
ros::Duration(1.0).sleep();
continue;
//使用try catch语句块来进行判断是否有异常；
//try进行判断，如果有异常进入catch语句，ex.what()返回char数组
//continue:结束本次循环，本次循环continue后的部分不再执行，开始下一次循环判断
//这里注意区分break，break是永久终止循环，break后面的不再执行，while循环也不再执行
}
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2)+
pow(transform.getOrigin().y(), 2));
//transform.getOrigin().x()获取transform中的变换数值，即得到turtle2在turtle1坐标系下的x值；
//transform.getOrigin().y()得到turtle2在turtle1坐标系下的y值；
//这里主要通过到turtle1的距离和角度的变换来计算turtle2的线速度和角速度
turtle_vel.publish(vel_msg);
//新的速度通过turtle2/cmd_cel话题来发布，更新turtle2的位姿
rate.sleep();
}
return 0;
};
三、编译
1.CMakeList.txt
共三处需要做修改：
1.
2.
3.
2.package.xml
共一处需要进行修改：
3.编写launch文件start_demo.launch
四、执行
1.启动launch
//执行
roscore
roslaunch learning_tf start_demo.launch
Launch文件运行以后，可以看到turtle1_tf_broadcaster、turtle2_tf_broadcaster开始进行广播，同时turtlesim仿真节点启动。
2.启动listener
rosrun learning_tf turtle_tf_listener
启动listener节点，listener程序中调用spawn服务产生turtle2，同时进行turtle1、turtle2之间的tf变换，运行过程中捕捉到一次error，即最开始没有正确进行turtle1与turtle2的tf变换，在进入下一次while循环时便能正确进行tf变换。
根据tf变换，给turtle2发布速度指令，产生上图所示的一个追逐过程。
3.启动键盘控制节点
rosrun turtlesim turtle_teleop_key
光标停留在运行键盘控制的终端下，使用上下左右键控制turtle1运动，turtle2会跟随发生变化，如下图所示：
小结
昨天北京迎来了2020年冬季的第一场雪，校园很美；
我觉得冬季更适合一个人静心看书，大家一起加油~