三分钟云课实践速通--C/C++程序设计--

这部分内容研发从2014年(indigo)到2026年，已经有12年经历了，博客中之放出了大概1%的研发内容。

echo "260501更新后密钥并完成Teb配置示例："
 
git clone https://gitcode.com/ZhangRelay1/donut.git
 
sudo apt-key add ~/donut/ros.key
 
gpg --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
 
gpg --export C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 | sudo tee /usr/share/keyrings/ros.gpg > /dev/null
 
sudo sh -c 'echo "deb [signed-by=/usr/share/keyrings/ros.gpg] https://mirrors.aliyun.com/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list'
 
sudo rm /etc/apt/sources.list.d/ros2-latest.list
 
sudo apt update

echo "260501更新后密钥并完成Teb配置示例："
 
git clone https://gitcode.com/ZhangRelay1/donut.git
 
sudo apt-key add ~/donut/ros.key
 
gpg --keyserver 'hkp://keyserver.ubuntu.com' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
 
gpg --export C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 | sudo tee /usr/share/keyrings/ros.gpg > /dev/null
 
sudo sh -c 'echo "deb [signed-by=/usr/share/keyrings/ros.gpg] https://mirrors.aliyun.com/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list'
 
sudo rm /etc/apt/sources.list.d/ros2-latest.list
 
sudo apt update

---

原创 适配 Ubuntu 16.04/20.04 + ROS Kinetic/Noetic 移动机器人开发文章标签：# 学习 #笔记 #ubuntu #C/C++ #ROS #移动机器人 #机器人工程

动手编译运行，这部分智能大模型错误太多。C/C++ 机器人工程专属示例（完全匹配 Ubuntu+ROS 环境）精准列表所有代码均为 ROS 环境真实可编译运行，按 C/C++ 学习路径 + 机器人工程落地场景 分类拆解，每个示例标注精准编译运行命令、核心 C/C++ 知识点、机器人工程适配说明，均可直接复制到终端一键运行。

一、C 语言基础与机器人数据处理类（移动机器人硬件入门核心）

核心定位：覆盖 C 语言全基础语法，是机器人传感器数据处理、IO 控制、指令输出的底层基石，完全匹配大学《C 语言程序设计》核心考点与移动机器人底层开发工程需求。

表格

示例精准一键运行命令	示例名称	核心 C/C++ 知识点	机器人工程适配说明
echo "代码" > robot_status_indicator.c && gcc robot_status_indicator.c -o robot_status_indicator && ./robot_status_indicator	机器人运行状态指示程序	C 语言基础结构、变量与基本数据类型、标准输入输出、主函数执行流程	对应机器人运行状态、电源通断、故障报警指示灯逻辑，是机器人最基础的状态可视化单元，理解 C 语言程序从编译到执行的完整流程
echo "代码" > laser_data_process.c && gcc laser_data_process.c -o laser_data_process && ./laser_data_process	激光雷达数据遍历与极值计算程序	数组、指针、指针与数组的关系、for 循环分支结构	对应移动机器人 2D 激光雷达数据处理，计算最近障碍物距离、有效数据筛选，是 TEB 局部规划器避障逻辑的底层数据基础
echo "代码" > robot_sensor_filter.c && gcc robot_sensor_filter.c -o robot_sensor_filter && ./robot_sensor_filter	传感器滑动均值滤波函数实现	函数定义与声明、函数传参、返回值、数组作为函数参数	对应机器人 IMU、力传感器、温度传感器的硬件数据滤波，纯 C 语言实现数据防抖，提升机器人测量精度与控制稳定性
echo "代码" > robot_motor_control.c && gcc robot_motor_control.c -o robot_motor_control && ./robot_motor_control	电机驱动分支控制程序	if-else 分支、switch-case 多条件判断、逻辑运算符	对应机器人直流电机正反转、启停、限速控制，实现急停触发、限位保护的底层逻辑判断，是机器人执行器控制的基础单元
echo "代码" > robot_odom_calc.c && gcc robot_odom_calc.c -o robot_odom_calc && ./robot_odom_calc	机器人里程计解算结构体实现	结构体定义、结构体成员访问、结构体作为函数参数	对应移动机器人编码器脉冲数据解析、里程计位姿解算，用结构体封装机器人位姿 (x,y,yaw)、速度、时间戳核心数据，是机器人导航的底层位姿基础

二、C++ 面向对象核心类（机器人传感与驱动模块封装核心）

核心定位：覆盖 C++ 面向对象全核心特性，实现机器人传感器、执行器、规划器的模块化封装，是 ROS 节点开发、TEB 规划器二次开发的核心语法基础，完全匹配大学《C++ 程序设计》核心内容与机器人工程开发需求。

表格

