【ROS2】中级:tf2-编写监听器 (C++)

最新推荐文章于 2024-07-21 21:32:15 发布

十年一梦实验室

最新推荐文章于 2024-07-21 21:32:15 发布

阅读量960

点赞数 24

文章标签： c++ 开发语言

本文链接：https://blog.csdn.net/cxyhjl/article/details/140391870

版权

目标：学习如何使用 tf2 获取帧变换。

教程级别：中级

时间：10 分钟

背景
先决条件
任务
- 1.编写监听节点
- 2.更新启动文件
- 3.构建
- 4.运行
摘要

背景

在之前的教程中，我们创建了一个 tf2 广播器来发布乌龟的姿态到 tf2。

在本教程中，我们将创建一个 tf2 监听器以开始使用 tf2。

先决条件

本教程假设您已经完成了 tf2 静态广播器教程（C++）https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Static-Broadcaster-Cpp.html 和 tf2 广播器教程（C++）https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Cpp.html。在上一个教程中，我们创建了一个 learning_tf2_cpp 包，我们将继续在此基础上进行工作。

任务

1. 编写监听节点

让我们首先创建源文件。转到我们在上一个教程中创建的 learning_tf2_cpp 包。在 src 目录中，通过输入以下命令下载示例监听器代码：

cxy@ubuntu2404-cxy:~/ros2_ws/src/learning_tf2_cpp/src$ wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/turtle_tf2_listener.cpp

使用您喜欢的文本编辑器打开文件。

#include <chrono>  // 包含C++标准库的时间库
#include <functional>  // 包含C++标准库的函数库
#include <memory>  // 包含C++标准库的内存库
#include <string>  // 包含C++标准库的字符串库


#include "geometry_msgs/msg/transform_stamped.hpp"  // 包含geometry_msgs包的TransformStamped消息
#include "geometry_msgs/msg/twist.hpp"  // 包含geometry_msgs包的Twist消息
#include "rclcpp/rclcpp.hpp"  // 包含ROS2的rclcpp库
#include "tf2/exceptions.h"  // 包含tf2的异常处理库
#include "tf2_ros/transform_listener.h"  // 包含tf2_ros的TransformListener库
#include "tf2_ros/buffer.h"  // 包含tf2_ros的Buffer库
#include "turtlesim/srv/spawn.hpp"  // 包含turtlesim的Spawn服务


using namespace std::chrono_literals;  // 使用std::chrono_literals命名空间


class FrameListener : public rclcpp::Node  // 定义一个名为FrameListener的类，继承自rclcpp::Node
{
public:
  FrameListener()  // FrameListener类的构造函数
  : Node("turtle_tf2_frame_listener"),  // 初始化节点名为"turtle_tf2_frame_listener"
    turtle_spawning_service_ready_(false),  // 初始化乌龟生成服务的状态为未准备好
    turtle_spawned_(false)  // 初始化乌龟的生成状态为未生成
  {
    // 声明并获取`target_frame`参数
    target_frame_ = this->declare_parameter<std::string>("target_frame", "turtle1");


    tf_buffer_ =
      std::make_unique<tf2_ros::Buffer>(this->get_clock());  // 创建一个新的tf2_ros::Buffer对象
    tf_listener_ =
      std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);  // 创建一个新的tf2_ros::TransformListener对象


    // 创建一个生成乌龟的客户端
    spawner_ =
      this->create_client<turtlesim::srv::Spawn>("spawn");


    // 创建turtle2速度发布器
    publisher_ =
      this->create_publisher<geometry_msgs::msg::Twist>("turtle2/cmd_vel", 1);


    // 每秒调用一次on_timer函数
    timer_ = this->create_wall_timer(
      1s, [this]() {return this->on_timer();});
  }


