判断frame是否已创建_大白菜的ROS笔记（8）（创建TF广播和监听，内容很多，细节满满）...

于 2020-11-26 15:03:19 发布
阅读量143 收藏 1
点赞数
文章标签： 判断frame是否已创建

04c2af87970a2294725d1b84d3d20fcf.png
  1. 创建TF广播和监听
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>     需要调用pose话题

std::string turtle_name;    全局变量用于保存命令行或节点参数，注意小写，这里是C++定义字符串，之前话题是消息类型。

void poseCallback(const turtlesim::PoseConstPtr& msg) 对应头文件pose消息
                                                    可以不加&。 PoseConstPtr乌龟程序专用。不加const则&也不能加。
{
    // tf广播器
    static tf::TransformBroadcaster br;

    // 根据乌龟当前的位姿，设置相对于世界坐标系的坐标变换
    tf::Transform transform;                        用于储存相对位置
    transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); Vector3是TF中    定义的数据类型xyz
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transform.setRotation(q);

1bdc5b38db1f012cd89d393ce0b9f53f.png

注意修改格式，原程序中平移和旋转采用了两种格式。上图格式互换。control+点击看类的定义，针对定义的格式来编写，roboware自带查看成员方便。除去源代码的方式还有：

方法1 tf::Vector3 a； 方法2 tf::Vector3 a(msg->x,msg->y,0.0);；

a.setX(msg->x); transform.setOrigin( a );

transform.setOrigin( a );

C++小括号赋值只能在定义的时候（类的对象，构造函数，只能在声明的时候对对象进行初始化赋值,如果不在声明的时候赋值，则只能像方法1，对象调用方法赋值），不能在调用的时候。比如上面transform后面a就是调用。transform.setOrigin( a.setX(msg->x) );是错误的

方法3 transform.setRotation(tf::Quaternion(0, 0, msg->theta));

方法4 tf::Quaternion q(0, 0, msg->theta);

transform.setRotation(q);

同理，都是在声明的时候初始化，如果没有初始化，则只能对q调用setRPY时候赋值。方法3如同平移源代码是tf::Quaternion类省略定义对象构造函数直接对类赋值（C++可以这样写），然后前面setRotation方法调用刚才类中输出的变量。省略RPY是因为看到Quaternion中自动识别输入参数个数，3为RPY,4为四元数。

例如，以下函数具有一个接收 Rectangle 对象的形参：
void displayRectangle(Rectangle r)
{
   cout << "Length = " << r.getLength() << endl;
    cout << "Width = " << r.getWidth() << endl;
    cout << "Area = " << r.getArea() << endl;    '
}

以下代码创建了一个长度为 15 和宽度为 10 的 Rectangle 对象 box，然后将 box 对象作为参数传递给了 displayRectangle 函数：
Rectangle box(15, 10);displayRectangle(box);

则该函数将输出以下信息：Length = 15 。。。。。

类 class A

方式1 A a（赋值） 构造函数，a传给函数参数

方式2 A（赋值），省略对象，无法直接传给函数做参数，需要如下图定义个中

间变量a或者直接在函数中使用这种方式类名非对象传参，如下图和程序中的

vector3等。

方式3 A b b.c(赋值) 不使用构造函数赋值，则只能通过对象调用方法赋值

604e437e1da3d211125790c58a0f43d1.png

// 发布坐标变换，

br.sendTransform

(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

}

br.sendTransform，广播把当前now()时刻坐标变化插入TF树并进行发布，发布消息类型为tf::StampedTransform，其中包括坐标变换，时间戳，源坐标，目标坐标turtle_name同时定义了目标坐标系名字。

注意前面是这个类StampedTransform储存着谁到谁的变换信息，相对数据储存在基类transform（在前面定义tf::Transform transform;）中。然后通过tf::TransformBroadcaster类中的函数sendTransform（）发送到tf中。

int main(int argc, char** argv)
{
    // 初始化节点
    ros::init(argc, argv, "my_tf_broadcaster");
    if (argc != 2)
    {
        ROS_ERROR("need turtle name as argument"); 
        return -1;                                        等效1
    };
    turtle_name = argv[1];                    终端或launch args获取变量名

    // 订阅乌龟的pose信息
    ros::NodeHandle node;
    ros::Subscriber sub 
= node.subscribe(turtle_name+"/pose",10, &poseCallback); 订阅话题名，调用回
调函数，&可不加函数作为参数，turtle_name+"/pose"相当于/turtle1/pose
                                          与前面话题订阅类似，注意，所有订阅没有指明消息类型，发布有。因为订阅在回掉函数中定义了与发布话题消息有关的消息类型。

    ros::spin();

    return 0;
};