示例精准一键运行命令	示例名称	核心 C/C++ 知识点	机器人工程适配说明
echo "代码" > TebTrajectory.cpp && g++ TebTrajectory.cpp -o TebTrajectory && ./TebTrajectory	TEB 轨迹类封装实现	类与对象、构造函数 / 析构函数、成员变量 / 成员函数、封装特性	对应 TEB 局部规划器的轨迹点管理、路径长度计算、轨迹点增删改查，实现轨迹数据的私有化封装与标准化接口，是 TEB 算法 C++ 实现的核心基础
echo "代码" > RobotSensor.cpp && g++ RobotSensor.cpp -o RobotSensor && ./RobotSensor	机器人传感器基类与多态实现	继承、虚函数、纯虚函数、运行时多态	对应机器人激光雷达、IMU、超声传感器的统一接口封装，用多态实现不同传感器的统一数据读取接口，大幅简化 ROS 多传感融合节点的代码架构
echo "代码" > RobotConstraint.cpp && g++ RobotConstraint.cpp -o RobotConstraint && ./RobotConstraint	TEB 约束条件多态实现	公有继承、虚函数重写、接口与实现分离	对应 TEB 规划器的障碍物约束、速度约束、加速度约束、动力学约束，用基类定义统一校验接口，派生类实现不同约束的具体逻辑，完美适配 TEB 算法的多目标优化架构
echo "代码" > RobotMotorDriver.cpp && g++ RobotMotorDriver.cpp -o RobotMotorDriver && ./RobotMotorDriver	机器人电机驱动重载实现	函数重载、运算符重载、默认参数	对应机器人不同型号电机、差分驱动 / 阿克曼驱动的统一驱动接口，用函数重载适配不同的参数输入格式，实现同接口多场景适配，简化机器人运动控制代码
echo "代码" > RobotParamManager.cpp && g++ RobotParamManager.cpp -o RobotParamManager && ./RobotParamManager	机器人参数管理单例模式实现	静态成员、单例设计模式、访问控制符	对应 ROS 机器人全局参数管理，实现 TEB 规划参数、电机控制参数、传感器标定参数的全局唯一访问，避免参数重复加载与数据不一致，是工业级机器人代码的核心设计模式

三、C++ STL 与算法类（机器人控制与数据处理核心）

核心定位：覆盖 C++ 标准模板库核心容器与算法，实现机器人海量传感数据、路径数据、参数数据的高效管理，是 TEB 路径优化、多传感数据融合、实时控制的核心工具，直接对接机器人算法开发工程需求。

表格

示例精准一键运行命令	示例名称	核心 C/C++ 知识点	机器人工程适配说明
echo "代码" > GlobalPathManager.cpp && g++ GlobalPathManager.cpp -o GlobalPathManager && ./GlobalPathManager	全局路径 vector 容器管理	vector 容器、迭代器、容器增删改查、动态数组特性	对应移动机器人全局路径规划的路径点存储、遍历、插入与删除，适配 TEB 规划器对全局路径的实时读取与跟踪需求，替代原生数组实现动态内存安全管理
echo "代码" > TebParamConfig.cpp && g++ TebParamConfig.cpp -o TebParamConfig && ./TebParamConfig	TEB 参数 map 容器管理	map/unordered_map 关联容器、键值对存储、参数快速查找	对应 TEB 规划器的权重参数、速度约束、加速度约束、障碍物约束的键值对管理，实现参数的快速查找、动态修改与批量配置，完美适配 ROS 参数服务器的参数读写逻辑
echo "代码" > SensorDataCache.cpp && g++ SensorDataCache.cpp -o SensorDataCache && ./SensorDataCache	传感器数据循环队列实现	queue/deque 容器、先进先出特性、循环缓存设计	对应机器人激光雷达、IMU 高频数据的实时缓存与历史数据读取，实现滑动窗口滤波、数据时序对齐，是机器人实时控制系统的核心数据缓存方案
echo "代码" > PathOptimize.cpp && g++ PathOptimize.cpp -o PathOptimize && ./PathOptimize	路径点排序与去重算法实现	algorithm 标准算法库、sort 排序、unique 去重、find 查找	对应 TEB 规划器的路径点预处理、全局路径优化、冗余点剔除，用 STL 标准算法实现高效的路径数据处理，避免重复造轮子，提升代码稳定性与执行效率
echo "代码" > RobotStateSet.cpp && g++ RobotStateSet.cpp -o RobotStateSet && ./RobotStateSet	机器人状态集合管理	set 容器、元素唯一性、有序存储特性	对应机器人运行状态、故障状态、导航状态的去重与有序管理，实现机器人状态机的快速状态切换与合法性校验，是机器人上层逻辑控制的核心数据结构

四、ROS 与 C++ 融合 机器人节点开发类（移动机器人工程落地核心）

核心定位：C/C++ 语法与 ROS 机器人操作系统的深度融合案例，将 C++ 面向对象特性与 ROS 导航栈、TEB 规划器开发无缝对接，直接对应移动机器人下位机控制、导航节点开发、TEB 二次开发，是 ROS 机器人开发的核心参考。

表格

示例精准运行命令	示例名称	核心 C/C++ 知识点	机器人工程适配说明
先创建 ROS 功能包，catkin_make编译后rosrun运行	ROS 速度指令发布节点	ROS C++ API、节点句柄、发布者对象、类封装 ROS 节点	对应 TEB 规划器的速度指令输出，向 /cmd_vel 话题发布机器人线速度 / 角速度控制指令，实现移动机器人的运动控制，是 ROS 导航的核心执行节点
先创建 ROS 功能包，catkin_make编译后rosrun运行	ROS 激光雷达数据订阅节点	ROS 订阅者对象、回调函数、const 常量引用、传感数据解析	对应 TEB 规划器的障碍物感知输入，订阅 /scan 激光雷达话题，解析距离数据并计算最近障碍物距离，为 TEB 避障规划提供实时环境数据
先创建 ROS 功能包，catkin_make编译后rosrun运行	TEB 参数动态配置节点	ROS 参数服务器、dynamic_reconfigure C++ 接口、参数动态修改	对应 TEB 规划器的实时参数调优，实现 weight_obstacle、max_vel_x、acc_lim_x 等核心参数的动态修改，无需重启节点即可优化 TEB 规划效果
先创建 ROS 功能包，catkin_make编译后rosrun运行	机器人里程计发布节点	ROS 时间戳、tf 坐标变换、结构体封装位姿数据、发布频率控制	对应移动机器人里程计位姿发布，向 ROS 系统发布机器人实时位姿与 tf 坐标变换，为 TEB 规划器提供机器人当前位姿与运动状态反馈
先创建 ROS 功能包，catkin_make编译后rosrun运行	机器人导航目标点监听节点	ROS action 客户端、move_base 导航接口、回调函数异步处理	对应 TEB 规划器的导航目标设置，监听 /rviz/2d_nav_goal 目标点，向 move_base 发送导航请求，实现 TEB 规划器的完整导航闭环控制

