编写一个tf listener

listerner

1 #include <ros/ros.h>
2 #include <tf/transform_listener.h>
3 #include <geometry_msgs/Twist.h>
4 #include <turtlesim/Spawn.h>
5
6 int main(int argc, char** argv){
7   ros::init(argc, argv, "my_tf_listener");
8
9   ros::NodeHandle node;
10
11   ros::service::waitForService("spawn");
12   ros::ServiceClient add_turtle =
13     node.serviceClient<turtlesim::Spawn>("spawn");
14   turtlesim::Spawn srv;
15   add_turtle.call(srv);
16
17   ros::Publisher turtle_vel =
18     node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
19
20   tf::TransformListener listener;
21
22   ros::Rate rate(10.0);
23   while (node.ok()){
24     tf::StampedTransform transform;
25     try{
26       listener.lookupTransform("/turtle2", "/turtle1",
27                                ros::Time(0), transform);
28     }
//

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

29 catch (tf::TransformException &ex) {
30 ROS_ERROR("%s",ex.what());
31 ros::Duration(1.0).sleep();
32 continue;
33 }
34
35 geometry_msgs::Twist vel_msg;
36 vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
37 transform.getOrigin().x());
38 vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
39 pow(transform.getOrigin().y(), 2));
40 turtle_vel.publish(vel_msg);
41
//在这里，变换用于计算turtle2的新的线性和角速度，基于它与turtle1的距离和角度。新的速度在主题“turtle2 / cmd_vel”中发布，sim将使用它来更新turtle2的移动。
42 rate.sleep();
43 }
44 return 0;
45 };

CMakeLists.txt与launch文件里比较类似