ROS 2边学边练(41)-- 使用基于tf2_ros::MessageFilter带标记(位姿、时间...)的数据类型

本文主要是介绍ROS 2边学边练(41)-- 使用基于tf2_ros::MessageFilter带标记(位姿、时间...)的数据类型,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

前言

        此篇将介绍如何利用tf2来使用传感器数据(如单声道和立体声摄像机以及雷达)。

        假设我们创建了一只海龟叫turtle3,它的里程计不大好用,为了监视turtle3的活动轨迹,有台头顶摄像机被安装到该海龟的背上(负碑的赑屃),并且实时发布相对于世界坐标系的PointStamped消息(包含位姿和时间)。

        有只叫turtle1的海龟想要知道turtle3相对自己的位姿(在turtle1坐标系中)。

        于是乎turtle1订阅了摄像机发布的关于turtle3位姿的主题,并等待可用的转换数据以便执行其他操作。为了方便实现这个目标,我们可以利用tf2_ros::MessageFilter。

        tf2_ros::MessageFilter会订阅任何带有头(header)的消息并缓存起来,直到可以将该消息从源坐标系变换到目标坐标系为止。

动动手

        需要实现两个节点,一个python实现一个C++实现,其中python的实现需要在learning_tf2_py功能包下,如果前期一直用的learning_tf2_cpp包而没有此包的话,请在工作空间根路径的src下执行如下命令:

$ros2 pkg create --build-type ament_python --license Apache-2.0 -- learning_tf2_py

写一个PointStamped消息的广播节点

        我们先实现一个广播turtle3 PointStamped位置消息的节点代码(python)。

        进入learning_tf2_py包路径下/src/learning_tf2_py/learning_tf2_py,执行如下命令下载传感器消息广播节点的例子代码turtle_tf2_message_broadcaster.py:

$wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_message_broadcaster.py

内容如下:

from geometry_msgs.msg import PointStamped
from geometry_msgs.msg import Twistimport rclpy
from rclpy.node import Nodefrom turtlesim.msg import Pose
from turtlesim.srv import Spawnclass PointPublisher(Node):def __init__(self):super().__init__('turtle_tf2_message_broadcaster')# Create a client to spawn a turtleself.spawner = self.create_client(Spawn, 'spawn')# Boolean values to store the information# if the service for spawning turtle is availableself.turtle_spawning_service_ready = False# if the turtle was successfully spawnedself.turtle_spawned = False# if the topics of turtle3 can be subscribedself.turtle_pose_cansubscribe = Falseself.timer = self.create_timer(1.0, self.on_timer)def on_timer(self):if self.turtle_spawning_service_ready:if self.turtle_spawned:self.turtle_pose_cansubscribe = Trueelse:if self.result.done():self.get_logger().info(f'Successfully spawned {self.result.result().name}')self.turtle_spawned = Trueelse:self.get_logger().info('Spawn is not finished')else:if self.spawner.service_is_ready():# Initialize request with turtle name and coordinates# Note that x, y and theta are defined as floats in turtlesim/srv/Spawnrequest = Spawn.Request()request.name = 'turtle3'request.x = 4.0request.y = 2.0request.theta = 0.0# Call requestself.result = self.spawner.call_async(request)self.turtle_spawning_service_ready = Trueelse:# Check if the service is readyself.get_logger().info('Service is not ready')if self.turtle_pose_cansubscribe:self.vel_pub = self.create_publisher(Twist, 'turtle3/cmd_vel', 10)self.sub = self.create_subscription(Pose, 'turtle3/pose', self.handle_turtle_pose, 10)self.pub = self.create_publisher(PointStamped, 'turtle3/turtle_point_stamped', 10)def handle_turtle_pose(self, msg):vel_msg = Twist()vel_msg.linear.x = 1.0vel_msg.angular.z = 1.0self.vel_pub.publish(vel_msg)ps = PointStamped()ps.header.stamp = self.get_clock().now().to_msg()ps.header.frame_id = 'world'ps.point.x = msg.xps.point.y = msg.yps.point.z = 0.0self.pub.publish(ps)def main():rclpy.init()node = PointPublisher()try:rclpy.spin(node)except KeyboardInterrupt:passrclpy.shutdown()
代码分析
# Initialize request with turtle name and coordinates
# Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
request = Spawn.Request()
request.name = 'turtle3'
request.x = 4.0
request.y = 2.0
request.theta = 0.0
# Call request
self.result = self.spawner.call_async(request)

        on_timer回调函数中,我们通过异步调用turtlesim中的Spawn服务孵化出turtle3,并给予turtle3初始位置(4, 2, 0)。

