【ROS2】演示:为有损网络使用服务质量设置

十年一梦实验室

于 2024-07-22 11:03:49 发布

阅读量461

点赞数 12

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

版权

背景
先决条件
运行演示
- 命令行选项
- 添加网络流量

背景

请阅读有关 QoS 设置的文档页面，以获取有关 ROS 2 中可用支持的背景信息。

在这个演示中，我们将生成一个发布相机图像的节点和另一个订阅图像并在屏幕上显示图像的节点。然后，我们将模拟它们之间的丢包网络连接，并展示不同的服务质量设置如何处理不良链接。

先决条件

本教程假设您已安装并运行 ROS 2 和 OpenCV。有关安装说明，请参阅 OpenCV 文档。您还需要 ROS 包 image_tools 。

cxy@cxy-Ubuntu2404:~$ sudo apt-get install ros-jazzy-image-tools
[sudo] password for cxy: 
Reading package lists... Done
Building dependency tree... Done
Reading state information... Done
The following NEW packages will be installed:
  ros-jazzy-image-tools
0 upgraded, 1 newly installed, 0 to remove and 5 not upgraded.
Need to get 185 kB of archives.
After this operation, 1,052 kB of additional disk space will be used.
Get:1 http://packages.ros.org/ros2/ubuntu noble/main amd64 ros-jazzy-image-tools amd64 0.33.4-1noble.20240703.161128 [185 kB]
Fetched 185 kB in 4s (46.2 kB/s)                
Selecting previously unselected package ros-jazzy-image-tools.
(Reading database ... 270207 files and directories currently installed.)
Preparing to unpack .../ros-jazzy-image-tools_0.33.4-1noble.20240703.161128_amd64.deb ...
Unpacking ros-jazzy-image-tools (0.33.4-1noble.20240703.161128) ...
Setting up ros-jazzy-image-tools (0.33.4-1noble.20240703.161128) ...

源码安装:

git clone https://github.com/ros2/demos.git -b jazzy

运行演示

在运行演示之前，请确保您的计算机已连接一个可用的网络摄像头。

一旦你安装了 ROS 2，请源化你的设置文件：

source ~/.bashrc

然后运行：

cxy@cxy-Ubuntu2404:~$ ros2 run image_tools showimage
[INFO] [1721611318.045341212] [showimage]: Subscribing to topic 'image'
[INFO] [1721611322.141314647] [showimage]: Received image #camera_frame
Received image #camera_frame
[INFO] [1721611323.644344772] [showimage]: Received image #camera_frame
Received image #camera_frame

还没有任何事情发生。 showimage 是一个订阅节点，正在等待 image 主题的发布者。

注意：您必须稍后使用 Ctrl-C 关闭 showimage 进程。您不能仅仅关闭窗口。

在一个单独的终端中，源安装文件并运行发布者节点：

cxy@cxy-Ubuntu2404:~$ ros2 run image_tools cam2image
[ WARN:0@1.283] global ./modules/videoio/src/cap_gstreamer.cpp (1405) open OpenCV | GStreamer warning: Cannot query video position: status=0, value=-1, duration=-1
[INFO] [1721611322.128570743] [cam2image]: Publishing image #1
[INFO] [1721611322.161307078] [cam2image]: Publishing image #2

这将从您的网络摄像头发布图像。如果您的计算机没有连接摄像头，可以使用命令行选项发布预定义的图像。

ros2 run image_tools cam2image --ros-args -p burger_mode:=True

在此窗口中，您将看到终端输出：

[INFO] [1715662452.055277255] [cam2image]: Publishing image #1
[INFO] [1715662452.119336061] [cam2image]: Publishing image #2
[INFO] [1715662452.187315139] [cam2image]: Publishing image #3
...

将弹出一个标题为“视图”的窗口，显示您的摄像头画面。在第一个窗口中，您将看到来自订阅者的输出：

[INFO] [1715662452.188906764] [showimage]: Received image #camera_frame
Received image #camera_frame
[INFO] [1715662452.252836919] [showimage]: Received image #camera_frame
Received image #camera_frame
[INFO] [1715662452.320878578] [showimage]: Received image #camera_frame
Received image #camera_frame
...

注意

macOS 用户：如果这些示例不起作用或您收到类似 ddsi_conn_write failed -1 的错误，则需要增加系统范围的 UDP 数据包大小

$ sudo sysctl -w net.inet.udp.recvspace=209715
$ sudo sysctl -w net.inet.udp.maxdgram=65500

这些更改在重启后不会保留。如果您希望更改保留，请将这些行添加到 /etc/sysctl.conf （如果文件尚不存在，请创建该文件）：

net.inet.udp.recvspace=209715
net.inet.udp.maxdgram=65500

命令行选项

在你的一个终端中，向原始命令添加一个 -h 标志：

cxy@cxy-Ubuntu2404:~$ ros2 run image_tools showimage -h
Usage: showimage [-h] [--ros-args [-p param:=value] ...]
Subscribe to an image topic and show the images.
Example: ros2 run image_tools showimage --ros-args -p reliability:=best_effort


Options:
  -h, --help  Display this help message and exit


