ROS2 导航播报 & 轨迹模拟 —— 计划

ChengQianO2026-03-25 17:10

目标 ：在 ROS2 下用 C++ 实现导航语音播报，并用生成的播报内容模拟机器人/无人车的行驶轨迹。
环境 ：Ubuntu + ROS2 (Humble/Iron/Jazzy) + ament_cmake 构建
语言：C++17

📋 整体学习路线图

复制代码 
阶段1: ROS2 基础 + C++  →  阶段2: TF & 坐标系     →  阶段3: 导航框架 Nav2
   ↓                          ↓                          ↓
阶段4: 轨迹生成与发布    →  阶段5: 语音播报集成     →  阶段6: 可视化与仿真
   ↓                          ↓                          ↓
阶段7: 综合实战项目      →  阶段8: 进阶优化         →  完成 🎉

阶段 1：ROS2 基础入门 + C++ 开发基础（1~2 周）

1.1 核心概念

概念 说明 C++ 关键类
Node（节点） ROS2 最小执行单元 rclcpp::Node
Topic（话题） 发布/订阅异步通信 create_publisher / create_subscription
Service（服务） 请求/响应同步通信 create_service / create_client
Action（动作） 带反馈的长时间任务 rclcpp_action::create_server/client
Parameter（参数） 节点可配置参数 declare_parameter / get_parameter
Timer（定时器） 周期性回调 create_wall_timer

1.2 C++ 前置知识（ROS2 大量使用）

C++ 特性 ROS2 中的用途 示例
std::shared_ptr 所有 ROS2 对象生命周期管理 std::make_shared<MyNode>()
Lambda 表达式 回调函数 [this](const Msg::SharedPtr msg){...}
std::bind 绑定成员函数为回调 std::bind(&Class::func, this, _1)
模板 消息类型泛型 create_publisher<std_msgs::msg::String>(...)
auto 简化类型声明 auto pub = create_publisher<...>(...)
std::chrono 定时器时间间隔 std::chrono::milliseconds(50)

1.3 动手练习

bash 复制代码 
# 安装 ROS2（以 Humble 为例）
sudo apt install ros-humble-desktop

# 配置环境
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
source ~/.bashrc

# 运行示例：小乌龟
ros2 run turtlesim turtlesim_node
ros2 run turtlesim turtle_teleop_key

# 查看话题
ros2 topic list
ros2 topic echo /turtle1/pose

1.4 创建第一个 C++ 包

bash 复制代码 
cd /home/ubuntu/ros/src

# build-type 用 ament_cmake，依赖用 rclcpp（不是 rclpy）
ros2 pkg create --build-type ament_cmake my_first_pkg \
    --dependencies rclcpp std_msgs

1.5 最简节点示例

cpp 复制代码 
// src/hello_node.cpp
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

class HelloNode : public rclcpp::Node
{
public:
    HelloNode() : Node("hello_node")
    {
        publisher_ = this->create_publisher<std_msgs::msg::String>(
            "hello_topic", 10);
        timer_ = this->create_wall_timer(
            std::chrono::seconds(1),
            std::bind(&HelloNode::timer_callback, this));
        RCLCPP_INFO(this->get_logger(), "Hello 节点已启动!");
    }

private:
    void timer_callback()
    {
        auto msg = std_msgs::msg::String();
        msg.data = "Hello ROS2 C++! count=" + std::to_string(count_++);
        publisher_->publish(msg);
        RCLCPP_INFO(this->get_logger(), "发布: '%s'", msg.data.c_str());
    }

    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
    size_t count_ = 0;
};

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

1.6 CMakeLists.txt 基本结构

cmake 复制代码 
cmake_minimum_required(VERSION 3.8)
project(my_first_pkg)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

add_executable(hello_node src/hello_node.cpp)
ament_target_dependencies(hello_node rclcpp std_msgs)

install(TARGETS hello_node DESTINATION lib/${PROJECT_NAME})

ament_package()

1.7 编译与运行

bash 复制代码 
cd /home/ubuntu/ros
colcon build --packages-select my_first_pkg
source install/setup.bash
ros2 run my_first_pkg hello_node

1.8 学习资源

阶段 2：TF2 坐标变换与里程计（1~2 周）

2.1 为什么需要学 TF2？

复制代码 
map → odom → base_link → sensor_link
 │            │              │
地图坐标  里程计坐标    机器人底盘     传感器坐标

2.2 核心知识点

  • 坐标系（Frame） ：map、odom、base_link、base_footprint
  • TF 广播器（Broadcaster）：发布坐标变换
  • TF 监听器（Listener）：查询坐标变换
  • 静态 TF vs 动态 TF

2.3 关键头文件

cpp 复制代码 
#include <nav_msgs/msg/odometry.hpp>                // 里程计
#include <geometry_msgs/msg/transform_stamped.hpp>  // TF 变换
#include <nav_msgs/msg/path.hpp>                    // 路径
#include <geometry_msgs/msg/pose_stamped.hpp>       // 位姿

