ROS2演示：手动组合节点实现进程内通信
目录
背景
安装演示程序
运行和理解演示
背景
ROS 应用程序通常由执行狭窄(narrow)任务并与系统其他部分解耦的单个“节点”组成。这促进了故障隔离、更快的开发、模块化和代码重用，但往往以性能为代价。在最初开发 ROS 1 之后，高效组合节点的需求变得明显，并开发了 Nodelets。在 ROS 2 中，我们旨在通过解决一些需要重组节点的基本问题来改进 Nodelets 的设计。
在这个演示中，我们将重点介绍如何手动组合节点，通过分别定义节点，但在不同的流程布局中组合它们，而不更改节点的代码或限制其功能。
安装演示程序
请参阅安装说明以了解安装 ROS 2 的详细信息。
如果您已经从软件包安装了 ROS 2，请确保已安装 ros-jazzy-intra-process-demo 。如果您下载了存档或从源代码构建了 ROS 2，(/home/cxy/ros2_jazzy/src/ros2/demos/intra_process_demo )它将已经是安装的一部分。
cxy@cxy-Ubuntu2404:~$ ros2 pkg prefix intra_process_demo
/home/cxy/ros2_jazzy/install/intra_process_demo运行和理解演示
有几种不同的演示：有些是玩具问题，旨在突出进程内通信功能的特点，有些是端到端的示例，使用 OpenCV 并展示了重新组合节点成不同配置的能力。
两个节点管道演示
此演示旨在展示当使用 std::unique_ptr 进行发布和订阅时，进程内发布/订阅连接可以实现消息的零拷贝传输。
首先让我们看看源代码：
https://github.com/ros2/demos/blob/jazzy/intra_process_demo/src/two_node_pipeline/two_node_pipeline.cpp
#include <chrono> // 引入用于时间相关操作的头文件
#include <cinttypes> // 引入用于整数类型的头文件
#include <cstdio> // 引入标准输入输出头文件
#include <memory> // 引入智能指针相关头文件
#include <string> // 引入字符串操作头文件
#include <utility> // 引入用于实用功能的头文件
#include "rclcpp/rclcpp.hpp" // 引入 ROS 2 C++ 客户端库的头文件
#include "std_msgs/msg/int32.hpp" // 引入 ROS 2 中 Int32 消息类型的头文件
using namespace std::chrono_literals; // 使用 chrono 时间文字常量的命名空间
// 生产消息的节点
struct Producer : public rclcpp::Node
{
Producer(const std::string & name, const std::string & output)
: Node(name, rclcpp::NodeOptions().use_intra_process_comms(true)) // 构造函数初始化节点，启用内部进程通信
{
// 在指定的主题上创建一个发布者
pub_ = this->create_publisher<std_msgs::msg::Int32>(output, 10); // 创建一个 Int32 消息类型的发布者，队列长度为 10
std::weak_ptr<std::remove_pointer<decltype(pub_.get())>::type> captured_pub = pub_; // 捕获发布者的弱引用
// 创建一个定时器，每秒大约触发一次，调用回调函数
auto callback = [captured_pub]() -> void {
auto pub_ptr = captured_pub.lock(); // 锁定弱引用，获取发布者的强引用
if (!pub_ptr) {
return; // 如果发布者已被销毁，则退出回调函数
}
static int32_t count = 0; // 静态变量，用于计数
std_msgs::msg::Int32::UniquePtr msg(new std_msgs::msg::Int32()); // 创建一个 Int32 消息的唯一指针
msg->data = count++; // 设置消息数据并自增计数
printf(
"Published message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data,
reinterpret_cast<std::uintptr_t>(msg.get())); // 打印消息值和地址
pub_ptr->publish(std::move(msg)); // 发布消息
};
timer_ = this->create_wall_timer(1s, callback); // 创建墙定时器，每秒调用一次回调函数
}
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub_; // 发布者指针
rclcpp::TimerBase::SharedPtr timer_; // 定时器指针
};
// 消费消息的节点
struct Consumer : public rclcpp::Node
{
Consumer(const std::string & name, const std::string & input)
: Node(name, rclcpp::NodeOptions().use_intra_process_comms(true)) // 构造函数初始化节点，启用内部进程通信
{
// 在指定的主题上创建一个订阅者，接收到新消息时打印消息内容
sub_ = this->create_subscription<std_msgs::msg::Int32>(
input,
10,
[](std_msgs::msg::Int32::UniquePtr msg) {
printf(
" Received message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data,
reinterpret_cast<std::uintptr_t>(msg.get())); // 打印接收到的消息值和地址
});
}
rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sub_; // 订阅者指针
};
int main(int argc, char * argv[])
{
setvbuf(stdout, NULL, _IONBF, BUFSIZ); // 设置标准输出流不进行缓冲
rclcpp::init(argc, argv); // 初始化 ROS 2 系统
rclcpp::executors::SingleThreadedExecutor executor; // 创建单线程执行器
auto producer = std::make_shared<Producer>("producer", "number"); // 创建生产者节点
auto consumer = std::make_shared<Consumer>("consumer", "number"); // 创建消费者节点
executor.add_node(producer); // 将生产者节点添加到执行器
executor.add_node(consumer); // 将消费者节点添加到执行器
executor.spin(); // 启动执行器，处理节点的回调
rclcpp::shutdown(); // 关闭 ROS 2 系统
return 0; // 程序结束
}
# 查看消息 详细信息
cxy@cxy-Ubuntu2404:~$ ros2 interface show std_msgs/msg/Int32
# This was originally provided as an example message.
# It is deprecated as of Foxy
# It is recommended to create your own semantically meaningful message.
# However if you would like to continue using this please use the equivalent in example_msgs.
int32 data
cxy@cxy-Ubuntu2404:~$ ros2 interface list
正如您通过查看 main 函数所看到的，我们有一个生产者节点和一个消费者节点，我们将它们添加到单线程执行器中，然后调用 spin。
如果你查看 Producer 结构体中“producer”节点的实现，你会看到我们创建了一个发布者，它在“number”主题上发布，并且有一个定时器，它会定期创建一个新消息，打印出其在内存中的地址及其内容的值，然后发布它。
“消费者”节点稍微简单一些，您可以在 Consumer 结构体中看到它的实现，因为它只订阅“数字”主题并打印它收到的消息的地址和值。
预期是生产者将打印出一个地址和值，消费者将打印出一个匹配的地址和值。这表明进程内通信确实有效，并且避免了不必要的复制，至少对于简单的图形来说是这样。
让我们通过执行 ros2 run intra_process_demo two_node_pipeline 可执行文件来运行演示（别忘了先获取设置文件的源代码）：
$ ros2 run intra_process_demo two_node_pipeline
Published message with value: 0, and address: 0x7fb02303faf0
Published message with value: 1, and address: 0x7fb020cf0520
Received message with value: 1, and address: 0x7fb020cf0520
Published message with value: 2, and address: 0x7fb020e12900
Received message with value: 2, and address: 0x7fb020e12900
Published message with value: 3, and address: 0x7fb020cf0520
Received message with value: 3, and address: 0x7fb020cf0520
Published message with value: 4, and address: 0x7fb020e12900
Received message with value: 4, and address: 0x7fb020e12900
Published message with value: 5, and address: 0x7fb02303cea0
Received message with value: 5, and address: 0x7fb02303cea0
[...]有一件事你会注意到，那就是消息大约每秒钟跳动一次。这是因为我们告诉计时器大约每秒钟触发一次。
您可能还注意到，第一条消息（值为 0 ）没有相应的“收到消息...”行。这是因为发布/订阅是“尽力而为”，我们没有启用“锁存”行为。这意味着如果发布者在订阅建立之前发布消息，订阅将不会收到该消息。这种竞争条件可能导致前几条消息丢失。在这种情况下，由于它们每秒只出现一次，通常只有第一条消息会丢失。
最后，您可以看到具有相同值的“已发布消息...”和“已接收消息...”行也具有相同的地址。这表明接收到的消息的地址与发布的消息的地址相同，并且它不是副本。这是因为我们使用 std::unique_ptr 发布和订阅消息，这允许消息的所有权在系统中安全地移动。您也可以使用 const & 和 std::shared_ptr 进行发布和订阅，但在这种情况下不会发生零拷贝。
循环管道演示
此演示与之前的类似，但不同之处在于，生产者不会为每次迭代创建新消息，而是仅使用一个消息实例。这是通过在图中创建一个循环并通过外部使其中一个节点在旋转执行器之前发布来“启动”通信来实现的。
https://github.com/ros2/demos/blob/jazzy/intra_process_demo/src/cyclic_pipeline/cyclic_pipeline.cpp
#include <chrono> // 引入用于时间相关操作的头文件
#include <cinttypes> // 引入用于整数类型的头文件
#include <cstdio> // 引入标准输入输出头文件
#include <memory> // 引入智能指针相关头文件
#include <string> // 引入字符串操作头文件
#include <utility> // 引入用于实用功能的头文件
#include "rclcpp/rclcpp.hpp" // 引入 ROS 2 C++ 客户端库的头文件
#include "std_msgs/msg/int32.hpp" // 引入 ROS 2 中 Int32 消息类型的头文件
using namespace std::chrono_literals; // 使用 chrono 时间文字常量的命名空间
// 这个节点接收一个 Int32 消息，等待 1 秒，然后递增并发送该消息
struct IncrementerPipe : public rclcpp::Node
{
IncrementerPipe(const std::string & name, const std::string & in, const std::string & out)
: Node(name, rclcpp::NodeOptions().use_intra_process_comms(true)) // 构造函数初始化节点，启用内部进程通信
{
// 在输出主题上创建一个发布者
pub = this->create_publisher<std_msgs::msg::Int32>(out, 10); // 创建一个 Int32 消息类型的发布者，队列长度为 10
std::weak_ptr<std::remove_pointer<decltype(pub.get())>::type> captured_pub = pub; // 捕获发布者的弱引用
// 在输入主题上创建一个订阅者
sub = this->create_subscription<std_msgs::msg::Int32>(
in,
10,
[captured_pub](std_msgs::msg::Int32::UniquePtr msg) { // 回调函数接收消息
auto pub_ptr = captured_pub.lock(); // 锁定弱引用，获取发布者的强引用
if (!pub_ptr) {
return; // 如果发布者已被销毁，则退出回调函数
}
printf(
"Received message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data,
reinterpret_cast<std::uintptr_t>(msg.get())); // 打印接收到的消息值和地址
printf(" sleeping for 1 second...\n");
if (!rclcpp::sleep_for(1s)) {
return; // 如果睡眠失败（例如按下 Ctrl-C），则返回
}
printf(" done.\n");
msg->data++; // 递增消息的数据
printf(
"Incrementing and sending with value: %d, and address: 0x%" PRIXPTR "\n", msg->data,
reinterpret_cast<std::uintptr_t>(msg.get())); // 打印递增后的消息值和地址
pub_ptr->publish(std::move(msg)); // 发布消息到输出主题
});
}
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub; // 发布者指针
rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sub; // 订阅者指针
};
int main(int argc, char * argv[])
{
setvbuf(stdout, NULL, _IONBF, BUFSIZ); // 设置标准输出流不进行缓冲
rclcpp::init(argc, argv); // 初始化 ROS 2 系统
rclcpp::executors::SingleThreadedExecutor executor; // 创建单线程执行器
// 创建一个简单的循环，通过连接两个 IncrementerPipe 的输入和输出主题
// 期望消息在它们之间传递时地址不变
auto pipe1 = std::make_shared<IncrementerPipe>("pipe1", "topic1", "topic2"); // 创建第一个管道节点
auto pipe2 = std::make_shared<IncrementerPipe>("pipe2", "topic2", "topic1"); // 创建第二个管道节点
rclcpp::sleep_for(1s); // 等待订阅建立以避免竞态条件
// 发布第一条消息（启动循环）
std::unique_ptr<std_msgs::msg::Int32> msg(new std_msgs::msg::Int32());
msg->data = 42;
printf(
"Published first message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data,
reinterpret_cast<std::uintptr_t>(msg.get())); // 打印发布的第一条消息的值和地址
pipe1->pub->publish(std::move(msg)); // 通过第一个管道节点发布消息
executor.add_node(pipe1); // 将第一个管道节点添加到执行器
executor.add_node(pipe2); // 将第二个管道节点添加到执行器
executor.spin(); // 启动执行器，处理节点的回调
rclcpp::shutdown(); // 关闭 ROS 2 系统
return 0; // 程序结束
}订阅的lambda函数
与之前的演示不同，此演示仅使用一个节点，两次实例化，具有不同的名称和配置。图最终变成 pipe1 -> pipe2 -> pipe1 … 循环。
该行 pipe1->pub->publish(msg); 启动了该过程，但从那时起，消息在节点之间来回传递，每个节点在其自己的订阅回调中调用发布。
这里的期望是节点每秒钟来回传递一次消息，每次递增消息的值。由于消息是作为 unique_ptr 发布和订阅的，因此从一开始创建的同一消息会被连续使用。
为了测试这些期望，让我们运行它：
$ ros2 run intra_process_demo cyclic_pipeline
Published first message with value: 42, and address: 0x7fd2ce0a2bc0
Received message with value: 42, and address: 0x7fd2ce0a2bc0
sleeping for 1 second...
done.
Incrementing and sending with value: 43, and address: 0x7fd2ce0a2bc0
Received message with value: 43, and address: 0x7fd2ce0a2bc0
sleeping for 1 second...
done.
Incrementing and sending with value: 44, and address: 0x7fd2ce0a2bc0
Received message with value: 44, and address: 0x7fd2ce0a2bc0
sleeping for 1 second...
done.
Incrementing and sending with value: 45, and address: 0x7fd2ce0a2bc0
Received message with value: 45, and address: 0x7fd2ce0a2bc0
sleeping for 1 second...
done.
Incrementing and sending with value: 46, and address: 0x7fd2ce0a2bc0
Received message with value: 46, and address: 0x7fd2ce0a2bc0
sleeping for 1 second...
done.
Incrementing and sending with value: 47, and address: 0x7fd2ce0a2bc0
Received message with value: 47, and address: 0x7fd2ce0a2bc0
sleeping for 1 second...
[...]您应该在每次迭代中看到不断增加的数字，从 42 开始……因为 42，并且在整个过程中它重复使用相同的消息，正如指针地址所示，它们不会改变，从而避免了不必要的复制。
图像管道演示
在这个演示中，我们将使用 OpenCV 来捕获、注释，然后查看图像。
注意
如果您使用的是 macOS，并且这些示例不起作用或收到类似 ddsi_conn_write failed -1 的错误，那么您需要增加系统范围的 UDP 数据包大小
$ sudo sysctl -w net.inet.udp.recvspace=209715
$ sudo sysctl -w net.inet.udp.maxdgram=65500这些更改在重启后将不会持续。简单的管道
首先，我们将有一个由三个节点组成的管道，排列如下： camera_node -> watermark_node -> image_view_node
// image_pipeline_all_in_one.cpp
#include <memory> // 引入智能指针相关头文件
#include "rclcpp/rclcpp.hpp" // 引入 ROS 2 C++ 客户端库的头文件
#include "image_pipeline/camera_node.hpp" // 引入自定义的 CameraNode 头文件
#include "image_pipeline/image_view_node.hpp" // 引入自定义的 ImageViewNode 头文件
#include "image_pipeline/watermark_node.hpp" // 引入自定义的 WatermarkNode 头文件
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv); // 初始化 ROS 2 系统
rclcpp::executors::SingleThreadedExecutor executor; // 创建单线程执行器
// 将节点连接成一个流水线：camera_node -> watermark_node -> image_view_node
std::shared_ptr<CameraNode> camera_node = nullptr; // 初始化 CameraNode 指针为空
try {
camera_node = std::make_shared<CameraNode>("image"); // 创建 CameraNode 节点，主题名为 "image"
} catch (const std::exception & e) {
fprintf(stderr, "%s Exiting ..\n", e.what()); // 如果创建失败，打印错误信息并退出
return 1;
}
auto watermark_node =
std::make_shared<WatermarkNode>("image", "watermarked_image", "Hello world!"); // 创建 WatermarkNode 节点，输入主题为 "image"，输出主题为 "watermarked_image"，水印内容为 "Hello world!"
auto image_view_node = std::make_shared<ImageViewNode>("watermarked_image"); // 创建 ImageViewNode 节点，订阅主题为 "watermarked_image"
executor.add_node(camera_node); // 将 CameraNode 节点添加到执行器
executor.add_node(watermark_node); // 将 WatermarkNode 节点添加到执行器
executor.add_node(image_view_node); // 将 ImageViewNode 节点添加到执行器
executor.spin(); // 启动执行器，处理节点的回调
rclcpp::shutdown(); // 关闭 ROS 2 系统
return 0; // 程序结束
}camera_node 从您计算机上的相机设备 0 读取信息，在图像上写入一些信息并发布。 watermark_node 订阅 camera_node 的输出，并在发布之前添加更多文本。最后， image_view_node 订阅 watermark_node 的输出，在图像上写入更多文本，然后使用 cv::imshow 进行可视化。
在每个节点中，正在发送或已接收的消息地址或两者都被写入图像。水印和图像视图节点被设计为在不复制图像的情况下修改图像，因此只要节点在同一进程中并且图形保持在如上所述的管道中组织，图像上印刻的地址应该都是相同的。
注意
在某些系统上（我们在 Linux 上见过这种情况），打印到屏幕上的地址可能不会改变。这是因为相同的唯一指针正在被重用。在这种情况下，管道仍在运行。
让我们通过执行以下可执行文件来运行演示：
ros2 run intra_process_demo image_pipeline_all_in_one你应该看到这样的内容：
您可以通过按空格键暂停图像渲染，再次按空格键恢复。您也可以按 q 或 ESC 退出。
如果你暂停图像查看器，你应该能够比较图像上写的地址，并看到它们是相同的。
具有两个图像查看器的管道
现在让我们看一个与上面类似的例子，只是它有两个图像视图节点。所有节点仍在同一进程中，但现在应该会显示两个图像视图窗口。（macOS 用户注意：您的图像视图窗口可能会重叠）。让我们使用以下命令运行它：
ros2 run intra_process_demo image_pipeline_with_two_image_view
// image_pipeline_with_two_image_view.cpp
#include <memory> // 引入智能指针相关头文件
#include "rclcpp/rclcpp.hpp" // 引入 ROS 2 C++ 客户端库的头文件
#include "image_pipeline/camera_node.hpp" // 引入自定义的 CameraNode 头文件
#include "image_pipeline/image_view_node.hpp" // 引入自定义的 ImageViewNode 头文件
#include "image_pipeline/watermark_node.hpp" // 引入自定义的 WatermarkNode 头文件
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv); // 初始化 ROS 2 系统
rclcpp::executors::SingleThreadedExecutor executor; // 创建单线程执行器
// 将节点连接成一个流水线：camera_node -> watermark_node -> image_view_node
// 以及一个额外的分支： \-> image_view_node2
std::shared_ptr<CameraNode> camera_node = nullptr; // 初始化 CameraNode 指针为空
try {
camera_node = std::make_shared<CameraNode>("image"); // 创建 CameraNode 节点，主题名为 "image"
} catch (const std::exception & e) {
fprintf(stderr, "%s Exiting..\n", e.what()); // 如果创建失败，打印错误信息并退出
return 1;
}
auto watermark_node =
std::make_shared<WatermarkNode>("image", "watermarked_image", "Hello world!"); // 创建 WatermarkNode 节点，输入主题为 "image"，输出主题为 "watermarked_image"，水印内容为 "Hello world!"
auto image_view_node = std::make_shared<ImageViewNode>("watermarked_image"); // 创建 ImageViewNode 节点，订阅主题为 "watermarked_image"
auto image_view_node2 = std::make_shared<ImageViewNode>("watermarked_image", "image_view_node2"); // 创建第二个 ImageViewNode 节点，订阅主题为 "watermarked_image"，节点名为 "image_view_node2"
executor.add_node(camera_node); // 将 CameraNode 节点添加到执行器
executor.add_node(watermark_node); // 将 WatermarkNode 节点添加到执行器
executor.add_node(image_view_node); // 将第一个 ImageViewNode 节点添加到执行器
executor.add_node(image_view_node2); // 将第二个 ImageViewNode 节点添加到执行器
executor.spin(); // 启动执行器，处理节点的回调
rclcpp::shutdown(); // 关闭 ROS 2 系统
return 0; // 程序结束
}就像上一个例子一样，您可以使用空格键暂停渲染，再次按下空格键继续。您可以停止更新以检查写入屏幕的指针。
如上图所示，我们有一张图片，其中所有指针都相同，然后另一张图片的前两个条目与第一张图片的指针相同，但第二张图片的最后一个指针不同。要理解为什么会发生这种情况，请考虑图的拓扑结构：
camera_node -> watermark_node -> image_view_node
-> image_view_node2camera_node 和 watermark_node 之间的链接可以使用相同的指针而无需复制，因为只有一个进程内订阅需要接收消息。但是对于 watermark_node 和两个图像视图节点之间的链接，关系是一对多，因此如果图像视图节点使用 unique_ptr 回调，则不可能将相同指针的所有权传递给两个节点。然而，它可以传递给其中一个。哪个节点会获得原始指针没有定义，而是最后一个接收的节点。
请注意，图像视图节点没有订阅 unique_ptr 回调。相反，它们订阅了 const shared_ptr 。这意味着系统将相同的 shared_ptr 传递给两个回调。当处理第一个进程内订阅时，内部存储的 unique_ptr 将提升为 shared_ptr 。每个回调将共享同一消息的所有权。
管道与进程间查看器
另一件重要的事情是避免在进行进程间订阅时中断进程内零拷贝行为。为了测试这一点，我们可以运行第一个图像管道演示， image_pipeline_all_in_one ，然后运行一个独立的 image_view_node 实例（不要忘记在终端中为它们加上 ros2 run intra_process_demo 前缀）。这将看起来像这样：
要同时暂停两个图像很难，因此图像可能无法对齐，但重要的是要注意 image_pipeline_all_in_one 图像视图在每一步都显示相同的地址。这意味着即使订阅了外部视图，进程内零拷贝也得以保留。你还可以看到，进程间图像视图在前两行文本中有不同的进程 ID，而在第三行文本中有独立图像查看器的进程 ID。
附录：
camera_node.cpp
#include <memory> // 引入智能指针相关头文件
#include "image_pipeline/camera_node.hpp" // 引入自定义的 CameraNode 头文件
#include "rclcpp/rclcpp.hpp" // 引入 ROS 2 C++ 客户端库的头文件
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv); // 初始化 ROS 2 系统
std::shared_ptr<CameraNode> camera_node = nullptr; // 初始化 CameraNode 指针为空
try {
camera_node = std::make_shared<CameraNode>("image"); // 创建 CameraNode 节点，主题名为 "image"
} catch (const std::exception & e) {
fprintf(stderr, "%s Exiting..\n", e.what()); // 如果创建失败，打印错误信息并退出
return 1; // 返回错误代码 1
}
rclcpp::spin(camera_node); // 开始处理 CameraNode 节点的回调
rclcpp::shutdown(); // 关闭 ROS 2 系统
return 0; // 程序结束，返回 0
}camera_node.hpp
#ifndef IMAGE_PIPELINE__CAMERA_NODE_HPP_
#define IMAGE_PIPELINE__CAMERA_NODE_HPP_
#include <chrono>
#include <sstream>
#include <string>
#include <thread>
#include <utility>
#include "opencv2/highgui/highgui.hpp" // 引入 OpenCV 头文件，用于捕捉和处理图像
#include "rclcpp/rclcpp.hpp" // 引入 ROS 2 C++ 客户端库的头文件
#include "sensor_msgs/msg/image.hpp" // 引入 ROS 2 图像消息类型
#include "common.hpp" // 引入自定义的通用头文件
/// 使用 OpenCV 从相机捕捉图像并发布图像的节点。
/// 图像带有该进程的 ID 以及消息指针的注释。
class CameraNode final : public rclcpp::Node
{
public:
/// \brief 构造一个用于捕捉视频的 CameraNode 对象
/// \param output 使用的输出主题名
/// \param node_name 使用的节点名
/// \param watermark 是否在发布前添加水印到图像
/// \param device 使用的相机设备
/// \param width 捕捉视频的宽度
/// \param height 捕捉视频的高度
explicit CameraNode(
const std::string & output, const std::string & node_name = "camera_node",
bool watermark = true, int device = 0, int width = 320, int height = 240)
: Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true)),
canceled_(false), watermark_(watermark)
{
// 初始化 OpenCV
cap_.open(device);
// TODO: 当不再支持 Xenial 时移除预编译检查
#if CV_MAJOR_VERSION < 3
cap_.set(CV_CAP_PROP_FRAME_WIDTH, static_cast<double>(width));
cap_.set(CV_CAP_PROP_FRAME_HEIGHT, static_cast<double>(height));
#else
cap_.set(cv::CAP_PROP_FRAME_WIDTH, static_cast<double>(width));
cap_.set(cv::CAP_PROP_FRAME_HEIGHT, static_cast<double>(height));
#endif
if (!cap_.isOpened()) {
throw std::runtime_error("无法打开视频流！");
}
// 在输出主题上创建发布者。
pub_ = this->create_publisher<sensor_msgs::msg::Image>(output, rclcpp::SensorDataQoS());
// 创建摄像头读取循环。
thread_ = std::thread([this]() { return this->loop(); });
}
~CameraNode()
{
// 确保在关闭时加入线程。
canceled_.store(true);
if (thread_.joinable()) {
thread_.join();
}
}
/// \brief 捕捉并发布数据直到程序关闭
void loop()
{
// 在运行时...
while (rclcpp::ok() && !canceled_.load()) {
// 从 OpenCV 捕捉一帧图像。
cap_ >> frame_;
if (frame_.empty()) {
continue;
}
// 创建一个新的 Image 消息的 unique_ptr 进行存储。
sensor_msgs::msg::Image::UniquePtr msg(new sensor_msgs::msg::Image());
if (watermark_) {
std::stringstream ss;
// 在图像上添加该进程的 ID 和消息指针地址。
ss << "pid: " << GETPID() << ", ptr: " << msg.get();
draw_on_image(frame_, ss.str(), 20);
}
// 将 OpenCV 图像打包到 ROS 图像消息中。
set_now(msg->header.stamp);
msg->header.frame_id = "camera_frame";
msg->height = frame_.rows;
msg->width = frame_.cols;
msg->encoding = mat_type2encoding(frame_.type());
msg->is_bigendian = false;
msg->step = static_cast<sensor_msgs::msg::Image::_step_type>(frame_.step);
msg->data.assign(frame_.datastart, frame_.dataend);
pub_->publish(std::move(msg)); // 发布消息。
}
}
private:
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_; // 图像消息的发布者
std::thread thread_; // 用于运行图像捕捉和发布的线程
std::atomic<bool> canceled_; // 用于控制线程的原子变量
/// 是否在图像上添加显示进程 ID 和指针位置的水印
bool watermark_;
cv::VideoCapture cap_; // OpenCV 视频捕捉对象
cv::Mat frame_; // 存储捕捉到的帧
};
#endif // IMAGE_PIPELINE__CAMERA_NODE_HPP_common.hpp
#ifndef IMAGE_PIPELINE__COMMON_HPP_
#define IMAGE_PIPELINE__COMMON_HPP_
#ifdef _WIN32
#include <process.h>
#define GETPID _getpid // 在 Windows 上使用 _getpid 获取进程 ID
#else
#include <unistd.h>
#define GETPID getpid // 在非 Windows 系统上使用 getpid 获取进程 ID
#endif
#include <chrono> // 引入时间相关库
#include <string> // 引入字符串库
#include "opencv2/opencv.hpp" // 引入 OpenCV 库
#include "builtin_interfaces/msg/time.hpp" // 引入 ROS 内置时间消息类型
// 将编码类型转换为 OpenCV 矩阵类型
int
encoding2mat_type(const std::string & encoding)
{
if (encoding == "mono8") {
return CV_8UC1;
} else if (encoding == "bgr8") {
return CV_8UC3;
} else if (encoding == "mono16") {
return CV_16SC1;
} else if (encoding == "rgba8") {
return CV_8UC4;
}
throw std::runtime_error("不支持的矩阵类型");
}
// 将 OpenCV 矩阵类型转换为编码类型
std::string
mat_type2encoding(int mat_type)
{
switch (mat_type) {
case CV_8UC1:
return "mono8";
case CV_8UC3:
return "bgr8";
case CV_16SC1:
return "mono16";
case CV_8UC4:
return "rgba8";
default:
throw std::runtime_error("不支持的编码类型");
}
}
// 设置当前时间到 ROS 时间消息
void set_now(builtin_interfaces::msg::Time & time)
{
std::chrono::nanoseconds now = std::chrono::high_resolution_clock::now().time_since_epoch();
if (now <= std::chrono::nanoseconds(0)) {
time.sec = time.nanosec = 0;
} else {
time.sec = static_cast<builtin_interfaces::msg::Time::_sec_type>(now.count() / 1000000000);
time.nanosec = now.count() % 1000000000;
}
}
// 在图像上绘制文字
void
draw_on_image(cv::Mat & image, const std::string & text, int height)
{
cv::Mat c_mat = image;
cv::putText(
c_mat,
text.c_str(),
cv::Point(10, height),
cv::FONT_HERSHEY_SIMPLEX,
0.3,
cv::Scalar(0, 255, 0));
}
#endif // IMAGE_PIPELINE__COMMON_HPP_watermark_node.hpp
#ifndef IMAGE_PIPELINE__WATERMARK_NODE_HPP_
#define IMAGE_PIPELINE__WATERMARK_NODE_HPP_
#include <memory>
#include <sstream>
#include <string>
#include <utility>
#include "opencv2/opencv.hpp" // 引入 OpenCV 库
#include "rclcpp/rclcpp.hpp" // 引入 ROS 2 库
#include "sensor_msgs/msg/image.hpp" // 引入图像消息类型
#include "common.hpp" // 引入通用工具函数和宏
/// 该节点接收图像，添加水印文本后再发布图像
class WatermarkNode final : public rclcpp::Node
{
public:
/// \brief 构造一个 WatermarkNode 对象，接受图像、添加水印并重新发布
/// \param input 要订阅的主题名
/// \param output 要发布带水印图像的主题名
/// \param text 要添加到图像上的文本
/// \param node_name 节点名称
explicit WatermarkNode(
const std::string & input, const std::string & output, const std::string & text,
const std::string & node_name = "watermark_node")
: Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true)) // 使用节点选项初始化节点
{
rclcpp::SensorDataQoS qos; // 设置 QoS 为传感器数据
// 在输出主题上创建一个发布者
pub_ = this->create_publisher<sensor_msgs::msg::Image>(output, qos);
std::weak_ptr<std::remove_pointer<decltype(pub_.get())>::type> captured_pub = pub_; // 弱指针捕获发布者
// 在输入主题上创建一个订阅者
sub_ = this->create_subscription<sensor_msgs::msg::Image>(
input,
qos,
[captured_pub, text](sensor_msgs::msg::Image::UniquePtr msg) { // 接收到消息时的回调函数
auto pub_ptr = captured_pub.lock(); // 锁定发布者指针
if (!pub_ptr) {
return; // 如果发布者指针无效，则返回
}
// 从图像消息创建一个 cv::Mat（不进行复制）
cv::Mat cv_mat(
msg->height, msg->width,
encoding2mat_type(msg->encoding),
msg->data.data());
// 在图像上添加进程 ID、指针地址和水印文本
std::stringstream ss;
ss << "pid: " << GETPID() << ", ptr: " << msg.get() << " " << text;
draw_on_image(cv_mat, ss.str(), 40); // 在图像上绘制文本
pub_ptr->publish(std::move(msg)); // 发布带水印的图像消息
});
}
private:
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_; // 发布者
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_; // 订阅者
cv::VideoCapture cap_; // 视频捕获对象
cv::Mat frame_; // 图像帧
};
#endif // IMAGE_PIPELINE__WATERMARK_NODE_HPP_watermark_node.cpp
#include <memory> // 引入内存管理相关的库
#include "image_pipeline/watermark_node.hpp" // 引入水印节点头文件
#include "rclcpp/rclcpp.hpp" // 引入 ROS 2 库
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv); // 初始化 ROS 2
auto watermark_node =
std::make_shared<WatermarkNode>("image", "watermarked_image", "Hello world!"); // 创建一个水印节点实例
rclcpp::spin(watermark_node); // 开始处理节点的回调函数
rclcpp::shutdown(); // 关闭 ROS 2
return 0;
}image_view_node.hpp
#ifndef IMAGE_PIPELINE__IMAGE_VIEW_NODE_HPP_
#define IMAGE_PIPELINE__IMAGE_VIEW_NODE_HPP_
#include <sstream>
#include <string>
#include "opencv2/highgui/highgui.hpp" // 引入 OpenCV 高级 GUI 模块
#include "rclcpp/rclcpp.hpp" // 引入 ROS 2 库
#include "sensor_msgs/msg/image.hpp" // 引入图像消息类型
#include "common.hpp" // 引入通用工具函数和宏
/// 接收 sensor_msgs/Image 消息并使用 OpenCV 渲染它们的节点
class ImageViewNode final : public rclcpp::Node
{
public:
/// \brief 构造一个用于可视化图像数据的 ImageViewNode
/// \param input 要订阅的主题名
/// \param node_name 节点名称
/// \param watermark 是否在显示图像前添加水印
explicit ImageViewNode(
const std::string & input, const std::string & node_name = "image_view_node",
bool watermark = true)
: Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true)) // 使用节点选项初始化节点
{
// 在输入主题上创建一个订阅者
sub_ = this->create_subscription<sensor_msgs::msg::Image>(
input,
rclcpp::SensorDataQoS(), // 使用传感器数据 QoS
[node_name, watermark](sensor_msgs::msg::Image::ConstSharedPtr msg) { // 接收到消息时的回调函数
// 从图像消息创建一个 cv::Mat（不进行复制）
cv::Mat cv_mat(
msg->height, msg->width,
encoding2mat_type(msg->encoding),
const_cast<unsigned char *>(msg->data.data()));
if (watermark) {
// 在图像上添加进程 ID 和指针地址
std::stringstream ss;
ss << "pid: " << GETPID() << ", ptr: " << msg.get();
draw_on_image(cv_mat, ss.str(), 60); // 在图像上绘制文本
}
// 显示图像
cv::Mat c_mat = cv_mat;
cv::imshow(node_name.c_str(), c_mat);
char key = cv::waitKey(1); // 监听按键
if (key == 27 /* ESC */ || key == 'q') {
rclcpp::shutdown(); // 按下 ESC 或 q 键关闭节点
}
if (key == ' ') { // 按下空格键暂停直到再次按下空格键
key = '\0';
while (key != ' ') {
key = cv::waitKey(1);
if (key == 27 /* ESC */ || key == 'q') {
rclcpp::shutdown(); // 按下 ESC 或 q 键关闭节点
}
if (!rclcpp::ok()) {
break; // 如果节点已经关闭，跳出循环
}
}
}
});
}
private:
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_; // 订阅者
cv::VideoCapture cap_; // 视频捕获对象
cv::Mat frame_; // 图像帧
};
#endif // IMAGE_PIPELINE__IMAGE_VIEW_NODE_HPP_image_view_node.cpp
#include <memory> // 引入内存管理相关的库
#include "image_pipeline/image_view_node.hpp" // 引入图像查看节点头文件
#include "rclcpp/rclcpp.hpp" // 引入 ROS 2 库
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv); // 初始化 ROS 2
auto image_view_node = std::make_shared<ImageViewNode>("watermarked_image"); // 创建一个图像查看节点实例，订阅名为 "watermarked_image" 的主题
rclcpp::spin(image_view_node); // 开始处理节点的回调函数
rclcpp::shutdown(); // 关闭 ROS 2
return 0;
}
扫一扫
3511
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
