Gmapping从开始到放弃—写一个TF 监听

这篇文章主要 记录如何监听一个TF广播，通过监听tf，我们可以避免繁琐的旋转矩阵的计算，而直接获取我们需要的相关信息.当然也是接着上一篇文章创建的开发包继续走下去
（1）在my_tf文件下的src下新建一个文件命名turtle_tf_listener.cpp.
添加代码如下

#include <ros/ros.h>
#include <tf/transform_listener.h>  //transformListener是接受tf消息的头文件 
#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);

  ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);  //定义发布的消息的类型

  tf::TransformListener listener; //定义监听器

  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;//定义存放变换关系的变量
    try{
    listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(10.0) );//等待
      listener.lookupTransform("/turtle2", "/turtle1",
                               ros::Time(0), transform);//参数分别是destination_frame, original_frame,time,transform.我们是想把/turtle1的坐标信息转换位turtle2 。transform 这里是用于存储最后总的结果
    }
    catch (tf::TransformException &ex) {   //出现异常的处理
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }
//上面存储在transform中的turtle1的距离和角度的信息，用于计算turtle2De 线速度和角速度，这个新的计算结果发布在“turtle2/cmd_vel”用来更型turtle2的运动
    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;
};

分析：由于tf的会把监听的内容存放到一个缓存中，再读取内容，而这个过程可能会有几毫秒的延迟，所以，tf的监听器并不能立即监听到“现在”的变换，所以如果不使用try,catch函数会导致报错：
不可以把ros::Time(0)改成ros::time::now()，因为监听做不到实时，会有几毫秒的延迟。ros::Time(0)指最近时刻存储的数据，ros::time::now()则指当下，如果非要使用ros::time::now,则需要结合waitForTransform()使用，具体见:
http://wiki.ros.org/tf/Tutorials/tf%20and%20Time%20%28C++%29
实际中ros::Time(0)大多数情况下可以满足要求。

“world” passed to lookupTransform argument target_frame does not exist. ”

（2）在CMakeList.txt中添加我们刚刚新建的文件，需要编译的文件

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

（3）catkin_make 编译
（4）为了查看效果在之前的.launch文件中添加下面的一行

<node pkg="my_tf" type="turtle_tf_listener"
          name="listener" />

（5）运行roslaunch my_tf start_demo.launch

具体的解释在代码中也是有给出了，所以我们直接运行结果查看

可以发现我们在按移动turtle1的时候turtle2也会跟在后面走，那是因为turtle2监听了turtle1转换到turtle2的TF 消息了，可以查看下一
rosrun tf tf_echo /turtle1 /turtle2

不过好像并看不出来什么,但是我们可以从代码中可以明白一点什么