#include <tf2_ros/transform_broadcaster.h>          // 动态 TF
#include <tf2_ros/static_transform_broadcaster.h>   // 静态 TF
#include <tf2_ros/transform_listener.h>             // TF 监听
#include <tf2_ros/buffer.h>                         // TF 缓存
#include <tf2/LinearMath/Quaternion.h>              // 四元数

2.4 TF2 广播器示例

cpp 复制代码 
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h>

class TfBroadcasterNode : public rclcpp::Node
{
public:
    TfBroadcasterNode() : Node("tf_broadcaster"), angle_(0.0)
    {
        tf_broadcaster_ =
            std::make_unique<tf2_ros::TransformBroadcaster>(*this);
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(50),
            std::bind(&TfBroadcasterNode::broadcast_cb, this));
    }

private:
    void broadcast_cb()
    {
        geometry_msgs::msg::TransformStamped t;
        t.header.stamp = this->get_clock()->now();
        t.header.frame_id = "map";
        t.child_frame_id = "base_link";

        t.transform.translation.x = 3.0 * std::cos(angle_);
        t.transform.translation.y = 3.0 * std::sin(angle_);
        t.transform.translation.z = 0.0;

        tf2::Quaternion q;
        q.setRPY(0, 0, angle_ + M_PI / 2.0);
        t.transform.rotation.x = q.x();
        t.transform.rotation.y = q.y();
        t.transform.rotation.z = q.z();
        t.transform.rotation.w = q.w();

        tf_broadcaster_->sendTransform(t);
        angle_ += 0.02;
    }

    std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
    rclcpp::TimerBase::SharedPtr timer_;
    double angle_;
};

2.5 TF2 监听器示例

cpp 复制代码 
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>

class TfListenerNode : public rclcpp::Node
{
public:
    TfListenerNode() : Node("tf_listener")
    {
        tf_buffer_ =
            std::make_unique<tf2_ros::Buffer>(this->get_clock());
        tf_listener_ =
            std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
        timer_ = this->create_wall_timer(
            std::chrono::seconds(1),
            std::bind(&TfListenerNode::timer_cb, this));
    }

private:
    void timer_cb()
    {
        try {
            auto t = tf_buffer_->lookupTransform(
                "map", "base_link", tf2::TimePointZero);
            RCLCPP_INFO(this->get_logger(),
                "base_link 在 map 中: (%.2f, %.2f)",
                t.transform.translation.x,
                t.transform.translation.y);
        } catch (const tf2::TransformException &ex) {
            RCLCPP_WARN(this->get_logger(), "TF 失败: %s", ex.what());
        }
    }

    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
    rclcpp::TimerBase::SharedPtr timer_;
};

2.6 动手练习

bash 复制代码 
sudo apt install ros-humble-tf2-ros ros-humble-tf2-tools \
    ros-humble-tf2-geometry-msgs

ros2 run tf2_tools view_frames
ros2 run tf2_ros tf2_echo map odom

阶段 3：Nav2 导航框架（2~3 周）

复制代码 
                     ┌──────────────┐
                     │ BT Navigator  │ ← 行为树控制导航流程
                     └──────┬───────┘
                ┌───────────┼───────────┐
                ▼           ▼           ▼
        ┌──────────┐ ┌──────────┐ ┌──────────┐
        │ Planner  │ │Controller│ │ Recovery │
        │ Server   │ │  Server  │ │  Server  │
        └──────────┘ └──────────┘ └──────────┘
         全局规划      局部控制      异常恢复
bash 复制代码 
sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup
sudo apt install ros-humble-turtlebot3*

echo "export TURTLEBOT3_MODEL=waffle" >> ~/.bashrc
source ~/.bashrc

3.3 关键话题

话题/服务 类型 说明
/plan nav_msgs/Path 全局路径
/local_plan nav_msgs/Path 局部路径
/cmd_vel geometry_msgs/Twist 速度指令
/odom nav_msgs/Odometry 里程计
/navigate_to_pose Action 导航到目标点
/follow_waypoints Action 沿路径点导航
cpp 复制代码 
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <tf2/LinearMath/Quaternion.h>

using NavigateToPose = nav2_msgs::action::NavigateToPose;
using GoalHandle = rclcpp_action::ClientGoalHandle<NavigateToPose>;

class Nav2Client : public rclcpp::Node
{
public:
    Nav2Client() : Node("nav2_client")
    {
        client_ = rclcpp_action::create_client<NavigateToPose>(
            this, "navigate_to_pose");
    }