五、综合项目类（移动机器人系统级应用）

核心定位：C/C++ 全流程开发、多模块联动的系统级案例，对应移动机器人完整导航系统开发、TEB 规划器二次开发，融合 C 语言基础、C++ 面向对象、STL、ROS C++ 开发全知识点，完全匹配机器人专业课程设计、竞赛项目、实机开发需求。

表格

示例精准运行命令	示例名称	核心 C/C++ 知识点	机器人工程适配说明
基于 ROS 工作空间编译运行	TEB 局部规划器简化版 C++ 实现	面向对象设计、多态、STL 容器、ROS 导航插件机制	基于 C++ 实现 TEB Timed Elastic Band 核心算法，适配 ROS navigation 导航栈插件接口，可直接替换原生 TEB 规划器，实现移动机器人的局部路径优化与避障
基于 ROS 工作空间编译运行	移动机器人自主避障控制系统	多线程编程、回调函数同步、传感数据融合、状态机设计	融合激光雷达避障、TEB 路径规划、电机速度控制全流程，用 C++ 实现机器人从环境感知→路径规划→运动执行的完整自主避障闭环
跨平台直接编译运行	机器人串口通信下位机程序	C 语言文件 IO、串口驱动、数据帧解析、循环缓冲区设计	对应机器人主控与下位机 STM32/Arduino 的串口通信，实现编码器数据读取、电机控制指令下发，是移动机器人硬件与上层 ROS 系统的通信桥梁

---

机器人工程专属 C/C++ 核心实战｜一键运行，边跑边懂

完全沿用参考文档操作规范，适配 Ubuntu 系统，所有案例均贴合移动机器人 TEB 导航核心场景，复制到终端即可一键编译运行，每步附代码 + 运行命令 + 工程结论。

实战 1：机器人激光雷达数据处理（C 语言基础入门必做）

实验目标：掌握 C 语言指针、数组、循环核心语法，实现激光雷达数据的最近障碍物距离计算，解决 TEB 规划器避障的底层数据处理需求，验证 C 语言基础语法的工程落地。

bash运行

echo "
#include <stdio.h>

int main() {
    // 模拟移动机器人激光雷达10组距离数据，单位：米
    double laser_data[] = {1.2, 0.8, 1.5, 0.4, 2.0, 0.6, 1.8, 0.3, 2.2, 1.0};
    int data_size = sizeof(laser_data)/sizeof(double);
    
    // 指针访问数组首元素，初始化最近距离
    double min_dist = *laser_data;
    int min_index = 0;

    // 指针遍历数组，查找最近障碍物
    for(int i=1; i<data_size; i++) {
        if(*(laser_data + i) < min_dist) {
            min_dist = *(laser_data + i);
            min_index = i;
        }
    }

    // 输出TEB规划所需的避障数据
    printf("===== 机器人激光雷达数据处理结果 =====\n");
    printf("TEB规划器检测到最近障碍物距离: %.2f m\n", min_dist);
    printf("障碍物对应激光雷达角度: %d°\n", min_index * 36);
    printf("=======================================\n");
    
    return 0;
}
" > robot_laser_calc.c && gcc robot_laser_calc.c -o robot_laser_calc && ./robot_laser_calc

仿真验证与工程结论

• 运行后直接输出 TEB 避障所需的最近障碍物距离与角度，验证 C 语言指针、数组的核心用法，理解内存地址与数据访问的底层逻辑；
• 修改激光雷达数据数组，可直接模拟不同障碍物场景，代码可无缝移植到 ROS 激光数据订阅节点中，为 TEB 规划器提供实时避障数据；
• 工程结论：C 语言指针与数组是机器人传感器数据处理的底层核心，直接决定了 TEB 规划器的实时性与数据处理效率，是机器人 C/C++ 开发的入门基石。

实战 2：TEB 约束条件面向对象封装（C++ 核心必做）

实验目标：掌握 C++ 类、继承、多态核心特性，实现 TEB 规划器多约束条件的模块化封装，解决 TEB 算法多目标优化的代码架构设计需求，理解面向对象开发的工程价值。

bash运行

echo "
#include <iostream>

// TEB约束基类：定义统一接口
class TebConstraint {
public:
    // 纯虚函数：约束校验接口
    virtual bool checkConstraint() = 0;
    virtual ~TebConstraint() {}
};

// 派生类：障碍物约束
class ObstacleConstraint : public TebConstraint {
private:
    double min_obstacle_dist;
    double safe_dist;
public:
    ObstacleConstraint(double dist, double safe) : min_obstacle_dist(dist), safe_dist(safe) {}
    
    bool checkConstraint() override {
        std::cout << "[TEB障碍物约束校验] 最近障碍物距离: " << min_obstacle_dist << "m，安全距离: " << safe_dist << "m" << std::endl;
        return min_obstacle_dist > safe_dist;
    }
};

// 派生类：速度约束
class VelocityConstraint : public TebConstraint {
private:
    double current_vel;
    double max_vel;
public:
    VelocityConstraint(double vel, double max) : current_vel(vel), max_vel(max) {}
    
    bool checkConstraint() override {
        std::cout << "[TEB速度约束校验] 当前速度: " << current_vel << "m/s，最大限速: " << max_vel << "m/s" << std::endl;
        return current_vel < max_vel;
    }
};

// 派生类：加速度约束
class AccelerationConstraint : public TebConstraint {
private:
    double current_acc;
    double max_acc;
public:
    AccelerationConstraint(double acc, double max) : current_acc(acc), max_acc(max) {}
    
