ROS 2从入门到精通系列（十三）：传感器集成 - 从硬件到数据流

学会集成各种传感器，构建完整的感知系统。

---

引言

一个完整的机器人系统需要各种传感器来感知环境：相机、激光雷达、惯性测量单元(IMU)、深度传感器等。本文教你如何将这些硬件设备集成到ROS2系统中。

---

一、常见传感器和ROS2驱动

1.1 传感器生态

1.2 ROS2驱动和包的查找

# 在ROS包索引中搜索传感器驱动
# https://index.ros.org/

# 查找特定品牌的驱动
sudo apt search ros-humble | grep lidar
sudo apt search ros-humble | grep camera

# 安装驱动包
sudo apt install ros-humble-usb-cam                    # USB摄像头
sudo apt install ros-humble-realsense2-camera          # Intel RealSense
sudo apt install ros-humble-rplidar-ros                # RPLidar激光雷达
sudo apt install ros-humble-imu-tools                  # IMU工具

---

二、USB摄像头集成

2.1 安装和配置

# 安装USB摄像头驱动
sudo apt install ros-humble-usb-cam

# 验证摄像头设备
ls -la /dev/video*

# 输出示例:
# /dev/video0  # 第一个摄像头
# /dev/video1  # 第二个摄像头

2.2 启动摄像头节点

# 使用默认配置启动摄像头
ros2 run usb_cam usb_cam_node_exe

# 查看发布的话题
ros2 topic list

# 输出:
# /image_raw
# /camera_info

2.3 Launch文件配置

创建 camera_launch.py：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='usb_cam',
            executable='usb_cam_node_exe',
            name='camera',
            output='screen',
            parameters=[
                {'video_device': '/dev/video0'},
                {'camera_name': 'camera'},
                {'camera_frame_id': 'camera_link'},
                {'framerate': 30.0},
                {'pixel_format': 'mjpeg'},
                {'image_width': 640},
                {'image_height': 480},
            ]
        )
    ])

2.4 使用摄像头数据

#!/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 ImageSubscriber(Node):
    def __init__(self):
        super().__init__('image_subscriber')

        # 创建ROS-OpenCV桥接
        self.bridge = CvBridge()

        # 订阅图像话题
        self.sub = self.create_subscription(
            Image,
            '/image_raw',
            self.image_callback,
            10
        )

    def image_callback(self, msg):
        # 将ROS Image消息转换为OpenCV图像
        cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')

        # 处理图像（示例：灰度化）
        gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)

        # 显示（仅用于调试，生产环境应保存结果）
        # cv2.imshow('Image', gray)
        # cv2.waitKey(1)

        self.get_logger().info(f'收到图像: {msg.width}x{msg.height}')

def main(args=None):
    rclpy.init(args=args)
    node = ImageSubscriber()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

---

三、激光雷达集成

3.1 常见激光雷达驱动

# RPLidar (RP A1, A2, A3)
sudo apt install ros-humble-rplidar-ros

# Sick激光雷达
sudo apt install ros-humble-sicktoolbox-wrapper

# Velodyne 3D激光
sudo apt install ros-humble-velodyne

# Livox激光雷达
git clone https://github.com/Livox-SDK/livox_ros_driver2.git
colcon build --packages-select livox_ros_driver2

3.2 RPLidar配置和启动

创建 lidar_launch.py：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='rplidar_ros',
            executable='rplidar_composition',
            name='rplidar',
            parameters=[
                {'serial_port': '/dev/ttyUSB0'},  # 串口设备
                {'serial_baudrate': 115200},       # 波特率
                {'frame_id': 'lidar_link'},        # 坐标系
                {'inverted': False},               # 是否反向
                {'angle_compensate': True},        # 角度补偿
                {'scan_mode': 'Standard'},         # 扫描模式
            ],
            output='screen'
        ),

        # 发布LIDAR到base_link的变换
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            arguments=['0', '0', '0.1', '0', '0', '0', 'base_link', 'lidar_link']
        )
    ])

3.3 处理激光扫描数据

#!/usr/bin/env python3
"""
处理激光雷达点云数据
"""

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
import numpy as np

class LidarProcessor(Node):
    def __init__(self):
        super().__init__('lidar_processor')

        self.sub = self.create_subscription(
            LaserScan,
            '/scan',
            self.scan_callback,
            10
        )

    def scan_callback(self, msg):
        """处理激光扫描"""
        # 提取扫描数据
        ranges = np.array(msg.ranges)
        angles = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges))

        # 过滤无效数据
        valid_mask = np.isfinite(ranges) & (ranges > msg.range_min) & (ranges < msg.range_max)
        valid_ranges = ranges[valid_mask]
        valid_angles = angles[valid_mask]

        # 计算统计信息
        if len(valid_ranges) > 0:
            min_distance = np.min(valid_ranges)
            max_distance = np.max(valid_ranges)
            avg_distance = np.mean(valid_ranges)

            self.get_logger().info(
                f'扫描数据: 有效点={len(valid_ranges)}, '
                f'最近={min_distance:.2f}m, 最远={max_distance:.2f}m, '
                f'平均={avg_distance:.2f}m'
            )

def main(args=None):
    rclpy.init(args=args)
    node = LidarProcessor()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