    void send_goal(double x, double y, double yaw)
    {
        if (!client_->wait_for_action_server(std::chrono::seconds(5))) {
            RCLCPP_ERROR(this->get_logger(), "Action 服务不可用!");
            return;
        }

        auto goal = NavigateToPose::Goal();
        goal.pose.header.frame_id = "map";
        goal.pose.header.stamp = this->get_clock()->now();
        goal.pose.pose.position.x = x;
        goal.pose.pose.position.y = y;

        tf2::Quaternion q;
        q.setRPY(0, 0, yaw);
        goal.pose.pose.orientation.x = q.x();
        goal.pose.pose.orientation.y = q.y();
        goal.pose.pose.orientation.z = q.z();
        goal.pose.pose.orientation.w = q.w();

        auto options =
            rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
        options.feedback_callback =
            [this](GoalHandle::SharedPtr,
                   const std::shared_ptr<const NavigateToPose::Feedback> fb) {
                RCLCPP_INFO(this->get_logger(),
                    "剩余: %.2f m, 耗时: %d s",
                    fb->distance_remaining,
                    fb->navigation_time.sec);
            };
        options.result_callback =
            [this](const GoalHandle::WrappedResult &result) {
                if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
                    RCLCPP_INFO(this->get_logger(), "导航成功!");
                else
                    RCLCPP_WARN(this->get_logger(), "导航失败!");
            };

        client_->async_send_goal(goal, options);
    }

private:
    rclcpp_action::Client<NavigateToPose>::SharedPtr client_;
};

3.5 动手练习

bash 复制代码 
ros2 launch nav2_bringup tb3_simulation_launch.py
ros2 topic echo /navigate_to_pose/_action/feedback

阶段 4：轨迹生成与发布（2~3 周）⭐ 核心阶段

4.1 学习目标

  • 用 C++ 生成预定义轨迹（直线、圆弧、S 弯、路径点序列）
  • 发布轨迹为 nav_msgs/Path
  • 模拟机器人沿轨迹运动（发布 TF + Odometry）

4.2 轨迹生成节点（完整 C++）

cpp 复制代码 
// src/trajectory_publisher.cpp
#include <cmath>
#include <string>
#include <vector>

#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/path.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>

struct TrajPt { double x, y, yaw; };

class TrajectoryPublisher : public rclcpp::Node
{
public:
    TrajectoryPublisher() : Node("trajectory_publisher"), t_(0.0)
    {
        this->declare_parameter<std::string>("trajectory_type", "circle");
        this->declare_parameter<double>("speed", 0.5);
        this->declare_parameter<double>("radius", 5.0);

        path_pub_ = this->create_publisher<nav_msgs::msg::Path>(
            "/planned_path", 10);
        odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>(
            "/odom", 10);
        tf_bc_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

        traj_ = generate_trajectory();
        publish_full_path();

        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(50),
            std::bind(&TrajectoryPublisher::on_timer, this));

        RCLCPP_INFO(this->get_logger(),
            "轨迹节点启动! type=%s, pts=%zu",
            this->get_parameter("trajectory_type").as_string().c_str(),
            traj_.size());
    }

private:
    std::vector<TrajPt> generate_trajectory()
    {
        auto type = this->get_parameter("trajectory_type").as_string();
        std::vector<TrajPt> pts;

        if (type == "circle") {
            double r = this->get_parameter("radius").as_double();
            for (int i = 0; i < 360; ++i) {
                double a = i * M_PI / 180.0;
                pts.push_back({r*cos(a), r*sin(a), a + M_PI/2});
            }
        } else if (type == "line") {
            for (int i = 0; i < 200; ++i)
                pts.push_back({i * 0.1, 0.0, 0.0});
        } else if (type == "s_curve") {
            for (int i = 0; i < 400; ++i) {
                double s = i * 0.05;
                pts.push_back({
                    s, 3.0*sin(s*0.5),
                    atan2(1.5*cos(s*0.5), 1.0)});
            }
        } else if (type == "waypoints") {
            std::vector<std::pair<double,double>> wps = {
                {0,0},{5,0},{5,5},{10,5},{10,10},{0,10},{0,0}};
            for (size_t i = 0; i+1 < wps.size(); ++i) {
                double x1=wps[i].first, y1=wps[i].second;
                double x2=wps[i+1].first, y2=wps[i+1].second;
                int steps = (int)(std::hypot(x2-x1,y2-y1) / 0.1);
                double yaw = atan2(y2-y1, x2-x1);
                for (int s = 0; s < steps; ++s) {
                    double r = (double)s / steps;
                    pts.push_back({
                        x1+(x2-x1)*r, y1+(y2-y1)*r, yaw});
                }
            }
        }
        return pts;
    }

    void publish_full_path()
    {
        nav_msgs::msg::Path path;
        path.header.frame_id = "map";
        path.header.stamp = this->get_clock()->now();
        for (auto &p : traj_) {
            geometry_msgs::msg::PoseStamped ps;
            ps.header = path.header;
            ps.pose.position.x = p.x;
            ps.pose.position.y = p.y;
            tf2::Quaternion q; q.setRPY(0, 0, p.yaw);
            ps.pose.orientation.x = q.x();
            ps.pose.orientation.y = q.y();
            ps.pose.orientation.z = q.z();
            ps.pose.orientation.w = q.w();
            path.poses.push_back(ps);
        }
        path_pub_->publish(path);
    }

    void on_timer()
    {
        if (traj_.empty()) return;
        double spd = this->get_parameter("speed").as_double();
        size_t idx = (size_t)(t_ * spd * 20.0) % traj_.size();
        auto &p = traj_[idx];
        auto now = this->get_clock()->now();

        // TF: map → base_link
        geometry_msgs::msg::TransformStamped tf;
        tf.header.stamp = now;
        tf.header.frame_id = "map";
        tf.child_frame_id = "base_link";
        tf.transform.translation.x = p.x;
        tf.transform.translation.y = p.y;
        tf2::Quaternion q; q.setRPY(0, 0, p.yaw);
        tf.transform.rotation.x = q.x();
        tf.transform.rotation.y = q.y();
        tf.transform.rotation.z = q.z();
        tf.transform.rotation.w = q.w();
        tf_bc_->sendTransform(tf);

        // Odometry
        nav_msgs::msg::Odometry odom;
        odom.header.stamp = now;
        odom.header.frame_id = "map";
        odom.child_frame_id = "base_link";
        odom.pose.pose.position.x = p.x;
        odom.pose.pose.position.y = p.y;
        odom.pose.pose.orientation = tf.transform.rotation;
        odom_pub_->publish(odom);

        t_ += 0.05;
        if ((int)(t_*20) % 100 == 0) publish_full_path();
    }

    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_;
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
    std::unique_ptr<tf2_ros::TransformBroadcaster> tf_bc_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::vector<TrajPt> traj_;
    double t_;
};

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

