ros2教程（一）：使用python和C++发布摄像头原始图像和压缩图像

1. 使用python发布图像

在ROS 2中，可以通过使用rclpy库来发布压缩图像和原始图像。发布原始图像可以使用sensor_msgs.msg.Image消息类型，压缩图像则使用sensor_msgs.msg.CompressedImage消息类型。

#!/usr/bin/env python3# function: usbcam publish raw image or compressed image
# author: xxx
# Date: 2024.06.29
# version: v0.1import rclpy
from rclpy.node import Node
import cv2
from cv_bridge import CvBridge, CvBridgeError
import numpy as np
import time
from sensor_msgs.msg import Image, CompressedImageclass NodePublisher(Node):def __init__(self,name):super().__init__(name)self.get_logger().info("usb cam node created!")def main(args=None):#image sizeheight = 480width =  640#capture frequencyfrequency = 10#compressed flagcompressed_flag = True#image compressed quality %img_quality = 50 #usb cam device idcapture = cv2.VideoCapture(0)#ubuntu: check /dev/video*capture.set(cv2.CAP_PROP_FRAME_WIDTH, width)    capture.set(cv2.CAP_PROP_FRAME_HEIGHT, height)capture.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'))  # init rclpy noderclpy.init()node = NodePublisher("usb_cam_image") if compressed_flag: # create compressed image topicsimage_compressed_pub = node.create_publisher(CompressedImage, "/usb_cam_image/compressed", 10)         else: # create raw image topicsimage_pub = node.create_publisher(Image, "/usb_cam_image", 10) # create compressed image messagemsg_compressed_img = CompressedImage()msg_compressed_img.format = "jpeg"bridge = CvBridge() n = 30 // frequencycount = 0while True:       ret, frame = capture.read()  if count % n == 0:np_frame = np.array(cv2.flip(frame, 1))           if compressed_flag:_, compressed_image = cv2.imencode('.jpg', np_frame, [int(cv2.IMWRITE_JPEG_QUALITY), img_quality])msg_compressed_img.data = compressed_image.tobytes()    image_compressed_pub.publish(msg_compressed_img) else:                    img_raw = bridge.cv2_to_imgmsg(np_frame, encoding="bgr8") image_pub.publish(img_raw) count = 0count += 1

相应的setup.py文件如下：

from setuptools import setuppackage_name = 'py_usb_cam_record'setup(name=package_name,version='0.0.0',packages=[package_name],data_files=[('share/ament_index/resource_index/packages',['resource/' + package_name]),('share/' + package_name, ['package.xml']),],install_requires=['setuptools'],zip_safe=True,maintainer='xxx',maintainer_email='[e-mail]',description='TODO: Package description',license='TODO: License declaration',tests_require=['pytest'],entry_points={'console_scripts': ['py_usb_cam_record = py_usb_cam_record.py_usb_cam_record:main'],},
)

2. 使用C++发布图像

在ROS 2中，使用C++发布原始图像和压缩图像可以通过image_transport库来实现，原始图像使用sensor_msgs::msg::Image，而压缩图像可以通过image_transport::Publisher发布为sensor_msgs::msg::CompressedImage。

使用C++发布原始图像

/*=================================================*  function: usbcam publish raw image or compressed image
*  Author:   xxx
*  Date:     2024.06.29===================================================*/#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "opencv2/opencv.hpp"
#include "cv_bridge/cv_bridge.h"using namespace std::chrono_literals;class CameraPublisher : public rclcpp::Node {
public:CameraPublisher(): Node("camera_publisher"), count_(0) {publisher_ = this->create_publisher<sensor_msgs::msg::Image>("camera/image", 10);timer_ = this->create_wall_timer(100ms, std::bind(&CameraPublisher::publishImage, this));cap_ = cv::VideoCapture(0); // Open default cameraprintf("record raw image!\n");if (!cap_.isOpened()) {RCLCPP_ERROR(this->get_logger(), "Failed to open camera");}}private:void publishImage() {cv::Mat frame;cap_ >> frame; // Capture a frame from the cameraif (frame.empty()) {RCLCPP_ERROR(this->get_logger(), "Failed to capture frame");return;}cv::Mat resized_frame;cv::resize(frame, resized_frame, cv::Size(640, 480), cv::INTER_LINEAR);auto msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", resized_frame).toImageMsg();publisher_->publish(*msg);count_++;printf("record raw image: %d\r", count_);}rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;rclcpp::TimerBase::SharedPtr timer_;cv::VideoCapture cap_;int count_;
};int main(int argc, char **argv) {rclcpp::init(argc, argv);auto node = std::make_shared<CameraPublisher>();rclcpp::spin(node);rclcpp::shutdown();return 0;
}