Parameters:
  reliability  Reliability QoS setting. Either 'reliable' (default) or 'best_effort'
  history  History QoS setting. Either 'keep_last' (default) or 'keep_all'.
    If 'keep_last', then up to N samples are stored where N is the depth
  depth    Depth of the publisher queue. Only honored if history QoS is 'keep_last'. Default value is 10
  show_image  Show the image. Either 'true' (default) or 'false'
  window_name  Name of the display window. Default value is the topic name

ros2 run image_tools showimage -h
用法: showimage [-h] [--ros-args [-p param:=value] ...]
订阅一个图像主题并显示图像。
示例: ros2 run image_tools showimage --ros-args -p reliability:=best_effort


选项:
  -h, --help  显示此帮助信息并退出


参数:
  reliability  可靠性QoS设置。可以是'reliable'（默认）或'best_effort'
  history  历史QoS设置。可以是'keep_last'（默认）或'keep_all'。
    如果是'keep_last'，则最多存储N个样本，其中N是深度
  depth    发布者队列的深度。仅在历史QoS为'keep_last'时有效。默认值为10
  show_image  显示图像。可以是'true'（默认）或'false'
  window_name  显示窗口的名称。默认值是主题名称

添加网络流量

警告

本演示的这一部分在 RTI 的 Connext DDS 和 Fast-DDS 上无法运行。当在同一主机上运行多个节点时，这些 DDS 实现使用共享内存和回环接口。降低回环接口的吞吐量不会影响共享内存，因此两个节点之间的流量不会受到影响。

注意

下一节是特定于 Linux 的。

然而，对于 macOS 和 Windows，你可以使用“Network Link Conditioner”（xcode 工具套件的一部分）和“Clumsy”（http://jagt.github.io/clumsy/index.html）等工具来实现类似的效果，但它们不会在本教程中介绍。

我们将使用 Linux 网络流量控制工具， tc (http://linux.die.net/man/8/tc)。

sudo tc qdisc add dev lo root netem loss 5%

这个神奇的咒语将模拟本地回环设备上的 5%数据包丢失。如果您使用更高分辨率的图像（例如 --ros-args -p width:=640 -p height:=480 ），您可能需要尝试较低的数据包丢失率（例如 1% ）。

接下来我们启动 cam2image 和 showimage ，我们很快会注意到两个程序似乎都减慢了图像传输的速度。这是由默认 QoS 设置的行为引起的。在有损信道上强制可靠性意味着发布者（在本例中为 cam2image ）将重新发送网络数据包，直到收到消费者（即 showimage ）的确认。

现在让我们尝试运行两个程序，但使用更合适的设置。首先，我们将使用 -p reliability:=best_effort 选项来启用尽力而为的通信。发布者现在只会尝试传递网络数据包，并且不期望消费者的确认。我们现在看到 showimage 端的一些帧被丢弃了，所以在运行 showimage 的 shell 中的帧号将不再是连续的。

ros2 run image_tools cam2image --ros-args -p reliability:=best_effort
 ros2 run image_tools showimage --ros-args --param reliability:=best_effort

完成后，请记得删除排队规则

sudo tc qdisc delete dev lo root netem loss 5%

附录：

cxy@cxy-Ubuntu2404:~/second_ros2_ws/src/demos/image_tools$ tree
.
├── CHANGELOG.rst
├── CMakeLists.txt
├── doc
│   └── qos-best-effort.png
├── Doxyfile
├── img
│   └── result.png
├── include
│   └── image_tools
│       ├── cv_mat_sensor_msgs_image_type_adapter.hpp
│       └── visibility_control.h
├── package.xml
├── README.md
├── src
│   ├── burger.cpp
│   ├── burger.hpp
│   ├── cam2image.cpp
│   ├── cv_mat_sensor_msgs_image_type_adapter.cpp
│   ├── policy_maps.hpp
│   └── showimage.cpp
└── test
    ├── cam2image.txt
    ├── showimage.regex
    └── test_executables_demo.py.in


7 directories, 18 files

CMakeLists.txt

cmake_minimum_required(VERSION 3.5)  # 设置CMake的最低版本为3.5


project(image_tools)  # 定义项目名称为image_tools


# Default to C++17
if(NOT CMAKE_CXX_STANDARD)  # 如果没有设置C++标准
  set(CMAKE_CXX_STANDARD 17)  # 设置C++标准为C++17
  set(CMAKE_CXX_STANDARD_REQUIRED ON)  # 强制使用C++17标准
endif()


if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")  # 如果使用的是GNU编译器或Clang编译器
  add_compile_options(-Wall -Wextra -Wpedantic)  # 添加编译选项：显示所有警告、额外警告和严格警告
endif()


find_package(ament_cmake REQUIRED)  # 查找ament_cmake包，必需
find_package(rclcpp REQUIRED)  # 查找rclcpp包，必需
find_package(rclcpp_components REQUIRED)  # 查找rclcpp_components包，必需
find_package(sensor_msgs REQUIRED)  # 查找sensor_msgs包，必需
find_package(std_msgs REQUIRED)  # 查找std_msgs包，必需
find_package(OpenCV REQUIRED COMPONENTS core highgui imgcodecs imgproc videoio)  # 查找OpenCV包及其组件


add_library(${PROJECT_NAME} SHARED  # 添加共享库
  src/burger.cpp
  src/cam2image.cpp
  src/cv_mat_sensor_msgs_image_type_adapter.cpp
  src/showimage.cpp
)
target_compile_definitions(${PROJECT_NAME}
  PRIVATE "IMAGE_TOOLS_BUILDING_DLL")  # 定义编译宏
target_include_directories(${PROJECT_NAME} PUBLIC
  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"  # 设置包含目录
  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
target_link_libraries(${PROJECT_NAME}
  rclcpp::rclcpp  # 链接rclcpp库
  ${sensor_msgs_TARGETS}  # 链接sensor_msgs库
  ${std_msgs_TARGETS}  # 链接std_msgs库
  rclcpp_components::component  # 链接rclcpp_components库
  ${OpenCV_LIBS})  # 链接OpenCV库
rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "image_tools::Cam2Image" EXECUTABLE cam2image)  # 注册节点Cam2Image
rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "image_tools::ShowImage" EXECUTABLE showimage)  # 注册节点ShowImage


