ROS TF坐标变换 - 静态坐标变换

目录

  • 一、静态坐标变换(C++实现)
  • 二、静态坐标变换(Python实现)

如前文所属,ROS通过广播的形式告知各模块的位姿关系,接下来详述这一机制的代码实现。

模块间的位置关系有两种类型,一种是相对固定的,称为静态坐标变换,一种是相对不固定,变化的,称为动态坐标变换。

一、静态坐标变换(C++实现)

所谓静态坐标变换,是指两个坐标系之间的相对位置是固定的。比如机器人底盘上安装了一个激光雷达,他和底盘组成一个刚体,它们的相对位姿不会随机器人的运动而变化,他们之间的坐标变换即属于静态坐标变换。

假设激光雷达相对与底盘的欧拉位姿为(0.5, 0.0, 0.3; 0.0, 0.0, 0.0)

雷达检测到的障碍物位置为(2.0, 2.5, 0.3)

若要计算障碍物和底盘的相对位置,就可以通过雷达到底盘的坐标变换来计算,步骤如下:

  1. 雷达(laser)发布自己和底盘(base_link)的相对静态坐标
  2. 避障模块监听雷达(laser)和底盘(base_link)的相对坐标关系,并通过tf 计算障碍物位置。

首先创建 tf2_learning 包,命令如下:(这一步不是必须,这里只是为了方便清晰的说明,也可以使用已有的包,在包里新增节点等方法)

catkin_creat_pkg tf2_learning roscpp rospy geometry_msgs std_msgs tf2 tf2_geometry_msgs tf2_ros

创建后,文件结构如下:

ROS TF坐标变换 - 静态坐标变换_第1张图片

在创建的 tf2_learning 包路径下有一个 src 目录,在这里存储C++源码,我们创建 static_frame_broadcast.cppstatic_frame_listen.cpp ,修改 CMakeLists.txt ,添加如下内容:

add_executable(${PROJECT_NAME}_broadcast src/static_frame_broadcast.cpp)
add_executable(${PROJECT_NAME}_listen src/static_frame_listen.cpp)

target_link_libraries(${PROJECT_NAME}_broadcast
  ${catkin_LIBRARIES}
)

target_link_libraries(${PROJECT_NAME}_listen
  ${catkin_LIBRARIES}
)

static_frame_broadcast.cpp 实现广播子坐标系相对于父坐标系的静态坐标,内容如下:

#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

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

    // 创建静态坐标转换广播器
    tf2_ros::StaticTransformBroadcaster broadcaster;

    // 创建坐标系信息
    geometry_msgs::TransformStamped ts;
    // --设置头信息
    ts.header.seq = 100;
    ts.header.stamp = ros::Time::now();
    ts.header.frame_id = "base_link";
    // --设置子级坐标系
    ts.child_frame_id = "laser";
    // --设置子坐标系相对于父坐标系的平移偏移量
    ts.transform.translation.x = 0.5;
    ts.transform.translation.y = 0.0;
    ts.transform.translation.z = 0.3;
    // --设置子坐标系相对于父坐标系的旋转偏移量
    // --将欧拉角转换成四元数
    tf2::Quaternion qtn; // tf2的四元数类
    qtn.setRPY(0, 0, 0); // 设置欧拉角
    // 获取旋转的四元数值
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();

    // 广播器发布坐标系信息
    broadcaster.sendTransform(ts);

    ros::spin();

    return 0;
}

static_frame_listen.cpp 实现订阅静态坐标转换关系,并利用该关系将雷达坐标系的点转换到 base_link 坐标系,内容如下:

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

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

    ros::NodeHandle nh;

    // 创建 TF 订阅节点
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate rate(1);
    while (ros::ok())
    {
        // 生成一个坐标点, 模拟雷达检测到的障碍物坐标点(雷达坐标系下的坐标)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "laser";
        point_laser.header.stamp = ros::Time::now();
        point_laser.point.x = 2.0;
        point_laser.point.y = 2.5;
        point_laser.point.z = 0.3;

        // 转换坐标点, 计算障碍物坐标点在 base_link 下的坐标
        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser, "base_link");
            ROS_INFO("point_base: (%.2f, %.2f, %.2f), frame: %s",
                     point_base.point.x, point_base.point.y, point_base.point.z,
                     point_base.header.frame_id.c_str());
        }
        catch (const std::exception &e)
        {
            ROS_ERROR("%s", e.what());
        }

        rate.sleep();
        ros::spinOnce();
    }

    return 0;
}