---

四、IMU传感器集成

4.1 安装IMU驱动

# 通用IMU工具
sudo apt install ros-humble-imu-tools

# 特定品牌驱动
sudo apt install ros-humble-xsens-mti-driver        # Xsens
sudo apt install ros-humble-microstrain-inertial    # MicroStrain

4.2 IMU数据处理

#!/usr/bin/env python3
"""
IMU传感器数据处理
"""

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
import math

class IMUProcessor(Node):
    def __init__(self):
        super().__init__('imu_processor')

        self.sub = self.create_subscription(
            Imu,
            '/imu/data',
            self.imu_callback,
            10
        )

    def imu_callback(self, msg):
        """处理IMU数据"""
        # 加速度
        ax = msg.linear_acceleration.x
        ay = msg.linear_acceleration.y
        az = msg.linear_acceleration.z
        accel_magnitude = math.sqrt(ax**2 + ay**2 + az**2)

        # 角速度
        wx = msg.angular_velocity.x
        wy = msg.angular_velocity.y
        wz = msg.angular_velocity.z
        angular_speed = math.sqrt(wx**2 + wy**2 + wz**2)

        # 四元数表示的姿态
        qx = msg.orientation.x
        qy = msg.orientation.y
        qz = msg.orientation.z
        qw = msg.orientation.w

        self.get_logger().info(
            f'加速度: {accel_magnitude:.2f}m/s², '
            f'角速度: {angular_speed:.2f}rad/s'
        )

def main(args=None):
    rclpy.init(args=args)
    node = IMUProcessor()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

---

五、RGB-D深度传感器

5.1 RealSense集成

# 安装RealSense驱动
sudo apt install ros-humble-realsense2-camera

# 启动RealSense节点
ros2 launch realsense2_camera rs_launch.py

# 发布的话题：
# /camera/color/image_raw      - RGB图像
# /camera/depth/image_rect_raw - 深度图像
# /camera/pointcloud           - 点云

5.2 点云处理

#!/usr/bin/env python3
"""
处理点云数据
"""

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
import numpy as np

class PointCloudProcessor(Node):
    def __init__(self):
        super().__init__('pointcloud_processor')

        self.sub = self.create_subscription(
            PointCloud2,
            '/camera/pointcloud',
            self.cloud_callback,
            10
        )

    def cloud_callback(self, msg):
        """处理点云"""
        # 提取点云数据
        point_count = msg.width * msg.height

        self.get_logger().info(
            f'点云: {point_count} 个点, '
            f'分辨率: {msg.width}x{msg.height}'
        )

def main(args=None):
    rclpy.init(args=args)
    node = PointCloudProcessor()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

---

六、多传感器融合系统架构

6.1 系统设计模式

6.2 同步多个传感器

#!/usr/bin/env python3
"""
多传感器同步处理
"""

import rclpy
from rclpy.node import Node
from message_filters import ApproximateTimeSynchronizer, Subscriber
from sensor_msgs.msg import Image, LaserScan

class MultiSensorFusion(Node):
    def __init__(self):
        super().__init__('multi_sensor_fusion')

        # 创建订阅者
        image_sub = Subscriber(self, Image, '/image_raw')
        scan_sub = Subscriber(self, LaserScan, '/scan')

        # 创建时间同步器
        self.ts = ApproximateTimeSynchronizer(
            [image_sub, scan_sub],
            queue_size=10,
            slop=0.5  # 允许0.5秒的时间差
        )
        self.ts.registerCallback(self.fusion_callback)

    def fusion_callback(self, image_msg, scan_msg):
        """同步处理图像和激光数据"""
        self.get_logger().info(
            f'融合: 图像时间={image_msg.header.stamp.sec}, '
            f'激光时间={scan_msg.header.stamp.sec}'
        )

def main(args=None):
    rclpy.init(args=args)
    node = MultiSensorFusion()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

---

七、传感器标定和校准

7.1 相机标定

# 使用ROS camera_calibration工具
ros2 run camera_calibration cameracalibrator \
  --size 8x6 --square 0.108 \
  image:=/image_raw camera:=/camera_info

# 标定结果保存到~/.ros/camera_info/

7.2 IMU标定

# IMU加速度计零点校准
# 将IMU放在水平面上，采集静止数据

# IMU陀螺仪零点校准
# 保持IMU不动，采集偏差数据

---

八、本文要点总结

✅ 常见传感器：

• 视觉：摄像头、RGB-D、立体
• 测距：2D/3D激光雷达
• 惯性：IMU、加速度计、陀螺仪
• 距离：超声波、红外

✅ 集成步骤：

1. 安装硬件驱动包
2. 配置设备参数
3. 启动传感器节点
4. 订阅数据话题
5. 处理和融合数据

✅ 关键消息类型：

• Image - 图像
• LaserScan - 2D激光
• PointCloud2 - 3D点云
• Imu - 惯性测量

✅ 常用工具：

• cv_bridge - ROS图像桥接
• camera_calibration - 相机标定
• message_filters - 传感器同步

---

下一篇预告：《ROS2从入门到精通系列（十四）：时间管理与模拟时钟》

🎯 掌握传感器集成，你就掌握了机器人的"感官"！