4.3 从文件加载轨迹（C++）

cpp 复制代码 
#include <fstream>
#include <sstream>
#include <vector>
#include <stdexcept>
// sudo apt install nlohmann-json3-dev
#include <nlohmann/json.hpp>

struct WaypointData { double x, y, yaw; std::string action; };

// 从 JSON 加载
std::vector<WaypointData> load_json(const std::string &path)
{
    std::ifstream ifs(path);
    if (!ifs) throw std::runtime_error("无法打开: " + path);
    nlohmann::json j; ifs >> j;
    std::vector<WaypointData> pts;
    for (auto &wp : j["waypoints"])
        pts.push_back({wp["x"], wp["y"], wp["yaw"],
                        wp.value("action","")});
    return pts;
}

// 从 CSV 加载
std::vector<WaypointData> load_csv(const std::string &path)
{
    std::ifstream ifs(path);
    if (!ifs) throw std::runtime_error("无法打开: " + path);
    std::vector<WaypointData> pts;
    std::string line;
    std::getline(ifs, line);  // 跳过表头
    while (std::getline(ifs, line)) {
        std::stringstream ss(line); std::string tok;
        WaypointData p;
        std::getline(ss,tok,','); p.x   = std::stod(tok);
        std::getline(ss,tok,','); p.y   = std::stod(tok);
        std::getline(ss,tok,','); p.yaw = std::stod(tok);
        pts.push_back(p);
    }
    return pts;
}

JSON 轨迹文件示例：

json 复制代码 
{
  "waypoints": [
    {"x": 0.0,  "y": 0.0,  "yaw": 0.0,    "action": "start"},
    {"x": 5.0,  "y": 0.0,  "yaw": 0.0,    "action": "turn_left"},
    {"x": 5.0,  "y": 5.0,  "yaw": 1.5708, "action": "announce_arrival"},
    {"x": 10.0, "y": 5.0,  "yaw": 0.0,    "action": "destination"}
  ]
}

阶段 5：语音/导航播报集成（2~3 周）⭐ 核心阶段

5.1 播报系统架构

复制代码 
┌────────────┐     ┌──────────────┐     ┌──────────────┐
│ 轨迹跟踪    │ ──→ │ 导航事件检测  │ ──→ │  播报文本生成 │
│ (Odom订阅)  │     │ (转弯/到达等) │     │ (String发布)  │
└────────────┘     └──────────────┘     └──────────────┘
                                               │
                                               ▼
                                        ┌──────────────┐
                                        │  TTS 语音节点 │
                                        │ (espeak/flite)│
                                        └──────────────┘

5.2 导航事件检测与播报节点（完整 C++）

cpp 复制代码 
// src/navigation_announcer.cpp
#include <cmath>
#include <string>
#include <vector>
#include <utility>
#include <limits>
#include <sstream>
#include <iomanip>

#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <nav_msgs/msg/path.hpp>
#include <std_msgs/msg/string.hpp>

