一、环境配置

1.安装ROS2 

https://blog.csdn.net/m0_73745340/article/details/135281023?fromshare=blogdetail&sharetype=blogdetail&sharerId=135281023&sharerefer=PC&sharesource=2301_81967246&sharefrom=from_link

参考鱼香大佬下载安装ROS2

2.摄像头调用

# 安装摄像头工具
sudo apt install v4l-utils
# 查看摄像头设备
v4l2-ctl --list-devices
# 测试摄像头
sudo apt install cheese
cheese

3.导入OpenCV

# ROS 2 Humble已包含OpenCV，也可单独安装
sudo apt install python3-opencv

二、创建工作空间和功能包

1.创建工作空间

# 创建新的工作空间
mkdir -p ~/ros2_face_recognition_ws/src
cd ~/ros2_face_recognition_ws/src

2.创建功能包

ros2 pkg create face_recognition --build-type ament_python --dependencies rclpy cv_bridge sensor_msgs std_msgs image_transport

3.安装OpenCV数据文件

# 安装OpenCV数据文件（包含人脸检测器）
sudo apt update
sudo apt install opencv-data

# 检查文件是否安装成功
ls /usr/share/opencv4/haarcascades/haarcascade_frontalface_default.xml

4.创建文件结构

cd face_recognition
mkdir -p launch
mkdir -p face_recognition

三、人脸识别检测相关代码

1. 创建人脸检测节点

cat > face_recognition/face_detector.py << 'EOF'
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from std_msgs.msg import String
from cv_bridge import CvBridge
import cv2
import os

class FaceDetector(Node):
    def __init__(self):
        super().__init__('face_detector')
        
        # 初始化CV Bridge
        self.bridge = CvBridge()
        
        # 创建订阅者（摄像头图像）
        self.subscription = self.create_subscription(
            Image,
            '/camera/image_raw',
            self.image_callback,
            10)
        
        # 创建发布者
        self.image_pub = self.create_publisher(Image, '/face_detection/result', 10)
        self.status_pub = self.create_publisher(String, '/face_detection/status', 10)
        
        # 加载人脸检测器
        self.face_cascade = None
        self.load_face_detector()
        
        if self.face_cascade is not None:
            self.get_logger().info('🚀 人脸检测节点已启动，等待摄像头数据...')
        else:
            self.get_logger().error('❌ 人脸检测器加载失败，节点无法正常工作')
    
    def load_face_detector(self):
        """加载人脸检测器，尝试多种路径"""
        cascade_paths = [
            # OpenCV 4.x 路径
            '/usr/share/opencv4/haarcascades/haarcascade_frontalface_default.xml',
            # OpenCV 3.x 路径
            '/usr/share/opencv/haarcascades/haarcascade_frontalface_default.xml',
            # 系统路径
            '/usr/local/share/opencv4/haarcascades/haarcascade_frontalface_default.xml',
        ]
        
        for path in cascade_paths:
            if os.path.exists(path):
                self.face_cascade = cv2.CascadeClassifier(path)
                if not self.face_cascade.empty():
                    self.get_logger().info(f'✅ 从 {path} 加载人脸检测器')
                    return
        
        # 如果以上路径都失败，尝试使用cv2.data（如果存在）
        try:
            # 检查cv2是否有data属性
            if hasattr(cv2, 'data'):
                cascade_file = cv2.data.haarcascades + 'haarcascade_frontalface_default.xml'
                self.face_cascade = cv2.CascadeClassifier(cascade_file)
                if not self.face_cascade.empty():
                    self.get_logger().info('✅ 使用cv2.data加载人脸检测器')
                    return
        except Exception as e:
            self.get_logger().warning(f'使用cv2.data失败: {e}')
        
        # 如果还是失败，提供安装建议
        self.get_logger().error('❌ 无法加载人脸检测器')
        self.get_logger().info('💡 请确保已安装: sudo apt install opencv-data')
        self.face_cascade = None
    
    def image_callback(self, msg):
        # 检查人脸检测器是否加载成功
        if self.face_cascade is None:
            return
            
        try:
            # 将ROS图像消息转换为OpenCV格式
            cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
        except Exception as e:
            self.get_logger().error(f'图像转换错误: {str(e)}')
            return
        
        # 转换为灰度图像进行人脸检测
        gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
        
        # 人脸检测
        try:
            faces = self.face_cascade.detectMultiScale(
                gray,
                scaleFactor=1.1,
                minNeighbors=5,
                minSize=(30, 30)
            )
        except Exception as e:
            self.get_logger().error(f'人脸检测错误: {str(e)}')
            return
        
        # 绘制检测结果
        result_image = cv_image.copy()
        face_count = len(faces)
        
        for (x, y, w, h) in faces:
            cv2.rectangle(result_image, (x, y), (x+w, y+h), (0, 255, 0), 2)
            cv2.putText(result_image, 'Face', (x, y-10), 
                       cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 2)
        
        # 添加计数信息
        cv2.putText(result_image, f'Faces: {face_count}', (10, 30),
                   cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2)
        
        # 发布结果图像
        try:
            result_msg = self.bridge.cv2_to_imgmsg(result_image, 'bgr8')
            self.image_pub.publish(result_msg)
        except Exception as e:
            self.get_logger().error(f'结果图像发布错误: {str(e)}')
        
        # 发布检测状态
        status_msg = String()
        status_msg.data = f'检测到 {face_count} 张人脸'
        self.status_pub.publish(status_msg)
        
        if face_count > 0:
            self.get_logger().info(f'👤 检测到 {face_count} 张人脸', throttle_duration_sec=2.0)

