ROS

2024-07-14 08:12| 来源: 网络整理| 查看: 265

ROS | TF坐标系变换的编程实现 1. 创建功能包2. 节点编程2.1 案例说明2.2 TF坐标系广播器编程2.3 TF坐标系监听者编程 3. 配置与编译3.1 在CMaKeLists.txt中添加编译选项3.2 编译文件 4. 话题可视化

1. 创建功能包

在ROS工作空间ROS_ws的src文件夹目录下创建一个功能包，命名为tf_lidar_task，并编译完成。在这里插入图片描述

2. 节点编程 2.1 案例说明

广播并监听机器人的坐标变换，已知激光雷达和机器人底盘的坐标关系，求解激光雷达数据在底盘坐标系下的坐标值。在这里插入图片描述

2.2 TF坐标系广播器编程

在功能包下面的src文件夹目录下创建一个空文件lidar_broadcaster.cpp，并打开所创建的文件，输入以下代码。

#include #include int main(int argc, char** argv) { //初始化ROS节点 ros::init(argc, argv, "robot_tf_publisher"); //创建节点句柄 ros::NodeHandle n; //设置TF广播频率为100Hz ros::Rate r(100); //创建tf::TransformBroadcaster类的对象 tf::TransformBroadcaster broadcaster; while(n.ok()) { //广播TF坐标系变换关系到ROS系统中 broadcaster.sendTransform( tf::StampedTransform( tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)), ros::Time::now(), "base_link", "base_laser")); r.sleep(); } }

说明：

头文件ros/ros.h包含了标准ROS类的声明，在每一个ROS程序中都需要包含它。头文件tf/transform_broadcaster.h用来完成TF树的广播，之后会使用到tf::TransformBroadcaster类的实例。main函数中一开始都是类似的，初始化ROS节点，创建节点句柄，从而启动ROS节点。ros::Rate r(100);的作用是设置TF坐标系广播频率为100Hz。tf::TransformBroadcaster broadcaster;的作用是创建一个tf::TransformBroadcaster类的对象，用来广播坐标系base_link -> base_laser的变换关系。broadcaster.sendTransform(tf::StampedTransform())的作用是通过tf::TransformBroadcaster类调用sendTransform()函数，向系统中广播参考系之间的坐标变换关系，其中所广播的变换关系的数据类型是tf::StampedTransform，它包含了四个参数。第一个参数是存储坐标系之间变换关系的变量，此处tf::Transform是tf::StampedTransform的父类，它包含了两个参数。第一个参数表示坐标的旋转变换，通过tf::Quaternion四元数来存储旋转变换的参数，我们用到的两个参考系之间没有发生旋转变换，因此倾斜角、滚动角和偏航角都是0。第二个参数表示坐标的位移变换，这里我们将base_link作为父节点，base_laser作为子节点，从base_link节点到base_laser节点的变换关系为(x: 0.1m, y: 0.0m, z: 0.2m)，将位移量填入到Vector3向量中。第二个参数是广播TF变换关系的时间戳，这里使用ros::Time::now()表示当前时间。第三个参数是传递的父节点坐标系的名称，即base_link。第四个参数是传递的子节点坐标系的名称，即base_laser。 tf::StampedTransform及tf::Transform类的定义： http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1StampedTransform.html http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1Transform.html r.sleep();的作用是为了延时，保证以100Hz的频率广播TF变换关系。

相关资料：http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF

2.3 TF坐标系监听者编程

在功能包下面的src文件夹目录下创建一个空文件lidar_listener.cpp，并打开所创建的文件，输入以下代码。

#include #include #include //当接收到TF消息时，自动调用该回调函数 void transformPoint(const tf::TransformListener& listener) { geometry_msgs::PointStamped laser_point; laser_point.header.frame_id = "base_laser"; laser_point.header.stamp = ros::Time(); laser_point.point.x = 1.0; laser_point.point.y = 0.2; laser_point.point.z = 0.0; try { geometry_msgs::PointStamped base_point; listener.transformPoint("base_link", laser_point, base_point); ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f", laser_point.point.x, laser_point.point.y, laser_point.point.z, base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec()); } catch(tf::TransformException& ex) { ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what()); } } int main(int argc, char** argv) { ros::init(argc, argv, "robot_tf_listener"); ros::NodeHandle n; tf::TransformListener listener(ros::Duration(10)); ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener))); ros::spin(); }

说明：