TF监听器，TF消息广播后其他节点就可以监听TF消息，从而获取需要的坐标变换。获取turtle2相对于turtle1的变换从而控制turtle2移动。

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>               需要调用twist话题
#include <turtlesim/Spawn.h>                    需要调用spawn服务

int main(int argc, char** argv)
{
    // 初始化节点
    ros::init(argc, argv, "my_tf_listener");

    ros::NodeHandle node;

    // 通过服务调用，产生第二只乌龟turtle2
ros::service::waitForService("spawn"); 等待，直到服务"spawn"出现，在launch中下面启动乌龟节点。

a2bb1aad30d6f6cb007a3df7d4c7470f.png 
。
只有这样才能请求此服务再产生一个乌龟。如果删waitForService,则还没产生spawn
服务就可能直接进行下面的程序，无法获取turtle2数据waitForService是静态函数
只能::调用，不能对象调用。
    ros::ServiceClient add_turtle =
    node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    // 定义turtle2的速度控制发布器
    ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10); 可以加/

    // tf监听器,自动开始接收TF树信息
    tf::TransformListener listener;

//循环频率一秒10次循环。
    ros::Rate rate(10.0);                             
    while (node.ok())                                 可以换成ros::ok()
    {
        tf::StampedTransform transform;               消息类型
        try                                           try-catch结构，捕捉异常，当try里面语句异常时，执行catch
        {
            // 开始在TF树中查找turtle2与turtle1的坐标变换
   listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));
waitForTransform()函数,返回bool值，评估变换是否有效,基本API：
bool tf::TransformListener::waitForTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const 
source_frame target_frame在指定时间time等待坐标变换关系，所以waitForTransform()实际上会阻塞直到两个海龟之间的变换可用（这通常需要几毫秒）或者如果变换不可用,直到达到超时。所以设置 ros::Duration &timeout。Duration(3.0)：为waitForTransform()函数的结束条件：最多等待3秒，如果提前得到了坐标的转换信息，直接结束等待。
listener.lookupTransform("/turtle2", "/turtle1",ros::Time(0), transform);
lookupTransform()是一个更底层的方法用于返回两个坐标系的变换。
这个方法是 tf库的核心方法。大部分transform的方法都是终端用户使用，而这个方法设计在transform()方法内使用的。
void tf::TransformListener::lookupTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const 
在时间time，ros::Time(0)当前时刻也就是最新一次bg的坐标变换，上使用从source_frame到target_frame的变换填充transform，

为什么要讲这个，这是因为 ROS 的 tf 在进行坐标之间的转换变换不是实时的转换，它有一个缓冲器来存放一段时间的坐标转换数据，所以有时，可能没有当前时间的坐标转换数据，使用 lookupTransform() 函数就可以得到你想的某个时间的坐标数据，前提是缓冲区里要有这个时间的坐标数据，tf 的坐标转换是自动计算的，所以有时，你想得到当前的时间的坐标转换数据，你调用 lookupTransform() 函数去获取，但是缓冲器里还没有来得及去计算现在的坐标转换数据，就是说：现在还没有。如果你非要获取，就会出错，所以你要使用 waitForTransform() 函数来等待 tf 的坐标转换线程得到你想要的时间点的坐标转换数据。 
简单的说：waitForTransform() 就是一个安全程序。
Time(0):是tf缓存里的第一个tf信息。 now():是当前这个时间的tf信息。
如果把lookuptransfrom中的Time(0)换成now(),会报错，因为每个监听器有一个缓冲区，它存储来自不同tf广播者的所有坐标变换。 当广播者发出变换时，变换进入缓冲区之前需要一些时间（通常是几个毫秒）。 因此，当您在时间“now”请求坐标系变换时，您应该等待几毫秒以获得该信息

 }
        catch (tf::TransformException &ex) 
        {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        // 根据turtle1和turtle2之间的坐标变换，计算turtle2需要运动的线速度和角速度
        // 并发布速度控制指令，使turtle2向turtle1移动
        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)); pow(x,y)求的是x的y次方; 加上
        turtle_vel.publish(vel_msg);

        rate.sleep();
    }
    return 0;
};

与话题服务一样，也需要配置CMakeList.txt

aea8bb8789b4ade49ac98ad4424697cf.png

1b889d16d3bd523dd2a76834fde76f02.png

