ROS学习之tf.3编写一个tf接收器（C++）

ROS.tf3编写一个tf接收器（C++）

声明：本教程来自于ROS.WIKI，本人在整理过程中可能出现一些错误和问题，有问题可以去查看官网版本也可以咨询本人

在之前的教程中，我们创建了一个tf广播公司向tf发布一只乌龟的姿势。
在本教程中，我们将创建一个tf接收器来开始使用tf。

1.1如何创建一个tf接收器
我们先创建源文件。
转到我们在上一个教程中创建的包：

 $ roscd learning_tf

1.2启动您的编辑器并将以下代码粘贴到名为src
/ turtle_tf_listener.cpp的新文件中。

#include 

#include 

#include 

#include 

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("spawn"); 

 turtlesim::Spawn srv; 

 add_turtle.call(srv); 

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

 tf::TransformListener listener; 

 ros::Rate rate(10.0); 

 while(node.ok()) 

 { 

  tf::StampedTransform transform; 

  try 

  { 

  
listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),transform);

  } 

  catch(tf::TransformException &ex) 

  { 

   ROS_ERROR("%s",ex.what()); 

   ros::Duration(1.0).sleep(); 

   continue; 

  } 

  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));

  turtle_vel.publish(vel_msg); 

  rate.sleep(); 

 } 

 return 0; 

}

如果在运行时出现错误“Lookup
would require extrapolation into the past”，则可以尝试使用此替代代码来调用侦听器：

try {

   
listener.waitForTransform(destination_frame, original_frame,
ros::Time(0), ros::Duration(10.0) );

   
listener.lookupTransform(destination_frame, original_frame,
ros::Time(0), transform);

} catch
(tf::TransformException ex) {

   
ROS_ERROR("%s",ex.what());

}

1.3代码解析

（1）
 #include 

tf软件包提供了一个TransformListener的实现来帮助简化接收转换的任务。
要使用TransformListener，我们需要包含tf
/ transform_listener.h头文件。
（2）
tf::TransformListener listener;

在这里，我们创建一个TransformListener对象。一旦创建了监听器，它就开始接收通过线路进行的tf转换，并缓冲它们长达10秒。TransformListener对象的范围应该保持不变，否则它的缓存将无法填充，几乎每个查询都会失败。一种常见的方法是使TransformListener对象成为类的成员变量。

（3）try

     {

      listener.lookupTransform("/turtle2", "/turtle1",

      ros::Time(0), transform);

     }

在这里，真正的工作完成了，我们向侦听器查询特定的转换。我们来看看这四个参数：
我们希望从frame/ turtle1到frame/ turtle2的转换。
我们想要改变的时间。提供ros ::Time（0）只会为我们提供最新的可用转换。
我们存储结果变换的对象。

（4）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的新的线性和角速度，基于它与turtle1的距离和角度。新速度发布在主题“turtle2/ cmd_vel”中，sim将使用它来更新turtle2的移动。

2.1运行侦听器

现在我们创建了代码，让我们先编译它。打开CMakeLists.txt文件，并在底部添加以下行：

add_executable(turtle_tf_listener
src/turtle_tf_listener.cpp)

target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

进入您的工作空间编译

 $ catkin_make

如果一切顺利，你的devel/ lib /learning_tf文件夹中应该有一个名为turtle_tf_listener的二进制文件。
如果是这样，我们已经准备好添加这个演示的启动文件。在您的文本编辑器中，打开名为start_demo.launch的启动文件，然后在块内合并下面的节点块：

 
 ...
 
 

首先，确保你停止了前一篇教程中的启动文件（使用ctrl-c）。现在您已准备好开始您的完整海龟演示：

 $ roslaunch learning_tf start_demo.launch

要想看看事情是否有效，只需使用箭头键在第一只乌龟周围驾驶（确保您的终端窗口处于活动状态，而不是您的模拟器窗口），并且您会在第一只乌龟之后看到第一只乌龟！

当turtlesim启动时，您可能会看到：
[ERROR]
1253915565.300572000: Frame id /turtle2 does not exist! When trying
to transform between /turtle1 and /turtle2.

[ERROR]
1253915565.401172000: Frame id /turtle2 does not exist! When trying
to transform between /turtle1 and /turtle2.

发生这种情况是因为我们的听众在收到关于龟2的消息之前正在计算变换，因为它需要一点时间才能在turtlesim中产卵并开始广播一个tf帧。

现在您已准备好转到下一个教程，您将在其中学习如何添加框架（C++）