class NavigationAnnouncer : public rclcpp::Node
{
public:
    NavigationAnnouncer()
    : Node("navigation_announcer"), seg_(0), has_pose_(false)
    {
        this->declare_parameter<double>("waypoint_threshold", 1.0);
        this->declare_parameter<double>("off_track_threshold", 3.0);
        this->declare_parameter<double>("turn_angle_threshold", 30.0);

        odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "/odom", 10,
            std::bind(&NavigationAnnouncer::odom_cb, this,
                      std::placeholders::_1));
        path_sub_ = this->create_subscription<nav_msgs::msg::Path>(
            "/planned_path", 10,
            std::bind(&NavigationAnnouncer::path_cb, this,
                      std::placeholders::_1));
        ann_pub_ = this->create_publisher<std_msgs::msg::String>(
            "/navigation/announcement", 10);
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(500),
            std::bind(&NavigationAnnouncer::check, this));

        RCLCPP_INFO(this->get_logger(), "导航播报节点已启动!");
    }

private:
    void odom_cb(const nav_msgs::msg::Odometry::SharedPtr msg)
    {
        cx_ = msg->pose.pose.position.x;
        cy_ = msg->pose.pose.position.y;
        has_pose_ = true;
    }

    void path_cb(const nav_msgs::msg::Path::SharedPtr msg)
    {
        path_.clear();
        for (auto &p : msg->poses)
            path_.emplace_back(p.pose.position.x, p.pose.position.y);
        seg_ = 0;
        RCLCPP_INFO(this->get_logger(), "收到路径: %zu 点", path_.size());
    }

    void check()
    {
        if (!has_pose_ || path_.empty()) return;

        double wp_th = this->get_parameter("waypoint_threshold").as_double();
        double off_th = this->get_parameter("off_track_threshold").as_double();
        double turn_th = this->get_parameter("turn_angle_threshold").as_double();

        // 事件1: 到达路径点
        if (seg_ < path_.size()) {
            double d = std::hypot(cx_ - path_[seg_].first,
                                  cy_ - path_[seg_].second);
            if (d < wp_th) {
                seg_++;
                double pct = 100.0 * seg_ / path_.size();
                std::ostringstream o;
                o << "已到达第 " << seg_ << " 个路径点，进度 "
                  << std::fixed << std::setprecision(0) << pct << "%";
                announce(o.str());
            }
        }

        // 事件2: 转弯检测
        if (seg_ > 0 && seg_ + 1 < path_.size()) {
            double turn = detect_turn(seg_);
            if (std::abs(turn) > turn_th) {
                std::string dir = (turn > 0) ? "左转" : "右转";
                std::ostringstream o;
                o << "前方即将" << dir << "，角度 "
                  << std::fixed << std::setprecision(0)
                  << std::abs(turn) << " 度";
                announce(o.str());
            }
        }

        // 事件3: 到达终点
        if (!path_.empty()) {
            double de = std::hypot(cx_ - path_.back().first,
                                   cy_ - path_.back().second);
            if (de < wp_th) announce("已到达目的地！导航结束。");
        }

        // 事件4: 偏离路径
        double md = min_dist();
        if (md > off_th) {
            std::ostringstream o;
            o << "注意：偏离路径 " << std::fixed
              << std::setprecision(1) << md << " 米，重新规划中...";
            announce(o.str());
        }
    }

    double detect_turn(size_t i)
    {
        if (i < 1 || i + 1 >= path_.size()) return 0.0;
        double a1 = atan2(path_[i].second - path_[i-1].second,
                          path_[i].first  - path_[i-1].first);
        double a2 = atan2(path_[i+1].second - path_[i].second,
                          path_[i+1].first  - path_[i].first);
        double t = (a2 - a1) * 180.0 / M_PI;
        while (t >  180) t -= 360;
        while (t < -180) t += 360;
        return t;
    }

    double min_dist()
    {
        double best = std::numeric_limits<double>::max();
        for (auto &p : path_)
            best = std::min(best, std::hypot(cx_-p.first, cy_-p.second));
        return best;
    }

    void announce(const std::string &text)
    {
        if (text == last_) return;
        auto msg = std_msgs::msg::String();
        msg.data = text;
        ann_pub_->publish(msg);
        RCLCPP_INFO(this->get_logger(), "📢 %s", text.c_str());
        last_ = text;
    }

    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
    rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr ann_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::vector<std::pair<double,double>> path_;
    size_t seg_;
    double cx_ = 0, cy_ = 0;
    bool has_pose_;
    std::string last_;
};

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

5.3 TTS 语音合成方案（C++）

方案 A：espeak-ng（最简单，离线，支持中文）
bash 复制代码 
sudo apt install espeak-ng
cpp 复制代码 
#include <cstdlib>
#include <string>

class EspeakTTS {
public:
    void speak(const std::string &text) {
        // -v zh 中文语音, & 后台播放不阻塞
        std::string cmd = "espeak-ng -v zh \"" + text + "\" &";
        std::system(cmd.c_str());
    }
};
方案 B：Flite（C 原生库，适合嵌入式）
bash 复制代码 
sudo apt install flite libflite1
cpp 复制代码 
#include <flite/flite.h>

