编写一个tf listener

来源：互联网发布：邮币卡交易平台软件编辑：程序博客网时间：2024/06/03 16:33
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文件里比较类似
阅读全文
0 0