install(
  TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}  # 安装目标文件
  ARCHIVE DESTINATION lib  # 安装归档文件到lib目录
  LIBRARY DESTINATION lib  # 安装库文件到lib目录
  RUNTIME DESTINATION bin)  # 安装可执行文件到bin目录


install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})  # 安装包含目录


# TODO(sloretz) stop exporting old-style CMake variables in the future
ament_export_libraries(${PROJECT_NAME})  # 导出库
ament_export_targets(${PROJECT_NAME})  # 导出目标
ament_export_dependencies(rclcpp_components)  # 导出依赖


if(BUILD_TESTING)  # 如果启用了测试
  find_package(ament_lint_auto REQUIRED)  # 查找ament_lint_auto包，必需
  ament_lint_auto_find_test_dependencies()  # 查找测试依赖


  find_package(ament_cmake_pytest REQUIRED)  # 查找ament_cmake_pytest包，必需
  find_package(launch_testing_ament_cmake REQUIRED)  # 查找launch_testing_ament_cmake包，必需
  find_package(rmw_implementation_cmake REQUIRED)  # 查找rmw_implementation_cmake包，必需


  # These are the regex's for validating the output of the executables.
  set(RCLCPP_DEMO_SHOWIMAGE_EXPECTED_OUTPUT "${CMAKE_CURRENT_SOURCE_DIR}/test/showimage")  # 设置showimage的预期输出
  set(RCLCPP_DEMO_CAM2IMAGE_EXPECTED_OUTPUT "${CMAKE_CURRENT_SOURCE_DIR}/test/cam2image")  # 设置cam2image的预期输出


  macro(testing_targets)  # 定义宏testing_targets
    set(RCLCPP_DEMO_CAM2IMAGE_EXECUTABLE $<TARGET_FILE:cam2image>)  # 设置cam2image可执行文件
    set(RCLCPP_DEMO_SHOWIMAGE_EXECUTABLE $<TARGET_FILE:showimage>)  # 设置showimage可执行文件


    configure_file(
      test/test_executables_demo.py.in
      test_showimage_cam2image${target_suffix}.py.genexp
      @ONLY
    )  # 配置文件


    file(GENERATE
      OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/test_showimage_cam2image${target_suffix}_$<CONFIG>.py"
      INPUT "${CMAKE_CURRENT_BINARY_DIR}/test_showimage_cam2image${target_suffix}.py.genexp"
    )  # 生成文件


    add_launch_test(
      "${CMAKE_CURRENT_BINARY_DIR}/test_showimage_cam2image${target_suffix}_$<CONFIG>.py"
      TARGET test_showimage_cam2image${target_suffix}
      ENV
      RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation}
      RMW_IMPLEMENTATION=${rmw_implementation}
      TIMEOUT 30
    )  # 添加启动测试
  endmacro()


  call_for_each_rmw_implementation(testing_targets)  # 为每个rmw实现调用testing_targets


endif()


ament_package()  # 定义ament包

package.xml

<!-- 声明XML版本为1.0 -->
<?xml version="1.0"?>
<!-- 声明XML模型的链接和模型类型 -->
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<!-- 声明包的格式为2 -->
<package format="2">
  <!-- 声明包的名称为image_tools -->
  <name>image_tools</name>
  <!-- 声明包的版本为0.33.4 -->
  <version>0.33.4</version>
  <!-- 声明包的描述 -->
  <description>Tools to capture and play back images to and from DDS subscriptions and publications.</description>


  <!-- 声明包的维护者 -->
  <maintainer email="aditya.pande@openrobotics.org">Aditya Pande</maintainer>
  <maintainer email="audrow@openrobotics.org">Audrow Nash</maintainer>


  <!-- 声明包的许可证 -->
  <license>Apache License 2.0</license>


  <!-- 声明包的作者 -->
  <author email="dthomas@osrfoundation.org">Dirk Thomas</author>
  <author email="mabel@openrobotics.org">Mabel Zhang</author>


  <!-- 声明构建工具依赖 -->
  <buildtool_depend>ament_cmake</buildtool_depend>
  <!-- 声明构建依赖 -->
  <build_depend>libopencv-dev</build_depend>
  <build_depend>rclcpp</build_depend>
  <build_depend>rclcpp_components</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>std_msgs</build_depend>


  <!-- 声明执行依赖 -->
  <exec_depend>libopencv-dev</exec_depend>
  <exec_depend>rclcpp</exec_depend>
  <exec_depend>rclcpp_components</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>std_msgs</exec_depend>


  <!-- 声明测试依赖 -->
  <test_depend>ament_cmake_pytest</test_depend>
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>
  <test_depend>launch</test_depend>
  <test_depend>launch_ros</test_depend>
  <test_depend>launch_testing</test_depend>
  <test_depend>launch_testing_ament_cmake</test_depend>
  <test_depend>launch_testing_ros</test_depend>
  <test_depend>rmw_implementation_cmake</test_depend>


  <!-- 声明导出信息 -->
  <export>
    <!-- 声明构建类型 -->
    <build_type>ament_cmake</build_type>
  </export>
