使用stamped数据类型与tf2_ros: MessageFilter [待校准@8578]

Goal目标: Learn了解如何使用 “tf2_ros:: messagefilter” 处理stamped数据类型。 [待校准@8579]

Tutorial教程级别: Advanced高级 [待校准@7643]

时间: 10分钟 [Alyssa@7452]

背景

本教程介绍如何在tf2中使用传感器数据。传感器数据的一些真实例子是: [待校准@8580]

• 单声道和立体声相机 [待校准@8581]

• 激光扫描 [待校准@8582]

假设一只名为 turtle3 的新乌龟被创造出来了，它没有很好的里程计，但是有一个头顶摄像机跟踪它的位置，并把它作为与 world 帧相关的 PointStamped 信息发布。 [待校准@8583]

[需手动修复的语法]``turtle1`` wants to know where turtle3 is与自身相比。 [待校准@8584]

要做到这一点， turtle1 必须倾听 turtle3 姿势被发表的话题，等到转换成想要的帧准备好，然后再进行操作。为了使这变得更容易，请使用 “tf2_ros::MessageFilter `` is very useful. The `` tf2_ros:: messageFilter'' 将订阅带有标头的任何ROS 2消息并将其缓存，直到可以将其转换为目标帧为止。 [待校准@8585]

设置示例 [待校准@8586]

1写入标记消息的广播器节点 [待校准@8587]

本教程会成立例程应用具有节点 (Python) 广播 PointStamped 位置信息 turtle3 。 [待校准@8588]

首先，让我们创建源文件。 [待校准@8589]

转到我们在上一教程中创建的 learning_tf2_py package 。在 src/learning_tf2_py/learning_tf2_py 目录中，通过输入以下命令下载示例传感器消息广播器代码: [待校准@8590]

LinuxmacOSWindows

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