self.vel_pub = self.create_publisher(Twist, '/turtle3/cmd_vel', 10)
self.sub = self.create_subscription(Pose, '/turtle3/pose', self.handle_turtle_pose, 10)
self.pub = self.create_publisher(PointStamped, '/turtle3/turtle_point_stamped', 10)

        之后节点发布主题/turtle3/cmd_vel及主题/turtle3/turtle_point_stamped数据,并订阅了/turtle3/pose主题,当进来消息后会调用handle_turtle_pose回调函数来处理这些消息。

vel_msg = Twist()
vel_msg.linear.x = 1.0
vel_msg.angular.z = 1.0
self.vel_pub.publish(vel_msg)ps = PointStamped()
ps.header.stamp = self.get_clock().now().to_msg()
ps.header.frame_id = 'world'
ps.point.x = msg.x
ps.point.y = msg.y
ps.point.z = 0.0
self.pub.publish(ps)

        最后,在回调函数handle_turtle_pose中,我们初始化了turtle3的Twist类型消息(半径为1米的圆周运动)并发布了它们,接着我们将接收到的Pose消息解析并填充到PointStamped消息中,最后发布了这个PointStamped类型消息。

写一个启动文件

        在learning_tf2_py包中的launch文件夹内创建turtle_tf2_sensor_message_launch.py文件用来运行我们的例子。

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Nodedef generate_launch_description():return LaunchDescription([DeclareLaunchArgument('target_frame', default_value='turtle1',description='Target frame name.'),Node(package='turtlesim',executable='turtlesim_node',name='sim',output='screen'),Node(package='turtle_tf2_py',executable='turtle_tf2_broadcaster',name='broadcaster1',parameters=[{'turtlename': 'turtle1'}]),Node(package='turtle_tf2_py',executable='turtle_tf2_broadcaster',name='broadcaster2',parameters=[{'turtlename': 'turtle3'}]),Node(package='turtle_tf2_py',executable='turtle_tf2_message_broadcaster',name='message_broadcaster',),])
添加入口点

        我们必须在src/learning_tf2_py路径下的setup.py文件中添加入口点才能让ros2 run命令启动我们的节点。将下面语句添加到'console_scripts':括弧内:

'turtle_tf2_message_broadcaster = learning_tf2_py.turtle_tf2_message_broadcaster:main',
构建

        进入工作空间根路径,分别执行如下命令进行依赖检查和最终包的构建工作。

$rosdep install -i --from-path src --rosdistro iron -y
$colcon build --packages-select learning_tf2_py

写一个消息过滤器/监听器节点

        现在,为了可靠地在turtle1的坐标系下获取turtle3的流式PointStamped数据,我们将创建消息过滤器/监听器节点的源文件。

        进入src/learning_tf2_cpp/src路径,执行下面的命令下载turtle_tf2_message_filter.cpp文件:

$wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/turtle_tf2_message_filter.cpp

内容如下:

#include <chrono>
#include <memory>
#include <string>#include "geometry_msgs/msg/point_stamped.hpp"
#include "message_filters/subscriber.h"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/create_timer_ros.h"
#include "tf2_ros/message_filter.h"
#include "tf2_ros/transform_listener.h"
#ifdef TF2_CPP_HEADERS#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#else#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#endifusing namespace std::chrono_literals;class PoseDrawer : public rclcpp::Node
{
public:PoseDrawer(): Node("turtle_tf2_pose_drawer"){// Declare and acquire `target_frame` parametertarget_frame_ = this->declare_parameter<std::string>("target_frame", "turtle1");std::chrono::duration<int> buffer_timeout(1);tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());// Create the timer interface before call to waitForTransform,// to avoid a tf2_ros::CreateTimerInterfaceException exceptionauto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(this->get_node_base_interface(),this->get_node_timers_interface());tf2_buffer_->setCreateTimerInterface(timer_interface);tf2_listener_ =std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);point_sub_.subscribe(this, "/turtle3/turtle_point_stamped");tf2_filter_ = std::make_shared<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>>(point_sub_, *tf2_buffer_, target_frame_, 100, this->get_node_logging_interface(),this->get_node_clock_interface(), buffer_timeout);// Register a callback with tf2_ros::MessageFilter to be called when transforms are availabletf2_filter_->registerCallback(&PoseDrawer::msgCallback, this);}private:void msgCallback(const geometry_msgs::msg::PointStamped::SharedPtr point_ptr){geometry_msgs::msg::PointStamped point_out;try {tf2_buffer_->transform(*point_ptr, point_out, target_frame_);RCLCPP_INFO(this->get_logger(), "Point of turtle3 in frame of turtle1: x:%f y:%f z:%f\n",point_out.point.x,point_out.point.y,point_out.point.z);} catch (const tf2::TransformException & ex) {RCLCPP_WARN(// Print exception which was caughtthis->get_logger(), "Failure %s\n", ex.what());}}std::string target_frame_;std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;message_filters::Subscriber<geometry_msgs::msg::PointStamped> point_sub_;std::shared_ptr<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>> tf2_filter_;
};int main(int argc, char * argv[])
{rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<PoseDrawer>());rclcpp::shutdown();return 0;
}
代码分析
#include "geometry_msgs/msg/point_stamped.hpp"
#include "message_filters/subscriber.h"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/create_timer_ros.h"
#include "tf2_ros/message_filter.h"
#include "tf2_ros/transform_listener.h"
#ifdef TF2_CPP_HEADERS#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#else#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#endif

        首先需包含tf2_ros::MessageFilter、tf2、ros2等相关头文件,以使能相关API。

std::string target_frame_;
std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
message_filters::Subscriber<geometry_msgs::msg::PointStamped> point_sub_;
std::shared_ptr<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>> tf2_filter_;

        其次声明有关tf2_ros::Buffer、tf2_ros::TransformListener及tf2_ros::MessageFilter的全局变量。

PoseDrawer()
: Node("turtle_tf2_pose_drawer")
{// Declare and acquire `target_frame` parametertarget_frame_ = this->declare_parameter<std::string>("target_frame", "turtle1");std::chrono::duration<int> buffer_timeout(1);tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());// Create the timer interface before call to waitForTransform,// to avoid a tf2_ros::CreateTimerInterfaceException exceptionauto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(this->get_node_base_interface(),this->get_node_timers_interface());tf2_buffer_->setCreateTimerInterface(timer_interface);tf2_listener_ =std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);point_sub_.subscribe(this, "/turtle3/turtle_point_stamped");tf2_filter_ = std::make_shared<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>>(point_sub_, *tf2_buffer_, target_frame_, 100, this->get_node_logging_interface(),this->get_node_clock_interface(), buffer_timeout);// Register a callback with tf2_ros::MessageFilter to be called when transforms are availabletf2_filter_->registerCallback(&PoseDrawer::msgCallback, this);
}

        第三,ROS 2中的message_filters::Subscriber必须使用主题("/turtle3/turtle_point_stamped")进行初始化。同时,tf2_ros::MessageFilter也必须使用那个Subscriber对象(point_sub_)进行初始化。在MessageFilter构造函数中值得注意的其他参数包括目标帧(target_frame)和回调函数(callback)。目标帧是确保canTransform能够成功执行的目标坐标系。当数据准备就绪时,回调函数(msgCallback)就会被调用。