</package>

showimage.cpp

#include <iostream>  // 包含输入输出流库
#include <sstream>  // 包含字符串流库
#include <string>  // 包含字符串库
#include <vector>  // 包含向量库


#include "opencv2/core/mat.hpp"  // 包含OpenCV核心矩阵库
#include "opencv2/highgui.hpp"  // 包含OpenCV高层GUI库
#include "opencv2/imgproc.hpp"  // 包含OpenCV图像处理库


#include "rcl_interfaces/msg/parameter_descriptor.hpp"  // 包含参数描述符消息
#include "rclcpp/rclcpp.hpp"  // 包含rclcpp库
#include "rclcpp_components/register_node_macro.hpp"  // 包含注册节点宏
#include "sensor_msgs/msg/image.hpp"  // 包含图像消息


#include "image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp"  // 包含自定义类型适配器
#include "image_tools/visibility_control.h"  // 包含可见性控制头文件


#include "./policy_maps.hpp"  // 包含策略映射头文件


RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(
  image_tools::ROSCvMatContainer,
  sensor_msgs::msg::Image);  // 使用自定义类型作为ROS消息类型


namespace image_tools  // 定义命名空间image_tools
{
class ShowImage : public rclcpp::Node  // 定义ShowImage类，继承自rclcpp::Node
{
public:
  IMAGE_TOOLS_PUBLIC
  explicit ShowImage(const rclcpp::NodeOptions & options)  // 显式构造函数
  : Node("showimage", options)  // 调用基类构造函数，初始化节点名称为"showimage"
  {
    setvbuf(stdout, NULL, _IONBF, BUFSIZ);  // 设置标准输出缓冲区
    // Do not execute if a --help option was provided
    if (help(options.arguments())) {  // 如果提供了--help选项，则不执行
      // TODO(jacobperron): Replace with a mechanism for a node to "unload" itself
      // from a container.
      exit(0);  // 退出程序
    }
    parse_parameters();  // 解析参数
    initialize();  // 初始化
  }


private:
  IMAGE_TOOLS_LOCAL
  void initialize()  // 初始化函数
  {
    // Set quality of service profile based on command line options.
    auto qos = rclcpp::QoS(
      rclcpp::QoSInitialization(
        // The history policy determines how messages are saved until taken by
        // the reader.
        // KEEP_ALL saves all messages until they are taken.
        // KEEP_LAST enforces a limit on the number of messages that are saved,
        // specified by the "depth" parameter.
        history_policy_,  // 设置历史策略
        // Depth represents how many messages to store in history when the
        // history policy is KEEP_LAST.
        depth_  // 设置深度
    ));
    // The reliability policy can be reliable, meaning that the underlying transport layer will try
    // ensure that every message gets received in order, or best effort, meaning that the transport
    // makes no guarantees about the order or reliability of delivery.
    qos.reliability(reliability_policy_);  // 设置可靠性策略
    auto callback =
      this {  // 定义回调函数
        process_image(container, show_image_, this->get_logger());  // 处理图像
      };


    RCLCPP_INFO(this->get_logger(), "Subscribing to topic '%s'", topic_.c_str());  // 打印订阅信息
    sub_ = create_subscription<image_tools::ROSCvMatContainer>(topic_, qos, callback);  // 创建订阅


    if (window_name_ == "") {  // 如果没有自定义窗口名称
      // If no custom window name is given, use the topic name
      window_name_ = sub_->get_topic_name();  // 使用主题名称作为窗口名称
    }
  }


