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

相关推荐
扛枪的书生2 小时前
Jenkins 学习总结
linux
strive programming2 小时前
Linux从入门到精通——基础IO(简洁清晰版)
linux·运维·服务器
节点小宝2 小时前
一站式部署：支持Windows、macOS、Linux三端的统一方案
linux·运维·macos
乌鸦9442 小时前
《库制作与原理》
linux·动态库
ZhengEnCi3 小时前
L1D-Linux系统Node.js部署Claude Code完全指南 🚀
linux·ai编程·claude
hnxaoli3 小时前
统信小程序（十一）快捷地址栏
linux·python·小程序
黄昏晓x3 小时前
Linux----网络
linux·网络·arm开发
小比特＿蓝光3 小时前
Linux开发工具
linux·运维·服务器
大熊背3 小时前
ISP离线模式应用（二）-如何利用 ISP 离线模式 加速 3DNR 收敛
linux·算法·rtos·isp pipeline·3dnr
热门推荐
01GitHub 镜像站点02Qwen3.5 开源全解析：从 0.8B 到 397B，代际升级 + 全场景选型指南03围棋-html版本04小黑课堂计算机二级WPSoffice题库软件下载安装教程（2026年3月最新版）05OpenClaw 使用和管理 MCP 完全指南06班级宠物园部署指南07UV安装并设置国内源08“wsl --install -d Ubuntu-22.04”下载慢，中国地区离线安装 Ubuntu 22.04 WSL方法（亲测2025年5月6日）09让 Trae IDE 智能体 “读懂”文档 Excel+PDF+DOCX ：mcp-documents-reader 工具使用指南10OpenClaw Control UI安全上下文访问配置