private:
  void on_timer()  // 定义on_timer函数
{
    // 将帧名称存储在变量中，这些变量将用于
    // 计算转换
    std::string fromFrameRel = target_frame_.c_str();  // 目标帧的名称
    std::string toFrameRel = "turtle2";  // turtle2的帧名称


    if (turtle_spawning_service_ready_) {  // 如果乌龟生成服务已准备好
      if (turtle_spawned_) {  // 如果乌龟已经生成
        geometry_msgs::msg::TransformStamped t;  // 定义一个TransformStamped消息


        // 查找target_frame和turtle2帧之间的转换
        // 并发送速度命令让turtle2达到target_frame
        try {
          t = tf_buffer_->lookupTransform(
            toFrameRel, fromFrameRel,
            tf2::TimePointZero);  // 查找转换
        } catch (const tf2::TransformException & ex) {  // 捕获并处理异常
          RCLCPP_INFO(
            this->get_logger(), "Could not transform %s to %s: %s",
            toFrameRel.c_str(), fromFrameRel.c_str(), ex.what());
          return;
        }


        geometry_msgs::msg::Twist msg;  // 定义一个Twist消息


        static const double scaleRotationRate = 1.0;  // 定义旋转速度的缩放因子
        msg.angular.z = scaleRotationRate * atan2(
          t.transform.translation.y,
          t.transform.translation.x);  // 计算并设置z轴的角速度


        static const double scaleForwardSpeed = 0.5;  // 定义前进速度的缩放因子
        msg.linear.x = scaleForwardSpeed * sqrt(
          pow(t.transform.translation.x, 2) +
          pow(t.transform.translation.y, 2));  // 计算并设置x轴的线速度


        publisher_->publish(msg);  // 发布消息
      } else {
        RCLCPP_INFO(this->get_logger(), "Successfully spawned");  // 打印成功生成乌龟的信息
        turtle_spawned_ = true;  // 设置乌龟已经生成
      }
    } else {
      // 检查服务是否准备好
      if (spawner_->service_is_ready()) {  // 如果服务已经准备好
        // 使用乌龟的名字和坐标初始化请求
        // 注意在turtlesim/srv/Spawn中，x，y和theta被定义为浮点数
        auto request = std::make_shared<turtlesim::srv::Spawn::Request>();  // 定义一个Spawn服务的请求
        request->x = 4.0;  // 设置乌龟的x坐标
        request->y = 2.0;  // 设置乌龟的y坐标
        request->theta = 0.0;  // 设置乌龟的初始角度
        request->name = "turtle2";  // 设置乌龟的名字


        // 调用请求
        using ServiceResponseFuture =
          rclcpp::Client<turtlesim::srv::Spawn>::SharedFuture;  // 定义一个类型别名
        auto response_received_callback =  [this](ServiceResponseFuture future) { // 定义一个回调函数
            auto result = future.get();  // 获取结果
            if (strcmp(result->name.c_str(), "turtle2") == 0) {  // 如果结果的名字是"turtle2"
              turtle_spawning_service_ready_ = true;  // 设置乌龟生成服务已经准备好
            } else {
              RCLCPP_ERROR(this->get_logger(), "Service callback result mismatch");  // 打印错误信息
            }
          };
        auto result = spawner_->async_send_request(request, response_received_callback);  // 异步发送请求
      } else {
        RCLCPP_INFO(this->get_logger(), "Service is not ready");  // 打印服务未准备好的信息
      }
    }
  }


  // 布尔值用于存储信息
  // 如果生成乌龟的服务可用
  bool turtle_spawning_service_ready_;
  // 如果乌龟已成功生成
  bool turtle_spawned_;
  rclcpp::Client<turtlesim::srv::Spawn>::SharedPtr spawner_{nullptr};  // 定义一个Spawn服务的客户端
  rclcpp::TimerBase::SharedPtr timer_{nullptr};  // 定义一个定时器
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_{nullptr};  // 定义一个Twist消息的发布器
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};  // 定义一个TransformListener
  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;  // 定义一个Buffer
  std::string target_frame_;  // 定义一个字符串用于存储目标帧的名字
};


int main(int argc, char * argv[])  // 主函数
{
  rclcpp::init(argc, argv);  // 初始化ROS
  rclcpp::spin(std::make_shared<FrameListener>());  // 创建FrameListener对象并开始处理ROS事件
  rclcpp::shutdown();  // 关闭ROS
  return 0;  // 返回0表示程序正常结束
}

1.1 检查代码

要了解生成海龟服务背后的工作原理，请参考编写一个简单的服务和客户端（C++）教程 https://docs.ros.org/en/jazzy/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Service-And-Client.html 。

现在，让我们来看一下与获取帧变换相关的代码。 tf2_ros 包含一个 TransformListener 类，这使得接收变换的任务变得更容易。

#include "tf2_ros/transform_listener.h"

在这里，我们创建一个 TransformListener 对象。监听器创建后，它开始通过网络接收 tf2 变换，并将它们缓冲最多 10 秒。

tf_listener_ =
  std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

最后，我们查询监听器以获取特定转换。我们调用 lookup_transform 方法，传入以下参数：

目标框架
来源框架
我们想要变换的时间

提供 tf2::TimePointZero 将只会得到最新可用的转换。所有这些都包裹在一个 try-catch 块中，以处理可能的异常。

t = tf_buffer_->lookupTransform(
  toFrameRel, fromFrameRel,
  tf2::TimePointZero);

所得到的变换代表了目标乌龟（fromFrameRel）相对于 turtle2 （toFrameRel）的位置和方向。然后使用乌龟之间的角度来计算速度指令以跟随目标乌龟。有关 tf2 的更多一般信息，请参阅概念部分中的 tf2 页面 https://docs.ros.org/en/jazzy/Concepts/Intermediate/About-Tf2.html 。

1.2 CMakeLists.txt

返回到 learning_tf2_cpp 目录， CMakeLists.txt 和 package.xml 文件位于那里。