  IMAGE_TOOLS_LOCAL
  bool help(const std::vector<std::string> args)  // 帮助函数
  {
    if (std::find(args.begin(), args.end(), "--help") != args.end() ||
      std::find(args.begin(), args.end(), "-h") != args.end())  // 如果提供了--help或-h选项
    {
      std::stringstream ss;  // 创建字符串流
      ss << "Usage: showimage [-h] [--ros-args [-p param:=value] ...]" << std::endl;  // 打印用法
      ss << "Subscribe to an image topic and show the images." << std::endl;  // 打印描述
      ss << "Example: ros2 run image_tools showimage --ros-args -p reliability:=best_effort";  // 打印示例
      ss << std::endl << std::endl;
      ss << "Options:" << std::endl;  // 打印选项
      ss << "  -h, --help\tDisplay this help message and exit";  // 打印帮助选项
      ss << std::endl << std::endl;
      ss << "Parameters:" << std::endl;  // 打印参数
      ss << "  reliability\tReliability QoS setting. Either 'reliable' (default) or 'best_effort'";  // 打印可靠性参数
      ss << std::endl;
      ss << "  history\tHistory QoS setting. Either 'keep_last' (default) or 'keep_all'.";  // 打印历史参数
      ss << std::endl;
      ss << "\t\tIf 'keep_last', then up to N samples are stored where N is the depth";  // 打印深度参数
      ss << std::endl;
      ss << "  depth\t\tDepth of the publisher queue. Only honored if history QoS is 'keep_last'.";  // 打印深度参数
      ss << " Default value is 10";  // 打印默认值
      ss << std::endl;
      ss << "  show_image\tShow the image. Either 'true' (default) or 'false'";  // 打印显示图像参数
      ss << std::endl;
      ss << "  window_name\tName of the display window. Default value is the topic name";  // 打印窗口名称参数
      ss << std::endl;
      std::cout << ss.str();  // 输出帮助信息
      return true;  // 返回true
    }
    return false;  // 返回false
  }


  IMAGE_TOOLS_LOCAL
  void parse_parameters()  // 解析参数函数
  {
    // Parse 'reliability' parameter
    rcl_interfaces::msg::ParameterDescriptor reliability_desc;  // 创建参数描述符
    reliability_desc.description = "Reliability QoS setting for the image subscription";  // 设置描述
    reliability_desc.additional_constraints = "Must be one of: ";  // 设置附加约束
    for (auto entry : name_to_reliability_policy_map) {  // 遍历可靠性策略映射
      reliability_desc.additional_constraints += entry.first + " ";  // 添加约束
    }
    const std::string reliability_param = this->declare_parameter(
      "reliability", "reliable", reliability_desc);  // 声明参数
    auto reliability = name_to_reliability_policy_map.find(reliability_param);  // 查找参数
    if (reliability == name_to_reliability_policy_map.end()) {  // 如果参数无效
      std::ostringstream oss;  // 创建字符串流
      oss << "Invalid QoS reliability setting '" << reliability_param << "'";  // 打印错误信息
      throw std::runtime_error(oss.str());  // 抛出运行时错误
    }
    reliability_policy_ = reliability->second;  // 设置可靠性策略


    // Parse 'history' parameter
    rcl_interfaces::msg::ParameterDescriptor history_desc;  // 创建参数描述符
    history_desc.description = "History QoS setting for the image subscription";  // 设置描述
    history_desc.additional_constraints = "Must be one of: ";  // 设置附加约束
    for (auto entry : name_to_history_policy_map) {  // 遍历历史策略映射
      history_desc.additional_constraints += entry.first + " ";  // 添加约束
    }
    const std::string history_param = this->declare_parameter(
      "history", name_to_history_policy_map.begin()->first, history_desc);  // 声明参数
    auto history = name_to_history_policy_map.find(history_param);  // 查找参数
    if (history == name_to_history_policy_map.end()) {  // 如果参数无效
      std::ostringstream oss;  // 创建字符串流
      oss << "Invalid QoS history setting '" << history_param << "'";  // 打印错误信息
      throw std::runtime_error(oss.str());  // 抛出运行时错误
    }
    history_policy_ = history->second;  // 设置历史策略


    // Declare and get remaining parameters
    depth_ = this->declare_parameter("depth", 10);  // 声明深度参数
    show_image_ = this->declare_parameter("show_image", true); 
    window_name_ = this->declare_parameter("window_name", "");  // 声明窗口名称参数
  }


  /// Convert the ROS Image message to an OpenCV matrix and display it to the user.
  // \param[in] container The image message to show.
  IMAGE_TOOLS_LOCAL
  void process_image(
    const image_tools::ROSCvMatContainer & container, bool show_image, rclcpp::Logger logger)  // 处理图像函数
  {
    RCLCPP_INFO(logger, "Received image #%s", container.header().frame_id.c_str());  // 打印接收图像信息
    std::cerr << "Received image #" << container.header().frame_id.c_str() << std::endl;  // 打印接收图像信息


    if (show_image) {  // 如果显示图像
      cv::Mat frame = container.cv_mat();  // 获取图像矩阵


      if (frame.type() == CV_8UC3 /* rgb8 */) {  // 如果图像类型为CV_8UC3
        cv::cvtColor(frame, frame, cv::COLOR_RGB2BGR);  // 转换颜色空间
      } else if (frame.type() == CV_8UC2) {  // 如果图像类型为CV_8UC2
        container.is_bigendian() ? cv::cvtColor(frame, frame, cv::COLOR_YUV2BGR_UYVY) :
        cv::cvtColor(frame, frame, cv::COLOR_YUV2BGR_YUYV);  // 转换颜色空间
      }


      // Show the image in a window
      cv::imshow(window_name_, frame);  // 在窗口中显示图像
      // Draw the screen and wait for 1 millisecond.
      cv::waitKey(1);  // 刷新窗口并等待1毫秒
    }
  }


