ROS学习过程中的错误记录2 - 编写简单的消息发布器和订阅器(C++)

<think>我们正在编写一个C++程序，用于在ROS 2中创建订阅者来接收和处理来自ros2bag的图像消息。ros2bag是ROS 2中用于记录和回放话题数据的工具。当回放时，它会将之前记录的消息发布到相应的话题上，因此我们可以像订阅普通话题一样订阅这些消息。 步骤： 1. 创建一个ROS 2节点。 2. 在该节点中创建一个订阅者，订阅图像话题（例如："/camera/image_raw"）。 3. 在订阅回调函数中，接收sensor_msgs::msg::Image类型的消息。 4. 处理图像消息（例如：转换为OpenCV格式，然后进行图像处理或保存）。 注意：我们需要使用image_transport或cv_bridge来将ROS图像消息转换为OpenCV的Mat格式。 以下是一个完整的示例代码，包括必要的头文件和CMakeLists.txt配置。 代码示例： ```cpp #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/image.hpp" #include "cv_bridge/cv_bridge.h" #include "opencv2/opencv.hpp" class ImageSubscriber : public rclcpp::Node { public: ImageSubscriber() : Node("image_subscriber") { // 创建订阅者，订阅图像话题 subscription_ = this->create_subscription<sensor_msgs::msg::Image>( "/camera/image_raw", // 话题名称，根据实际录制的话题名调整 10, // 队列长度 std::bind(&ImageSubscriber::image_callback, this, std::placeholders::_1)); } private: void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { try { // 将ROS图像消息转换为OpenCV的Mat格式 cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8"); cv::Mat image = cv_ptr->image; // 在这里进行图像处理，例如显示图像 cv::imshow("Received Image", image); cv::waitKey(1); // 注意：waitKey(1)用于刷新显示，但如果在非主线程中使用可能会有问题，这里仅作示例 // 或者保存图像 // static int count = 0; // std::string filename = "image_" + std::to_string(count++) + ".jpg"; // cv::imwrite(filename, image); } catch (cv_bridge::Exception& e) { RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); } } rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_; }; int main(int argc, char * argv[]) { rclcpp::init(argc, argv); auto node = std::make_shared<ImageSubscriber>(); rclcpp::spin(node); rclcpp::shutdown(); return 0; } ``` 接下来，我们需要配置CMakeLists.txt文件来编译这个节点。假设我们的文件名为`image_subscriber.cpp`，放在`src`目录下。 ```cmake cmake_minimum_required(VERSION 3.5) project(image_subscriber) # 设置C++标准 set(CMAKE_CXX_STANDARD 14) # 查找依赖包 find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(cv_bridge REQUIRED) find_package(OpenCV REQUIRED) # 创建可执行文件 add_executable(image_subscriber src/image_subscriber.cpp) # 链接库 target_link_libraries(image_subscriber rclcpp::rclcpp sensor_msgs::sensor_msgs cv_bridge ${OpenCV_LIBRARIES} ) # 安装目标 install(TARGETS image_subscriber DESTINATION lib/${PROJECT_NAME} ) # 导出依赖 ament_package() ``` 使用说明： 1. 将上述C++代码保存为`image_subscriber.cpp`，放在工作空间的`src`目录下。 2. 将CMakeLists.txt内容保存到`CMakeLists.txt`文件中。 3. 编译工作空间：`colcon build` 4. 运行ros2bag回放（假设已经录制了图像话题）： ```bash ros2 bag play <your_bag_directory> ``` 5. 运行订阅者节点： ```bash source install/setup.bash ros2 run image_subscriber image_subscriber ``` 注意事项： - 确保在运行程序前已经安装了必要的ROS 2包，包括`sensor_msgs`、`cv_bridge`和`OpenCV`。 - 图像话题名称需要与录制时的话题名称一致。如果录制时的话题不是"/camera/image_raw"，请修改订阅的话题名称。 - 在回调函数中，避免进行耗时的操作，以免阻塞其他回调。如果需要进行大量计算，建议使用多线程或异步处理。 通过以上步骤，我们就可以创建一个订阅者节点来接收并处理来自ros2bag的图像消息了。 §§相关问题§§ 1. 如何在ROS 2中录制和回放图像话题？ 2. 如何将ROS 2图像消息转换为OpenCV格式？ 3. 在ROS 2中处理图像时如何避免阻塞主线程？</think>### 在ROS 2中使用C++创建图像订阅者处理ros2bag消息 以下是完整的C++实现方案，用于在ROS 2中创建订阅者接收和处理来自ros2bag的图像消息： #### 完整代码实现 ```cpp #include <rclcpp/rclcpp.hpp> #include <sensor_msgs/msg/image.hpp> #include <cv_bridge/cv_bridge.h> #include <opencv2/opencv.hpp> #include <fstream> class ImageSubscriber : public rclcpp::Node { public: ImageSubscriber() : Node("image_subscriber") { // 创建图像订阅者 subscription_ = this->create_subscription<sensor_msgs::msg::Image>( "/camera/image_raw", // 订阅的话题名称（与ros2bag录制的话题一致） 10, // QoS队列长度 [this](const sensor_msgs::msg::Image::SharedPtr msg) { this->image_callback(msg); }); RCLCPP_INFO(this->get_logger(), "图像订阅者已启动，等待ros2bag消息..."); } private: void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { try { // 1. 将ROS图像消息转换为OpenCV格式 cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8"); cv::Mat image = cv_ptr->image; // 2. 处理图像（示例：显示和保存） cv::imshow("Received Image", image); cv::waitKey(1); // 刷新显示 // 3. 将图像数据写入txt文件 save_image_data(msg); RCLCPP_INFO(this->get_logger(), "成功处理图像: 时间戳=%d.%d, 尺寸=%dx%d", msg->header.stamp.sec, msg->header.stamp.nanosec, msg->width, msg->height); } catch (const cv_bridge::Exception& e) { RCLCPP_ERROR(this->get_logger(), "图像转换错误: %s", e.what()); } } void save_image_data(const sensor_msgs::msg::Image::SharedPtr msg) { // 创建输出文件（追加模式） std::ofstream outfile("image_data.txt", std::ios::app); if (!outfile.is_open()) { RCLCPP_ERROR(this->get_logger(), "无法打开输出文件"); return; } // 写入元数据 outfile << "=== 图像帧 ===" << "\n"; outfile << "时间戳: " << msg->header.stamp.sec << "." << msg->header.stamp.nanosec << "\n"; outfile << "尺寸: " << msg->width << "x" << msg->height << "\n"; outfile << "编码: " << msg->encoding << "\n"; outfile << "步长: " << msg->step << "\n"; // 写入前10个像素值（示例） outfile << "前10像素值: "; for (size_t i = 0; i < 10 && i < msg->data.size(); ++i) { outfile << static_cast<int>(msg->data[i]) << " "; } outfile << "\n\n"; outfile.close(); } rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_; }; int main(int argc, char * argv[]) { rclcpp::init(argc, argv); auto node = std::make_shared<ImageSubscriber>(); rclcpp::spin(node); rclcpp::shutdown(); return 0; } ``` #### CMakeLists.txt 配置 ```cmake cmake_minimum_required(VERSION 3.5) project(image_subscriber) # 设置C++标准 set(CMAKE_CXX_STANDARD 14) # 查找依赖包 find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(cv_bridge REQUIRED) find_package(OpenCV REQUIRED) # 创建可执行文件 add_executable(image_subscriber src/image_subscriber.cpp) # 链接库 target_link_libraries(image_subscriber rclcpp::rclcpp sensor_msgs::sensor_msgs cv_bridge ${OpenCV_LIBS} ) # 安装目标 install(TARGETS image_subscriber DESTINATION lib/${PROJECT_NAME} ) # 导出依赖 ament_export_dependencies(rclcpp sensor_msgs cv_bridge OpenCV) ament_package() ``` #### 使用步骤 1. **编译程序**： ```bash colcon build --packages-select image_subscriber ``` 2. **运行ros2bag回放**： ```bash ros2 bag play <your_bag_directory> --loop ``` 3. **运行订阅者节点**： ```bash source install/setup.bash ros2 run image_subscriber image_subscriber ``` 4. **查看输出**： - 实时显示接收的图像 - 图像数据保存在`image_data.txt`文件中 #### 关键组件解析 1. **图像订阅者**： - 使用`create_subscription`创建图像话题订阅者[^2] - 指定QoS队列长度确保消息不丢失 - 回调函数接收`sensor_msgs::msg::Image`类型消息 2. **图像转换**： - 使用`cv_bridge::toCvCopy`转换ROS图像为OpenCV格式 - 支持多种像素格式（BGR8, MONO8等） 3. **数据处理**： - 使用OpenCV显示图像 - 将元数据和部分像素值写入文本文件 - 可扩展为图像处理算法（如边缘检测、目标识别） 4. **错误处理**： - 捕获`cv_bridge`异常 - 检查文件打开状态 - 使用`RCLCPP_ERROR`记录错误 #### 注意事项 1. **话题匹配**： - 确保订阅的话题名称与ros2bag录制的话题一致 - 使用`ros2 topic list`查看可用话题 2. **消息类型**： - 仅处理`sensor_msgs/msg/Image`类型消息 - 其他图像格式（如CompressedImage）需额外处理 3. **性能优化**： ```cpp // 在构造函数中添加QoS配置 rclcpp::QoS qos(10); qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); subscription_ = this->create_subscription<...>(..., qos, ...); ``` 4. **大文件处理**： - 避免在回调中执行耗时操作 - 使用多线程处理图像保存 - 考虑使用ROS 2的`image_transport`