ROS2 C++开发系列18-STL容器实战：deque缓存激光雷达数据｜priority_queue调度任务

📺 配套视频：ROS2 C++开发系列18-STL容器实战：deque缓存激光雷达数据｜priority_queue调度任务

在机器人软件开发中，数据的高效管理与调度是核心难点。无论是处理高频的传感器流、维护机器人的运动状态，还是调度紧急任务，选择合适的标准模板库（STL）容器都能显著提升系统的实时性与稳定性。本教程将深入探讨 C++ STL 中的常用容器，通过实际代码示例，解析 deque、list、set、map、stack、queue 以及 priority_queue 在机器人场景下的具体应用与底层逻辑。

双端队列 deque：高效处理传感器数据流

在机器人系统中，传感器（如激光雷达、IMU）产生的数据往往以流的形式持续到达。我们需要一种既能快速在末尾添加新数据，又能快速从头部读取并移除旧数据的结构。std::deque（Double-Ended Queue）正是为此设计的。

为什么选择 deque？

与 std::vector 相比，vector 在头部插入或删除元素时效率较低，因为需要移动所有后续元素。而 deque 采用分块存储机制，允许在两端以常数时间 $O(1)$ 进行插入和删除操作。这对于模拟“先进先出”（FIFO）且数据量动态变化的传感器缓存非常理想。

实战：模拟传感器读数处理

以下代码演示了如何使用 deque 模拟一串浮点型传感器数据。我们在尾部推入新读数，在头部处理并移除已读数的数据。

#include <iostream>
#include <deque> // 必须包含此头文件

int main() {
    // 声明一个 float 类型的双端队列，用于存储传感器数据
    std::deque<float> sensorData;

    // 1. 在尾部添加新读数 (push_back)
    sensorData.push_back(2.5);
    sensorData.push_back(3.1);
    sensorData.push_back(4.7);

    // 2. 处理前端读数：获取并移除
    std::cout << "处理前端读数: " << sensorData.front() << std::endl;
    sensorData.pop_front(); // 移除队首元素

    // 3. 继续添加更多读数
    sensorData.push_back(5.5);
    sensorData.push_back(6.8);

    // 4. 再次处理前端读数
    std::cout << "处理前端读数: " << sensorData.front() << std::endl;
    sensorData.pop_front();

    // 5. 显示剩余数据
    std::cout << "剩余传感器数据:" << std::endl;
    for (float val : sensorData) {
        std::cout << val << " ";
    }
    std::cout << std::endl;

    return 0;
}

代码解析：

push_back(val)：在队列尾部追加元素。
front()：返回队列第一个元素的引用，但不删除它。
pop_front()：删除队列第一个元素。
范围 for 循环：自动遍历当前队列中的所有元素，适合打印或进一步处理。

易错点：在对空队列调用 front() 或 pop_front() 之前，务必检查队列是否为空，否则会导致未定义行为或程序崩溃。

迭代器与基础容器遍历

迭代器是 C++ 中访问容器元素的通用接口。虽然范围 for 循环语法简洁，但理解底层迭代器机制对于高级操作（如条件过滤、并发修改）至关重要。

迭代器的基本用法

迭代器类似于指针，指向容器中的特定位置。begin() 指向第一个元素，end() 指向最后一个元素之后的“哨兵”位置。

#include <iostream>
#include <vector>

int main() {
    std::vector<int> sensorData = {10, 20, 30, 40, 50};

    // 使用迭代器遍历并打印原始数据
    std::cout << "原始数据: ";
    for (auto it = sensorData.begin(); it != sensorData.end(); ++it) {
        std::cout << *it << " "; // *it 解引用，获取当前值
    }
    std::cout << std::endl;

    // 使用迭代器修改元素（例如翻倍）
    for (auto it = sensorData.begin(); it != sensorData.end(); ++it) {
        *it *= 2; // 修改当前指向的值
    }

    // 使用范围 for 循环打印修改后的数据
    std::cout << "修改后数据: ";
    for (const auto& value : sensorData) {
        std::cout << value << " ";
    }
    std::cout << std::endl;

    return 0;
}

原理说明：

auto it = ...：利用类型推导简化迭代器声明。
*it：解引用操作符，用于读写迭代器指向的值。
范围 for 循环内部实际上也是由编译器生成的迭代器逻辑，但在简单遍历场景下更易于阅读。

链表容器：list 与 forward_list

当需要在容器中间频繁插入或删除元素时，std::list 和 std::forward_list 是比 vector 更好的选择。它们基于双向或单向链表实现，插入和删除的时间复杂度为 $O(1)$ （已知位置时）。