  rclcpp::Subscription<image_tools::ROSCvMatContainer>::SharedPtr sub_;  // 订阅对象
  size_t depth_ = rmw_qos_profile_default.depth;  // 深度参数
  rmw_qos_reliability_policy_t reliability_policy_ = rmw_qos_profile_default.reliability;  // 可靠性策略
  rmw_qos_history_policy_t history_policy_ = rmw_qos_profile_default.history;  // 历史策略
  bool show_image_ = true;  // 是否显示图像
  std::string topic_ = "image";  // 主题名称
  std::string window_name_;  // 窗口名称
};


}  // namespace image_tools


RCLCPP_COMPONENTS_REGISTER_NODE(image_tools::ShowImage)  // 注册节点

cam2image.cpp

#include <chrono>  // 包含时间库
#include <memory>  // 包含智能指针库
#include <sstream>  // 包含字符串流库
#include <string>  // 包含字符串库
#include <utility>  // 包含实用工具库
#include <vector>  // 包含向量库


#include "opencv2/core/mat.hpp"  // 包含OpenCV核心矩阵库
#include "opencv2/core.hpp"  // 包含OpenCV核心库
#include "opencv2/highgui.hpp"  // 包含OpenCV高层GUI库
#include "opencv2/videoio.hpp"  // 包含OpenCV视频输入输出库


#include "rcl_interfaces/msg/parameter_descriptor.hpp"  // 包含参数描述符消息
#include "rclcpp/rclcpp.hpp"  // 包含rclcpp库
#include "rclcpp_components/register_node_macro.hpp"  // 包含注册节点宏
#include "std_msgs/msg/bool.hpp"  // 包含布尔消息


#include "image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp"  // 包含自定义类型适配器
#include "image_tools/visibility_control.h"  // 包含可见性控制头文件


#include "./burger.hpp"  // 包含汉堡头文件
#include "./policy_maps.hpp"  // 包含策略映射头文件


RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(
  image_tools::ROSCvMatContainer,
  sensor_msgs::msg::Image);  // 使用自定义类型作为ROS消息类型


namespace image_tools  // 定义命名空间image_tools
{
class Cam2Image : public rclcpp::Node  // 定义Cam2Image类，继承自rclcpp::Node
{
public:
  IMAGE_TOOLS_PUBLIC
  explicit Cam2Image(const rclcpp::NodeOptions & options)  // 显式构造函数
  : Node("cam2image", options),  // 调用基类构造函数，初始化节点名称为"cam2image"
    is_flipped_(false),  // 初始化是否翻转图像为false
    publish_number_(1u)  // 初始化发布编号为1
  {
    setvbuf(stdout, NULL, _IONBF, BUFSIZ);  // 设置标准输出缓冲区
    // Do not execute if a --help option was provided
    if (help(options.arguments())) {  // 如果提供了--help选项，则不执行
      // TODO(jacobperron): Replace with a mechanism for a node to "unload" itself
      // from a container.
      exit(0);  // 退出程序
    }
    parse_parameters();  // 解析参数
    initialize();  // 初始化
  }


private:
  IMAGE_TOOLS_LOCAL
  void initialize()  // 初始化函数
  {
    auto qos = rclcpp::QoS(
      rclcpp::QoSInitialization(
        // The history policy determines how messages are saved until taken by
        // the reader.
        // KEEP_ALL saves all messages until they are taken.
        // KEEP_LAST enforces a limit on the number of messages that are saved,
        // specified by the "depth" parameter.
        history_policy_,  // 设置历史策略
        // Depth represents how many messages to store in history when the
        // history policy is KEEP_LAST.
        depth_  // 设置深度
    ));
    // The reliability policy can be reliable, meaning that the underlying transport layer will try
    // ensure that every message gets received in order, or best effort, meaning that the transport
    // makes no guarantees about the order or reliability of delivery.
    qos.reliability(reliability_policy_);  // 设置可靠性策略
    pub_ = create_publisher<image_tools::ROSCvMatContainer>("image", qos);  // 创建发布者


    // Subscribe to a message that will toggle flipping or not flipping, and manage the state in a
    // callback
    auto callback = this -> void  // 定义回调函数
      {
        this->is_flipped_ = msg->data;  // 设置是否翻转图像
        RCLCPP_INFO(this->get_logger(), "Set flip mode to: %s", this->is_flipped_ ? "on" : "off");  // 打印翻转模式信息
      };
    // Set the QoS profile for the subscription to the flip message.
    sub_ = create_subscription<std_msgs::msg::Bool>(
      "flip_image", rclcpp::SensorDataQoS(), callback);  // 创建订阅


    if (!burger_mode_) {  // 如果不是汉堡模式
      // Initialize OpenCV video capture stream.
      cap.open(device_id_);  // 打开视频捕获设备


      // Set the width and height based on command line arguments.
      cap.set(cv::CAP_PROP_FRAME_WIDTH, static_cast<double>(width_));  // 设置宽度
      cap.set(cv::CAP_PROP_FRAME_HEIGHT, static_cast<double>(height_));  // 设置高度
      if (!cap.isOpened()) {  // 如果视频流未打开
        RCLCPP_ERROR(this->get_logger(), "Could not open video stream");  // 打印错误信息
        throw std::runtime_error("Could not open video stream");  // 抛出运行时错误
      }
    }


    // Start main timer loop
    timer_ = this->create_wall_timer(
      std::chrono::milliseconds(static_cast<int>(1000.0 / freq_)),  // 设置定时器频率
      this {return this->timerCallback();});  // 设置定时器回调函数
  }