private:void msgCallback(const geometry_msgs::msg::PointStamped::SharedPtr point_ptr){geometry_msgs::msg::PointStamped point_out;try {tf2_buffer_->transform(*point_ptr, point_out, target_frame_);RCLCPP_INFO(this->get_logger(), "Point of turtle3 in frame of turtle1: x:%f y:%f z:%f\n",point_out.point.x,point_out.point.y,point_out.point.z);} catch (const tf2::TransformException & ex) {RCLCPP_WARN(// Print exception which was caughtthis->get_logger(), "Failure %s\n", ex.what());}}

        最后,在回调函数msgCallback中,当数据准备好的时候会调用transform函数将数据转换为目标坐标系视角下的对应数据,并会将结果数据输出到控制台。

package,xml添加依赖

        我们需要增加下面两行内容到package.xml:

<depend>message_filters</depend>
<depend>tf2_geometry_msgs</depend>
CMakeLists.txt

        同样,CMakeLists.txt文件也需添加下面两行内容:

find_package(message_filters REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)

        下面内容是为了处理不同版本的ROS:

if(TARGET tf2_geometry_msgs::tf2_geometry_msgs)get_target_property(_include_dirs tf2_geometry_msgs::tf2_geometry_msgs INTERFACE_INCLUDE_DIRECTORIES)
else()set(_include_dirs ${tf2_geometry_msgs_INCLUDE_DIRS})
endif()find_file(TF2_CPP_HEADERSNAMES tf2_geometry_msgs.hppPATHS ${_include_dirs}NO_CACHEPATH_SUFFIXES tf2_geometry_msgs
)

        接着,我们还需加上下面内容,我们将消息过滤器/监听器节点可执行文件命名为turtle_tf2_message_filter:

add_executable(turtle_tf2_message_filter src/turtle_tf2_message_filter.cpp)
ament_target_dependencies(turtle_tf2_message_filtergeometry_msgsmessage_filtersrclcpptf2tf2_geometry_msgstf2_ros
)if(EXISTS ${TF2_CPP_HEADERS})target_compile_definitions(turtle_tf2_message_filter PUBLIC -DTF2_CPP_HEADERS)
endif()

        最后再加上安装信息,使ros2 run命令能够根据路径找到可执行文件:

install(TARGETSturtle_tf2_message_filterDESTINATION lib/${PROJECT_NAME})
构建

        执行依赖检查和最终构建:

$rosdep install -i --from-path src --rosdistro iron -y
$colcon build --packages-select learning_tf2_cpp

运行

        新开一个终端,进入工作空间根路径,source下环境(. install/setup.bash),首先启动几个节点(PointStamped消息的广播节点):

$ros2 launch learning_tf2_py turtle_tf2_sensor_message_launch.py

如果上述命令提示找不到turtle_tf2_sensor_message_launch.py文件也可以直接进入launch路径执行,如下图所示。 

 

        启动完成后会有两只小海龟,turtle3在做圆周运动,turtle1静止不动,我们可以再开启一个终端执行如下命令控制turtle1:

$ros2 run turtlesim turtle_teleop_key

        我们可以订阅查看下turtle3/turtle_point_stamped主题的消息:

$ros2 topic echo /turtle3/turtle_point_stamped

        这些都完成之后,我们再运行下最后构建的消息过滤器/监听器节点:

$ros2 run learning_tf2_cpp turtle_tf2_message_filter

        如果一切OK,我们会在终端看到下面的信息(turtle3在turtle1坐标系中的位姿数据):

本篇完。 

这篇关于ROS 2边学边练(41)-- 使用基于tf2_ros::MessageFilter带标记(位姿、时间...)的数据类型的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/949997

相关文章

服务器集群同步时间手记

1.时间服务器配置(必须root用户) (1)检查ntp是否安装 [root@node1 桌面]# rpm -qa|grep ntpntp-4.2.6p5-10.el6.centos.x86_64fontpackages-filesystem-1.41-1.1.el6.noarchntpdate-4.2.6p5-10.el6.centos.x86_64 (2)修改ntp配置文件 [r

中文分词jieba库的使用与实景应用(一)

知识星球:https://articles.zsxq.com/id_fxvgc803qmr2.html 目录 一.定义: 精确模式(默认模式): 全模式: 搜索引擎模式: paddle 模式(基于深度学习的分词模式): 二 自定义词典 三.文本解析   调整词出现的频率 四. 关键词提取 A. 基于TF-IDF算法的关键词提取 B. 基于TextRank算法的关键词提取

使用SecondaryNameNode恢复NameNode的数据

1)需求: NameNode进程挂了并且存储的数据也丢失了,如何恢复NameNode 此种方式恢复的数据可能存在小部分数据的丢失。 2)故障模拟 (1)kill -9 NameNode进程 [lytfly@hadoop102 current]$ kill -9 19886 (2)删除NameNode存储的数据(/opt/module/hadoop-3.1.4/data/tmp/dfs/na

Hadoop数据压缩使用介绍

一、压缩原则 (1)运算密集型的Job,少用压缩 (2)IO密集型的Job,多用压缩 二、压缩算法比较 三、压缩位置选择 四、压缩参数配置 1)为了支持多种压缩/解压缩算法,Hadoop引入了编码/解码器 2)要在Hadoop中启用压缩,可以配置如下参数