编译后,执行 rosrun tf2_learning tf2_learning_broadcast 开始广播坐标,此时打开rviz订阅TF看到TF树模型,操作与结果如下:

  • 输入命令:rviz
  • 在启动的 rviz 中设置 Fixed Framebase_link
  • 点击左下的 Add 按钮,在弹出的窗口中选择 TF 组件,即可显示坐标关系。

ROS TF坐标变换 - 静态坐标变换_第2张图片

继续执行命令rosrun tf2_learning tf2_learning_listen可以看到转换后的坐标,以及所属父坐标系,如下:

ROS TF坐标变换 - 静态坐标变换_第3张图片

其中,ERROR是由于节点刚起来时,TF数据还未来得及写入缓存,导致base_link不存在,可以发现第二次调用就没有报错了,实际使用中,可以等待要操作的frame存在再做转换,如下:

tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
// _frameExists()返回指定frame是否存在于tf树中
if (!buffer._frameExists("base_link"))
{
    ROS_WARN("base_link frame does not exist.");
}

二、静态坐标变换(Python实现)

在创建的 tf2_learning 包路径下 src 目录的同级,创建一个 scripts 目录,在这里存储脚本(如python脚本),我们创建 tf2_learning_broadcast.py 以实现坐标广播,编辑内容如下:

#! /usr/bin/env python

import rospy
import tf
import tf2_ros
from geometry_msgs.msg import TransformStamped


if __name__ == "__main__":

    # 初始化 ROS 节点
    rospy.init_node("static_frame_broadcast_py")

    # 创建静态坐标广播器
    broadcaster = tf2_ros.StaticTransformBroadcaster()

    # 创建并组织被广播的消息
    tfs = TransformStamped()
    # -- 头信息
    tfs.header.frame_id = "base_link" # 父坐标系
    tfs.header.stamp = rospy.Time.now()
    tfs.header.seq = 101
    # -- 子坐标系
    tfs.child_frame_id = "laser"
    # -- 坐标系相对信息
    # ---- 相对于父坐标系的平移偏移量
    tfs.transform.translation.x = 0.5
    tfs.transform.translation.y = 0.0
    tfs.transform.translation.z = 0.3
    # ---- 相对于父坐标系的旋转偏移量
    # ---- 设置欧拉角,并将欧拉角转换成四元数
    qtn = tf.transformations.quaternion_from_euler(0, 0, 0)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]

    # 广播器发送消息
    broadcaster.sendTransform(tfs)

    # spin
    rospy.spin()

创建 tf2_learning_listen.py 以订阅静态坐标转换关系,并利用该关系将雷达坐标系的点转换到 base_link 坐标系,编辑内容如下:

#! /usr/bin/env python

import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped

if __name__ == "__main__":
    # 初始化 ROS 节点
    rospy.init_node("static_frame_listen")
    # 创建 TF 订阅对象
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        # 生成一个坐标点, 模拟雷达检测到的障碍物坐标点(雷达坐标系下的坐标)
        point_laser = PointStamped()
        point_laser.header.frame_id = "laser"
        point_laser.header.stamp = rospy.Time.now()
        point_laser.point.x = 2.0
        point_laser.point.y = 2.5
        point_laser.point.z = 0.3

        try:
            # 转换坐标点, 计算障碍物坐标点在 base_link 下的坐标
            point_base = buffer.transform(point_laser, "base_link")
            rospy.loginfo("point_base: (%.2f, %.2f, %.2f), frame: %s",
                          point_base.point.x,
                          point_base.point.y,
                          point_base.point.z,
                          point_base.header.frame_id)
        except Exception as e:
            rospy.logerr("%s", e)

        # spin
        rate.sleep()

你可能感兴趣的:(ROS,ROS,TF2,机器人,静态坐标变换,C++,Python,坐标系变换)