1. 项目概述
在机器人导航领域,实时语音播报和轨迹可视化是提升人机交互体验的关键功能。这个项目基于ROS2框架,使用C++17开发了一套完整的导航播报与轨迹模拟系统。系统能够根据预设路径或实时规划的轨迹,生成自然语言导航指令,并通过语音合成技术进行播报,同时可视化展示机器人的运动轨迹。
项目采用模块化设计,主要包含以下几个核心组件:
- 轨迹生成与发布模块
- 导航事件检测模块
- 语音播报生成模块
- TTS语音合成模块
- 可视化与仿真模块
2. 环境准备与基础配置
2.1 系统环境要求
项目运行需要以下基础环境:
- Ubuntu 22.04 LTS(推荐)或20.04 LTS
- ROS2 Humble/Iron/Jazzy版本
- C++17编译器
- ament_cmake构建系统
2.2 ROS2安装与配置
对于Ubuntu 22.04用户,建议安装ROS2 Humble版本:
bash复制# 设置locale
sudo apt update && sudo apt install locales
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8
# 添加ROS2仓库
sudo apt install software-properties-common
sudo add-apt-repository universe
sudo apt update && sudo apt install curl -y
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
# 安装ROS2基础包
sudo apt update
sudo apt install ros-humble-desktop
# 环境配置
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
source ~/.bashrc
2.3 创建工作空间
创建一个新的ROS2工作空间用于本项目开发:
bash复制mkdir -p ~/nav_ws/src
cd ~/nav_ws
colcon build
source install/setup.bash
3. 核心模块实现
3.1 轨迹生成与发布模块
轨迹生成是系统的基础功能,负责创建和发布机器人的运动路径。我们实现了多种轨迹类型:
cpp复制// src/trajectory_publisher.cpp
#include <cmath>
#include <string>
#include <vector>
#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/path.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h>
class TrajectoryPublisher : public rclcpp::Node {
public:
TrajectoryPublisher() : Node("trajectory_publisher") {
// 参数声明
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);
// 生成轨迹
generate_trajectory();
// 定时器
timer_ = this->create_wall_timer(
std::chrono::milliseconds(50),
std::bind(&TrajectoryPublisher::publish_path, this));
}
private:
void generate_trajectory() {
auto type = this->get_parameter("trajectory_type").as_string();
double radius = this->get_parameter("radius").as_double();
if (type == "circle") {
for (int i = 0; i < 360; ++i) {
double angle = i * M_PI / 180.0;
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = radius * cos(angle);
pose.pose.position.y = radius * sin(angle);
tf2::Quaternion q;
q.setRPY(0, 0, angle + M_PI/2);
pose.pose.orientation.x = q.x();
pose.pose.orientation.y = q.y();
pose.pose.orientation.z = q.z();
pose.pose.orientation.w = q.w();
path_.poses.push_back(pose);
}
}
// 其他轨迹类型实现...
}
void publish_path() {
path_.header.stamp = this->get_clock()->now();
path_.header.frame_id = "map";
path_pub_->publish(path_);
}
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_;
rclcpp::TimerBase::SharedPtr timer_;
nav_msgs::msg::Path path_;
};
3.2 导航事件检测模块
导航事件检测模块负责分析机器人运动状态,识别关键事件如转弯、到达路径点等:
cpp复制// src/navigation_announcer.cpp
#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") {
// 参数配置
this->declare_parameter<double>("waypoint_threshold", 1.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_callback, this, std::placeholders::_1));
path_sub_ = this->create_subscription<nav_msgs::msg::Path>(
"/planned_path", 10, std::bind(&NavigationAnnouncer::path_callback, this, std::placeholders::_1));
// 发布器
announcement_pub_ = this->create_publisher<std_msgs::msg::String>("/navigation/announcement", 10);
}
private:
void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
current_pose_ = msg->pose.pose;
check_navigation_events();
}
void path_callback(const nav_msgs::msg::Path::SharedPtr msg) {
path_ = *msg;
current_segment_ = 0;
}
void check_navigation_events() {
if (path_.poses.empty()) return;
// 检查是否到达路径点
double distance = calculate_distance(current_pose_, path_.poses[current_segment_].pose);
if (distance < this->get_parameter("waypoint_threshold").as_double()) {
current_segment_++;
announce_arrival();
}
// 检查转弯事件
if (current_segment_ > 0 && current_segment_ < path_.poses.size() - 1) {
double turn_angle = calculate_turn_angle(
path_.poses[current_segment_-1].pose,
path_.poses[current_segment_].pose,
path_.poses[current_segment_+1].pose);
if (abs(turn_angle) > this->get_parameter("turn_angle_threshold").as_double()) {
announce_turn(turn_angle);
}
}
}
void announce_arrival() {
auto msg = std_msgs::msg::String();
msg.data = "已到达第 " + std::to_string(current_segment_) + " 个路径点";
announcement_pub_->publish(msg);
}
void announce_turn(double angle) {
auto msg = std_msgs::msg::String();
msg.data = "前方即将" + (angle > 0 ? "左转" : "右转") +
",角度 " + std::to_string(abs(angle)) + " 度";
announcement_pub_->publish(msg);
}
// 其他辅助函数...
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 announcement_pub_;
nav_msgs::msg::Path path_;
geometry_msgs::msg::Pose current_pose_;
size_t current_segment_ = 0;
};
3.3 语音合成模块
语音合成模块将文本导航指令转换为语音输出,支持多种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>("engine", "espeak");
this->declare_parameter<std::string>("voice", "zh");
sub_ = this->create_subscription<std_msgs::msg::String>(
"/navigation/announcement", 10,
std::bind(&TTSNode::announcement_callback, this, std::placeholders::_1));
}
private:
void announcement_callback(const std_msgs::msg::String::SharedPtr msg) {
std::string engine = this->get_parameter("engine").as_string();
std::string voice = this->get_parameter("voice").as_string();
if (engine == "espeak") {
std::string cmd = "espeak-ng -v " + voice + " \"" + msg->data + "\" &";
std::system(cmd.c_str());
} else if (engine == "flite") {
std::string cmd = "flite -t \"" + 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_;
};
4. 系统集成与测试
4.1 启动文件配置
创建一个launch文件来同时启动所有模块:
python复制# launch/full_demo.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='nav_trajectory_sim',
executable='trajectory_publisher',
name='trajectory_publisher',
parameters=[{
'trajectory_type': 'circle',
'radius': 5.0,
'speed': 0.5
}]
),
Node(
package='nav_trajectory_sim',
executable='navigation_announcer',
name='navigation_announcer',
parameters=[{
'waypoint_threshold': 1.0,
'turn_angle_threshold': 30.0
}]
),
Node(
package='nav_trajectory_sim',
executable='tts_node',
name='tts_node',
parameters=[{
'engine': 'espeak',
'voice': 'zh'
}]
)
])
4.2 构建与运行
构建项目并运行演示:
bash复制cd ~/nav_ws
colcon build --packages-select nav_trajectory_sim
source install/setup.bash
ros2 launch nav_trajectory_sim full_demo.launch.py
4.3 RViz2可视化
在RViz2中配置以下显示项:
- 添加
Path显示,订阅/planned_path话题 - 添加
TF显示,查看坐标系关系 - 添加
RobotModel显示(如有URDF模型) - 添加
Marker显示,用于可视化导航事件
5. 进阶功能与优化
5.1 动态轨迹加载
支持从JSON或CSV文件加载预定义的轨迹:
cpp复制std::vector<Waypoint> load_trajectory_from_file(const std::string& filename) {
std::vector<Waypoint> waypoints;
if (filename.ends_with(".json")) {
// JSON格式解析
std::ifstream file(filename);
nlohmann::json data;
file >> data;
for (auto& wp : data["waypoints"]) {
Waypoint point;
point.x = wp["x"];
point.y = wp["y"];
point.yaw = wp.value("yaw", 0.0);
point.action = wp.value("action", "");
waypoints.push_back(point);
}
} else if (filename.ends_with(".csv")) {
// CSV格式解析
std::ifstream file(filename);
std::string line;
while (std::getline(file, line)) {
std::stringstream ss(line);
std::string token;
Waypoint point;
std::getline(ss, token, ',');
point.x = std::stod(token);
std::getline(ss, token, ',');
point.y = std::stod(token);
std::getline(ss, token, ',');
point.yaw = std::stod(token);
waypoints.push_back(point);
}
}
return waypoints;
}
5.2 多语言支持
扩展语音播报模块支持多语言:
cpp复制std::string translate_to_language(const std::string& text, const std::string& lang) {
static const std::map<std::string, std::map<std::string, std::string>> translations = {
{"en", {
{"已到达路径点", "Reached waypoint"},
{"前方左转", "Turn left ahead"},
// 更多翻译...
}},
{"es", {
{"已到达路径点", "Punto de ruta alcanzado"},
{"前方左转", "Gire a la izquierda adelante"},
// 更多翻译...
}}
};
if (translations.count(lang) && translations.at(lang).count(text)) {
return translations.at(lang).at(text);
}
return text;
}
5.3 性能优化技巧
- 消息共享:使用
std::shared_ptr避免消息拷贝 - 定时器优化:根据实际需求调整发布频率
- 资源管理:合理使用
tf2缓存减少计算开销 - 异步处理:将TTS语音合成放在独立线程执行
cpp复制// 使用shared_ptr优化消息传递
void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
// 直接使用共享指针,避免拷贝
current_odom_ = msg;
process_odometry();
}
// 异步TTS处理
void speak_async(const std::string& text) {
std::thread([this, text]() {
std::string cmd = "espeak-ng -v zh \"" + text + "\"";
std::system(cmd.c_str());
}).detach();
}
6. 常见问题与解决方案
6.1 TF坐标问题
问题:TF树不完整导致坐标转换失败
解决方案:
- 检查所有必要的坐标系是否都正确发布
- 使用
tf2_tools检查TF树结构:bash复制
ros2 run tf2_tools view_frames - 确保时间戳同步,必要时使用
tf2::TimePointZero获取最新变换
6.2 语音合成延迟
问题:语音播报有明显延迟
优化方案:
- 使用更轻量的TTS引擎如
flite - 预加载常用语音片段
- 降低语音质量换取速度(如espeak的
-s参数调整语速)
6.3 路径跟踪不准确
问题:机器人偏离预定路径
调试方法:
- 检查里程计数据是否准确
- 调整控制器参数(PID增益)
- 增加路径跟踪的容错阈值
- 在RViz中可视化实际轨迹与规划路径的偏差
6.4 系统资源占用高
优化建议:
- 降低各节点的发布频率
- 使用
rqt_top监控资源占用 - 对计算密集型操作(如路径插值)进行性能分析
- 考虑使用
rclcpp::QoS调整消息队列深度
7. 项目扩展方向
7.1 实际机器人部署
将系统部署到真实机器人需要:
- 适配实际机器人的驱动接口
- 处理真实的传感器噪声
- 增加安全监控机制
- 优化语音播报的室外可听性
7.2 与SLAM系统集成
结合SLAM实现自主导航:
- 订阅SLAM系统生成的地图
- 动态更新导航路径
- 处理实时障碍物避障
- 增加重定位状态播报
7.3 云端语音服务集成
替换本地TTS为云端服务:
- 使用如Google Cloud TTS或Azure Speech服务
- 实现网络请求的异步处理
- 增加离线回退机制
- 支持更自然的语音合成
7.4 多机器人协同
扩展为多机器人系统:
- 增加机器人ID识别
- 协调多机路径规划
- 区分不同机器人的语音播报
- 实现机器人间的状态同步