list：双向链表

std::list 支持双向遍历，可以在头部和尾部高效操作，也支持在任意位置插入。

#include <iostream>
#include <list>
#include <string>

int main() {
    // 创建存储字符串动作的双向链表
    std::list<std::string> robot_actions;

    // 在尾部添加动作
    robot_actions.push_back("move");
    robot_actions.push_back("rotate");
    robot_actions.push_back("scan");
    
    // 在头部添加动作（注意：题目字幕中虽提及push_front，此处演示完整流程）
    // robot_actions.push_front("initialize"); 
    // 根据字幕逻辑，我们主要展示 push_back 后的遍历
    robot_actions.push_back("grasp");
    robot_actions.push_back("initialize");

    // 遍历并打印
    std::cout << "机器人动作列表:" << std::endl;
    for (const auto& action : robot_actions) {
        std::cout << action << std::endl;
    }

    return 0;
}

forward_list：单向链表

std::forward_list 是单向链表，只支持向前遍历。它的内存开销比 list 更小，因为每个节点只需要一个指向下一个节点的指针。适用于只需从头到尾处理的数据流，如日志记录或单向消息队列。

#include <iostream>
#include <forward_list>

int main() {
    // 创建存储 double 类型传感器读数的单向链表
    std::forward_list<double> sensor_readings;

    // 在头部添加元素 (push_front)
    sensor_readings.push_front(1.5);
    sensor_readings.push_front(2.7);
    sensor_readings.push_front(3.2);
    sensor_readings.push_front(0.8); // 最新的数据在最前面

    // 遍历打印
    std::cout << "传感器读数 (最近优先):" << std::endl;
    for (double reading : sensor_readings) {
        std::cout << reading << std::endl;
    }

    return 0;
}

小结：如果不需要在中间随机插入，且对内存敏感，优先选择 forward_list；如果需要双向操作，选择 list。

关联容器：set, multiset, map, multimap

关联容器通过键（Key）来组织数据，查找效率通常为 $O(\log N)$ 。它们在去重、映射配置参数等方面极具优势。

set 与 multiset：唯一性与重复性管理

std::set 自动对元素排序并去除重复项；std::multiset 允许重复元素并保持有序。

#include <iostream>
#include <set>

int main() {
    // Set: 自动去重并排序
    std::set<int> unique_landmarks = {10, 20, 30, 40, 20, 30};
    std::cout << "唯一地标 (Set): ";
    for (int landmark : unique_landmarks) {
        std::cout << landmark << " ";
    }
    std::cout << std::endl; // 输出: 10 20 30 40

    // Multiset: 保留重复项
    std::multiset<std::string> repeated_commands = {"move", "rotate", "scan", "move", "grasp"};
    std::cout << "重复命令 (Multiset): ";
    for (const auto& cmd : repeated_commands) {
        std::cout << cmd << " ";
    }
    std::cout << std::endl; // 输出: move move rotate scan grasp

    return 0;
}

map 与 multimap：键值对映射

std::map 存储唯一的键值对，常用于存储传感器名称到数值的映射。std::multimap 允许一个键对应多个值。

#include <iostream>
#include <map>
#include <string>

int main() {
    // Map: 唯一键映射
    std::map<std::string, double> sensor_readings;
    sensor_readings["temperature"] = 25.0;
    sensor_readings["humidity"] = 60.0;
    sensor_readings["pressure"] = 1013.0;

    std::cout << "传感器读数 (Map):" << std::endl;
    for (const auto& pair : sensor_readings) {
        std::cout << pair.first << ": " << pair.second << std::endl;
    }

    // Multimap: 多值映射
    std::multimap<std::string, std::string> robot_commands;
    robot_commands.insert({"move", "forward"});
    robot_commands.insert({"move", "backward"});
    robot_commands.insert({"rotate", "left"});
    robot_commands.insert({"rotate", "right"});

    std::cout << "\n机器人指令 (Multimap):" << std::endl;
    for (const auto& command : robot_commands) {
        std::cout << command.first << " -> " << command.second << std::endl;
    }

    return 0;
}

关键区别：map 的键必须是唯一的，插入重复键会覆盖原值（取决于方法）；multimap 的键可以重复，适合一对多关系。

栈 stack 与队列 queue：后进先出与先进先出

这两种容器适配器提供了受限的访问方式，分别对应 LIFO（Last In First Out）和 FIFO（First In First Out）策略。

Stack：递归与回溯

