ROS学习之路———C++编写tf2广播器及侦听器

2021-04-18

tf2 简介

tf2是转换库的第二代，可让用户随时间跟踪多个坐标系。tf2保持时间缓冲的树结构中的坐标系之间的关系，并允许用户在任意所需的时间点在任意两个坐标系之间转换点，向量等，任何用户基本上都会使用tf2进行两项任务，即监听转换和广播转换。

tf2

编写tf2广播器

广播转换：将坐标系的相对姿势发送到系统的其余部分。一个系统可以有许多广播公司，每个广播公司都提供有关机器人不同部分的信息

创建一个learning_tf2软件包
首先，我们将创建一个catkin包，该包将用于本教程和后续教程。这个名为learning_tf2的软件包将取决于 tf2，tf2_ros，roscpp，rospy和turtlesim

catkin_create_pkg learning_tf2 tf tf2_ros roscpp rospy turtlesim
roscd learning_tf2 t

将乌龟姿势发布到tf2的相关代码
创建一个名为src /的文件夹，并启动您喜欢的编辑器，将以下代码粘贴到一个名为src / turtle_tf2_broadcaster.cpp的新文件中

#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
//tf2软件包提供了TransformBroadcaster的实现，以帮助简化发布转换的任务
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallbacks(const turtlesim::PoseConstPtr &msg)
{
    //在这里，我们创建一个TransformBroadcaster对象br，稍后将使用它通过网络发送转换
    static tf2_ros::TransformBroadcaster br;
    //在这里，我们创建一个Transform对象，并稍后为其提供适当的元数据
    geometry_msgs::TransformStamped transformStamped;

    //我们需要给要发布的转换加上时间戳，我们只需将其标记为当前时间ros::Time::now()即可
    transformStamped.header.stamp = ros::Time::now();
    //然后，我们需要设置要创建的链接的父框架的名称，在本例中为“ world”
    transformStamped.header.frame_id = "world";
    //最后，我们需要设置要创建的链接的子节点的名称，在这种情况下，这就是乌龟本身的名称
    transformStamped.child_frame_id = turtle_name;

    //在这里，我们将信息从3D乌龟姿势复制到3D变换中
    transformStamped.transform.translation.x = msg->x;
    transformStamped.transform.translation.y = msg->y;
    transformStamped.transform.translation.z = 0.0;
    tf2::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transformStamped.transform.rotation.x = q.x();
    transformStamped.transform.rotation.y = q.y();
    transformStamped.transform.rotation.z = q.z();
    transformStamped.transform.rotation.w = q.w();

    //这是完成实际工作的地方。使用TransformBroadcaster发送转换仅需要传递转换本身
    //sendTransform和StampedTransform的父级和子级顺序相反
    br.sendTrannsform(transformStamped);
}

int main(int argc, char **argv)
{
    ros::intt(argc, argv, "my_tf2_broadcaster");
    ros::NodeHandle private_node("~");

    if (!private_node.hasParam("turtle"))
    {
        if (argc != 2)
        {
            ROS_ERROR("need turtle name as argument");
            return -1;
        };
        turtle_name = argv[1];
    }
    else
    {
        private_node.getParam("turtle", turtle_name);
    }

    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name + "/pose", 10, $poseCallbacks);

    ros::spin();
    return 0;
}

修改CMakeList.txt文件
打开CMakeLists.txt文件，并添加以下内容：

add_executable（turtle_tf2_broadcaster src / turtle_tf2_broadcaster.cpp）
target_link_libraries（turtle_tf2_broadcaster
 $ {catkin_LIBRARIES}
）

编译

catkin_make

成功后可以在lib 文件中获得名为 turtle_tf2_broadcaster 的二进制文件

创建启动文件start_demo.launch

