191 lines
6.4 KiB
Plaintext
191 lines
6.4 KiB
Plaintext
#include "path_curve.h"
|
||
#include <fstream>
|
||
#include <sstream>
|
||
#include <iostream>
|
||
|
||
// CSV加载功能实现
|
||
bool PathCurve::loadFromCSV(const std::string& filename, bool has_header) {
|
||
std::ifstream file(filename);
|
||
if (!file.is_open()) {
|
||
std::cerr << "Error: Cannot open file " << filename << std::endl;
|
||
return false;
|
||
}
|
||
|
||
std::vector<PathPoint> points;
|
||
std::string line;
|
||
int line_num = 0;
|
||
|
||
// 跳过表头
|
||
if (has_header && std::getline(file, line)) {
|
||
line_num++;
|
||
}
|
||
|
||
while (std::getline(file, line)) {
|
||
line_num++;
|
||
// 跳过空行和注释行
|
||
if (line.empty() || line[0] == '#') {
|
||
continue;
|
||
}
|
||
|
||
std::stringstream ss(line);
|
||
std::string token;
|
||
std::vector<double> values;
|
||
bool parse_error = false;
|
||
|
||
// 解析CSV行
|
||
while (std::getline(ss, token, ',')) {
|
||
try {
|
||
// 去除前后空格
|
||
size_t start = token.find_first_not_of(" \t\r\n");
|
||
size_t end = token.find_last_not_of(" \t\r\n");
|
||
if (start == std::string::npos) {
|
||
// 空token，跳过整行
|
||
parse_error = true;
|
||
break;
|
||
}
|
||
std::string trimmed = token.substr(start, end - start + 1);
|
||
values.push_back(std::stod(trimmed));
|
||
} catch (const std::exception&) {
|
||
std::cerr << "Error parsing line " << line_num << ": " << line << std::endl;
|
||
parse_error = true;
|
||
break;
|
||
}
|
||
}
|
||
|
||
// 如果解析出错或值数量不足，跳过整行
|
||
if (parse_error) {
|
||
continue;
|
||
}
|
||
|
||
// 根据列数创建路径点
|
||
if (values.size() >= 2) {
|
||
PathPoint p;
|
||
p.x = values[0];
|
||
p.y = values[1];
|
||
p.theta = (values.size() >= 3) ? values[2] : 0.0;
|
||
p.kappa = (values.size() >= 4) ? values[3] : 0.0;
|
||
points.push_back(p);
|
||
}
|
||
}
|
||
|
||
file.close();
|
||
|
||
if (points.empty()) {
|
||
std::cerr << "Error: No valid path points loaded from " << filename << std::endl;
|
||
return false;
|
||
}
|
||
|
||
// 设置路径点（会自动计算theta和kappa）
|
||
setPathPoints(points);
|
||
|
||
std::cout << "Successfully loaded " << points.size() << " points from " << filename << std::endl;
|
||
return true;
|
||
}
|
||
|
||
// CSV保存功能实现
|
||
bool PathCurve::saveToCSV(const std::string& filename) const {
|
||
std::ofstream file(filename);
|
||
if (!file.is_open()) {
|
||
std::cerr << "Error: Cannot create file " << filename << std::endl;
|
||
return false;
|
||
}
|
||
|
||
// 写入表头
|
||
file << "# Custom Path Data\n";
|
||
file << "# x(m), y(m), theta(rad), kappa(1/m)\n";
|
||
|
||
// 写入路径点
|
||
for (const auto& p : path_points_) {
|
||
file << p.x << ", " << p.y << ", " << p.theta << ", " << p.kappa << "\n";
|
||
}
|
||
|
||
file.close();
|
||
std::cout << "Successfully saved " << path_points_.size() << " points to " << filename << std::endl;
|
||
return true;
|
||
}
|
||
|
||
// Catmull-Rom样条插值实现
|
||
void PathCurve::generateSpline(const std::vector<PathPoint>& key_points,
|
||
int num_points,
|
||
double tension) {
|
||
if (key_points.size() < 2) {
|
||
std::cerr << "Error: Need at least 2 key points for spline interpolation" << std::endl;
|
||
return;
|
||
}
|
||
|
||
path_points_.clear();
|
||
path_points_.reserve(num_points);
|
||
|
||
// 参数化张力 (0 = 最平滑, 1 = 最紧)
|
||
double s = (1.0 - tension) / 2.0;
|
||
|
||
// 对每一段进行插值
|
||
int segments = static_cast<int>(key_points.size()) - 1;
|
||
int points_per_segment = num_points / segments;
|
||
|
||
for (int seg = 0; seg < segments; ++seg) {
|
||
// 获取控制点（使用边界处理）
|
||
PathPoint p0 = (seg == 0) ? key_points[0] : key_points[seg - 1];
|
||
PathPoint p1 = key_points[seg];
|
||
PathPoint p2 = key_points[seg + 1];
|
||
PathPoint p3 = (seg == segments - 1) ? key_points[seg + 1] : key_points[seg + 2];
|
||
|
||
// 对每一段生成点
|
||
int points_in_this_segment = (seg == segments - 1) ?
|
||
(num_points - seg * points_per_segment) : points_per_segment;
|
||
|
||
for (int i = 0; i < points_in_this_segment; ++i) {
|
||
double t = static_cast<double>(i) / points_per_segment;
|
||
double t2 = t * t;
|
||
double t3 = t2 * t;
|
||
|
||
// Catmull-Rom 基函数
|
||
double b0 = s * ((-t3 + 2.0*t2 - t));
|
||
double b1 = s * ((3.0*t3 - 5.0*t2) / 2.0) + 1.0;
|
||
double b2 = s * ((-3.0*t3 + 4.0*t2 + t) / 2.0);
|
||
double b3 = s * (t3 - t2);
|
||
|
||
// 调整系数以确保通过控制点
|
||
if (seg > 0 || i > 0) {
|
||
b0 *= 2.0;
|
||
b1 = b1 * 2.0 - b0 / 2.0;
|
||
b2 = b2 * 2.0 - b3 / 2.0;
|
||
b3 *= 2.0;
|
||
}
|
||
|
||
PathPoint p;
|
||
p.x = b0*p0.x + b1*p1.x + b2*p2.x + b3*p3.x;
|
||
p.y = b0*p0.y + b1*p1.y + b2*p2.y + b3*p3.y;
|
||
p.theta = 0.0; // 将由setPathPoints计算
|
||
p.kappa = 0.0; // 将由setPathPoints计算
|
||
|
||
path_points_.push_back(p);
|
||
}
|
||
}
|
||
|
||
// 重新计算所有点的theta和kappa
|
||
for (size_t i = 0; i < path_points_.size(); ++i) {
|
||
if (i == 0 && path_points_.size() > 1) {
|
||
double dx = path_points_[i + 1].x - path_points_[i].x;
|
||
double dy = path_points_[i + 1].y - path_points_[i].y;
|
||
path_points_[i].theta = std::atan2(dy, dx);
|
||
} else if (i == path_points_.size() - 1 && path_points_.size() > 1) {
|
||
double dx = path_points_[i].x - path_points_[i - 1].x;
|
||
double dy = path_points_[i].y - path_points_[i - 1].y;
|
||
path_points_[i].theta = std::atan2(dy, dx);
|
||
} else if (path_points_.size() > 2) {
|
||
double dx = path_points_[i + 1].x - path_points_[i - 1].x;
|
||
double dy = path_points_[i + 1].y - path_points_[i - 1].y;
|
||
path_points_[i].theta = std::atan2(dy, dx);
|
||
|
||
if (i > 0 && i < path_points_.size() - 1) {
|
||
path_points_[i].kappa = computeCurvature(
|
||
path_points_[i - 1], path_points_[i], path_points_[i + 1]);
|
||
}
|
||
}
|
||
}
|
||
|
||
std::cout << "Generated spline with " << path_points_.size()
|
||
<< " points from " << key_points.size() << " key points" << std::endl;
|
||
}
|