头文件ros/ros.h包含了标准ROS类的声明，在每一个ROS程序中都需要包含它。头文件geometry_msgs/PointStamped.h包含了坐标系和时间戳等信息，之后会使用geometry_msgs::PointStamped类创建一个虚拟点。头文件tf/transform_listener.h用于创建TF变换的监听者，之后会使用到tf::TransformListener类的对象，该对象会自动订阅ROS中的TF消息，并且管理所有的变换关系数据。transformPoint(const tf::TransformListener& listener)该回调函数每当接收到TF消息时，都会被自动调用。在回调函数中，我们需要完成数据从坐标系base_laser到base_link的坐标变换。 geometry_msgs::PointStamped laser_point; laser_point.header.frame_id = "base_laser"; laser_point.header.stamp = ros::Time(); laser_point.point.x = 1.0; laser_point.point.y = 0.2; laser_point.point.z = 0.0; 这里创建了一个geometry_msgs::PointStamped类型的虚拟点，该类型包含标准的header和point两种消息结构，因此可以在消息中添加发布数据的时间戳、参考系的ID以及该点的坐标。其中ros::Time()表示使用缓冲中最新的TF数据，这里不使用ros::Time::now()是因为每个监听器都有一个缓冲区，存储来自不同TF广播的所有坐标变换。当广播器发送TF变换时，数据进入缓冲区需要一段时间（通常是几毫秒）。因此如果在当前时刻就请求TF变换数据，缓冲区中还提取不到当前发布的变换，您应该等待一段时间，直到数据到达。 PointStamped的消息定义： http://docs.ros.org/api/geometry_msgs/html/msg/PointStamped.html try { geometry_msgs::PointStamped base_point; listener.transformPoint("base_link", laser_point, base_point); ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f", laser_point.point.x, laser_point.point.y, laser_point.point.z, base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec()); }

这里使用tf::TransformListener类的对象中的transformPoint()函数，将虚拟点的坐标数据从base_laser参考系转换到base_link参考系下，该函数包含三个参数。

第一个参数是需要转换到的参考系ID，即base_link。第二个参数是需要转换的原始数据，即上面创建的虚拟点的信息。第三个参数是存储TF变换完成后的数据的变量。

该函数执行完毕后，base_point中就包含了变换完成后的点坐标了，最后通过ROS_INFO()将坐标信息打印在ROS终端界面。

catch(tf::TransformException& ex) { ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what()); } 如果由于某种原因导致base_lase -> base_link的转换不可用（tf_broadcaster未运行等），则在执行transformPoint()时就会出现错误。为了确保能够正常处理，我们将捕获异常并为用户打印错误信息。ex.what()是tf::TransformException类的一个方法，它返回一个指向与异常相关的内容的C字符串的指针。此处必须用try-catch语句，否则会报错。main函数中一开始都是类似的，初始化ROS节点，创建节点句柄，从而启动ROS节点。tf::TransformListener listener(ros::Duration(10))的作用是通过tf::TransformListener创建一个TF变换的监听者，形参中的ros::Duration()函数表示所存储的TF变换数据的时间长度，TF功能包最大可以存储10s的数据，10s之后就会将之前的消息丢弃掉，这里将数据存储的时间长度设置为10s。tf::TransformListener类的定义： http://docs.ros.org/diamondback/api/tf/html/c++/classtf_1_1TransformListener.html#a7e41bd5e244f92709e22bffa2860605dros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener))); 此处创建了一个timer对象，通过ros::Duration()函数设置回调函数transformPoint()的执行频率为1Hz，通过boost::bind()函数将回调函数transformPoint()的参数与listener对象绑定，而当某些函数对象或值参数很大，拷贝代价很高，或者无法拷贝时，boost::bind()的使用就会受到限制，因此使用boost::ref()引用传递对象。ros::spin();的作用是让程序进入自循环的挂起状态，从而让程序以最好的效率监听TF变换并调用回调函数。 3. 配置与编译 3.1 在CMaKeLists.txt中添加编译选项

将如下位置中CATLIN_DEPENDS前面的“#”去掉，使能相关的依赖项。在这里插入图片描述

在如下位置进行配置，add_executable(lidar_broadcaster src/lidar_broadcaster.cpp)的作用是将src文件夹下的lidar_broadcaster.cpp文件编译成名为lidar_broadcaster的可执行文件。target_link_libraries(lidar_broadcaster ${catkin_LIBRARIES})的作用是将lidar_broadcaster可执行文件与ROS相关的库链接。

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

在这里插入图片描述