  /// Publish camera, or burger, image.
  IMAGE_TOOLS_LOCAL
  void timerCallback()  // 定时器回调函数
  {
    cv::Mat frame;  // 定义图像矩阵


    // Initialize a shared pointer to an Image message.
    // Get the frame from the video capture.
    if (burger_mode_) {  // 如果是汉堡模式
      frame = burger_cap.render_burger(width_, height_);  // 渲染汉堡图像
    } else {
      cap >> frame;  // 从视频捕获设备获取图像
    }


    // If no frame was grabbed, return early
    if (frame.empty()) {  // 如果未获取到图像
      return;  // 提前返回
    }


    // Conditionally flip the image
    if (is_flipped_) {  // 如果需要翻转图像
      cv::flip(frame, frame, 1);  // 翻转图像
    }


    // Conditionally show image
    if (show_camera_) {  // 如果需要显示图像
      // Show the image in a window called "cam2image".
      cv::imshow("cam2image", frame);  // 在窗口中显示图像
      // Draw the image to the screen and wait 1 millisecond.
      cv::waitKey(1);  // 刷新窗口并等待1毫秒
    }


    std_msgs::msg::Header header;  // 定义消息头
    header.frame_id = frame_id_;  // 设置帧ID
    header.stamp = this->now();  // 设置时间戳
    image_tools::ROSCvMatContainer container(frame, header);  // 创建图像容器


    // Publish the image message and increment the publish_number_.
    RCLCPP_INFO(get_logger(), "Publishing image #%zd", publish_number_++);  // 打印发布信息
    pub_->publish(std::move(container));  // 发布图像消息
  }


  IMAGE_TOOLS_LOCAL
  bool help(const std::vector<std::string> & args)  // 帮助函数
  {
    if (std::find(args.begin(), args.end(), "--help") != args.end() ||
      std::find(args.begin(), args.end(), "-h") != args.end())  // 如果提供了--help或-h选项
    {
      std::stringstream ss;  // 创建字符串流
      ss << "Usage: cam2image [-h] [--ros-args [-p param:=value] ...]" << std::endl;  // 打印用法
      ss << "Publish images from a camera stream." << std::endl;  // 打印描述
      ss << "Example: ros2 run image_tools cam2image --ros-args -p reliability:=best_effort";  // 打印示例
      ss << std::endl << std::endl;
      ss << "Options:" << std::endl;  // 打印选项
      ss << "  -h, --help\tDisplay this help message and exit";  // 打印帮助选项
      ss << std::endl << std::endl;
      ss << "Parameters:" << std::endl;  // 打印参数
      ss << "  reliability\tReliability QoS setting. Either 'reliable' (default) or 'best_effort'";  // 打印可靠性参数
      ss << std::endl;
      ss << "  history\tHistory QoS setting. Either 'keep_last' (default) or 'keep_all'.";  // 打印历史参数
      ss << std::endl;
      ss << "\t\tIf 'keep_last', then up to N samples are stored where N is the depth";  // 打
      ss << std::endl;
      ss << "  depth\t\tDepth of the publisher queue. Only honored if history QoS is 'keep_last'.";  // 打印深度参数
      ss << " Default value is 10";  // 打印默认值
      ss << std::endl;
      ss << "  frequency\tPublish frequency in Hz. Default value is 30";  // 打印频率参数
      ss << std::endl;
      ss << "  burger_mode\tProduce images of burgers rather than connecting to a camera";  // 打印汉堡模式参数
      ss << std::endl;
      ss << "  show_camera\tShow camera stream. Either 'true' or 'false' (default)";  // 打印显示摄像头参数
      ss << std::endl;
      ss << "  device_id\tDevice ID of the camera. 0 (default) selects the default camera device.";  // 打印设备ID参数
      ss << std::endl;
      ss << "  width\t\tWidth component of the camera stream resolution. Default value is 320";  // 打印宽度参数
      ss << std::endl;
      ss << "  height\tHeight component of the camera stream resolution. Default value is 240";  // 打印高度参数
      ss << std::endl;
      ss << "  frame_id\t\tID of the sensor frame. Default value is 'camera_frame'";  // 打印帧ID参数
      ss << std::endl << std::endl;
      ss << "Note: try running v4l2-ctl --list-formats-ext to obtain a list of valid values.";  // 打印提示信息
      ss << std::endl;
      std::cout << ss.str();  // 输出帮助信息
      return true;  // 返回true
    }
    return false;  // 返回false
  }


