使用带时间戳的数据类型与``tf2_ros::MessageFilter``
**目标:**了解如何使用``tf2_ros::MessageFilter``处理带时间戳的数据类型。
教程级别: 中级
时间: 10分钟
内容
背景
本教程解释了如何在tf2中使用传感器数据。一些传感器数据的真实世界示例包括:
摄像头,包括单目和双目
激光扫描
假设创建了一个名为“turtle3”的新海龟,它没有良好的里程计,但有一台高空摄像头跟踪它的位置,并将其作为与“world”坐标系相关的“PointStamped”消息发布。
“turtle1”想知道相对于自己,"turtle3"在哪里。
为了实现这一点,“turtle1”必须订阅发布“turtle3”姿态的主题,等待直到可以将其转换为所需坐标系,然后执行相应操作。为了简化此过程,“tf2_ros::MessageFilter”非常有用。“tf2_ros::MessageFilter”将订阅具有头信息的任何ROS 2消息,并将其缓存,直到可以将其转换为目标坐标系。
任务
1、编写发布PointStamped消息的广播节点
在本教程中,我们将设置一个演示应用程序,其中包含一个用Python编写的节点,用于广播“turtle3”的“PointStamped”位置消息。
首先,让我们创建源文件。
进入我们在之前教程中创建的“learning_tf2_py”包的“src/learning_tf2_py/learning_tf2_py”目录,并通过输入以下命令下载示例传感器消息广播器代码:
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_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
在 Windows 命令行提示符中:
curl -sk https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_message_broadcaster.py -o turtle_tf2_message_broadcaster.py
或者在powershell中执行:
curl https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_message_broadcaster.py -o turtle_tf2_message_broadcaster.py
使用您偏爱的文本编辑器打开该文件。
from geometry_msgs.msg import PointStamped
from geometry_msgs.msg import Twist
import rclpy
from rclpy.node import Node
from turtlesim.msg import Pose
from turtlesim.srv import Spawn
class PointPublisher(Node):
def __init__(self):
super().__init__('turtle_tf2_message_broadcaster')
# Create a client to spawn a turtle
self.spawner = self.create_client(Spawn, 'spawn')
# Boolean values to store the information
# if the service for spawning turtle is available
self.turtle_spawning_service_ready = False
# if the turtle was successfully spawned
self.turtle_spawned = False
# if the topics of turtle3 can be subscribed
self.turtle_pose_cansubscribe = False
self.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 = True
else:
if self.result.done():
self.get_logger().info(
f'Successfully spawned {self.result.result().name}')
self.turtle_spawned = True
else:
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/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)
self.turtle_spawning_service_ready = True
else:
# Check if the service is ready
self.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.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)
def main():
rclpy.init()
node = PointPublisher()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
rclpy.shutdown()
1.1 检查代码
现在让我们来看一下代码。首先,在“on_timer”回调函数中,我们通过异步调用“turtlesim”的“Spawn”服务来生成“turtle3”,并在准备好龟生成服务时将其位置初始化为(4,2,0)。
# 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)
之后,该节点发布“turtle3/cmd_vel”主题、“turtle3/turtle_point_stamped”主题,并订阅“turtle3/pose”主题,并在每次接收到消息时运行回调函数“handle_turtle_pose”。
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)
最后,在回调函数“handle_turtle_pose”中,我们初始化“turtle3”的“Twist”消息并发布它们,这将使“turtle3”沿着一个圆形运动。然后,我们使用传入的“Pose”消息填充“turtle3”的“PointStamped”消息并发布它们。
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)
1.2 编写启动文件
为了运行此演示,我们需要在包 learning_tf2_py
的 launch
子目录中创建一个名为 turtle_tf2_sensor_message.launch.py
的启动文件:
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
def 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',
),
])
1.3 添加入口点
为了让 ros2 run
命令能够运行你的节点,你必须在 setup.py``(位于 ``src/learning_tf2_py
目录中)中添加入口点。
在``'console_scripts':``括号之间添加以下行:
'turtle_tf2_message_broadcaster = learning_tf2_py.turtle_tf2_message_broadcaster:main',
1.4 构建
在你的工作空间的根目录中运行 rosdep
,以检查缺少的依赖项。
rosdep install -i --from-path src --rosdistro humble -y
rosdep
只能在 Linux 上运行,所以你需要自己安装 geometry_msgs
和 turtlesim
依赖项。
rosdep
只能在 Linux 上运行,所以你需要自己安装 geometry_msgs
和 turtlesim
依赖项。
然后我们可以构建该包:
colcon build --packages-select learning_tf2_py
colcon build --packages-select learning_tf2_py
colcon build --merge-install --packages-select learning_tf2_py
2 编写消息过滤器/监听器节点
现在,为了可靠地获取 turtle3
在 turtle1
坐标系下的实时流 PointStamped
数据,我们将创建消息过滤器/监听器节点的源文件。
进入之前教程中创建的``learning_tf2_cpp`` package。在``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
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/turtle_tf2_message_filter.cpp
在 Windows 命令行提示符中:
curl -sk wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/turtle_tf2_message_filter.cpp -o turtle_tf2_message_filter.cpp
或者在powershell中执行:
curl https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/turtle_tf2_message_filter.cpp -o 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"
#endif
using namespace std::chrono_literals;
class PoseDrawer : public rclcpp::Node
{
public:
PoseDrawer()
: Node("turtle_tf2_pose_drawer")
{
// Declare and acquire `target_frame` parameter
target_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 exception
auto 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 available
tf2_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 caught
this->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;
}
2.1 检查代码
首先,您必须包含``tf2_ros``包中的``tf2_ros::MessageFilter``头文件,以及先前使用过的``tf2``和``ros2``相关头文件。
#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::Buffer``、``tf2_ros::TransformListener``和``tf2_ros::MessageFilter``的持久实例。
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_;
第三,ROS 2中的``message_filters::Subscriber``必须使用该主题进行初始化。并且``tf2_ros::MessageFilter``必须使用该``Subscriber``对象进行初始化。``MessageFilter``构造函数中的其他值得注意的参数是``target_frame``和回调函数。目标框架是它将确保``canTransform``成功的框架。回调函数是数据准备就绪时将调用的函数。
PoseDrawer()
: Node("turtle_tf2_pose_drawer")
{
// Declare and acquire `target_frame` parameter
target_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 exception
auto 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 available
tf2_filter_->registerCallback(&PoseDrawer::msgCallback, this);
}
最后,当数据准备就绪时,回调方法将调用``tf2_buffer_->transform``并将输出打印到控制台。
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 caught
this->get_logger(), "Failure %s\n", ex.what());
}
}
2.2 添加依赖项
在构建``learning_tf2_cpp``包之前,请在该包的``package.xml``文件中添加另外两个依赖项:
<depend>message_filters</depend>
<depend>tf2_geometry_msgs</depend>
2.3 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_HEADERS
NAMES tf2_geometry_msgs.hpp
PATHS ${_include_dirs}
NO_CACHE
PATH_SUFFIXES tf2_geometry_msgs
)
之后,添加可执行文件,并将其命名为``turtle_tf2_message_filter``,稍后将在``ros2 run``中使用。
add_executable(turtle_tf2_message_filter src/turtle_tf2_message_filter.cpp)
ament_target_dependencies(
turtle_tf2_message_filter
geometry_msgs
message_filters
rclcpp
tf2
tf2_geometry_msgs
tf2_ros
)
if(EXISTS ${TF2_CPP_HEADERS})
target_compile_definitions(turtle_tf2_message_filter PUBLIC -DTF2_CPP_HEADERS)
endif()
最后,在其他现有节点下面添加``install(TARGETS…)``部分,以便``ros2 run``可以找到您的可执行文件:
install(TARGETS
turtle_tf2_message_filter
DESTINATION lib/${PROJECT_NAME})
2.4 构建
在你的工作空间的根目录中运行 rosdep
,以检查缺少的依赖项。
rosdep install -i --from-path src --rosdistro humble -y
rosdep
只能在 Linux 上运行,所以你需要自己安装 geometry_msgs
和 turtlesim
依赖项。
rosdep
只能在 Linux 上运行,所以你需要自己安装 geometry_msgs
和 turtlesim
依赖项。
现在打开一个新的终端,导航到您的工作空间的根目录,并使用以下命令重新构建软件包:
colcon build --packages-select learning_tf2_cpp
colcon build --packages-select learning_tf2_cpp
colcon build --merge-install --packages-select learning_tf2_cpp
打开一个新终端,进入你的工作空间的根目录,并源化设置文件:
. install/setup.bash
. install/setup.bash
# CMD
call install\setup.bat
# Powershell
.\install\setup.ps1
3 运行
首先,我们需要通过启动启动文件``turtle_tf2_sensor_message.launch.py``来运行多个节点(包括PointStamped消息的广播节点):
ros2 launch learning_tf2_py turtle_tf2_sensor_message.launch.py
这将在``turtlesim``窗口中带来两只乌龟,其中``turtle3``沿着一个圆圈移动,而``turtle1``起初不动。但是你可以在另一个终端中运行``turtle_teleop_key``节点来驱动``turtle1``移动:
ros2 run turtlesim turtle_teleop_key
现在,如果你回显主题``turtle3/turtle_point_stamped``:
ros2 topic echo /turtle3/turtle_point_stamped
那么将会有如下输出:
header:
stamp:
sec: 1629877510
nanosec: 902607040
frame_id: world
point:
x: 4.989276885986328
y: 3.073937177658081
z: 0.0
---
header:
stamp:
sec: 1629877510
nanosec: 918389395
frame_id: world
point:
x: 4.987966060638428
y: 3.089883327484131
z: 0.0
---
header:
stamp:
sec: 1629877510
nanosec: 934186680
frame_id: world
point:
x: 4.986400127410889
y: 3.105806589126587
z: 0.0
---
当演示运行时,打开另一个终端并运行消息过滤器/监听器节点:
ros2 run learning_tf2_cpp turtle_tf2_message_filter
如果运行正确,你应该会看到像下面这样的流式数据:
[INFO] [1630016162.006173900] [turtle_tf2_pose_drawer]: Point of turtle3 in frame of turtle1: x:-6.493231 y:-2.961614 z:0.000000
[INFO] [1630016162.006291983] [turtle_tf2_pose_drawer]: Point of turtle3 in frame of turtle1: x:-6.472169 y:-3.004742 z:0.000000
[INFO] [1630016162.006326234] [turtle_tf2_pose_drawer]: Point of turtle3 in frame of turtle1: x:-6.479420 y:-2.990479 z:0.000000
[INFO] [1630016162.006355644] [turtle_tf2_pose_drawer]: Point of turtle3 in frame of turtle1: x:-6.486441 y:-2.976102 z:0.000000