ROS2 视频采集节点实现

一个完整的ROS2视频采集节点的实现，使用OpenCV进行视频捕获并通过ROS2发布图像消息。

1. 创建功能包

首先创建一个新的ROS2功能包（如果还没有）：

bash

ros2 pkg create video_capture --build-type ament_python --dependencies rclpy sensor_msgs cv_bridge opencv-python

2. 实现视频采集节点

在video_capture/video_capture目录下创建video_capture_node.py文件：

python

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2

class VideoCaptureNode(Node):
    def __init__(self):
        super().__init__('video_capture_node')
        
        # 声明参数
        self.declare_parameter('video_source', 0)  # 默认使用摄像头0
        self.declare_parameter('frame_width', 640)
        self.declare_parameter('frame_height', 480)
        self.declare_parameter('fps', 30)
        self.declare_parameter('topic_name', 'video_frames')
        
        # 获取参数值
        video_source = self.get_parameter('video_source').value
        frame_width = self.get_parameter('frame_width').value
        frame_height = self.get_parameter('frame_height').value
        fps = self.get_parameter('fps').value
        topic_name = self.get_parameter('topic_name').value
        
        # 初始化视频捕获
        self.cap = cv2.VideoCapture(video_source)
        if not self.cap.isOpened():
            self.get_logger().error(f"Cannot open video source {video_source}")
            raise RuntimeError(f"Cannot open video source {video_source}")
        
        # 设置摄像头参数
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, frame_width)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, frame_height)
        self.cap.set(cv2.CAP_PROP_FPS, fps)
        
        # 初始化CV桥
        self.bridge = CvBridge()
        
        # 创建发布者
        self.publisher = self.create_publisher(Image, topic_name, 10)
        
        # 创建定时器
        timer_period = 1.0 / fps  # 秒
        self.timer = self.create_timer(timer_period, self.timer_callback)
        
        self.get_logger().info(f"Video capture node started. Publishing to '{topic_name}' at {fps} FPS")
    
    def timer_callback(self):
        ret, frame = self.cap.read()
        if ret:
            try:
                # 转换并发布图像消息
                msg = self.bridge.cv2_to_imgmsg(frame, "bgr8")
                self.publisher.publish(msg)
            except Exception as e:
                self.get_logger().error(f"Error converting frame: {e}")
        else:
            self.get_logger().warning("Failed to capture frame")
    
    def destroy_node(self):
        self.cap.release()
        self.get_logger().info("Video capture released")
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    
    try:
        node = VideoCaptureNode()
        rclpy.spin(node)
    except Exception as e:
        node.get_logger().error(f"Error in video capture node: {e}")
    finally:
        if 'node' in locals():
            node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

3. 添加启动文件

在功能包中创建launch目录，并添加video_capture.launch.py文件：

python

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='video_capture',
            executable='video_capture_node',
            name='video_capture_node',
            parameters=[
                {'video_source': 0},
                {'frame_width': 640},
                {'frame_height': 480},
                {'fps': 30},
                {'topic_name': 'video_frames'}
            ]
        )
    ])

4. 修改setup.py

确保setup.py包含正确的入口点：

python

from setuptools import setup

package_name = 'video_capture'

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']),
        ('share/' + package_name + '/launch', ['launch/video_capture.launch.py']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='your_name',
    maintainer_email='[email protected]',
    description='ROS2 video capture node',
    license='Apache License 2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'video_capture_node = video_capture.video_capture_node:main',
        ],
    },
)

5. 构建和运行

构建功能包：

bash

colcon build --packages-select video_capture
source install/setup.bash

运行节点：

bash

ros2 launch video_capture video_capture.launch.py

或者直接运行：

bash

ros2 run video_capture video_capture_node

6. 参数说明

可以通过以下方式修改参数：

• 在启动文件中修改

• 通过命令行参数修改：

  bash

• ros2 run video_capture video_capture_node --ros-args -p video_source:=0 -p frame_width:=1280 -p frame_height:=720

7. 查看图像

可以使用image_view节点查看发布的图像：

bash

ros2 run image_tools showimage --ros-args --remap image:=video_frames

注意事项

1. 确保已安装OpenCV和cv_bridge

2. 根据实际摄像头调整video_source参数（0通常为默认摄像头）

3. 对于视频文件，将video_source设置为文件路径

4. 节点会自动释放摄像头资源当被关闭时

这个实现提供了基本的视频采集功能，可以根据需要进一步扩展，例如添加图像处理、保存视频等功能。