当前位置：首页 > news >正文

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

news 来源：原创 2024/9/20 16:34:51

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

背景

请阅读有关 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_effortOptions:-h, --help  Display this help message and exitParameters: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 depthdepth    Depth of the publisher queue. Only honored if history QoS is 'keep_last'. Default value is 10show_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'时有效。默认值为10show_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_effortros2 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.in7 directories, 18 files

CMakeLists.txt

cmake_minimum_required(VERSION 3.5)  # 设置CMake的最低版本为3.5project(image_tools)  # 定义项目名称为image_tools# Default to C++17
if(NOT CMAKE_CXX_STANDARD)  # 如果没有设置C++标准set(CMAKE_CXX_STANDARD 17)  # 设置C++标准为C++17set(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.cppsrc/cam2image.cppsrc/cv_mat_sensor_msgs_image_type_adapter.cppsrc/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)  # 注册节点ShowImageinstall(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_targetsset(RCLCPP_DEMO_CAM2IMAGE_EXECUTABLE $<TARGET_FILE:cam2image>)  # 设置cam2image可执行文件set(RCLCPP_DEMO_SHOWIMAGE_EXECUTABLE $<TARGET_FILE:showimage>)  # 设置showimage可执行文件configure_file(test/test_executables_demo.py.intest_showimage_cam2image${target_suffix}.py.genexp@ONLY)  # 配置文件file(GENERATEOUTPUT "${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}ENVRCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation}RMW_IMPLEMENTATION=${rmw_implementation}TIMEOUT 30)  # 添加启动测试endmacro()call_for_each_rmw_implementation(testing_targets)  # 为每个rmw实现调用testing_targetsendif()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_PUBLICexplicit ShowImage(const rclcpp::NodeOptions & options)  // 显式构造函数: Node("showimage", options)  // 调用基类构造函数，初始化节点名称为"showimage"{setvbuf(stdout, NULL, _IONBF, BUFSIZ);  // 设置标准输出缓冲区// Do not execute if a --help option was providedif (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_LOCALvoid 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 namewindow_name_ = sub_->get_topic_name();  // 使用主题名称作为窗口名称}}IMAGE_TOOLS_LOCALbool 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_LOCALvoid parse_parameters()  // 解析参数函数{// Parse 'reliability' parameterrcl_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' parameterrcl_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 parametersdepth_ = 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_LOCALvoid 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_8UC3cv::cvtColor(frame, frame, cv::COLOR_RGB2BGR);  // 转换颜色空间} else if (frame.type() == CV_8UC2) {  // 如果图像类型为CV_8UC2container.is_bigendian() ? cv::cvtColor(frame, frame, cv::COLOR_YUV2BGR_UYVY) :cv::cvtColor(frame, frame, cv::COLOR_YUV2BGR_YUYV);  // 转换颜色空间}// Show the image in a windowcv::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_toolsRCLCPP_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_PUBLICexplicit Cam2Image(const rclcpp::NodeOptions & options)  // 显式构造函数: Node("cam2image", options),  // 调用基类构造函数，初始化节点名称为"cam2image"is_flipped_(false),  // 初始化是否翻转图像为falsepublish_number_(1u)  // 初始化发布编号为1{setvbuf(stdout, NULL, _IONBF, BUFSIZ);  // 设置标准输出缓冲区// Do not execute if a --help option was providedif (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_LOCALvoid 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// callbackauto 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 looptimer_ = this->create_wall_timer(std::chrono::milliseconds(static_cast<int>(1000.0 / freq_)),  // 设置定时器频率this {return this->timerCallback();});  // 设置定时器回调函数}/// Publish camera, or burger, image.IMAGE_TOOLS_LOCALvoid 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 earlyif (frame.empty()) {  // 如果未获取到图像return;  // 提前返回}// Conditionally flip the imageif (is_flipped_) {  // 如果需要翻转图像cv::flip(frame, frame, 1);  // 翻转图像}// Conditionally show imageif (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_;  // 设置帧IDheader.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_LOCALbool 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_LOCALvoid parse_parameters()  // 解析参数函数{// Parse 'reliability' parameterrcl_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' parameterrcl_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 parametersdepth_ = 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_toolsRCLCPP_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_的定义