def main(args=None):
    rclpy.init(args=args)
    face_detector = FaceDetector()
    
    # 如果人脸检测器加载失败，直接退出
    if face_detector.face_cascade is None:
        face_detector.get_logger().error('人脸检测器加载失败，节点退出')
        face_detector.destroy_node()
        rclpy.shutdown()
        return
    
    try:
        rclpy.spin(face_detector)
    except KeyboardInterrupt:
        print('\n🛑 人脸检测节点关闭')
    finally:
        face_detector.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
EOF

2. 创建摄像头节点

cat > face_recognition/camera_node.py << 'EOF'
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np

class CameraNode(Node):
    def __init__(self):
        super().__init__('camera_node')
        
        self.bridge = CvBridge()
        self.publisher = self.create_publisher(Image, '/camera/image_raw', 10)
        
        self.get_logger().info('📷 正在初始化摄像头...')
        
        # 创建虚拟测试图像
        self.virtual_image = self.create_virtual_image()
        
        # 尝试打开物理摄像头
        self.cap = None
        for i in range(3):  # 尝试设备0,1,2
            self.get_logger().info(f'尝试打开摄像头设备 /dev/video{i}...')
            self.cap = cv2.VideoCapture(i)
            if self.cap.isOpened():
                # 设置摄像头参数
                self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
                self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
                self.cap.set(cv2.CAP_PROP_FPS, 30)
                
                # 让摄像头预热
                for _ in range(5):
                    self.cap.read()
                
                # 验证摄像头能正常读取
                ret, frame = self.cap.read()
                if ret and frame is not None:
                    self.get_logger().info(f'✅ 成功打开物理摄像头 /dev/video{i}')
                    self.get_logger().info(f'📐 图像尺寸: {frame.shape}')
                    self.use_virtual = False
                    break
                else:
                    self.cap.release()
                    self.cap = None
            else:
                self.cap = None
        
        if self.cap is None:
            self.get_logger().warning('❌ 无法打开物理摄像头，使用虚拟摄像头')
            self.use_virtual = True
        
        self.timer = self.create_timer(0.033, self.timer_callback)  # 30Hz
        self.get_logger().info('🎯 摄像头节点已启动')
    
    def create_virtual_image(self):
        """创建虚拟测试图像"""
        img = np.ones((480, 640, 3), dtype=np.uint8) * 100
        
        # 绘制彩色矩形模拟场景
        cv2.rectangle(img, (100, 100), (200, 200), (255, 0, 0), -1)    # 蓝色
        cv2.rectangle(img, (300, 150), (400, 250), (0, 255, 0), -1)    # 绿色
        cv2.rectangle(img, (500, 200), (600, 300), (0, 0, 255), -1)    # 红色
        
        # 绘制圆形模拟人脸
        cv2.circle(img, (150, 350), 40, (200, 200, 200), -1)  # 人脸1
        cv2.circle(img, (140, 340), 8, (0, 0, 0), -1)         # 左眼
        cv2.circle(img, (160, 340), 8, (0, 0, 0), -1)         # 右眼
        cv2.ellipse(img, (150, 365), (20, 10), 0, 0, 180, (0, 0, 0), 2)  # 嘴巴
        
        cv2.circle(img, (450, 350), 35, (200, 200, 200), -1)  # 人脸2
        cv2.circle(img, (440, 340), 7, (0, 0, 0), -1)         # 左眼
        cv2.circle(img, (460, 340), 7, (0, 0, 0), -1)         # 右眼
        cv2.ellipse(img, (450, 365), (15, 8), 0, 0, 180, (0, 0, 0), 2)   # 嘴巴
        
        cv2.putText(img, 'Virtual Camera - Face Detection Test', (50, 450), 
                   cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
        return img
    
    def timer_callback(self):
        if self.use_virtual:
            # 使用虚拟图像
            msg = self.bridge.cv2_to_imgmsg(self.virtual_image, 'bgr8')
        else:
            # 使用物理摄像头
            ret, frame = self.cap.read()
            if ret and frame is not None:
                # 确保图像是BGR格式
                if len(frame.shape) == 2:  # 灰度图
                    frame = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR)
                msg = self.bridge.cv2_to_imgmsg(frame, 'bgr8')
            else:
                self.get_logger().warning('摄像头读取失败，使用虚拟图像')
                msg = self.bridge.cv2_to_imgmsg(self.virtual_image, 'bgr8')
        
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'camera_frame'
        self.publisher.publish(msg)
    
    def __del__(self):
        if hasattr(self, 'cap') and self.cap and self.cap.isOpened():
            self.cap.release()