现在打开 CMakeLists.txt ，添加可执行文件并将其命名为 turtle_tf2_listener ，稍后您将与 ros2 run 一起使用。

add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp)
ament_target_dependencies(
    turtle_tf2_listener
    geometry_msgs
    rclcpp
    tf2
    tf2_ros
    turtlesim
)

最后，添加 install(TARGETS…) 部分，以便 ros2 run 可以找到你的可执行文件：

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

2. 更新启动文件

打开位于 src/learning_tf2_cpp/launch 目录中名为 turtle_tf2_demo_launch.py 的启动文件，用你的文本编辑器，向启动描述中添加两个新节点，添加一个启动参数，并添加导入。结果文件应该看起来像：

from launch import LaunchDescription  # 导入LaunchDescription模块
from launch.actions import DeclareLaunchArgument  # 导入DeclareLaunchArgument模块
from launch.substitutions import LaunchConfiguration  # 导入LaunchConfiguration模块


from launch_ros.actions import Node  # 导入Node模块




def generate_launch_description():  # 定义一个名为generate_launch_description的函数
    return LaunchDescription([  # 返回一个LaunchDescription对象
        Node(  # 创建一个Node对象
            package='turtlesim',  # 设置包名为'turtlesim'
            executable='turtlesim_node',  # 设置可执行文件名为'turtlesim_node'
            name='sim'  # 设置节点名为'sim'
        ),
        Node(  # 创建一个Node对象
            package='learning_tf2_cpp',  # 设置包名为'learning_tf2_cpp'
            executable='turtle_tf2_broadcaster',  # 设置可执行文件名为'turtle_tf2_broadcaster'
            name='broadcaster1',  # 设置节点名为'broadcaster1'
            parameters=[  # 设置参数
                {'turtlename': 'turtle1'}  # 设置'turtlename'参数为'turtle1'
            ]
        ),
        DeclareLaunchArgument(  # 声明一个启动参数
            'target_frame', default_value='turtle1',  # 设置参数名为'target_frame'，默认值为'turtle1'
            description='Target frame name.'  # 设置参数描述为'Target frame name.'
        ),
        Node(  # 创建一个Node对象
            package='learning_tf2_cpp',  # 设置包名为'learning_tf2_cpp'
            executable='turtle_tf2_broadcaster',  # 设置可执行文件名为'turtle_tf2_broadcaster'
            name='broadcaster2',  # 设置节点名为'broadcaster2'
            parameters=[  # 设置参数
                {'turtlename': 'turtle2'}  # 设置'turtlename'参数为'turtle2'
            ]
        ),
        Node(  # 创建一个Node对象
            package='learning_tf2_cpp',  # 设置包名为'learning_tf2_cpp'
            executable='turtle_tf2_listener',  # 设置可执行文件名为'turtle_tf2_listener'
            name='listener',  # 设置节点名为'listener'
            parameters=[  # 设置参数
                {'target_frame': LaunchConfiguration('target_frame')}  # 设置'target_frame'参数为LaunchConfiguration对象的'target_frame'属性
            ]
        ),
    ])

这将声明一个 target_frame 启动参数，为我们将生成的第二只乌龟启动一个广播器，并启动一个将订阅这些转换的监听器。

3. 构建

在工作区的根目录运行 rosdep 以检查缺失的依赖项。

rosdep install -i --from-path src --rosdistro jazzy -y

仍在工作区的根目录下，构建您的包：

colcon build --packages-select learning_tf2_cpp

打开一个新的终端，导航到工作区的根目录，然后加载设置文件：

. install/setup.bash

4. 运行

现在您已准备好开始完整的海龟演示：

ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.py

您应该看到有两只乌龟的 turtle sim。在第二个终端窗口中输入以下命令：

ros2 run turtlesim turtle_teleop_key

要查看事物是否正常工作，只需使用箭头键驾驶第一只乌龟（确保您的终端窗口处于活动状态，而不是您的模拟器窗口），您会看到第二只乌龟跟随第一只乌龟！

摘要

在本教程中，您学习了如何使用 tf2 来获取帧变换的访问权限。您还完成了编写您在 tf2 教程介绍中首次尝试的 turtlesim 演示。https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Tf2/Introduction-To-Tf2.html

十年一梦实验室

关注

24
点赞
踩
19

收藏

觉得还不错? 一键收藏
0
评论
【ROS2】中级:tf2-编写监听器 (C++)

目标：学习如何使用 tf2 获取帧变换。教程级别：中级时间：10 分钟目录背景先决条件任务1.编写监听节点2.更新启动文件3.构建4.运行摘要背景在之前的教程中，我们创建了一个 tf2 广播器来发布乌龟的姿态到 tf2。在本教程中，我们将创建一个 tf2 监听器以开始使用 tf2。先决条件本教程假设您已经完成了 tf2 静态广播器教程（C++）https://docs.ros....
复制链接

扫一扫