class FliteTTS {
public:
    FliteTTS() {
        flite_init();
        voice_ = flite_voice_select(nullptr);
    }
    void speak(const std::string &text) {
        flite_text_to_speech(text.c_str(), voice_, "play");
    }
private:
    cst_voice *voice_;
};
方案 C：独立 TTS 节点（推荐，解耦架构）
cpp 复制代码 
// src/tts_node.cpp - 订阅播报文本并朗读
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <cstdlib>

class TTSNode : public rclcpp::Node
{
public:
    TTSNode() : Node("tts_node")
    {
        this->declare_parameter<std::string>("language", "zh");
        sub_ = this->create_subscription<std_msgs::msg::String>(
            "/navigation/announcement", 10,
            std::bind(&TTSNode::on_msg, this, std::placeholders::_1));
        RCLCPP_INFO(this->get_logger(), "TTS 节点已启动!");
    }
private:
    void on_msg(const std_msgs::msg::String::SharedPtr msg) {
        auto lang = this->get_parameter("language").as_string();
        std::string cmd = "espeak-ng -v " + lang
                          + " \"" + msg->data + "\" &";
        std::system(cmd.c_str());
        RCLCPP_INFO(this->get_logger(), "🔊 %s", msg->data.c_str());
    }
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;
};

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

5.4 播报内容设计

导航事件 播报示例
导航开始 "开始导航，目的地距离 120 米，预计 2 分钟到达"
前方转弯 "前方 10 米处左转"
到达路径点 "已到达第 3 个路径点，进度 60%"
进入减速区 "前方减速区域，正在降速"
遇到障碍 "前方检测到障碍物，正在绕行"
偏离路径 "已偏离路径，重新规划路线"
到达终点 "已到达目的地，导航结束"

阶段 6：RViz2 可视化与 Gazebo 仿真（1~2 周）

6.1 RViz2 显示项

  • Path ：/planned_path --- 规划路径
  • TF：坐标系变换关系
  • Odometry：里程计箭头
  • Marker/MarkerArray：播报点、转弯标记
  • RobotModel：URDF 模型

6.2 自定义 Marker（C++）

cpp 复制代码 
#include <visualization_msgs/msg/marker.hpp>

visualization_msgs::msg::Marker make_text_marker(
    double x, double y, const std::string &text, int id,
    rclcpp::Clock::SharedPtr clock)
{
    visualization_msgs::msg::Marker m;
    m.header.frame_id = "map";
    m.header.stamp = clock->now();
    m.ns = "announcements";
    m.id = id;
    m.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
    m.action = visualization_msgs::msg::Marker::ADD;
    m.pose.position.x = x;
    m.pose.position.y = y;
    m.pose.position.z = 2.0;
    m.pose.orientation.w = 1.0;
    m.scale.z = 0.5;
    m.color.r = 1.0f; m.color.g = 1.0f;
    m.color.b = 0.0f; m.color.a = 1.0f;
    m.text = text;
    return m;
}

6.3 Gazebo 仿真

bash 复制代码 
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
ros2 launch nav2_bringup navigation_launch.py
ros2 launch nav2_bringup rviz_launch.py

阶段 7：综合实战项目（2~3 周）

7.1 项目结构（C++ / ament_cmake）

复制代码 
/home/ubuntu/ros/src/
├── nav_trajectory_sim/                  # 主功能包
│   ├── include/nav_trajectory_sim/      # 头文件
│   │   ├── trajectory_publisher.hpp
│   │   ├── navigation_announcer.hpp
│   │   └── tts_engine.hpp
│   ├── src/                             # 源文件
│   │   ├── trajectory_publisher.cpp
│   │   ├── navigation_announcer.cpp
│   │   └── tts_node.cpp
│   ├── config/
│   │   ├── trajectory_params.yaml
│   │   └── announcer_params.yaml
│   ├── launch/
│   │   └── full_demo_launch.py
│   ├── trajectories/
│   │   ├── city_route.json
│   │   └── campus_route.csv
│   ├── CMakeLists.txt
│   └── package.xml
│
├── nav_announcer_msgs/                  # 自定义消息包
│   ├── msg/
│   │   └── NavigationEvent.msg
│   ├── CMakeLists.txt
│   └── package.xml
│
└── nav_description/                     # 机器人 URDF（可选）
    └── urdf/simple_robot.urdf.xacro

7.2 CMakeLists.txt（完整版）

cmake 复制代码 
cmake_minimum_required(VERSION 3.8)
project(nav_trajectory_sim)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# 依赖
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)

include_directories(include)

# 1) 轨迹发布
add_executable(trajectory_publisher src/trajectory_publisher.cpp)
ament_target_dependencies(trajectory_publisher
  rclcpp nav_msgs geometry_msgs tf2 tf2_ros)