def main():
    rclpy.init()
    node = CameraNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        print('\n🛑 摄像头节点关闭')
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
EOF

3.创建lanuch文件

cat > launch/face_detection.launch.py << 'EOF'
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        # 启动摄像头节点
        Node(
            package='face_recognition',
            executable='camera_node',
            name='camera_node',
            output='screen',
            emulate_tty=True
        ),
        
        # 启动人脸检测节点
        Node(
            package='face_recognition',
            executable='face_detector',
            name='face_detector',
            output='screen',
            emulate_tty=True
        ),
    ])
EOF

4. 更新package.xml

cat > package.xml << 'EOF'
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypelayout="1.0"?>
<package format="3">
  <name>face_recognition</name>
  <version>0.0.0</version>
  <description>Face recognition package using OpenCV and ROS2</description>
  <maintainer email="you@example.com">Your Name</maintainer>
  <license>Apache-2.0</license>

  <depend>rclpy</depend>
  <depend>cv_bridge</depend>
  <depend>sensor_msgs</depend>
  <depend>std_msgs</depend>
  <depend>image_transport</depend>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>

  <export>
    <build_type>ament_python</build_type>
  </export>
</package>
EOF

5. 更新setup.py

cat > setup.py << 'EOF'
from setuptools import setup

package_name = 'face_recognition'

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/face_detection.launch.py']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Your Name',
    maintainer_email='you@example.com',
    description='Face recognition package using OpenCV and ROS2',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'face_detector = face_recognition.face_detector:main',
            'camera_node = face_recognition.camera_node:main',
        ],
    },
)
EOF

6.设置权限并构建

# 设置执行权限
chmod +x face_recognition/face_detector.py
chmod +x face_recognition/camera_node.py

# 构建工作空间
cd ~/ros2_face_recognition_ws
colcon build --packages-select face_recognition
source install/setup.bash

四、代码实测

1.执行命令行

# 使用launch文件启动完整系统
ros2 launch face_recognition face_detection.launch.py

现在系统应该可以正常工作了！如果人脸检测器加载成功，您会看到：

• ✅ 从指定路径加载人脸检测器的消息

• 🚀 人脸检测节点已启动的消息

（如果加载失败，请检查 sudo apt install opencv-data 是否成功安装。）

🎯 系统运行成功！

现在您可以：

1. 查看实时检测结果

# 在另一个终端查看检测结果图像
ros2 run rqt_image_view rqt_image_view

选择 /face_detection/result 话题查看带人脸框的检测结果。

2.查看检测状态

# 查看实时检测状态
ros2 topic echo /face_detection/status

3.人脸识别效果

五、总结

🎊 

您已经成功完成了：

• ✅ ROS 2环境配置

• ✅ OpenCV集成

• ✅ 摄像头数据采集

• ✅ 实时人脸检测

• ✅ ROS话题通信

• ✅ 完整的系统集成