<启动>
    <！-Turtlesim节点->
   <node pkg =“ turtlesim” type =“ turtlesim_node” name =“ sim” />

   <node pkg =“ turtlesim” type =“ turtle_teleop_key” name =“ teleop” output =“ screen” />
   <！-轴->
   <param name =“ scale_linear” value =“ 2” type =“ double” />
   <param name =“ scale_angular” value =“ 2” type =“ double” />

   <node pkg =“ learning_tf2” type =“ turtle_tf2_broadcaster”
         args =“ / turtle1” name =“ turtle1_tf2_broadcaster” />
   <node pkg =“ learning_tf2” type =“ turtle_tf2_broadcaster”
         args =“ / turtle2” name =“ turtle2_tf2_broadcaster” />

 </ launch>

修改CMakeList.txt文件
在文件中的install 区域内将启动文件标记为安装文件
1
2
3
4
start_demo.launch
＃myfile2
DESTINATION $ {CATKIN_PACKAGE_SHARE_DESTINATION}
）
开始乌龟广播演示

roslaunch learning_tf2 start_demo.launch
结果显示一直乌龟在turtlesim窗口中
检查结果
使用ts_echo 工具检查乌龟姿势是否确实正在广播到tf2，在启动广播界面使用键盘方向键更改乌龟位置，查看乌龟姿势中个位置是否发生变化

编写tf2侦听器

监听装换：接收和缓冲系统中广播的所有坐标帧，并查询帧之间的特定变换
接下来就是实现如何使用tf2访问帧转换

创建tf2帧听器
首先在之前创建的learning_tf2 文件中创建 /src/ turtle_tf2_listener.cpp 文件，并完成将乌龟姿势发布到tf2中的代码：

#include <ros/ros.h>
//tf2软件包提供了TransformListener的实现，以帮助简化接收转换的任务。
//要使用TransformListener,我们需要包括tf2 / transform_listener.h头文件
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "my_tf2_listener");

    ros::NodeHandle node;

    ros::service::waitForServer("spawn");
    ros::ServiceClient spawner = node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn turtle;
    turtle.request.x = 4;
    turtle.request.y = 2;
    turtle.request.thera = 0;
    turtle.request.name = "turtle2";
    spawner.call(turtle);

    //发布话题Turtle2/cmd_vel
    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

    //在这里，我们创建一个TransformListener类监听对象,创建侦听器后，它将开始通过网络接收tf2转换
    tf2_ros::Buffer tfBuffer;
    tf2_ros::TransformListener tfListener(tfBuffer);

    ros::Rate rate(10.0);

    while (node.ok())
    {
        //创建一个StampedTransform对象存储变换结果数据
        geometry_msgs::TransformStamped transformStamped;
        //监听包装在一个try-catch块中以捕获可能的异常
        try
        {
            //向侦听器查询特定的转换，(想得到/turtle1到/turtle2的变换)，想要转换的时间ros::Time(0)提供了最新的可用转换
            transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));
        }
        catch (tf2::TransformException &ex)
        {
            ROS_WARN("%s", ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        //在这里，变换使用turtle1的距离和角度，为turtle2计算新的线和角速度
        geometry_msgs::Twist vel_msg;

        vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y, transformStamped.transform.translation.x);
        vel_msg.angular.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) + pow(transformStamped.transform.translation.y, 2));
        //发布话题Turtle2/cmd_vel，用来更新Turtle2的运动
        turtle_vel.publish(vel_msg);

        rate.sleep();
    }
    return 0;
}

运行监听器
现在我们已经创建了代码，让我们首先对其进行编译。打开CMakeLists.txt文件并插入：

add_executable（turtle_tf2_listener src / turtle_tf2_listener.cpp）
target_link_libraries（turtle_tf2_listener
 $ {catkin_LIBRARIES}
）

在已经创建的 /launch/start_demo.launch 的启动文件中添加下面的节点快信息到< launch >块内：

1 2	<node pkg =“ learning_tf2” type =“ turtle_tf2_listener” name =“ listener” />

检查结果

编译并开始乌龟演示

catkin_make
roslaunch learning_tf2 start_demo.launch

如果一切顺利则会有两个乌龟在turtlesim界面，在启动终端界面使用键盘方向键移动turtle1，会发现turtle2始终看行turtle1，并随着turtle1的移动二改变方向