# 2) 导航播报
add_executable(navigation_announcer src/navigation_announcer.cpp)
ament_target_dependencies(navigation_announcer
  rclcpp nav_msgs std_msgs)

# 3) TTS
add_executable(tts_node src/tts_node.cpp)
ament_target_dependencies(tts_node rclcpp std_msgs)

# 安装
install(TARGETS trajectory_publisher navigation_announcer tts_node
  DESTINATION lib/${PROJECT_NAME})

install(DIRECTORY launch config trajectories
  DESTINATION share/${PROJECT_NAME})

ament_package()

7.3 package.xml

xml 复制代码 
<?xml version="1.0"?>
<package format="3">
  <name>nav_trajectory_sim</name>
  <version>0.1.0</version>
  <description>ROS2 导航播报与轨迹模拟 (C++)</description>
  <maintainer email="you@email.com">your_name</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>std_msgs</depend>
  <depend>nav_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>visualization_msgs</depend>
  <depend>tf2</depend>
  <depend>tf2_ros</depend>

  <exec_depend>ros2launch</exec_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

7.4 自定义消息

复制代码 
# msg/NavigationEvent.msg
uint8 EVENT_START=0
uint8 EVENT_TURN_LEFT=1
uint8 EVENT_TURN_RIGHT=2
uint8 EVENT_WAYPOINT_REACHED=3
uint8 EVENT_OBSTACLE=4
uint8 EVENT_OFF_TRACK=5
uint8 EVENT_ARRIVED=6

uint8 event_type
string description
float64 distance_remaining
float64 progress_percent
geometry_msgs/PoseStamped current_pose

7.5 Launch 启动文件

python 复制代码 
# launch/full_demo_launch.py（Launch 文件仍用 Python 编写）
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    pkg = get_package_share_directory('nav_trajectory_sim')
    return LaunchDescription([
        DeclareLaunchArgument('trajectory_type', default_value='waypoints'),
        DeclareLaunchArgument('speed', default_value='0.5'),

        Node(package='nav_trajectory_sim',
             executable='trajectory_publisher',
             parameters=[{
                 'trajectory_type': LaunchConfiguration('trajectory_type'),
                 'speed': LaunchConfiguration('speed'),
                 'radius': 5.0,
             }], output='screen'),

        Node(package='nav_trajectory_sim',
             executable='navigation_announcer',
             parameters=[os.path.join(pkg, 'config', 'announcer_params.yaml')],
             output='screen'),

        Node(package='nav_trajectory_sim',
             executable='tts_node',
             parameters=[{'language': 'zh'}],
             output='screen'),

        Node(package='rviz2', executable='rviz2'),
    ])

7.6 参数配置

yaml 复制代码 
# config/announcer_params.yaml
navigation_announcer:
  ros__parameters:
    waypoint_threshold: 1.0
    off_track_threshold: 3.0
    turn_angle_threshold: 30.0
yaml 复制代码 
# config/trajectory_params.yaml
trajectory_publisher:
  ros__parameters:
    trajectory_type: "waypoints"
    speed: 0.5
    radius: 5.0

7.7 编译运行

bash 复制代码 
cd /home/ubuntu/ros
colcon build --packages-select nav_trajectory_sim
source install/setup.bash

# 一键启动
ros2 launch nav_trajectory_sim full_demo_launch.py

# 或手动分别启动
ros2 run nav_trajectory_sim trajectory_publisher \
    --ros-args -p trajectory_type:=circle -p speed:=0.3
ros2 run nav_trajectory_sim navigation_announcer
ros2 run nav_trajectory_sim tts_node
rviz2

阶段 8：进阶优化（持续学习）

8.1 QoS 策略优化

cpp 复制代码 
#include <rclcpp/qos.hpp>

// 高频位姿：允许丢包换低延迟
auto best_effort = rclcpp::QoS(rclcpp::KeepLast(1))
    .reliability(rclcpp::ReliabilityPolicy::BestEffort);
odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
    "/odom", best_effort, ...);

// 重要播报：保证可靠传输
auto reliable = rclcpp::QoS(rclcpp::KeepLast(10))
    .reliability(rclcpp::ReliabilityPolicy::Reliable);
ann_pub_ = create_publisher<std_msgs::msg::String>(
    "/navigation/announcement", reliable);

8.2 多线程执行器

cpp 复制代码 
int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto traj = std::make_shared<TrajectoryPublisher>();
    auto ann  = std::make_shared<NavigationAnnouncer>();

    rclcpp::executors::MultiThreadedExecutor exec(
        rclcpp::ExecutorOptions(), 2);
    exec.add_node(traj);
    exec.add_node(ann);
    exec.spin();
    rclcpp::shutdown();
    return 0;
}

8.3 组件化节点（零拷贝通信）

cpp 复制代码 
#include <rclcpp_components/register_node_macro.hpp>

// .cpp 末尾注册组件
RCLCPP_COMPONENTS_REGISTER_NODE(TrajectoryPublisher)
RCLCPP_COMPONENTS_REGISTER_NODE(NavigationAnnouncer)