    bool checkConstraint() override {
        std::cout << "[TEB加速度约束校验] 当前加速度: " << current_acc << "m/s²，最大限值: " << max_acc << "m/s²" << std::endl;
        return current_acc < max_acc;
    }
};

int main() {
    std::cout << "===== TEB局部规划器多约束校验 =====" << std::endl;
    
    // 多态调用：基类指针指向派生类对象
    TebConstraint* constraint1 = new ObstacleConstraint(0.5, 0.3);
    TebConstraint* constraint2 = new VelocityConstraint(0.4, 0.5);
    TebConstraint* constraint3 = new AccelerationConstraint(0.4, 0.5);

    // 统一接口批量校验
    bool is_path_valid = true;
    is_path_valid &= constraint1->checkConstraint();
    is_path_valid &= constraint2->checkConstraint();
    is_path_valid &= constraint3->checkConstraint();

    std::cout << "---------------------------------------" << std::endl;
    if(is_path_valid) {
        std::cout << "TEB路径校验通过，可正常执行导航" << std::endl;
    } else {
        std::cout << "TEB路径校验失败，重新规划路径" << std::endl;
    }
    std::cout << "=======================================" << std::endl;

    // 释放内存
    delete constraint1;
    delete constraint2;
    delete constraint3;
    
    return 0;
}
" > TebConstraintDemo.cpp && g++ TebConstraintDemo.cpp -o TebConstraintDemo && ./TebConstraintDemo

仿真验证与工程结论

• 运行后自动完成 TEB 规划器的障碍物、速度、加速度多约束校验，输出路径是否合法的判断结果，验证 C++ 继承、多态的核心特性；
• 新增约束条件只需新增派生类，无需修改原有校验逻辑，完美符合开闭原则，代码架构可直接复用至 ROS TEB 规划器的二次开发中；
• 工程结论：C++ 面向对象特性是机器人复杂算法模块化开发的核心，通过封装、继承、多态可大幅提升机器人代码的可扩展性、可维护性，是工业级机器人软件开发的标准范式。

实战 3：TEB 参数管理与 STL 应用（算法开发核心必做）

实验目标：掌握 C++ STL 容器与标准算法的核心用法，实现 TEB 规划器参数的高效管理与路径点预处理，解决机器人算法开发中的数据管理需求，理解 STL 在机器人工程中的落地价值。

bash运行

echo "
#include <iostream>
#include <vector>
#include <map>
#include <algorithm>
#include <string>

// 路径点结构体
struct PathPoint {
    double x;
    double y;
    PathPoint(double x_, double y_) : x(x_), y(y_) {}
};

int main() {
    std::cout << "===== TEB规划器STL应用演示 =====" << std::endl;

    // 1. map容器管理TEB核心参数
    std::map<std::string, double> teb_params;
    teb_params["max_vel_x"] = 0.5;
    teb_params["weight_obstacle"] = 50.0;
    teb_params["acc_lim_x"] = 0.5;
    teb_params["inflation_radius"] = 0.4;

    std::cout << "[TEB参数列表]" << std::endl;
    for(const auto& param : teb_params) {
        std::cout << param.first << ": " << param.second << std::endl;
    }
    std::cout << "-----------------------------------" << std::endl;

    // 2. vector容器管理全局路径点
    std::vector<PathPoint> global_path;
    global_path.emplace_back(0.0, 0.0);
    global_path.emplace_back(1.0, 2.0);
    global_path.emplace_back(2.0, 1.0);
    global_path.emplace_back(3.0, 3.0);
    global_path.emplace_back(2.0, 1.0); // 冗余点

    std::cout << "原始路径点数量: " << global_path.size() << std::endl;

    // 3. STL算法实现路径点去重
    std::sort(global_path.begin(), global_path.end(), [](const PathPoint& a, const PathPoint& b) {
        if(a.x != b.x) return a.x < b.x;
        return a.y < b.y;
    });
    auto last = std::unique(global_path.begin(), global_path.end(), [](const PathPoint& a, const PathPoint& b) {
        return a.x == b.x && a.y == b.y;
    });
    global_path.erase(last, global_path.end());

    std::cout << "去重后路径点数量: " << global_path.size() << std::endl;

    // 4. STL算法查找x坐标最大的路径点
    auto max_x_point = std::max_element(global_path.begin(), global_path.end(), 
        [](const PathPoint& a, const PathPoint& b) { return a.x < b.x; });
    std::cout << "全局路径终点坐标: (" << max_x_point->x << ", " << max_x_point->y << ")" << std::endl;
    std::cout << "===================================" << std::endl;

    return 0;
}
" > TebSTLDemo.cpp && g++ TebSTLDemo.cpp -o TebSTLDemo && ./TebSTLDemo

仿真验证与工程结论

• 运行后完成 TEB 参数管理、路径点去重、极值查找全流程，验证 STL 容器与标准算法的高效性，代码量远低于原生数组实现，且内存安全、执行效率高；
• map 容器的键值对管理完美适配 ROS 参数服务器，vector 容器的动态数组特性完美适配全局路径的动态更新，代码可直接移植到 ROS 导航节点中；
• 工程结论：C++ STL 是机器人算法开发的核心工具，无需重复造轮子，即可实现高效的数据管理与算法实现，大幅提升机器人代码的开发效率与稳定性，是 ROS 机器人开发的必备技能。

实战 4：ROS 速度发布节点开发（工程落地核心必做）

实验目标：掌握 ROS 与 C++ 融合开发的核心流程，实现 TEB 规划器的速度指令发布节点，打通 C/C++ 代码与 ROS 机器人系统的落地链路，理解 C/C++ 在真实机器人开发中的应用。