b3f36c49a5051b82bead55557e2ba542.png

注意没有add_dependecies没有依赖消息代码。

配置packag.xml

a753921cefb9e67c35733e1b36ba1c92.png

下面我们看看如何为空间新增一个坐标系corrot1，并且计算出turtle2相对于corrot1的位置关系。

注意：

其实我们要增加传感器或者改变原来传感器的位置，就要新增坐标系。tf会处理所引入的额外坐标系，且不会更改原来的坐标系统，tf坐标系树种不允许出现环，只能靠树的关系来建立，所以当你引入新坐标系一定要说明这个坐标的父系是谁和相对父系的坐标数据，然后tf才会处理它，最后可以通过tf库计算出它与任何坐标系的相对位置。这就是tf库的伟大。

最开始的那张图，tf树包含三个坐标系：world，turtle1和turtle2。 两只乌龟是世界的子系。

如果我们要向tf添加一个新坐标系，三个现有坐标系中的一个需要是父系，新坐标系将成为子系。我们将turtle1作为corrot1的父系。

加入新坐标代码：
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");
  ros::NodeHandle node;

  tf::TransformBroadcaster br;//用于广播的一个对象
  tf::Transform transform;//创建一个储存相对位置数据的对象

  ros::Rate rate(10.0);
  while (node.ok()){
    transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );//初始化  相距2米
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
   //广播，指明相对关系和数据
    br.sendTransform(tf::StampedTransform(transform,ros::Time::now(), "turtle1", "carrot1"));
    rate.sleep();
  }
  return 0;
};

是不是发现跟前面的广播代码是一样的，没错就是一样的。这里先创建一个变换对象transform，并且初始化。然后广播出去，指定carrot1的相对于父系turtle1的相对位置是tranform。这样就在坐标空间增加了新坐标carrot1。同时注意，carrot1所有速度位置都是基于turtule1了，后面会有小车例子。

编译，launch文件，然后CMakeList.txt添加文件

77137ac64470aa5cc357bbb3f2aad3f9.png

302b5715cee87c0ec4ba90cbeb8324ca.png

然后运行。你会发现乌龟什么都没改变，你只是广播了新增的点，什么也没干。但是在rviz中可以看到新增的坐标，移动turtle1，carrot1相对于turlte1的位置不变，相对世界点原点一直在变，而turtle2也一直追着turtle1在跑。

现在我们让turtle2跟着corrot1跑 ，下面的duration时间大小自定义。

398e3f5df308e5a79fb80f4fdcb15e95.png

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

在之前的监听中加入ROS_INFO不打印信息，需要在roslaunch中加入output=screen，在终端就不存在这个问题。

ece7fa87fb65f9c2eae7191bcb072905.png

这样运行起来之后，turtle1移动，carrot1相对于turlte1不动，然后turtle2追着corrot1跑。

会出现报错

153d2547826c3fb9b7908f8e04080049.png
  • 这是因为turtle2需要非零时间来生成并开始发布tf帧。 因此，第一次请求现在时间的/turtle2坐标系可能不存在，当请求转换时，转换可能不存在，并且第一次失败。 在第一次变换之后，所有的变换都存在，并且乌龟的行为如预期的那样。

检查结果

  • 现在，你应该能够使用箭头键（确保你的终端窗口是活跃的，而不是你的模拟器窗口），你会看到第二只乌龟跟随第一只乌龟！
  • 所以，你注意到乌龟的行为没有明显的区别。 这是因为实际的时间差只有几个毫秒。 但是为什么我们从Time(0)到now()进行这种改变？ 只是教你关于tf缓冲区和与它相关的时间延迟。对于真实的tf用例，使用Time(0)通常是完全正常的。

下面再说一个随着时间的移动，turtle2绕着turtle1绕圈。很简单，只要改变turlte1与carrot1的相对距离，这个相对距离随时间变化,carrot1绕着turtle1,turtle2追着carrot1，直观上自然就绕着turtle1绕圈了。

于是我们更改carrot1跟turtle1的距离：

transform.setOrigin(tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) );

transform.setRotation( tf::Quaternion(0, 0, 0, 1) );

看到没，对时间用正余弦然后当成距离。c1绕着t1转圈，t2追着c1，于是就形成了左边的乌龟轨迹，乌龟1不动，乌龟2绕着乌龟1转圈圈。此时我们还可以移动t1，轨迹就跟复杂了。

weixin_39988164
关注
  • 0
    点赞
  • 踩
  • 1
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值