使用tf库：编写tf监听器

最新推荐文章于 2024-07-06 21:29:07 发布

退休码农飞伯德

最新推荐文章于 2024-07-06 21:29:07 发布

阅读量1.7k

点赞数 1

分类专栏： ROS机器人操作系统 ROS学习之路文章标签： ROS

本文链接：https://blog.csdn.net/github_30605157/article/details/73658540

版权

ROS机器人操作系统同时被 2 个专栏收录

23 篇文章 14 订阅

订阅专栏

ROS学习之路

11 篇文章 4 订阅

订阅专栏

本文主要内容参考自ROS wiki:Writing a tf listener (C++)，在加入了自己的一些理解的同时，我也对原文进行了适当的修改。原文使用Creative Commons Attribution 3.0，本文使用知识共享署名-非商业性使用-相同方式共享 4.0 国际许可协议协议，若使用本文，请自觉遵守相关协议。

上一个教程中，我们创建了一个tf广播器来发布乌龟的位姿信息，这篇文章我们来学习下如何创建tf监听器。该节点的主要任务是监听tf中turtle2到turtle1转换，然后将转换信息转化为速度信息发送给turtle2。

使用到的库

ros基础库(ros/ros.h)就不用提了，必须要添加进来；其次就是tf中的transform_listener.h，这里也不多做说明。由于我们需要将监听到的消息转换为速度消息，因此我们需要geometry_msgs/Twist消息的头文件；另外，我们需要在同一窗口中再生成一个乌龟turtle2，而turtlesim功能包中spawn服务提供了此项功能，因此我们需要spawn服务的头文件。

note:和消息类似，关于服务的创建过程也可以在ROS基础教程中找到：Creating a ROS msg and srv。另外，在ROS初级教程中，我们也曾使用过spawn服务，只不过是使用相应的命令和参数，具体可以参考：Understanding ROS Services and Parameters当然，如果你希望了解spawn(再生)服务的详细信息的话，你可以参考这里：Spawn.h。

调用spawn服务

ros中的基础工作（如初始化、创建节点句柄等），我们就不介绍了。下面介绍下如何调用spawn服务，建议如果有不懂的地方可以查看ros的基础教程：Writing a Simple Service and Client (C++)。

首先等待spawn服务，直到它启动起来；然后创建spawn服务的客户端，以及spawn服务。

ros::service::waitForService("spawn");
ros::ServiceClient add_turtle =
    node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv;

然后使用客户端调用服务即可：

add_turtle.call(srv);

创建消息发布器

使用下面的命令即可创建消息发布器，这里我们设置消息的类型为geometry_msgs::Twist，发布主题为"turtle2/cmd_vel"，缓冲区的大小设置为10；

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

监听tf

首先，我们需要创建一个监听器用于监听是否存在需要的tf：

tf::TransformListener listener;

然后，我们使用一个循环，在循环中，我们主要完成以下工作：

监听器查找特定的tf
根据查找的tf信息计算相应的速度信息
将速度信息发布给turtle2

// 发布频率为10Hz
ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
    try{
      // 查找特定的tf
      listener.lookupTransform("/turtle2", "/turtle1",
                               ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }
    // 根据tf计算速度信息
    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));
    // 发布速度信息给turtle2                             
    turtle_vel.publish(vel_msg);

    rate.sleep();
  }

其中，lookupTransform函数，我们需要详细说明一下。前两个参数是指定从arg1到arg2的tf的，我们这里查找的是从turtle2到turtle1的转换；第三个参数是指定tf的时间的，我们这里使用的是ros::Time(0)，这样的话，我们可以获取到最近存在的tf；最后一个参数用来保存查找到的tf。

编译

打开learning_tf功能包下的CMakeList.txt文件，然后在其中添加一下代码：

add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

然后，进入你的工作空间编译即可：

cd %YOUR_CATKIN_WORKSPACE_HOME%/
catkin_make

运行

我们仅仅需要在上一篇文章中的launch文件中添加两行代码即可：

  <launch>
    ...
    <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle2" name="turtle2_tf_broadcaster" />
    <node pkg="learning_tf" type="turtle_tf_listener" 
          name="listener" />
  </launch>