ROS 机器人技术

Wesley13
• 阅读 749

上次我们学习了 TF 的基本概念和如何发布静态的 TF 坐标:

这次来总结下如何发布一个自定义的 TF 坐标转换,并监听这个变换。

一、编写 TF 广播者

进入上次创建的 learning_tf2 包中:

roscd learning_tf2

src 下新建一个 turtle_tf2_broadcaster.cpp 文件,代码如下:

#include <ros/ros.h>

// 存储要发布的坐标变换
#include <geometry_msgs/TransformStamped.h>

// 四元数
#include <tf2/LinearMath/Quaternion.h>

// 变换广播者
#include <tf2_ros/transform_broadcaster.h>

// 乌龟的位姿定义
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg) 
{
    // 创建 tf 广播对象
    static tf2_ros::TransformBroadcaster br;

    // 存储要发布的坐标变换消息
    geometry_msgs::TransformStamped transformStamped;

    // 变换的时间戳
    transformStamped.header.stamp = ros::Time::now();
    
    // 父坐标系名称
    transformStamped.header.frame_id = "world";
    
    // 当前要发布的坐标系名称 - 乌龟的名字
    transformStamped.child_frame_id = turtle_name;

    // 乌龟在二维平面运动,所以 z 坐标高度为 0
    transformStamped.transform.translation.x = msg->x;
    transformStamped.transform.translation.y = msg->y;
    transformStamped.transform.translation.z = 0.0;

    // 用四元数存储乌龟的旋转角
    tf2::Quaternion q;

    // 因为乌龟在二维平面运动,只能绕 z 轴旋转,所以 x,y 轴的旋转量为 0
    q.setRPY(0, 0, msg->theta);

    // 把四元数拷贝到要发布的坐标变换中
    transformStamped.transform.rotation.x = q.x();
    transformStamped.transform.rotation.y = q.y();
    transformStamped.transform.rotation.z = q.z();
    transformStamped.transform.rotation.w = q.w();
    
    // 用 tf 广播者把订阅的乌龟位姿发布到 tf 中
    br.sendTransform(transformStamped);
}

int main(int argc, char** argv)
{
    // 当前节点的名称
    ros::init(argc, argv, "my_tf2_broadcaster");
    ros::NodeHandle private_node("~");

    // 判断当前要广播的乌龟节点名字
    if (!private_node.hasParam("turtle")) {
        // launch 文件和命令行都没有传递乌龟名称,就直接退出
        if (argc != 2) {
            ROS_ERROR("need turtle name as argument");
            return -1;
        };

        // launch 文件中如果没有定义乌龟名称,就在命令行中加上
        turtle_name = argv[1];
    } else {
        // 从 launch 文件获取乌龟名称参数
        private_node.getParam("turtle", turtle_name);
    }

    ros::NodeHandle node;

    // 订阅一个节点的 pose msg,在回调函数中广播订阅的位姿消息到 tf2 坐标系统中
    // turtle_name 为 turtle1 时广播 turtle1 的位姿到 tf 中
    // turtle_name 为 turtle2 时广播 turtle2 的位姿到 tf 中
    ros::Subscriber sub = node.subscribe(turtle_name + "/pose", 10, &poseCallback);

    ros::spin();
    return 0;
};

这个程序的意思是订阅输入乌龟的 pose 话题,然后在 poseCallback 回调函数中发布 world 到乌龟的 TF 变换,注意这个程序可以接收不同乌龟的 pose 消息,只要运行时指定乌龟的名称 turtle_name 即可,代码注释很详细,其他的就不说了,然后添加编译规则:

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

直接编译:

catkin_make

基本上不会出问题,为了方便启动我们在 launch 文件中启动广播者:

<launch>
     <!-- 乌龟节点 -->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <!-- 控制乌龟运动的键盘节点 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    
    <!-- 线速度和角速度的定义,但是在这个例子中并没有用到哎... -->
    <param name="scale_linear" value="2" type="double"/>
    <param name="scale_angular" value="2" type="double"/>

    <!-- 第一个乌龟的 tf 广播者节点,参数为乌龟 1 的名字 /tutle1 -->
    <node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle1" name="turtle1_tf2_broadcaster" />
    
    <!-- 第二个乌龟的 tf 广播者节点,还是用相同的节点,只不过改变了传递的参数为乌龟 2 的名字 /turtle2 --> 
    <node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle2" name="turtle2_tf2_broadcaster" />

  </launch>

然后就可以直接启动了:

roslaunch learning_tf2 start_demo.launch

为了确定是否成功广播了变换,使用下面的命令查看一个变换的输出:

rosrun tf tf_echo /world /turtle1

如果在控制台输出类似下面的消息,则说明变换发布成功:

ROS 机器人技术

下面我们来编写一个 TF 接收者来使用我们上面发布的变换。

二、编写 TF 接收者

同样在 src 目录下创建 turtle_tf2_listener.cpp,代码如下:

#include <ros/ros.h>

// 接受 tf 变换
#include <tf2_ros/transform_listener.h>

// 转换消息 
#include <geometry_msgs/TransformStamped.h>

// 发布到乌龟 2 的运动消息:角速度和线速度
#include <geometry_msgs/Twist.h>

// 再生服务
#include <turtlesim/Spawn.h>

// 实现乌龟 2 跟随乌龟 1 运动
int main(int argc, char** argv)
{
    // 当前节点的名字
    ros::init(argc, argv, "my_tf2_listener");

    ros::NodeHandle node;
    ros::service::waitForService("spawn");
    ros::ServiceClient spawner = node.serviceClient<turtlesim::Spawn>("spawn");
    
    turtlesim::Spawn turtle;
    
    turtle.request.x = 4;
    turtle.request.y = 2;
    turtle.request.theta = 0;
    turtle.request.name = "turtle2";
    spawner.call(turtle);

    // 角速度和线速度消息发布者,用来发布计算后的新的速度消息
    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

    // tf 变换缓存区,最多缓存 10 秒
    tf2_ros::Buffer tfBuffer;

    // 创建监听 tf 变换对象,创建完毕即开始监听,通常定义为成员变量
    tf2_ros::TransformListener tfListener(tfBuffer);
    
    ros::Rate rate(10.0);
    while (node.ok()) {
        // 用来保存寻找的坐标变换
        geometry_msgs::TransformStamped transformStamped;
        try{
              // 寻找坐标变换
                transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));
        }
        catch (tf2::TransformException &ex) {
            ROS_WARN("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        // 用来保存角速度和线速度
        geometry_msgs::Twist vel_msg;

        // 新的角速度为寻找到的变换角速度的 4 倍 - 使得第二个乌龟的运动轨迹转弯更快,且轨迹是弧线
        vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y, transformStamped.transform.translation.x);
        
        // 新的线速度是寻找到的变换线速度的 0.5 倍 - 使得第二个乌龟的运动速度为第一个乌龟的一半
        vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) + pow(transformStamped.transform.translation.y, 2));
        
        // 发布新的速度消息,乌龟 2 节点的内部订阅了这个消息,所以乌龟 2 会收到新的角速度和线速度,以此产生跟随运动
        turtle_vel.publish(vel_msg);
      
        rate.sleep();
    }

    return 0;
};

这里关键的代码如下:

// 保存寻找的变换
geometry_msgs::TransformStamped transformStamped;

// 寻找 turtle1 到 turtle2 的坐标变换
// target_frame: turtle2 
// source_frame: turtle1
// ros::Time(0): 获取变换的时间,
transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));

同样添加编译规则:

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

然后编译:

catkin_make

在上面广播者的 launch 文件中加上接收者的启动:

<!-- 
  这个例子一共创建了 5 个节点:
    1. 乌龟节点,包含 2 个小乌龟
    2. 控制乌龟运动的键盘节点
    3. 第一个乌龟的 tf 广播者节点
    4. 第二个乌龟的 tf 广播者节点
    5. tf 坐标系统的监听节点,用来监听 2 个乌龟之间的坐标变换
-->
<launch>
     <!-- 乌龟节点,这个节点的内部应该是创建了 2 个乌龟...... -->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <!-- 控制乌龟运动的键盘节点 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    
    <!-- 线速度和角速度的定义,但是在这个例子中并没有用到哎... -->
    <param name="scale_linear" value="2" type="double"/>
    <param name="scale_angular" value="2" type="double"/>

    <!-- 第一个乌龟的 tf 广播者节点,参数为乌龟 1 的名字 /tutle1 -->
    <node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle1" name="turtle1_tf2_broadcaster" />
    
    <!-- 第二个乌龟的 tf 广播者节点,还是用相同的节点,只不过改变了传递的参数为乌龟 2 的名字 /turtle2 --> 
    <node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle2" name="turtle2_tf2_broadcaster" />

    <!-- 启动 tf 坐标系同的监听节点 -->
    <node pkg="learning_tf2" type="turtle_tf2_listener" name="listener" />

  </launch>

然后启动:

roslaunch learning_tf2 start_demo.launch

运行时会出现 2 个小乌龟,把窗口焦点放到终端,按上下左右键会发现第二个乌龟跟随第一个乌龟运动:

ROS 机器人技术

但是刚启动时终端会报个错误:

[ERROR] [1418082761.220546623]: "turtle2" passed to lookupTransform argument target_frame does not exist.
[ERROR] [1418082761.320422000]: "turtle2" passed to lookupTransform argument target_frame does not exist.

这是因为我们在 turtle2 还没有产生之前就寻找变换,导致没有找到它,为了解决这个问题可以在寻找变换前等待变换可用:

// 第四个参数是阻塞等待的超时时间
listener.waitForTransform("/turtle2", "/turtle1", ros::Time::now(), ros::Duration(3.0));

transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));

加上这句运行时就不会报错了,今天就写到这里,下次见:)

ROS 机器人技术

点赞
收藏
评论区
推荐文章
blmius blmius
3年前
MySQL:[Err] 1292 - Incorrect datetime value: ‘0000-00-00 00:00:00‘ for column ‘CREATE_TIME‘ at row 1
文章目录问题用navicat导入数据时,报错:原因这是因为当前的MySQL不支持datetime为0的情况。解决修改sql\mode:sql\mode:SQLMode定义了MySQL应支持的SQL语法、数据校验等,这样可以更容易地在不同的环境中使用MySQL。全局s
皕杰报表之UUID
​在我们用皕杰报表工具设计填报报表时,如何在新增行里自动增加id呢?能新增整数排序id吗?目前可以在新增行里自动增加id,但只能用uuid函数增加UUID编码,不能新增整数排序id。uuid函数说明:获取一个UUID,可以在填报表中用来创建数据ID语法:uuid()或uuid(sep)参数说明:sep布尔值,生成的uuid中是否包含分隔符'',缺省为
待兔 待兔
4个月前
手写Java HashMap源码
HashMap的使用教程HashMap的使用教程HashMap的使用教程HashMap的使用教程HashMap的使用教程22
Jacquelyn38 Jacquelyn38
3年前
2020年前端实用代码段,为你的工作保驾护航
有空的时候,自己总结了几个代码段,在开发中也经常使用,谢谢。1、使用解构获取json数据let jsonData  id: 1,status: "OK",data: 'a', 'b';let  id, status, data: number   jsonData;console.log(id, status, number )
Wesley13 Wesley13
3年前
ROS
ROS学习笔记01ROS(RobotOperatingSystem,机器人操作系统)提供一系列程序库和工具以帮助软件开发者创建机器人应用软件。它提供了硬件抽象、设备驱动、库函数、可视化、消息传递和软件包管理等诸多功能。ROS遵守BSD开源许可协议。本文内容为安装ROSNoetic并测试简单小乌龟仿真机器人的例子
Stella981 Stella981
3年前
ROS和Gazebo进行机器人仿真(一)
Gazebo是一种多机器人仿真器,可用于室内外机器人仿真。Gazebo在ROS中有良好的接口,包含ROS和Gazebo的所有控制。若要实现ROS到Gazebo的通信,我们必须安装ROSGazebo接口。应该安装以下软件包:$sudoaptinstallrosmelodicgazeborospkgs rosmelodicga
Wesley13 Wesley13
3年前
00:Java简单了解
浅谈Java之概述Java是SUN(StanfordUniversityNetwork),斯坦福大学网络公司)1995年推出的一门高级编程语言。Java是一种面向Internet的编程语言。随着Java技术在web方面的不断成熟,已经成为Web应用程序的首选开发语言。Java是简单易学,完全面向对象,安全可靠,与平台无关的编程语言。
Wesley13 Wesley13
3年前
MySQL部分从库上面因为大量的临时表tmp_table造成慢查询
背景描述Time:20190124T00:08:14.70572408:00User@Host:@Id:Schema:sentrymetaLast_errno:0Killed:0Query_time:0.315758Lock_
暗箭伤人 暗箭伤人
1年前
【www.ithunter.club】 20230922下午
不容易的2023年,我们一起努力【www.ithunter.club】(2023092208:00:00.8872062023092216:00:00.887206)1.人事招聘专员数名(可选远程或入职)2.招聘向坐标东京Yahoo、Shift、L
Python进阶者 Python进阶者
10个月前
Excel中这日期老是出来00:00:00,怎么用Pandas把这个去除
大家好,我是皮皮。一、前言前几天在Python白银交流群【上海新年人】问了一个Pandas数据筛选的问题。问题如下:这日期老是出来00:00:00,怎么把这个去除。二、实现过程后来【论草莓如何成为冻干莓】给了一个思路和代码如下:pd.toexcel之前把这