bash运行

# 第一步：创建ROS功能包（仅首次执行）
mkdir -p ~/teb_cpp_demo_ws/src && cd ~/teb_cpp_demo_ws/src
catkin_create_pkg teb_speed_pub roscpp geometry_msgs std_msgs
cd ~/teb_cpp_demo_ws && catkin_make
source devel/setup.bash

# 第二步：写入节点代码并编译运行
echo "
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

int main(int argc, char** argv) {
    // ROS节点初始化
    ros::init(argc, argv, "teb_velocity_publisher");
    ros::NodeHandle nh;

    // 创建速度指令发布者，对应TEB规划器的/cmd_vel输出
    ros::Publisher vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    // 设置循环频率10Hz，匹配TEB规划器的实时控制频率
    ros::Rate loop_rate(10);

    ROS_INFO("===== TEB速度发布节点启动成功 =====");
    ROS_INFO("向/cmd_vel话题发布机器人控制指令");
    ROS_INFO("=====================================");

    // 节点主循环
    while (ros::ok()) {
        // 构造TEB规划器输出的速度指令
        geometry_msgs::Twist cmd_vel;
        cmd_vel.linear.x = 0.3;   // TEB规划的线速度
        cmd_vel.angular.z = 0.1;  // TEB规划的角速度

        // 发布速度指令
        vel_pub.publish(cmd_vel);
        ROS_INFO("TEB发布指令：线速度%.2f m/s，角速度%.2f rad/s", cmd_vel.linear.x, cmd_vel.angular.z);

        // 循环等待
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}
" > ~/teb_cpp_demo_ws/src/teb_speed_pub/src/teb_speed_pub.cpp

# 第三步：修改CMakeLists.txt，添加编译规则
echo "
add_executable(teb_speed_pub src/teb_speed_pub.cpp)
target_link_libraries(teb_speed_pub \${catkin_LIBRARIES})
" >> ~/teb_cpp_demo_ws/src/teb_speed_pub/CMakeLists.txt

# 第四步：编译并运行节点
cd ~/teb_cpp_demo_ws && catkin_make
source devel/setup.bash
rosrun teb_speed_pub teb_speed_pub

仿真验证与工程结论

• 节点启动后，以 10Hz 频率向 ROS 系统发布 /cmd_vel 速度指令，可直接在 Stage 仿真环境中控制机器人运动，与 TEB 规划器的输出接口完全兼容；
• 基于 C++ 实现的 ROS 节点，执行效率远高于 Python 节点，完美适配 TEB 规划器对实时性的高要求，可直接扩展为 TEB 规划器的二次开发节点；
• 工程结论：C/C++ 是 ROS 机器人工业级开发的首选语言，基于 C++ 实现的 ROS 节点具备高实时性、高稳定性，是移动机器人导航、TEB 规划器开发、实机控制的核心技术。

---

纯通俗・无公式・兴趣驱动｜C/C++ 程序设计「机器人专属全通关概论」

完全沿用讲解风格：不写复杂语法定义、不搞课本硬推导、全靠工程逻辑 + 机器人场景类比 + 结合上面 4 段可直接运行的代码理解，一口气讲完 C/C++ 程序设计全部核心知识点，从头到尾连贯、通俗、好吸收，彻底打通 C/C++ 底层逻辑，完美适配机器人工程专业。

一、核心底层：到底什么是 C/C++ 程序设计？

抛开课本定义：C/C++ 程序设计，就是一套机器人硬件世界的标准化控制语言，你可以把它理解成「机器人硬件与算法的底层操作系统」。

• 普通上位机代码：控制机器人的软件界面、数据可视化；
• C/C++ 代码：直接控制机器人的电机启停、传感器数据读取、路径规划计算、安全保护触发，里面每一个语法、每一个关键字，都是机器人硬件控制、算法实时运行的底层依据；
• 只要给定机器人的传感器、执行器、导航算法，它的控制实时性、运行稳定性、硬件访问权限，就全部被 C/C++ 的特性固定死，不会乱变。

所有 C/C++ 程序设计，本质都是：研究机器人硬件与算法的标准化控制规则，并用这套规则设计、开发、优化机器人的完整控制系统。

二、C 语言基础篇：机器人硬件控制的根本（核心重点，占比 60%）

这是机器人工程最核心的部分，《机器人硬件设计》《传感器技术》《电机控制》全靠它打底。

• 变量与数据类型通俗理解：机器人数据的最小存储单元，对应机器人的速度、距离、角度、传感器数值，是所有机器人数据处理的起点；
• 指针与数组通俗理解：机器人内存数据的直接访问钥匙，对应激光雷达、IMU 的海量数组数据快速遍历，直接决定了机器人控制的实时性，是 C 语言最核心的灵魂；
• 分支与循环通俗理解：机器人逻辑判断与重复执行的核心，对应机器人的避障判断、急停触发、传感器数据循环采集，是机器人自动运行的基础；
• 函数通俗理解：机器人功能的模块化封装，对应滤波算法、里程计算法、电机控制算法的封装，实现一次编写、多次调用，是机器人代码可维护性的核心；
• 结构体通俗理解：机器人相关数据的打包盒子，对应机器人位姿、速度、传感器状态的打包管理，让零散的机器人数据形成标准化的整体，是复杂系统开发的基础。

三、C++ 面向对象篇：机器人算法与模块的核心

机器人的 TEB 路径规划、多传感融合、多关节控制，全靠它打底，是 ROS 系统的核心开发语言。

• 类与封装通俗理解 ：机器人功能模块的标准化黑盒，把传感器的底层数据读取、参数配置、算法计算封装到黑盒里，只对外暴露简单的调用接口，比如激光雷达类，只需要调用get_min_dist()就能拿到最近障碍物距离，不用管底层数据怎么读取；
• 继承与多态通俗理解 ：机器人同类功能的统一接口，比如 TEB 的各种约束条件，用基类定义统一的checkConstraint()接口，不同的约束派生类实现各自的校验逻辑，新增约束不用改原有代码，是机器人复杂算法架构设计的核心；
• 重载与重写通俗理解：机器人同功能的多场景适配，同一个电机控制函数，能适配差分驱动、阿克曼驱动不同的参数输入，不用写多个重复函数，大幅简化机器人代码的冗余度；
• 设计模式通俗理解：机器人工业级开发的标准化方案，比如单例模式实现全局参数管理，工厂模式实现传感器对象的统一创建，是工业机器人、商业机器人代码开发的通用标准。

四、C++ STL 篇：机器人数据与算法的核心工具

机器人的路径管理、参数配置、数据缓存、算法实现，全靠它打底，是机器人算法开发的效率神器。

• vector 容器通俗理解：机器人动态数组，对应全局路径点、传感器历史数据的存储，长度自动变化，不用手动管理内存，比原生数组安全 100 倍；
• map 容器通俗理解：机器人键值对参数本，对应 TEB 规划参数、PID 控制参数的管理，通过参数名就能快速查找、修改参数，完美适配 ROS 参数服务器；
• queue/deque 容器通俗理解：机器人数据循环队列，对应高频传感器数据的实时缓存，先进先出，实现滑动窗口滤波、时序数据对齐，是机器人实时控制的核心数据结构；
• STL 算法通俗理解：机器人算法的预制工具箱，排序、去重、查找、遍历，不用自己手写循环，一行代码就能实现复杂的数据处理，大幅提升开发效率，减少 bug。

五、ROS 与 C++ 融合篇：机器人工程落地的唯一通路

所有真实的 ROS 机器人、工业机器人，核心控制节点全都是 C++ 开发的，这是从课本到真实机器人的必经之路。

• ROS 节点通俗理解：C++ 类封装的机器人功能单元，一个节点对应一个功能，比如速度发布节点、激光数据订阅节点、TEB 规划节点，节点之间通过话题通信，实现机器人分布式控制系统；
• 发布者 / 订阅者通俗理解：C++ 实现的机器人数据传输通道，TEB 规划器通过发布者发速度指令，电机驱动节点通过订阅者收指令，实现解耦开发，互不影响；
• 回调函数通俗理解：机器人事件的异步响应机制，激光雷达有新数据来了，自动触发回调函数处理，不用一直循环查询，保证机器人控制的实时性；
• ROS 参数服务器通俗理解：C++ map 容器的 ROS 全局版，所有节点都能读取、修改 TEB 的规划参数，实现机器人参数的全局统一管理。

六、全部知识串联：C/C++ 程序设计完整逻辑链（机器人专属）

• C 语言基础 = 机器人硬件控制与数据处理的底层规则
• C++ 面向对象 = 机器人算法与模块的标准化封装
• C++ STL = 机器人数据与算法的高效工具集
• ROS C++ 开发 = C/C++ 能力到机器人工程落地的通路
• 代码编译运行 = 程序逻辑的落地验证

七、现实落地：机器人专业学 C/C++ 到底能干什么

完全脱离做题，讲真实用途：

• 移动机器人开发：用 C 语言实现底层传感器数据处理、电机驱动，用 C++ 实现 TEB 路径规划、ROS 导航节点开发，完成完整的自主导航机器人；
• 机械臂开发：用 C++ 实现正逆运动学解算、轨迹规划、力控算法，开发机械臂的实时控制节点；
• 机器人算法开发：用 C++ 实现 SLAM 算法、路径规划算法、多传感融合算法，保证算法的实时性与执行效率；
• 工业机器人开发：用 C/C++ 实现工业机器人的实时控制系统、安全保护逻辑、运动控制算法，满足工业场景的高可靠性、高实时性要求；
• 机器人故障诊断：用 C/C++ 实现机器人传感器数据异常检测、电机故障诊断、控制逻辑异常判断，实现机器人的在线健康监测；
• 机器人竞赛开发：用 C/C++ 开发机器人的自主导航、避障、目标识别全流程代码，保证代码的执行效率与稳定性，是机器人竞赛的核心竞争力。

八、最终极简总括（背诵级）

C/C++ 是机器人硬件与算法的底层控制语言，C 语言定硬件控制与数据处理的底层规则，C++ 面向对象定算法与模块的封装架构，STL 定数据处理的高效工具，ROS C++ 定工程落地的通路；整个 C/C++ 程序设计，就是：研究机器人的标准化控制规则，并用这套规则设计、开发、优化机器人的完整控制系统。

---

机器人专业「考点 + 专业课联动」精准通关

期末必考考点（机器人方向侧重）

• C 语言：变量与数据类型、指针与数组、分支与循环、函数、结构体，对应机器人传感器数据处理、底层控制逻辑开发；
• C++：类与对象、构造 / 析构函数、继承与多态、虚函数，对应机器人模块封装、算法架构设计；
• C++ STL：vector/map/queue 核心容器、sort/find 等标准算法，对应机器人路径管理、参数配置、数据缓存；
• 内存管理：指针操作、动态内存分配 / 释放、内存泄漏规避，对应机器人高稳定性程序开发；
• 高级特性：函数重载、运算符重载、模板、设计模式，对应工业级机器人代码开发。

直接联动核心专业课

• 《移动机器人技术》→ 传感器数据处理、TEB 路径规划、ROS 导航节点开发；
• 《机器人控制工程》→ 电机控制算法、PID 控制器实现、运动学解算代码开发；
• 《传感器与检测技术》→ 传感器数据解析、滤波算法实现、多传感数据融合；
• 《嵌入式系统与单片机》→ 下位机串口通信、电机驱动、传感器采集 C 语言开发；
• 《机器人 SLAM 技术》→ 激光 SLAM 算法、位姿解算、点云处理 C++ 实现；
• 《机器人故障诊断》→ 传感器异常检测、电机故障诊断、控制逻辑异常判断代码开发。

---

机器人工程专属速成通关法

1. 放弃无用刷题，聚焦核心场景不用死磕复杂的语法题、指针奇淫技巧题、与机器人无关的编程题。重点只学：C 语言指针与数组、函数与结构体；C++ 类与对象、继承与多态；STL 核心容器与算法；ROS C++ 节点开发，这些是机器人工程 100% 会用到的核心。

2. 建立「C/C++ = 机器人控制语言」的工程思维在你眼里：C 语言指针 = 机器人传感器数据快速访问的钥匙，C++ 类 = 机器人功能模块的黑盒封装，STL 容器 = 机器人数据的管理盒子，ROS 节点 = 机器人功能的执行单元。所有 C/C++ 语法，都是为了让机器人 "测得准、动得顺、规划得好、不宕机"。

3. 结合编译运行，从现象理解原理先跑通代码看运行结果，再拆解代码理解语法原理，最后修改参数验证规律，彻底摆脱课本死记硬背：

• 修改激光雷达数组，看最近障碍物距离输出变化→直观理解指针与数组的核心逻辑；
• 新增 TEB 约束派生类，看多态调用的效果→吃透继承与多态的工程价值；
• 增减 STL 容器的路径点，看算法处理的结果→掌握 STL 容器与算法的用法；
• 运行 ROS 节点，在 RViz 里看机器人运动→理解 C/C++ 代码到机器人真实运动的落地链路。
• **背诵级极简总结（考前直接背）**指针数组是 C 语言核心，搞定传感器数据处理；类与多态是 C++ 灵魂，搞定机器人模块封装；STL 容器是效率神器，搞定数据管理与算法实现；ROS C++ 是落地通路，搞定机器人真实导航控制；C/C++ + ROS + TEB 案例 = 机器人工程开发的核心竞争力，是从课本理论到真实机器人落地的必经之路。

---

基于 ROS Kinetic TEB 案例的《C/C++ 程序设计》课程重构

（适用于机器人专业・移动机器人设计与实践方向）

课程设计理念

以移动机器人 TEB 局部规划器为核心案例载体，将 C/C++ 知识点拆解融入「传感器数据处理→轨迹优化→ROS 节点开发→仿真验证」全流程，实现 "语法学习→算法实现→工程落地" 的闭环。

课程结构与内容规划

第一部分：C 语言基础与机器人数据处理（4 课时）

第 1 章 C 语言概述与机器人简单程序

• 核心知识点：C 语言结构、变量 / 数据类型、输入输出

• TEB 关联案例 ：

  c运行

  // 模拟输出TEB规划器的速度指令
  #include <stdio.h>
  int main() {
      double max_vel_x = 0.5;  // TEB最大线速度
      double cmd_vel = 0.3;     // 当前速度指令
      printf("TEB规划器启动 - 最大线速度: %.2f m/s\n", max_vel_x);
      printf("发布速度指令: %.2f m/s\n", cmd_vel);
      return 0;
  }

• 实践任务：扩展程序，输出 TEB 的角速度指令与障碍物权重参数。

第 2 章 指针、数组与激光雷达数据处理

• 核心知识点：指针、数组、指针与数组的关系

• TEB 关联案例 ：

  c运行

  // 用指针遍历激光雷达数据，计算最近障碍物距离
  #include <stdio.h>
  int main() {
      double laser_data[] = {1.2, 0.8, 1.5, 0.5, 2.0};  // 模拟激光距离
      int data_size = sizeof(laser_data)/sizeof(double);
      double min_dist = *laser_data;  // 指针访问首元素
  
      for (int i=1; i<data_size; i++) {
          if (*(laser_data+i) < min_dist) {  // 指针偏移访问
              min_dist = *(laser_data+i);
          }
      }
      printf("TEB检测到最近障碍物距离: %.2f m\n", min_dist);
      return 0;
  }

• 实践任务：编写函数，用指针实现激光雷达数据的滤波（取平均值）。

第二部分：C++ 进阶与 TEB 算法实现（8 课时）

第 3 章 类与对象：封装 TEB 轨迹模块

• 核心知识点：类、构造 / 析构函数、成员变量 / 函数

• TEB 关联案例 ：

  cpp运行

  #include <vector>
  #include <iostream>
  class TebTrajectory {
  private:
      std::vector<double> path_x;  // 轨迹x坐标
      std::vector<double> path_y;  // 轨迹y坐标
  public:
      TebTrajectory() {}  // 构造函数
      ~TebTrajectory() {} // 析构函数
  
      // 添加轨迹点
      void addPoint(double x, double y) {
          path_x.push_back(x);
          path_y.push_back(y);
      }
  
      // 输出轨迹信息
      void printTrajectory() {
          std::cout << "TEB轨迹点数量: " << path_x.size() << std::endl;
          for (int i=0; i<path_x.size(); i++) {
              std::cout << "点" << i << ": (" << path_x[i] << ", " << path_y[i] << ")" << std::endl;
          }
      }
  };
  
  int main() {
      TebTrajectory traj;
      traj.addPoint(0.0, 0.0);
      traj.addPoint(1.0, 1.0);
      traj.printTrajectory();
      return 0;
  }

• 实践任务 ：扩展TebTrajectory类，添加计算轨迹总长度的方法。

第 4 章 继承与多态：实现 TEB 约束条件

• 核心知识点：继承、虚函数、多态

• TEB 关联案例 ：

  cpp运行

  #include <iostream>
  // 基类：约束条件
  class TebConstraint {
  public:
      virtual bool checkConstraint() = 0;  // 纯虚函数
  };
  
  // 派生类：障碍物约束
  class ObstacleConstraint : public TebConstraint {
  private:
      double min_dist;
  public:
      ObstacleConstraint(double dist) : min_dist(dist) {}
      bool checkConstraint() override {
          std::cout << "检查障碍物约束，最小距离: " << min_dist << " m" << std::endl;
          return min_dist > 0.3;  // 假设安全距离为0.3m
      }
  };
  
  // 派生类：速度约束
  class VelocityConstraint : public TebConstraint {
  private:
      double max_vel;
  public:
      VelocityConstraint(double vel) : max_vel(vel) {}
      bool checkConstraint() override {
          std::cout << "检查速度约束，最大速度: " << max_vel << " m/s" << std::endl;
          return max_vel < 1.0;  // 假设最大限速1.0m/s
      }
  };
  
  int main() {
      TebConstraint* c1 = new ObstacleConstraint(0.5);
      TebConstraint* c2 = new VelocityConstraint(0.8);
      c1->checkConstraint();
      c2->checkConstraint();
      delete c1;
      delete c2;
      return 0;
  }

• 实践任务 ：添加AccelerationConstraint类，实现加速度约束检查。

第 5 章 STL 与 TEB 数据管理

• 核心知识点 ：vector、map、算法库

• TEB 关联案例 ：

  cpp运行

  #include <vector>
  #include <map>
  #include <algorithm>
  #include <iostream>
  
  int main() {
      // 用vector存储全局路径点
      std::vector<std::pair<double, double>> global_path;
      global_path.emplace_back(0.0, 0.0);
      global_path.emplace_back(1.0, 2.0);
      global_path.emplace_back(2.0, 1.0);
  
      // 用map存储TEB参数
      std::map<std::string, double> teb_params;
      teb_params["max_vel_x"] = 0.5;
      teb_params["weight_obstacle"] = 10.0;
  
      // 用STL算法查找路径中x坐标最大的点
      auto max_x_point = std::max_element(global_path.begin(), global_path.end(),
          [](const auto& a, const auto& b) { return a.first < b.first; });
      std::cout << "全局路径中x最大点: (" << max_x_point->first << ", " << max_x_point->second << ")" << std::endl;
  
      return 0;
  }

• 实践任务 ：使用std::sort对全局路径点按距离原点的远近排序。

第三部分：ROS 与 C++ 结合：TEB 节点开发（6 课时）

第 6 章 ROS 基础与 TEB 节点编写

• 核心知识点：ROS 节点、话题、发布者 / 订阅者

• TEB 关联案例 （简化版速度发布节点）：

  cpp运行

  #include <ros/ros.h>
  #include <geometry_msgs/Twist.h>
  
  int main(int argc, char** argv) {
      ros::init(argc, argv, "teb_velocity_publisher");
      ros::NodeHandle nh;
  
      // 创建速度指令发布者
      ros::Publisher vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
      ros::Rate loop_rate(10);  // 10Hz循环
  
      while (ros::ok()) {
          geometry_msgs::Twist cmd_vel;
          cmd_vel.linear.x = 0.3;   // TEB规划的线速度
          cmd_vel.angular.z = 0.1;  // TEB规划的角速度
          vel_pub.publish(cmd_vel);
  
          ros::spinOnce();
          loop_rate.sleep();
      }
      return 0;
  }

• 实践任务：编写订阅者节点，接收激光雷达数据并输出最近障碍物距离。

第 7 章 TEB 局部规划器简化实现与仿真

• 核心知识点：ROS 导航栈接口、参数服务器、动态参数配置
• 综合任务 ：
  1. 基于前面的TebTrajectory类和约束类，编写简化的 TEB 局部规划器 ROS 节点；
  2. 参考提供的 TEB 案例，在 Stage 仿真环境中启动robot_carlike_in_stage.launch；
  3. 用dynamic_reconfigure动态调整weight_obstacle、max_vel_x等参数，观察机器人导航效果。

第四部分：综合项目（2 课时）

项目：TEB 局部规划器参数调优与仿真验证

• 要求 ：
  1. 使用 C++ 编写 TEB 参数调优节点，根据激光雷达数据动态调整障碍物权重；
  2. 在 Stage 仿真中完成「狭窄通道通过」「动态障碍物避障」两个场景测试；
  3. 记录不同参数下的导航成功率，输出分析报告。

教学资源与工具

1. ROS 环境：Ubuntu 16.04 + ROS Kinetic（参考提供的 TEB 案例配置镜像源）；
2. TEB 功能包 ：ros-kinetic-teb-local-planner、ros-kinetic-teb-local-planner-tutorials；
3. 仿真工具 ：Stage 仿真器（ros-kinetic-stage-ros）。

考核方式

• 平时作业（40%）：各章节编程实践任务；
• 项目验收（60%）：综合项目代码、仿真演示与报告。

---

本文介绍了一个基于C/C++和ROS的移动机器人开发教程系列，重点围绕TEB局部规划器展开。内容涵盖12年研发积累，分为C语言基础、C++面向对象、STL容器算法、ROS节点开发四大模块，通过真实可运行的代码示例讲解机器人工程开发的核心技术。教程特色包括：1) 提供一键运行的终端命令；2) 每个示例标注核心知识点和工程适配说明；3) 从传感器数据处理到TEB规划器开发的完整链路；4) 适配Ubuntu 16.04/20.04和ROS Kinetic/Noetic环境。该教程旨在帮助学习者快速掌握机器人开发所需的C/C++编程技能和ROS应用能力。