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

代码分析

# 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 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',
        ),
    ])

添加入口点

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

'turtle_tf2_message_broadcaster = learning_tf2_py.turtle_tf2_message_broadcaster:main',

        另外为了使ros2 launch能够通过包名找到上面的launch文件，我们还需添加相关引入模块及安装信息（否则就会提示在share下找不到launch文件，因为那个路径下根本就未安装成功）：

import os
from glob import glob
from setuptools import find_packages, setup

package_name = 'launch_tutorial'

setup(
    # Other parameters ...
    data_files=[
        # ... Other data files
        # Include all launch files.
        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*')))
    ]
)

        完整的setup.py信息如下： 

import os
from glob import glob
from setuptools import find_packages, setup

package_name = 'learning_tf2_py'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*')))
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='mike',
    maintainer_email='mike@todo.todo',
    description='TODO: Package description',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        '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"
#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;
}

代码分析

#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` 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);
}

        第三，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 caught
        this->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_HEADERS
  NAMES tf2_geometry_msgs.hpp
  PATHS ${_include_dirs}
  NO_CACHE
  PATH_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_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()

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

install(TARGETS
  turtle_tf2_message_filter
  DESTINATION 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文件（原因及解决方法见上文setup.py文件的修改，主要是未添加安装信息，如cmake中的install()一样）也可以直接进入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坐标系中的位姿数据）：

本篇完。