// 优势:
// 1. 同进程内零拷贝通信
// 2. 减少进程切换开销
// 3. component_container 动态加载/卸载

8.4 进阶方向

方向 技术栈 说明
传感器融合 GPS + IMU + 轮式里程计 robot_localization（C++）
SLAM Cartographer / SLAM Toolbox 同步定位与建图
动态避障 Nav2 DWB Controller 实时躲避移动障碍
多机协作 ROS2 Namespace / DDS 多车编队
Nav2 插件 C++ 插件体系 自定义 Planner / Controller
V2X DDS / MQTT 车路协同
高精地图 Lanelet2 / OpenDrive 车道级导航

8.5 推荐开源项目

🗓️ 时间线总结

阶段 内容 时长 难度
1 ROS2 基础 + C++ 节点 1~2 周 ⭐
2 TF2 坐标变换 1~2 周 ⭐⭐
3 Nav2 导航框架 2~3 周 ⭐⭐⭐
4 轨迹生成与发布 2~3 周 ⭐⭐⭐
5 语音播报集成 2~3 周 ⭐⭐⭐
6 RViz2 / Gazebo 1~2 周 ⭐⭐
7 综合实战 2~3 周 ⭐⭐⭐⭐
8 进阶（组件化/多线程/插件） 持续 ⭐⭐⭐⭐⭐

总计约 12~18 周（有 C++ 基础可压缩到 8~12 周）

💡 快速开始（5 步见效果）

bash 复制代码 
# ① 环境准备
source /opt/ros/humble/setup.bash
cd /home/ubuntu/ros
colcon build && source install/setup.bash

# ② 创建 C++ 包
cd src
ros2 pkg create --build-type ament_cmake nav_trajectory_sim \
    --dependencies rclcpp std_msgs nav_msgs geometry_msgs \
    tf2 tf2_ros visualization_msgs

# ③ 复制代码
#    阶段 4.2 → src/trajectory_publisher.cpp
#    阶段 5.2 → src/navigation_announcer.cpp
#    阶段 5.3C → src/tts_node.cpp
#    阶段 7.2 → CMakeLists.txt

# ④ 编译
cd /home/ubuntu/ros
colcon build --packages-select nav_trajectory_sim
source install/setup.bash

# ⑤ 运行！
ros2 run nav_trajectory_sim trajectory_publisher &
ros2 run nav_trajectory_sim navigation_announcer &
ros2 run nav_trajectory_sim tts_node &
rviz2
# 在 RViz 中 Add → Path → topic: /planned_path → 看到轨迹！
# 终端会输出播报文字，扬声器会朗读中文导航提示！

最后更新：2026-03-24 | 语言：C++ | 构建系统：ament_cmake

相关推荐
FoldWinCard2 小时前
D5 Linux 网络及端口命令
linux·运维·服务器
零涂毕业设计4 小时前
毕业设计模块开发-OLED显示屏(IIC协议0.96寸）STM32 ESP32 FPGA Linux驱动免费分享
linux·stm32·单片机·嵌入式·esp32·fpga·oled
tedcloud1234 小时前
OmniRoute怎么部署？开源AI模型路由平台Linux部署教程
linux·服务器·人工智能·开源·音视频
霸道流氓气质4 小时前
KMS 密钥管理服务（Key Management Service）原理与实践
linux·服务器·数据库
SakitamaX4 小时前
LVS（Linux Virtual Server）概念详解
linux·运维·lvs
小张同学a.4 小时前
Linux系统管理
linux·运维
阿龙的工作记录5 小时前
深入理解Linux设备驱动模型
linux·嵌入式硬件·工作记录
代码村新手5 小时前
Linux的基本指令
linux·运维·服务器
hkj88086 小时前
Ubuntu 切换java版本
java·linux·ubuntu
撩得Android一次心动7 小时前
Linux编程笔记3【个人用】
linux·笔记
热门推荐
01GitHub 镜像站点02如何新建文件夹？ 电脑新建文件夹的4种方法032026年7月AI圈大地震：GPT-5.6被政府限制、Claude入驻Slack、Anthropic自研芯片04国内可直接用、免费额度/永久免费的大模型API清单（含 SiliconFlow、火山、阿里、智谱、百度、Kimi、DeepSeek、DMXAPI 等）05AI科技热点日报 | 2026年07月01日06幻兽帕鲁 - 服务器管理员权限与 GM 命令完全指南07AI 编程 IDE 全景解析 2026：Agent 全面接管开发链路08微信历史版本含下载地址（ Windows PC | 安卓 | MAC ）及设置微信不更新092026 国产 AI 大模型横评：DeepSeek、通义千问、Kimi、文心一言、星火、豆包谁更能打？10【解构】DeepSeek V4 发布：技术报告深度解读 + 横向对比六大开源模型，我们的判断是……