使用您喜欢的文本编辑器打开文件。 [待校准@8591]

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 = float(4)
                request.y = float(2)
                request.theta = float(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检查代码 [待校准@8367]

现在让我们看一下代码。首先，在 on_timer 调用回调函数，我们生成 turtle3 的异步调用的 Spawn 服务 turtlesim ，初始化位置 (4，2，0)，当海龟产卵服务准备好了。 [待校准@8592]

# 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 = float(4)
request.y = float(2)
request.theta = float(0)
Call request
self.result = self.spawner.call_async(request)

之后，节点发布话题 turtle3/cmd_vel ，话题 turtle3/turtle_point_stamped ，订阅话题 turtle3/pose ，并对每条传入的消息运行调用函数 handle_turtle_pose 。 [待校准@8593]

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 沿着一个圆圈移动。然后我们填补 PointStamped 信息 turtle3 传入 Pose 消息发布。 [待校准@8594]

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写launch文件 [待校准@8369]

为了运行此例程，我们需要创建launch文件 turtle_tf2_sensor_message.launch.py 在 launch 子目录包装 learning_tf2_py : [待校准@8595]

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添加入口点并构建包 [待校准@8596]

不要忘记在包的 setup.py 文件中添加可执行文件: [待校准@8597]

'console_scripts': [
    ...
    'turtle_tf2_message_broadcaster = learning_tf2_py.turtle_tf2_message_broadcaster:main',
],

然后我们可以构建包: [待校准@8598]

LinuxmacOSWindows

colcon build --packages-select learning_tf2_py

2编写消息过滤器/侦听器节点 [待校准@8599]

现在，获得串流 PointStamped 数据 turtle3 在帧的 turtle1 可靠，我们将创建源文件文件消息/侦听节点。 [待校准@8600]

转到我们在上一教程中创建的 learning_tf2_cpp package 。在 src/learning_tf2_cpp/src 目录中，通过输入以下命令下载文件 turtle_tf2_message_filter.cpp : [待校准@8601]

LinuxmacOSWindows

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

使用您喜欢的文本编辑器打开文件。 [待校准@8591]

#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

#include <chrono>
#include <memory>
#include <string>

using namespace std::chrono_literals;

class PoseDrawer : public rclcpp::Node
{
public:
  PoseDrawer()
  : Node("turtle_tf2_pose_drawer")
  {
    // Declare and acquire `target_frame` parameter
    this->declare_parameter<std::string>("target_frame", "turtle1");
    this->get_parameter("target_frame", target_frame_);

    typedef std::chrono::duration<int> seconds_type;
    seconds_type 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 (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检查代码 [待校准@8007]

首先，您必须包括 tf2_ros 包中的 “tf2_ros:: messagefilter” 标题，以及以前使用的 tf2 和 ros2 相关标题。 [待校准@8602]

#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 `` and `` tf2_ros: MessageFilter''。 [待校准@8603]

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_;

第三，必须使用该 Subscriber 对象初始化ROS 2 “消息过滤器:: 订阅者 `` must be initialized with the topic. And the `` tf2_ros:: 消息过滤器”。 MessageFilter 构造函数中注意的其他参数是 target_frame 函数和调用back函数。目标帧是确保 canTransform 成功的帧。调用back函数是在数据准备就绪时将被调用的函数。 [待校准@8604]

PoseDrawer()
: Node("turtle_tf2_pose_drawer")
{
  // Declare and acquire `target_frame` parameter
  this->declare_parameter<std::string>("target_frame", "turtle1");
  this->get_parameter("target_frame", target_frame_);

  typedef std::chrono::duration<int> seconds_type;
  seconds_type 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);
}

最后，当数据准备好时，调用back方法将调用 tf2_buffer_->transform ，并将输出打印到控制台。 [待校准@8605]

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 (tf2::TransformException & ex) {
    RCLCPP_WARN(
      // Print exception which was caught
      this->get_logger(), "Failure %s\n", ex.what());
  }
}

2.2构建包 [待校准@8606]

在构建 learning_tf2_cpp 包之前，请在此包的 package.xml 文件中添加另外两个依赖项: [待校准@8607]

<depend>message_filters</depend>
<depend>tf2_geometry_msgs</depend>

在 CMakeLists.txt 文件中，在现有依赖项下添加两行: [待校准@8608]

find_package(message_filters REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)

最重要的是，将这些行添加到依赖项下方: [待校准@8609]

find_file(TF2_CPP_HEADERS
  NAMES tf2_geometry_msgs.hpp
  PATHS ${tf2_geometry_msgs_INCLUDE_DIRS}
  NO_CACHE
  PATH_SUFFIXES tf2_geometry_msgs
)

之后，添加可执行文件并将其命名为 turtle_tf2_message_filter ，稍后将在 ros2 run 中使用。 [待校准@8610]

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
)

最后，添加 “安装 (目标……) `` section (below other existing nodes) so `` ros2运行” 可以找到您的可执行文件: [待校准@8611]

install(TARGETS
  turtle_tf2_message_filter
  DESTINATION lib/${PROJECT_NAME})

现在打开一个新终端，导航到工作区的根目录，然后使用以下命令重建包: [待校准@8612]

LinuxmacOSWindows

colcon build --packages-select learning_tf2_cpp

运行和结果 [待校准@8613]

首先我们需要运行多个节点 (包括广播器节点PointStamped消息) launch的launch文件 turtle_tf2_sensor_message.launch.py : [待校准@8614]

ros2 launch learning_tf2_py turtle_tf2_sensor_message.launch.py

这将打开带有两只海龟的 turtlesim 窗口， turtle3 沿着一个圆圈移动，而 turtle1 起初没有移动。但是你可以在另一个终端运行 turtle_teleop_key 节点来驱动 turtle1 移动: [待校准@8615]

ros2 run turtlesim turtle_teleop_key

现在如果你重复这个话题 turtle3/turtle_point_stamped : [待校准@8616]

ros2 topic echo /turtle3/turtle_point_stamped

然后会有如下输出: [待校准@8617]

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
---

例程运行时，打开另一个终端并运行消息筛选器/侦听器节点: [待校准@8618]

ros2 run learning_tf2_cpp turtle_tf2_message_filter

如果它运行正常，您应该看到如下流数据: [待校准@8619]

[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

总结

在本教程中，您学习了如何在tf2中使用传感器数据/消息。具体调用y来说，你学会了如何在一个话题上发布 PointStamped 信息，以及如何倾听这个话题，并用 “tf2_ros:: messagefilter” 转换 PointStamped 信息的帧。 [待校准@8620]