  IMAGE_TOOLS_LOCAL
  void parse_parameters()  // 解析参数函数
  {
    // Parse 'reliability' parameter
    rcl_interfaces::msg::ParameterDescriptor reliability_desc;  // 创建参数描述符
    reliability_desc.description = "Reliability QoS setting for the image publisher";  // 设置描述
    reliability_desc.additional_constraints = "Must be one of: ";  // 设置附加约束
    for (auto entry : name_to_reliability_policy_map) {  // 遍历可靠性策略映射
      reliability_desc.additional_constraints += entry.first + " ";  // 添加约束
    }
    const std::string reliability_param = this->declare_parameter(
      "reliability", "reliable", reliability_desc);  // 声明参数
    auto reliability = name_to_reliability_policy_map.find(reliability_param);  // 查找参数
    if (reliability == name_to_reliability_policy_map.end()) {  // 如果参数无效
      std::ostringstream oss;  // 创建字符串流
      oss << "Invalid QoS reliability setting '" << reliability_param << "'";  // 打印错误信息
      throw std::runtime_error(oss.str());  // 抛出运行时错误
    }
    reliability_policy_ = reliability->second;  // 设置可靠性策略


    // Parse 'history' parameter
    rcl_interfaces::msg::ParameterDescriptor history_desc;  // 创建参数描述符
    history_desc.description = "History QoS setting for the image publisher";  // 设置描述
    history_desc.additional_constraints = "Must be one of: ";  // 设置附加约束
    for (auto entry : name_to_history_policy_map) {  // 遍历历史策略映射
      history_desc.additional_constraints += entry.first + " ";  // 添加约束
    }
    const std::string history_param = this->declare_parameter(
      "history", name_to_history_policy_map.begin()->first, history_desc);  // 声明参数
    auto history = name_to_history_policy_map.find(history_param);  // 查找参数
    if (history == name_to_history_policy_map.end()) {  // 如果参数无效
      std::ostringstream oss;  // 创建字符串流
      oss << "Invalid QoS history setting '" << history_param << "'";  // 打印错误信息
      throw std::runtime_error(oss.str());  // 抛出运行时错误
    }
    history_policy_ = history->second;  // 设置历史策略


    // Declare and get remaining parameters
    depth_ = this->declare_parameter("depth", 10);  // 声明深度参数
    freq_ = this->declare_parameter("frequency", 30.0);  // 声明频率参数
    burger_mode_ = this->declare_parameter("burger_mode", false);  // 声明汉堡模式参数
    show_camera_ = this->declare_parameter("show_camera", false);  // 声明显示摄像头参数
    device_id_ = this->declare_parameter("device_id", 0);  // 声明设备ID参数
    width_ = this->declare_parameter("width", 320);  // 声明宽度参数
    height_ = this->declare_parameter("height", 240);  // 声明高度参数
    frame_id_ = this->declare_parameter("frame_id", "camera_frame");  // 声明帧ID参数
  }


  rclcpp::Publisher<image_tools::ROSCvMatContainer>::SharedPtr pub_;  // 发布者对象
  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr sub_;  // 订阅者对象
  rclcpp::TimerBase::SharedPtr timer_;  // 定时器对象
  cv::VideoCapture cap;  // 视频捕获对象
  Burger burger_cap;  // 汉堡捕获对象
  bool is_flipped_;  // 是否翻转图像
  bool burger_mode_;  // 是否为汉堡模式
  bool show_camera_;  // 是否显示摄像头
  size_t depth_;  // 深度参数
  double freq_;  // 频率参数
  int device_id_;  // 设备ID参数
  int width_;  // 宽度参数
  int height_;  // 高度参数
  std::string frame_id_;  // 帧ID参数
  size_t publish_number_;  // 发布编号
  rmw_qos_reliability_policy_t reliability_policy_;  // 可靠性策略
  rmw_qos_history_policy_t history_policy_;  // 历史策略
};


}  // namespace image_tools


RCLCPP_COMPONENTS_REGISTER_NODE(image_tools::Cam2Image)  // 注册节点

policy_maps.hpp

#ifndef POLICY_MAPS_HPP_  // 如果没有定义POLICY_MAPS_HPP_
#define POLICY_MAPS_HPP_  // 定义POLICY_MAPS_HPP_


#include <map>  // 包含map库
#include <string>  // 包含string库


namespace image_tools  // 定义命名空间image_tools
{


static  // 定义静态变量
std::map<std::string, rmw_qos_reliability_policy_t> name_to_reliability_policy_map = {  // 定义字符串到可靠性策略的映射
  {"reliable", RMW_QOS_POLICY_RELIABILITY_RELIABLE},  // 可靠性策略映射：reliable
  {"best_effort", RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT}  // 可靠性策略映射：best_effort
};


static  // 定义静态变量
std::map<std::string, rmw_qos_history_policy_t> name_to_history_policy_map = {  // 定义字符串到历史策略的映射
  {"keep_last", RMW_QOS_POLICY_HISTORY_KEEP_LAST},  // 历史策略映射：keep_last
  {"keep_all", RMW_QOS_POLICY_HISTORY_KEEP_ALL}  // 历史策略映射：keep_all
};


}  // 结束image_tools命名空间


#endif  // 结束对POLICY_MAPS_HPP_的定义

十年一梦实验室

关注

12
点赞
踩
10

收藏

觉得还不错? 一键收藏
0
评论
【ROS2】演示:为有损网络使用服务质量设置

目录背景先决条件运行演示命令行选项添加网络流量背景请阅读有关 QoS 设置的文档页面，以获取有关 ROS 2 中可用支持的背景信息。在这个演示中，我们将生成一个发布相机图像的节点和另一个订阅图像并在屏幕上显示图像的节点。然后，我们将模拟它们之间的丢包网络连接，并展示不同的服务质量设置如何处理不良链接。先决条件本教程假设您已安装并运行 ROS 2 和 OpenCV。有关安装说明，请...
复制链接

扫一扫