在机器人路径规划或函数调用栈中，std::stack 非常有用。

#include <iostream>
#include <stack>

int main() {
    std::stack<int> myStack;
    
    // 压栈
    myStack.push(10);
    myStack.push(20);
    myStack.push(30);

    std::cout << "栈顶元素: " << myStack.top() << std::endl; // 输出 30
    
    // 弹栈
    myStack.pop();
    std::cout << "更新后的栈顶: " << myStack.top() << std::endl; // 输出 20

    return 0;
}

Queue：任务调度与缓冲区

std::queue 严格遵循先进先出原则，常用于处理传感器数据队列或待执行的任务列表。

#include <iostream>
#include <queue>
#include <string>

int main() {
    std::queue<std::string> myQ;

    // 入队
    myQ.push("sensor data");
    myQ.push("robot command");
    myQ.push("navigation goal");

    std::cout << "队列首个元素: " << myQ.front() << std::endl; // 输出 sensor data
    
    // 出队
    myQ.pop();
    std::cout << "更新后的队首元素: " << myQ.front() << std::endl; // 输出 robot command

    return 0;
}

优先队列 priority_queue：基于优先级的任务调度

在机器人系统中，并非所有任务都同等重要。例如，“紧急停止”或“避障”任务的优先级远高于“记录日志”。std::priority_queue 允许根据优先级自动排列元素，确保高优先级任务最先被处理。

实现自定义比较规则

默认情况下，priority_queue 是大顶堆（最大值优先）。为了实现自定义优先级（如整数越小优先级越高，或字符串特定含义），通常需要定义结构体并重载 < 运算符。

#include <iostream>
#include <queue>
#include <vector>
#include <functional> // 用于 greater 等函数对象

// 定义任务结构体
struct Task {
    int priority; // 优先级数值，假设数值越小优先级越高（或根据需求定义）
    std::string description;

    // 重载 < 运算符以定义优先级顺序
    // 注意：priority_queue 默认是最大堆，即 a < b 为真时 b 排在前面
    // 如果我们希望 priority 小的排前面，需要反转比较逻辑
    bool operator<(const Task& other) const {
        return this->priority > other.priority; // 大于号表示小优先级排在前面（大顶堆变体）
    }
};

int main() {
    // 创建优先队列，存储 Task 类型
    std::priority_queue<Task> taskQueue;

    // 添加不同优先级的任务
    taskQueue.push({3, "常规巡检"});
    taskQueue.push({1, "紧急避障"});   // 优先级最高
    taskQueue.push({2, "充电请求"});

    // 按优先级处理任务
    while (!taskQueue.empty()) {
        Task currentTask = taskQueue.top();
        std::cout << "执行任务: " << currentTask.description 
                  << " (优先级: " << currentTask.priority << ")" << std::endl;
        taskQueue.pop();
    }

    return 0;
}

逻辑解析：

priority_queue 默认取出的是“最大”元素。
通过重载 operator<，我们定义了“谁更大”。在这里，this->priority > other.priority 意味着如果一个任务的优先级数值比另一个大，它就被视为“较小”，从而在最大堆中被排在后面。反之，数值小的被视为“较大”，排在堆顶。
这样实现了“数值越小，优先级越高”的效果，符合紧急任务调度的直觉。

小结：优先队列是实时系统中处理中断和高优事件的核心工具。务必根据业务逻辑正确定义比较规则。

总结与选型指南

在实际 ROS2 或 C++ 机器人开发中，没有“最好”的容器，只有“最合适”的容器。以下是基于性能的选型建议：

容器类型	核心特性	适用场景	时间复杂度 (插入/删除/查找)
`vector`	连续内存，随机访问快	静态数组，频繁读取，尾部增删	$O(1)$ / $O(N)$ / $O(1)$
`deque`	分段连续，两端操作快	传感器数据缓冲，滑动窗口	$O(1)$ / $O(1)$ / $O(N)$
`list`	双向链表，任意位置增删快	频繁中间插入删除，不要求随机访问	$O(1)$ / $O(1)$ / $O(N)$
`set/map`	红黑树，有序，唯一键	去重数据，配置映射，索引查找	$O(\log N)$ / $O(\log N)$ / $O(\log N)$
`priority_queue`	堆结构，自动排序	任务调度，Dijkstra 算法，事件驱动	$O(\log N)$ / $O(\log N)$ / $O(N)$

掌握这些容器的底层差异，能帮助你在编写高性能机器人代码时做出明智的选择，避免不必要的性能瓶颈。