使用C++发布压缩图像

/*=================================================*  function: usbcam publish raw image or compressed image
*  Author:   xxx
*  Date:     2024.06.29===================================================*/// ros2 run image_transport republish compressed in/compressed:=compressed_image raw out:=image_raw#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/compressed_image.hpp"
#include "opencv2/opencv.hpp"
#include "cv_bridge/cv_bridge.h"using namespace std::chrono_literals;class CameraPublisher : public rclcpp::Node {
public:CameraPublisher(): Node("camera_publisher"), count_(0) {publisher_ = this->create_publisher<sensor_msgs::msg::CompressedImage>("compressed_image", 10);        timer_ = this->create_wall_timer(100ms, std::bind(&CameraPublisher::publishImage, this));cap_ = cv::VideoCapture(0); // Open default cameraprintf("record compressed image!\n");if (!cap_.isOpened()) {RCLCPP_ERROR(this->get_logger(), "Failed to open camera");}}private:void publishImage() {cv::Mat frame;cap_ >> frame; // Capture a frame from the cameraif (frame.empty()) {RCLCPP_ERROR(this->get_logger(), "Failed to capture frame");return;}cv::Mat resized_frame;cv::resize(frame, resized_frame, cv::Size(640, 480), cv::INTER_LINEAR);std::vector<uchar> buf;cv::imencode(".jpg", resized_frame, buf, {cv::IMWRITE_JPEG_QUALITY, 80}); // Adjust JPEG quality (0-100 scale)sensor_msgs::msg::CompressedImage msg;msg.format = "jpeg";msg.data = buf;publisher_->publish(msg);count_++;printf("record compressed image: %d\r", count_);}rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr publisher_;rclcpp::TimerBase::SharedPtr timer_;cv::VideoCapture cap_;int count_;
};int main(int argc, char **argv) {rclcpp::init(argc, argv);auto node = std::make_shared<CameraPublisher>();rclcpp::spin(node);rclcpp::shutdown();return 0;
}

CMakeLists.txt文件内容如下：

cmake_minimum_required(VERSION 3.5)
project(usb_cam_record)# Default to C++14
if(NOT CMAKE_CXX_STANDARD)set(CMAKE_CXX_STANDARD 14)
endif()if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")add_compile_options(-Wall -Wextra -Wpedantic)
endif()# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(image_transport REQUIRED)
find_package(cv_bridge REQUIRED)  # If using OpenCV for image handling
find_package(OpenCV REQUIRED)#include_directories(${OpenCV_INCLUDE_DIRS})add_executable(usb_cam_record_raw_node src/usb_cam_record_raw.cpp)
add_executable(usb_cam_record_compressed_node src/usb_cam_record_compressed.cpp)ament_target_dependencies(usb_cam_record_raw_noderclcppsensor_msgscv_bridgeimage_transportOpenCV
) ament_target_dependencies(usb_cam_record_compressed_noderclcppsensor_msgscv_bridgeimage_transportOpenCV
) install(TARGETS usb_cam_record_raw_node usb_cam_record_compressed_nodeDESTINATION lib/${PROJECT_NAME})if(BUILD_TESTING)find_package(ament_lint_auto REQUIRED)# the following line skips the linter which checks for copyrights# uncomment the line when a copyright and license is not present in all source files#set(ament_cmake_copyright_FOUND TRUE)# the following line skips cpplint (only works in a git repo)# uncomment the line when this package is not in a git repo#set(ament_cmake_cpplint_FOUND TRUE)ament_lint_auto_find_test_dependencies()
endif()ament_package()

package.xml的文件配置如下：

<package format="3">
<name>usb_cam_record</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="[e-mail]">xxx</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rclcpp</build_depend>
<exec_depend>rclcpp</exec_depend>
<build_depend>sensor_msgs</build_depend>
<exec_depend>sensor_msgs</exec_depend>
<build_depend>image_transport</build_depend>
<exec_depend>image_transport</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>