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

最新推荐文章于 2024-09-17 23:15:58 发布

机械心

最新推荐文章于 2024-09-17 23:15:58 发布

阅读量569

点赞数 9

分类专栏：编程语言教程文章标签： python opencv 开发语言

本文链接：https://blog.csdn.net/a8598671/article/details/142290181

版权

编程语言教程专栏收录该内容

5 篇文章 0 订阅

订阅专栏

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.1


import 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, CompressedImage

 
class NodePublisher(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info("usb cam node created!")
 
def main(args=None):

    #image size
    height = 480
    width =  640

    #capture frequency
    frequency = 10

    #compressed flag
    compressed_flag = True

    #image compressed quality %
    img_quality = 50 

    #usb cam device id
    capture = 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 node
    rclpy.init()
    node = NodePublisher("usb_cam_image") 

    if compressed_flag: # create compressed image topics
        image_compressed_pub = node.create_publisher(CompressedImage, "/usb_cam_image/compressed", 10)         

    else: # create raw image topics
        image_pub = node.create_publisher(Image, "/usb_cam_image", 10) 


    # create compressed image message
    msg_compressed_img = CompressedImage()
    msg_compressed_img.format = "jpeg"

    bridge = CvBridge() 


    n = 30 // frequency

    count = 0

    while 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 = 0
        count += 1

相应的setup.py文件如下：

from setuptools import setup

package_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='xxx@gmail.com',
    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 camera

        printf("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 camera

        if (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 camera

        printf("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 camera

        if (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_node
  rclcpp
  sensor_msgs
  cv_bridge
  image_transport
  OpenCV
) 
  
ament_target_dependencies(usb_cam_record_compressed_node
  rclcpp
  sensor_msgs
  cv_bridge
  image_transport
  OpenCV
) 

install(TARGETS usb_cam_record_raw_node usb_cam_record_compressed_node
  DESTINATION 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="xxx@gmail.com">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>