262 lines
10 KiB
C++
262 lines
10 KiB
C++
#include "path_curve.h"
|
||
#include <iostream>
|
||
#include <string>
|
||
|
||
/**
|
||
* @brief 平滑路径生成器类
|
||
* 用于生成各种类型的平滑路径并保存为CSV文件
|
||
*/
|
||
class SmoothPathGenerator {
|
||
public:
|
||
/**
|
||
* @brief 生成圆弧平滑路径
|
||
* @param filename 输出文件名
|
||
* @param center_x 圆心X坐标
|
||
* @param center_y 圆心Y坐标
|
||
* @param radius 半径
|
||
* @param start_angle 起始角度（弧度）
|
||
* @param end_angle 终止角度（弧度）
|
||
* @param num_points 路径点数量
|
||
*/
|
||
static bool generateCircleArc(const std::string& filename,
|
||
double center_x, double center_y, double radius,
|
||
double start_angle, double end_angle,
|
||
int num_points = 200) {
|
||
PathCurve path;
|
||
path.generateCircleArc(center_x, center_y, radius,
|
||
start_angle, end_angle, num_points);
|
||
|
||
if (path.saveToCSV(filename)) {
|
||
std::cout << "✓ Circle arc path saved: " << filename << std::endl;
|
||
std::cout << " Points: " << num_points << std::endl;
|
||
std::cout << " Length: " << path.getPathLength() << " m" << std::endl;
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/**
|
||
* @brief 生成S型曲线（三次贝塞尔曲线）
|
||
* @param filename 输出文件名
|
||
* @param start_x 起点X
|
||
* @param start_y 起点Y
|
||
* @param end_x 终点X
|
||
* @param end_y 终点Y
|
||
* @param control_offset 控制点偏移量
|
||
* @param num_points 路径点数量
|
||
*/
|
||
static bool generateSCurve(const std::string& filename,
|
||
double start_x, double start_y,
|
||
double end_x, double end_y,
|
||
double control_offset = 3.0,
|
||
int num_points = 200) {
|
||
PathPoint p0(start_x, start_y);
|
||
PathPoint p1(start_x + control_offset, start_y + control_offset);
|
||
PathPoint p2(end_x - control_offset, end_y + control_offset);
|
||
PathPoint p3(end_x, end_y);
|
||
|
||
PathCurve path;
|
||
path.generateCubicBezier(p0, p1, p2, p3, num_points);
|
||
|
||
if (path.saveToCSV(filename)) {
|
||
std::cout << "✓ S-curve path saved: " << filename << std::endl;
|
||
std::cout << " Points: " << num_points << std::endl;
|
||
std::cout << " Length: " << path.getPathLength() << " m" << std::endl;
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/**
|
||
* @brief 生成样条曲线路径
|
||
* @param filename 输出文件名
|
||
* @param key_points 关键点数组
|
||
* @param num_points 生成的路径点总数
|
||
* @param tension 张力参数（0-1，越大越紧）
|
||
*/
|
||
static bool generateSpline(const std::string& filename,
|
||
const std::vector<PathPoint>& key_points,
|
||
int num_points = 200,
|
||
double tension = 0.5) {
|
||
if (key_points.size() < 2) {
|
||
std::cerr << "✗ Error: At least 2 key points required" << std::endl;
|
||
return false;
|
||
}
|
||
|
||
PathCurve path;
|
||
path.generateSpline(key_points, num_points, tension);
|
||
|
||
if (path.saveToCSV(filename)) {
|
||
std::cout << "✓ Spline path saved: " << filename << std::endl;
|
||
std::cout << " Key points: " << key_points.size() << std::endl;
|
||
std::cout << " Total points: " << num_points << std::endl;
|
||
std::cout << " Length: " << path.getPathLength() << " m" << std::endl;
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/**
|
||
* @brief 生成复杂的平滑路径（组合多个关键点）
|
||
* @param filename 输出文件名
|
||
* @param num_points 路径点数量
|
||
*/
|
||
static bool generateComplexPath(const std::string& filename,
|
||
int num_points = 300) {
|
||
// 定义关键点：模拟AGV在仓库中的复杂路径
|
||
std::vector<PathPoint> key_points = {
|
||
PathPoint(0.0, 0.0), // 起点
|
||
PathPoint(5.0, 0.5), // 第一段轻微转弯
|
||
PathPoint(8.0, 3.0), // 上升
|
||
PathPoint(10.0, 6.0), // 继续上升
|
||
PathPoint(11.0, 9.0), // 接近顶部
|
||
PathPoint(10.5, 12.0), // 转向
|
||
PathPoint(8.0, 13.0), // 左转
|
||
PathPoint(5.0, 13.5), // 继续左转
|
||
PathPoint(2.0, 12.0), // 下降
|
||
PathPoint(0.0, 10.0) // 终点
|
||
};
|
||
|
||
PathCurve path;
|
||
path.generateSpline(key_points, num_points, 0.3);
|
||
|
||
if (path.saveToCSV(filename)) {
|
||
std::cout << "✓ Complex smooth path saved: " << filename << std::endl;
|
||
std::cout << " Key points: " << key_points.size() << std::endl;
|
||
std::cout << " Total points: " << num_points << std::endl;
|
||
std::cout << " Length: " << path.getPathLength() << " m" << std::endl;
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/**
|
||
* @brief 生成环形平滑路径
|
||
* @param filename 输出文件名
|
||
* @param radius 半径
|
||
* @param num_points 路径点数量
|
||
*/
|
||
static bool generateLoop(const std::string& filename,
|
||
double radius = 5.0,
|
||
int num_points = 300) {
|
||
PathCurve path;
|
||
path.generateCircleArc(0.0, 0.0, radius, 0.0, 2 * M_PI, num_points);
|
||
|
||
if (path.saveToCSV(filename)) {
|
||
std::cout << "✓ Loop path saved: " << filename << std::endl;
|
||
std::cout << " Radius: " << radius << " m" << std::endl;
|
||
std::cout << " Points: " << num_points << std::endl;
|
||
std::cout << " Length: " << path.getPathLength() << " m" << std::endl;
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/**
|
||
* @brief 生成Figure-8路径（8字形）
|
||
* @param filename 输出文件名
|
||
* @param size 8字大小
|
||
* @param num_points 路径点数量
|
||
*/
|
||
static bool generateFigure8(const std::string& filename,
|
||
double size = 5.0,
|
||
int num_points = 400) {
|
||
std::vector<PathPoint> key_points = {
|
||
PathPoint(0.0, 0.0),
|
||
PathPoint(size, size),
|
||
PathPoint(size * 2, 0.0),
|
||
PathPoint(size, -size),
|
||
PathPoint(0.0, 0.0)
|
||
};
|
||
|
||
PathCurve path;
|
||
path.generateSpline(key_points, num_points, 0.4);
|
||
|
||
if (path.saveToCSV(filename)) {
|
||
std::cout << "✓ Figure-8 path saved: " << filename << std::endl;
|
||
std::cout << " Size: " << size << " m" << std::endl;
|
||
std::cout << " Points: " << num_points << std::endl;
|
||
std::cout << " Length: " << path.getPathLength() << " m" << std::endl;
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/**
|
||
* @brief 打印使用说明
|
||
*/
|
||
static void printUsage() {
|
||
std::cout << "\n========================================" << std::endl;
|
||
std::cout << " Smooth Path Generator" << std::endl;
|
||
std::cout << "========================================\n" << std::endl;
|
||
std::cout << "This tool generates various smooth paths for AGV navigation.\n" << std::endl;
|
||
std::cout << "Available path types:" << std::endl;
|
||
std::cout << " 1. Simple smooth path (default)" << std::endl;
|
||
std::cout << " 2. Circle arc" << std::endl;
|
||
std::cout << " 3. S-curve" << std::endl;
|
||
std::cout << " 4. Complex path" << std::endl;
|
||
std::cout << " 5. Loop path" << std::endl;
|
||
std::cout << " 6. Figure-8 path" << std::endl;
|
||
std::cout << "\nAll paths will be saved as CSV files.\n" << std::endl;
|
||
}
|
||
};
|
||
|
||
int main(int argc, char* argv[]) {
|
||
SmoothPathGenerator::printUsage();
|
||
|
||
std::cout << "Generating smooth paths...\n" << std::endl;
|
||
|
||
// 1. 生成默认的平滑路径 - smooth_path.csv
|
||
std::cout << "[1] Generating default smooth path..." << std::endl;
|
||
std::vector<PathPoint> default_key_points = {
|
||
PathPoint(0.0, 0.0),
|
||
PathPoint(3.0, 1.0),
|
||
PathPoint(6.0, 3.0),
|
||
PathPoint(9.0, 3.5),
|
||
PathPoint(12.0, 3.0)
|
||
};
|
||
SmoothPathGenerator::generateSpline("smooth_path.csv", default_key_points, 200, 0.5);
|
||
|
||
// 2. 生成圆弧路径
|
||
std::cout << "\n[2] Generating circle arc path..." << std::endl;
|
||
SmoothPathGenerator::generateCircleArc("smooth_path_arc.csv",
|
||
5.0, 0.0, 5.0,
|
||
M_PI, M_PI / 2, 150);
|
||
|
||
// 3. 生成S型曲线
|
||
std::cout << "\n[3] Generating S-curve path..." << std::endl;
|
||
SmoothPathGenerator::generateSCurve("smooth_path_scurve.csv",
|
||
0.0, 0.0, 10.0, 0.0, 2.5, 200);
|
||
|
||
// 4. 生成复杂路径
|
||
std::cout << "\n[4] Generating complex path..." << std::endl;
|
||
SmoothPathGenerator::generateComplexPath("smooth_path_complex.csv", 300);
|
||
|
||
// 5. 生成环形路径
|
||
std::cout << "\n[5] Generating loop path..." << std::endl;
|
||
SmoothPathGenerator::generateLoop("smooth_path_loop.csv", 5.0, 300);
|
||
|
||
// 6. 生成8字形路径
|
||
std::cout << "\n[6] Generating Figure-8 path..." << std::endl;
|
||
SmoothPathGenerator::generateFigure8("smooth_path_figure8.csv", 4.0, 400);
|
||
|
||
std::cout << "\n========================================" << std::endl;
|
||
std::cout << "✓ All smooth paths generated successfully!" << std::endl;
|
||
std::cout << "========================================\n" << std::endl;
|
||
|
||
std::cout << "Generated files:" << std::endl;
|
||
std::cout << " • smooth_path.csv - Default smooth path" << std::endl;
|
||
std::cout << " • smooth_path_arc.csv - Circle arc" << std::endl;
|
||
std::cout << " • smooth_path_scurve.csv - S-curve" << std::endl;
|
||
std::cout << " • smooth_path_complex.csv - Complex path" << std::endl;
|
||
std::cout << " • smooth_path_loop.csv - Loop path" << std::endl;
|
||
std::cout << " • smooth_path_figure8.csv - Figure-8 path" << std::endl;
|
||
|
||
std::cout << "\nYou can load these CSV files in the Qt GUI application:" << std::endl;
|
||
std::cout << " 1. Run: ./build/Debug/agv_qt_gui.exe" << std::endl;
|
||
std::cout << " 2. Select 'Load from CSV' in Path Type" << std::endl;
|
||
std::cout << " 3. Choose one of the generated CSV files" << std::endl;
|
||
|
||
return 0;
|
||
}
|