Makefile简明使用教程

文章目录 规则makefile文件的基本语法:加在命令前的特殊符号:.PHONY伪目标: Makefilev1 直观写法v2 加上中间过程v3 伪目标v4 变量 make 选项-f-n-C Make 是一种流行的构建工具,常用于将源代码转换成可执行文件或者其他形式的输出文件(如库文件、文档等)。Make 可以自动化地执行编译、链接等一系列操作。 规则 makefile文件

使用opencv优化图片(画面变清晰)

文章目录 需求影响照片清晰度的因素 实现降噪测试代码 锐化空间锐化Unsharp Masking频率域锐化对比测试 对比度增强常用算法对比测试 需求 对图像进行优化,使其看起来更清晰,同时保持尺寸不变,通常涉及到图像处理技术如锐化、降噪、对比度增强等 影响照片清晰度的因素 影响照片清晰度的因素有很多,主要可以从以下几个方面来分析 1. 拍摄设备 相机传感器:相机传

pdfmake生成pdf的使用

实际项目中有时会有根据填写的表单数据或者其他格式的数据,将数据自动填充到pdf文件中根据固定模板生成pdf文件的需求 文章目录 利用pdfmake生成pdf文件1.下载安装pdfmake第三方包2.封装生成pdf文件的共用配置3.生成pdf文件的文件模板内容4.调用方法生成pdf 利用pdfmake生成pdf文件 1.下载安装pdfmake第三方包 npm i pdfma

零基础学习Redis(10) -- zset类型命令使用

zset是有序集合,内部除了存储元素外,还会存储一个score,存储在zset中的元素会按照score的大小升序排列,不同元素的score可以重复,score相同的元素会按照元素的字典序排列。 1. zset常用命令 1.1 zadd  zadd key [NX | XX] [GT | LT]   [CH] [INCR] score member [score member ...]

git使用的说明总结

Git使用说明 下载安装(下载地址) macOS: Git - Downloading macOS Windows: Git - Downloading Windows Linux/Unix: Git (git-scm.com) 创建新仓库 本地创建新仓库:创建新文件夹,进入文件夹目录,执行指令 git init ,用以创建新的git 克隆仓库 执行指令用以创建一个本地仓库的

【北交大信息所AI-Max2】使用方法

BJTU信息所集群AI_MAX2使用方法 使用的前提是预约到相应的算力卡,拥有登录权限的账号密码,一般为导师组共用一个。 有浏览器、ssh工具就可以。 1.新建集群Terminal 浏览器登陆10.126.62.75 (如果是1集群把75改成66) 交互式开发 执行器选Terminal 密码随便设一个(需记住) 工作空间:私有数据、全部文件 加速器选GeForce_RTX_2080_Ti