文件内容：

使用C++读取rout.txt文件，文件内容为
22, start, -52.14, -43.38, 0.00, 90.4
22, waypoint, -52.15, -42.88, 0.00, 90.4
22, waypoint, -52.15, -42.38, 0.00, 90.4
22, waypoint, -52.15, -41.88, 0.00, 90.4
22, waypoint, -52.16, -41.38, 0.00, 90.4
22, waypoint, -52.16, -40.88, 0.00, 90.4
199, waypoint, -52.19, 42.33, 0.00, 89.8
199, end, -52.19, 42.57, 0.00, 89.8
22, start, -52.14, -43.39, 0.00, 90.4
22, waypoint, -52.15, -42.88, 0.00, 90.4
22, waypoint, -52.15, -42.38, 0.00, 90.4
22, waypoint, -52.15, -41.88, 0.00, 90.4
22, waypoint, -52.16, -41.38, 0.00, 90.4
188, waypoint, -52.16, -40.88, 0.00, 90.4
199, waypoint, -52.19, 42.33, 0.00, 89.8
199, waypoint, -52.19, 42.33, 0.00, 89.8
199, end, -52.19, 42.57, 0.00, 89.8

---

1. 数据结构定义

#include <iostream>
#include <fstream>
#include <vector>
#include <map>
#include <string>
#include <sstream>
#include <cmath> // for fabs

// 定义路径点结构体
struct Waypoint {
int route_id;
std::string point_type; // "start"/"waypoint"/"end"
double x, y, z;
double yaw;
};

// 存储每条路由的所有点
using Route = std::vector<Waypoint>;
// 按route_id组织路由
using RouteMap = std::map<int, Route>;

---

2. 文件读取与解析

RouteMap parse_routes_file(const std::string& filename) {
RouteMap routes;
std::ifstream file(filename);
std::string line;

while (std::getline(file, line)) {
// 跳过注释和空行
if (line.empty() || line[0] == '#') continue;

std::istringstream iss(line);
std::string token;
Waypoint wp;

// 解析每行数据
std::getline(iss, token, ',');
wp.route_id = std::stoi(token);

std::getline(iss, wp.point_type, ',');
wp.point_type.erase(0, wp.point_type.find_first_not_of(' '));

std::getline(iss, token, ',');
wp.x = std::stod(token);
std::getline(iss, token, ',');
wp.y = std::stod(token);
std::getline(iss, token, ',');
wp.z = std::stod(token);
std::getline(iss, token);
wp.yaw = std::stod(token);

// 添加到对应路由
routes[wp.route_id].push_back(wp);
}

return routes;
}

---

3. 路由完整性校验

void validate_routes(RouteMap& routes) {
for (auto it = routes.begin(); it != routes.end(); ) {
auto& points = it->second;
bool has_start = false, has_end = false;

for (const auto& wp : points) {
if (wp.point_type == "start") has_start = true;
if (wp.point_type == "end") has_end = true;
}

// 删除不完整路由
if (!has_start || !has_end) {
std::cerr << "警告：路由 " << it->first << " 缺少起点或终点，已忽略\n";
it = routes.erase(it);
} else {
++it;
}
}
}

---

4. 主程序与输出

int main() {
const std::string filename = "rout.txt";
RouteMap routes = parse_routes_file(filename);
validate_routes(routes);

// 打印所有路由信息
for (const auto& [route_id, waypoints] : routes) {
std::cout << "=== 路由 " << route_id << " ===" << std::endl;
for (const auto& wp : waypoints) {
printf("[%s] 坐标: (%.2f, %.2f, %.2f), 朝向: %.1f°\n",
wp.point_type.c_str(), wp.x, wp.y, wp.z, wp.yaw);
}
std::cout << std::endl;
}

return 0;
}

---

5. 编译与运行

编译命令（Linux/macOS）

g++ -std=c++11 route_parser.cpp -o route_parser

运行示例

./route_parser

---

6. 输出示例

=== 路由 22 ===
[start] 坐标: (-52.14, -43.38, 0.00), 朝向: 90.4°
[waypoint] 坐标: (-52.15, -42.88, 0.00), 朝向: 90.4°
...
[end] 坐标: (-52.19, 42.57, 0.00), 朝向: 89.8°

=== 路由 199 ===
[waypoint] 坐标: (-52.19, 42.33, 0.00), 朝向: 89.8°
...
[end] 坐标: (-52.19, 42.57, 0.00), 朝向: 89.8°

---

7. 高级功能扩展

计算路由长度

double calculate_route_length(const Route& route) {
double length = 0.0;
for (size_t i = 1; i < route.size(); ++i) {
double dx = route[i].x - route[i-1].x;
double dy = route[i].y - route[i-1].y;
length += std::hypot(dx, dy);
}
return length;
}

查找最近路由点

const Waypoint* find_closest_waypoint(const RouteMap& routes, double x, double y) {
const Waypoint* closest = nullptr;
double min_dist = std::numeric_limits<double>::max();

for (const auto& [_, route] : routes) {
for (const auto& wp : route) {
double dist = std::hypot(wp.x - x, wp.y - y);
if (dist < min_dist) {
min_dist = dist;
closest = &wp;
}
}
}
return closest;
}

---

关键点说明

1. 数据完整性处理：

• 自动忽略不完整的路由（缺少起点或终点）
• 保留原始坐标精度（不修改Y轴方向）

2. 性能优化：

• 使用std::map按route_id自动排序路由
• 流式读取处理大文件时内存友好

3. 扩展性：

• 可轻松添加新的字段（如车道ID、道路类型等）

此实现可直接集成到ROS或自动驾驶系统中，用于路径规划模块的初始化。如需处理更复杂的CARLA地图数据，可扩展为读取OpenDRIVE格式。