📡 第03课：话题(Topic)通信

📋 课程目标

• 深入理解话题通信的发布/订阅模型
• 掌握Python和C++的Publisher/Subscriber编写
• 理解QoS策略对通信的影响
• 学会自定义消息类型
• 掌握话题的命令行调试工具

🧠 话题通信模型

话题(Topic)是ROS2中最核心的通信方式，采用发布/订阅(Publish/Subscribe)模式。发布者(Publisher)向话题发送消息，订阅者(Subscriber)从话题接收消息，两者完全解耦。

📐 话题通信架构详解

🐍 Python：发布者节点

#!/usr/bin/env python3
"""激光雷达数据模拟发布者"""

import math
import random
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Header
from builtin_interfaces.msg import Time


class LidarSimulatorNode(Node):
    """模拟激光雷达传感器，发布LaserScan消息"""

    def __init__(self):
        super().__init__('lidar_simulator')

        # 参数
        self.declare_parameter('frame_id', 'laser_frame')
        self.declare_parameter('scan_frequency', 10.0)
        self.declare_parameter('num_readings', 360)
        self.declare_parameter('range_min', 0.1)
        self.declare_parameter('range_max', 10.0)
        self.declare_parameter('obstacle_distance', 3.0)

        frame_id = self.get_parameter('frame_id').value
        freq = self.get_parameter('scan_frequency').value
        self.num_readings = self.get_parameter('num_readings').value
        self.range_min = self.get_parameter('range_min').value
        self.range_max = self.get_parameter('range_max').value
        self.obstacle_dist = self.get_parameter('obstacle_distance').value

        # 传感器数据用BEST_EFFORT + KEEP_LAST
        qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST,
            depth=5
        )

        self.pub = self.create_publisher(LaserScan, 'scan', qos)
        self.timer = self.create_timer(1.0 / freq, self.publish_scan)

        self.frame_id = frame_id
        self.scan_count = 0

        self.get_logger().info(f'激光雷达模拟器启动 - {freq}Hz, {self.num_readings}点')

    def publish_scan(self):
        """发布一帧激光扫描数据"""
        now = self.get_clock().now()

        msg = LaserScan()
        msg.header = Header()
        msg.header.stamp = now.to_msg()
        msg.header.frame_id = self.frame_id

        # 扫描参数
        msg.angle_min = -math.pi
        msg.angle_max = math.pi
        msg.angle_increment = 2 * math.pi / self.num_readings
        msg.time_increment = 0.0
        msg.scan_time = 0.1
        msg.range_min = self.range_min
        msg.range_max = self.range_max

        # 生成模拟数据：前方有障碍物
        ranges = []
        for i in range(self.num_readings):
            angle = msg.angle_min + i * msg.angle_increment
            # 前方120°范围内有障碍物
            if -math.pi / 3 < angle < math.pi / 3:
                r = self.obstacle_dist + random.uniform(-0.2, 0.2)
            else:
                r = self.range_max - random.uniform(0, 0.5)
            ranges.append(max(self.range_min, min(self.range_max, r)))

        msg.ranges = ranges
        self.pub.publish(msg)
        self.scan_count += 1

        if self.scan_count % 50 == 0:
            self.get_logger().info(f'已发布 {self.scan_count} 帧扫描数据')


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

if __name__ == '__main__':
    main()

🐍 Python：订阅者节点

#!/usr/bin/env python3
"""激光雷达数据处理订阅者"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist


class ObstacleDetectorNode(Node):
    """障碍物检测节点：订阅激光数据 → 发布速度命令"""

    def __init__(self):
        super().__init__('obstacle_detector')

        self.declare_parameter('safe_distance', 1.0)
        self.declare_parameter('stop_distance', 0.5)

        self.safe_dist = self.get_parameter('safe_distance').value
        self.stop_dist = self.get_parameter('stop_distance').value

        # 订阅激光扫描
        qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST,
            depth=5
        )
        self.scan_sub = self.create_subscription(
            LaserScan, 'scan', self.scan_callback, qos
        )

        # 发布速度命令
        self.cmd_pub = self.create_publisher(Twist, 'cmd_vel', 10)

        self.get_logger().info(
            f'障碍物检测启动 - 安全距离: {self.safe_dist}m, '
            f'停止距离: {self.stop_dist}m'
        )

    def scan_callback(self, msg: LaserScan):
        """处理激光扫描数据"""
        if not msg.ranges:
            return

        # 分析前方扇区（±30°）
        center_idx = len(msg.ranges) // 2
        sector_size = len(msg.ranges) // 6  # ±30°
        front_ranges = msg.ranges[
            center_idx - sector_size : center_idx + sector_size
        ]

        # 过滤无效值
        valid_ranges = [r for r in front_ranges
                       if msg.range_min < r < msg.range_max]

        if not valid_ranges:
            return

        min_dist = min(valid_ranges)

        # 根据距离决定行为
        cmd = Twist()
        if min_dist < self.stop_dist:
            # 太近：停止并后退
            cmd.linear.x = -0.1
            cmd.angular.z = 0.5
            self.get_logger().warn(f'🔴 太近！{min_dist:.2f}m → 后退')
        elif min_dist < self.safe_dist:
            # 接近：减速并转向
            cmd.linear.x = 0.1
            cmd.angular.z = 0.3
            self.get_logger().warn(f'🟡 接近！{min_dist:.2f}m → 减速')
        else:
            # 安全：前进
            cmd.linear.x = 0.3
            cmd.angular.z = 0.0
            self.get_logger().debug(f'🟢 安全：{min_dist:.2f}m → 前进')

        self.cmd_pub.publish(cmd)


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

if __name__ == '__main__':
    main()

🔧 C++：发布者与订阅者

// scan_filter_cpp.cpp - 激光扫描过滤器(C++版)
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include <vector>
#include <algorithm>
#include <cmath>

class ScanFilterNode : public rclcpp::Node {
public:
    ScanFilterNode() : Node("scan_filter") {
        // 参数
        this->declare_parameter("min_range", 0.3);
        this->declare_parameter("max_range", 8.0);
        this->declare_parameter("filter_angle_min", -M_PI / 6);
        this->declare_parameter("filter_angle_max", M_PI / 6);

        min_range_ = this->get_parameter("min_range").as_double();
        max_range_ = this->get_parameter("max_range").as_double();

        // 订阅原始扫描
        scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
            "scan", rclcpp::SensorDataQoS(),
            std::bind(&ScanFilterNode::scan_callback, this, std::placeholders::_1));

        // 发布过滤后的扫描
        filtered_pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>(
            "scan_filtered", rclcpp::SensorDataQoS());

        RCLCPP_INFO(this->get_logger(), "扫描过滤器启动");
    }

private:
    void scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
        auto filtered = *msg;

        // 过滤范围外的值设为inf
        for (size_t i = 0; i < filtered.ranges.size(); ++i) {
            double r = filtered.ranges[i];
            if (r < min_range_ || r > max_range_) {
                filtered.ranges[i] = std::numeric_limits<float>::infinity();
            }
        }

        // 统计有效点数
        size_t valid_count = std::count_if(
            filtered.ranges.begin(), filtered.ranges.end(),
            [](float r) { return std::isfinite(r); });

        RCLCPP_DEBUG(this->get_logger(), "有效点数: %zu/%zu",
                     valid_count, filtered.ranges.size());

        filtered_pub_->publish(filtered);
    }

    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr filtered_pub_;
    double min_range_, max_range_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ScanFilterNode>());
    rclcpp::shutdown();
    return 0;
}

📊 QoS (Quality of Service) 详解

QoS是ROS2相比ROS1的重要改进，允许精确控制通信行为。

常用QoS预设

# Python QoS预设
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy

# 1. 传感器数据（高频，可丢包）
sensor_qos = QoSProfile(
    reliability=QoSReliabilityPolicy.BEST_EFFORT,
    history=QoSHistoryPolicy.KEEP_LAST,
    depth=5
)

# 2. 状态数据（需要可靠传输）
state_qos = QoSProfile(
    reliability=QoSReliabilityPolicy.RELIABLE,
    history=QoSHistoryPolicy.KEEP_LAST,
    depth=10
)

# 3. 参数配置（需要晚加入者也能获取）
config_qos = QoSProfile(
    reliability=QoSReliabilityPolicy.RELIABLE,
    history=QoSHistoryPolicy.KEEP_LAST,
    depth=1,
    durability=QoSDurabilityPolicy.TRANSIENT_LOCAL
)

# C++预设
auto sensor_qos = rclcpp::SensorDataQoS();  // BEST_EFFORT
auto service_qos = rclcpp::ServicesQoS();   // RELIABLE
auto param_qos = rclcpp::ParametersQoS();   // RELIABLE + TRANSIENT_LOCAL

📦 自定义消息类型

1. 创建消息定义文件

# 创建消息包
ros2 pkg create my_interfaces --build-type ament_cmake

# 创建msg目录
mkdir -p my_interfaces/msg

# 创建消息文件 my_interfaces/msg/SensorStatus.msg
# --- 内容如下 ---
string sensor_name
float64 value
float64 min_value
float64 max_value
bool is_healthy
string status_message
builtin_interfaces/Time timestamp

2. 配置package.xml和CMakeLists.txt

<!-- package.xml 添加依赖 -->
<depend>builtin_interfaces</depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

# CMakeLists.txt 添加消息生成
find_package(builtin_interfaces REQUIRED)
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/SensorStatus.msg"
  DEPENDENCIES builtin_interfaces
)

3. 构建并使用

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

# 查看消息定义
ros2 interface show my_interfaces/msg/SensorStatus

# 在Python中使用
from my_interfaces.msg import SensorStatus

msg = SensorStatus()
msg.sensor_name = 'lidar_front'
msg.value = 3.5
msg.is_healthy = True

🖥️ 话题命令行工具

🔄 话题重映射(Remapping)

运行时修改话题名称，实现灵活配置：

# 启动时重映射话题
ros2 run my_pkg lidar_simulator --ros-args \
  --remap scan:=/robot1/scan \
  --remap cmd_vel:=/robot1/cmd_vel

# 在代码中重映射（Python）
self.pub = self.create_publisher(LaserScan, 'scan', 10)
# 运行时: ros2 run ... --remap scan:=base_scan

📊 常用ROS2消息类型

🎯 练习题

🏆 成就解锁