agv/RCS-3000
8
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

initial

Browse Source
This commit is contained in:
CaiXiang
2025-11-14 16:09:58 +08:00
commit af65c2425d
74 changed files with 14650 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
src/agv_model.cpp Normal file
View File

@@ -0,0 +1,63 @@
#include "agv_model.h"
#include <algorithm>
#define _USE_MATH_DEFINES
#include <cmath>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
AGVModel::AGVModel(double wheelbase, double max_velocity, double max_steering_angle)
: wheelbase_(wheelbase)
, max_velocity_(max_velocity)
, max_steering_angle_(max_steering_angle) {
}
AGVModel::State AGVModel::derivative(const State& state, const Control& control) const {
State dstate;
// 单舵轮AGV运动学方程
// dx/dt = v * cos(theta)
// dy/dt = v * sin(theta)
// dtheta/dt = (v / L) * tan(delta)
// 其中 L 是轮距，delta 是舵轮转向角
dstate.x = control.v * std::cos(state.theta);
dstate.y = control.v * std::sin(state.theta);
dstate.theta = (control.v / wheelbase_) * std::tan(control.delta);
return dstate;
}
AGVModel::State AGVModel::update(const State& state, const Control& control, double dt) const {
// 限制控制输入
Control clamped_control = clampControl(control);
// 使用欧拉法更新状态
State dstate = derivative(state, clamped_control);
State new_state;
new_state.x = state.x + dstate.x * dt;
new_state.y = state.y + dstate.y * dt;
new_state.theta = state.theta + dstate.theta * dt;
// 归一化角度到 [-π, π]
while (new_state.theta > M_PI) new_state.theta -= 2.0 * M_PI;
while (new_state.theta < -M_PI) new_state.theta += 2.0 * M_PI;
return new_state;
}
AGVModel::Control AGVModel::clampControl(const Control& control) const {
Control clamped;
// 限制速度
clamped.v = std::max(-max_velocity_, std::min(max_velocity_, control.v));
// 限制转向角
clamped.delta = std::max(-max_steering_angle_,
std::min(max_steering_angle_, control.delta));
return clamped;
}

183
src/can/CANController.cpp Normal file
View File

@@ -0,0 +1,183 @@
/**
* CAN 控制器实现
*/
#include "can/CANController.h"
#include <iostream>
#include <cstring>
using namespace std;
CANController::CANController(DWORD device_type, DWORD device_index, DWORD can_index)
: m_device_type(device_type)
, m_device_index(device_index)
, m_can_index(can_index)
, m_initialized(false)
, m_callback(nullptr)
{
}
CANController::~CANController() {
Close();
}
bool CANController::Initialize(BYTE baud_t0, BYTE baud_t1, BYTE mode) {
if (m_initialized) {
cerr << "警告：CAN 设备已经初始化" << endl;
return true;
}
// 1. 打开设备
cout << "打开 CAN 设备..." << endl;
DWORD ret = VCI_OpenDevice(m_device_type, m_device_index, 0);
if (ret != 1) {
cerr << "错误：打开设备失败！" << endl;
return false;
}
// 2. 初始化 CAN 参数
cout << "配置 CAN 参数..." << endl;
VCI_INIT_CONFIG config;
memset(&config, 0, sizeof(VCI_INIT_CONFIG));
config.AccCode = 0x00000000; // 验收码
config.AccMask = 0xFFFFFFFF; // 屏蔽码（接收所有）
config.Filter = 1; // 滤波方式：接收所有类型
config.Timing0 = baud_t0; // 波特率定时器0
config.Timing1 = baud_t1; // 波特率定时器1
config.Mode = mode; // 工作模式
ret = VCI_InitCAN(m_device_type, m_device_index, m_can_index, &config);
if (ret != 1) {
cerr << "错误：初始化 CAN 失败！" << endl;
VCI_CloseDevice(m_device_type, m_device_index);
return false;
}
// 3. 启动 CAN
cout << "启动 CAN..." << endl;
ret = VCI_StartCAN(m_device_type, m_device_index, m_can_index);
if (ret != 1) {
cerr << "错误：启动 CAN 失败！" << endl;
VCI_CloseDevice(m_device_type, m_device_index);
return false;
}
// 4. 清空缓冲区
VCI_ClearBuffer(m_device_type, m_device_index, m_can_index);
m_initialized = true;
cout << "CAN 设备初始化成功！" << endl;
return true;
}
void CANController::Close() {
if (!m_initialized) {
return;
}
// 复位 CAN
VCI_ResetCAN(m_device_type, m_device_index, m_can_index);
// 关闭设备
VCI_CloseDevice(m_device_type, m_device_index);
m_initialized = false;
cout << "CAN 设备已关闭" << endl;
}
bool CANController::SendStandardFrame(UINT can_id, const BYTE* data, BYTE len) {
if (!m_initialized) {
cerr << "错误：CAN 设备未初始化！" << endl;
return false;
}
VCI_CAN_OBJ frame;
memset(&frame, 0, sizeof(VCI_CAN_OBJ));
frame.ID = can_id;
frame.SendType = 0; // 正常发送
frame.RemoteFlag = 0; // 数据帧
frame.ExternFlag = 0; // 标准帧
frame.DataLen = len > 8 ? 8 : len;
if (data != nullptr && len > 0) {
memcpy(frame.Data, data, frame.DataLen);
}
DWORD ret = VCI_Transmit(m_device_type, m_device_index, m_can_index, &frame, 1);
return (ret == 1);
}
bool CANController::SendExtendedFrame(UINT can_id, const BYTE* data, BYTE len) {
if (!m_initialized) {
cerr << "错误：CAN 设备未初始化！" << endl;
return false;
}
VCI_CAN_OBJ frame;
memset(&frame, 0, sizeof(VCI_CAN_OBJ));
frame.ID = can_id;
frame.SendType = 0; // 正常发送
frame.RemoteFlag = 0; // 数据帧
frame.ExternFlag = 1; // 扩展帧
frame.DataLen = len > 8 ? 8 : len;
if (data != nullptr && len > 0) {
memcpy(frame.Data, data, frame.DataLen);
}
DWORD ret = VCI_Transmit(m_device_type, m_device_index, m_can_index, &frame, 1);
return (ret == 1);
}
DWORD CANController::Receive(std::vector<VCI_CAN_OBJ>& frames, DWORD max_count) {
if (!m_initialized) {
cerr << "错误：CAN 设备未初始化！" << endl;
return 0;
}
frames.resize(max_count);
DWORD ret = VCI_Receive(m_device_type, m_device_index, m_can_index,
frames.data(), max_count, 0);
if (ret > 0) {
frames.resize(ret); // 调整为实际接收到的数量
// 如果设置了回调函数，调用它
if (m_callback) {
for (const auto& frame : frames) {
m_callback(frame);
}
}
} else {
frames.clear();
}
return ret;
}
DWORD CANController::GetReceiveNum() {
if (!m_initialized) {
return 0;
}
return VCI_GetReceiveNum(m_device_type, m_device_index, m_can_index);
}
bool CANController::ClearBuffer() {
if (!m_initialized) {
return false;
}
DWORD ret = VCI_ClearBuffer(m_device_type, m_device_index, m_can_index);
return (ret == 1);
}
bool CANController::GetDeviceInfo(VCI_BOARD_INFO& info) {
if (!m_initialized) {
return false;
}
DWORD ret = VCI_ReadBoardInfo(m_device_type, m_device_index, &info);
return (ret == 1);
}

109
src/can/CANController.h Normal file
View File

@@ -0,0 +1,109 @@
/**
* CAN 控制器封装类
* 功能：简化 CAN 设备的操作，提供易用的接口
*/
#ifndef CAN_CONTROLLER_H
#define CAN_CONTROLLER_H
#include "../../lib/ControlCAN.h"
#include <string>
#include <vector>
#include <functional>
/**
* CAN 控制器类
*/
class CANController {
public:
// 回调函数类型：接收到 CAN 数据时调用
using ReceiveCallback = std::function<void(const VCI_CAN_OBJ&)>;
/**
* 构造函数
* @param device_type 设备类型（VCI_USBCAN2 = 4）
* @param device_index 设备索引（第几个设备，从0开始）
* @param can_index CAN 通道索引（0 或 1）
*/
CANController(DWORD device_type = VCI_USBCAN2,
DWORD device_index = 0,
DWORD can_index = 0);
~CANController();
/**
* 初始化 CAN 设备
* @param baud_t0 波特率定时器0
* @param baud_t1 波特率定时器1
* @param mode 工作模式：0=正常，1=只听，2=自发自收
* @return 成功返回 true
*/
bool Initialize(BYTE baud_t0 = 0x00, BYTE baud_t1 = 0x1C, BYTE mode = 0);
/**
* 关闭 CAN 设备
*/
void Close();
/**
* 发送标准帧
* @param can_id CAN ID（11位）
* @param data 数据指针
* @param len 数据长度（最大8字节）
* @return 成功返回 true
*/
bool SendStandardFrame(UINT can_id, const BYTE* data, BYTE len);
/**
* 发送扩展帧
* @param can_id CAN ID（29位）
* @param data 数据指针
* @param len 数据长度（最大8字节）
* @return 成功返回 true
*/
bool SendExtendedFrame(UINT can_id, const BYTE* data, BYTE len);
/**
* 接收 CAN 数据（非阻塞）
* @param frames 接收缓冲区
* @param max_count 最大接收帧数
* @return 实际接收到的帧数
*/
DWORD Receive(std::vector<VCI_CAN_OBJ>& frames, DWORD max_count = 2500);
/**
* 获取接收缓冲区中的帧数量
*/
DWORD GetReceiveNum();
/**
* 清空接收缓冲区
*/
bool ClearBuffer();
/**
* 读取设备信息
*/
bool GetDeviceInfo(VCI_BOARD_INFO& info);
/**
* 设置接收回调函数
*/
void SetReceiveCallback(ReceiveCallback callback) {
m_callback = callback;
}
/**
* 是否已初始化
*/
bool IsInitialized() const { return m_initialized; }
private:
DWORD m_device_type;
DWORD m_device_index;
DWORD m_can_index;
bool m_initialized;
ReceiveCallback m_callback;
};
#endif // CAN_CONTROLLER_H

330
src/can/can_complete_example.cpp Normal file
View File

@@ -0,0 +1,330 @@
/**
* CAN 通信完整使用示例
* 包括：基础通信、AGV 运动控制、数据监控等场景
*/
#include <iostream>
#include <thread>
#include <chrono>
#include <iomanip>
#include "can/CANController.h"
using namespace std;
// AGV CAN 协议定义
namespace AGV_Protocol {
// CAN ID 定义
const UINT ID_MOTOR_CONTROL = 0x200; // 电机控制指令
const UINT ID_MOTOR_FEEDBACK = 0x201; // 电机反馈
const UINT ID_AGV_STATUS = 0x300; // AGV 状态
const UINT ID_SENSOR_DATA = 0x400; // 传感器数据
// 命令类型
const BYTE CMD_VELOCITY = 0x10; // 速度控制
const BYTE CMD_POSITION = 0x20; // 位置控制
const BYTE CMD_STOP = 0x30; // 停止
const BYTE CMD_RESET = 0x40; // 复位
}
/**
* 打印 CAN 帧信息
*/
void PrintCANFrame(const VCI_CAN_OBJ& frame) {
cout << "[接收] ID=0x" << hex << setw(3) << setfill('0') << frame.ID << dec;
cout << " (" << (frame.ExternFlag ? "扩展帧" : "标准帧");
cout << (frame.RemoteFlag ? ", 远程帧" : ", 数据帧") << ")";
cout << " 长度=" << (int)frame.DataLen << " 数据=[";
for (int i = 0; i < frame.DataLen; i++) {
cout << hex << setw(2) << setfill('0') << (int)frame.Data[i];
if (i < frame.DataLen - 1) cout << " ";
}
cout << "]" << dec << endl;
}
/**
* 示例1：基本 CAN 通信测试
*/
void Example1_BasicCommunication() {
cout << "\n========== 示例1：基本 CAN 通信测试 ==========" << endl;
CANController can;
// 初始化（波特率 500Kbps）
if (!can.Initialize(0x00, 0x1C, 0)) {
return;
}
// 读取设备信息
VCI_BOARD_INFO info;
if (can.GetDeviceInfo(info)) {
cout << "\n设备信息：" << endl;
cout << " 硬件版本: 0x" << hex << info.hw_Version << dec << endl;
cout << " 固件版本: 0x" << hex << info.fw_Version << dec << endl;
cout << " CAN 通道数: " << (int)info.can_Num << endl;
cout << " 序列号: " << info.str_Serial_Num << endl;
}
// 发送测试数据
cout << "\n发送测试数据..." << endl;
BYTE test_data[] = {0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88};
if (can.SendStandardFrame(0x123, test_data, 8)) {
cout << "发送成功：ID=0x123, 数据=[11 22 33 44 55 66 77 88]" << endl;
}
// 接收数据（持续3秒）
cout << "\n开始接收数据（3秒）..." << endl;
auto start = chrono::steady_clock::now();
int total_received = 0;
while (true) {
auto now = chrono::steady_clock::now();
auto elapsed = chrono::duration_cast<chrono::seconds>(now - start).count();
if (elapsed >= 3) break;
vector<VCI_CAN_OBJ> frames;
DWORD count = can.Receive(frames, 100);
if (count > 0) {
for (const auto& frame : frames) {
PrintCANFrame(frame);
total_received++;
}
}
this_thread::sleep_for(chrono::milliseconds(10));
}
cout << "接收完成，共接收 " << total_received << " 帧数据" << endl;
}
/**
* 示例2：AGV 速度控制
*/
void Example2_AGVVelocityControl() {
cout << "\n========== 示例2：AGV 速度控制 ==========" << endl;
CANController can;
// 初始化（波特率 250Kbps）
if (!can.Initialize(0x01, 0x1C, 0)) {
return;
}
// 设置接收回调（处理电机反馈）
can.SetReceiveCallback([](const VCI_CAN_OBJ& frame) {
if (frame.ID == AGV_Protocol::ID_MOTOR_FEEDBACK) {
// 解析电机反馈数据
int16_t speed = (frame.Data[0] << 8) | frame.Data[1];
int16_t current = (frame.Data[2] << 8) | frame.Data[3];
cout << " [反馈] 速度=" << speed << " RPM, 电流=" << current << " mA" << endl;
}
});
// AGV 速度控制函数
auto sendVelocityCommand = [&can](int16_t left_speed, int16_t right_speed) {
BYTE data[8] = {0};
data[0] = AGV_Protocol::CMD_VELOCITY; // 命令类型
data[1] = 0x00; // 保留
data[2] = (left_speed >> 8) & 0xFF; // 左轮速度高字节
data[3] = left_speed & 0xFF; // 左轮速度低字节
data[4] = (right_speed >> 8) & 0xFF; // 右轮速度高字节
data[5] = right_speed & 0xFF; // 右轮速度低字节
data[6] = 0x00; // 保留
data[7] = 0x00; // 校验和（简化）
return can.SendStandardFrame(AGV_Protocol::ID_MOTOR_CONTROL, data, 8);
};
// 场景1：直行
cout << "\n场景1：AGV 直行（速度 100 RPM）" << endl;
if (sendVelocityCommand(100, 100)) {
cout << "发送成功：左轮=100, 右轮=100" << endl;
}
this_thread::sleep_for(chrono::seconds(2));
// 场景2：左转
cout << "\n场景2：AGV 左转" << endl;
if (sendVelocityCommand(50, 100)) {
cout << "发送成功：左轮=50, 右轮=100" << endl;
}
this_thread::sleep_for(chrono::seconds(2));
// 场景3：右转
cout << "\n场景3：AGV 右转" << endl;
if (sendVelocityCommand(100, 50)) {
cout << "发送成功：左轮=100, 右轮=50" << endl;
}
this_thread::sleep_for(chrono::seconds(2));
// 场景4：停止
cout << "\n场景4：AGV 停止" << endl;
BYTE stop_data[] = {AGV_Protocol::CMD_STOP, 0, 0, 0, 0, 0, 0, 0};
if (can.SendStandardFrame(AGV_Protocol::ID_MOTOR_CONTROL, stop_data, 8)) {
cout << "发送停止命令" << endl;
}
// 接收反馈数据
cout << "\n接收反馈数据..." << endl;
for (int i = 0; i < 10; i++) {
vector<VCI_CAN_OBJ> frames;
can.Receive(frames, 100);
this_thread::sleep_for(chrono::milliseconds(100));
}
}
/**
* 示例3：CAN 数据监控
*/
void Example3_CANMonitor() {
cout << "\n========== 示例3：CAN 总线监控 ==========" << endl;
CANController can;
// 初始化为只听模式（不影响总线）
if (!can.Initialize(0x00, 0x1C, 1)) { // mode=1 只听模式
return;
}
cout << "开始监控 CAN 总线（10秒）..." << endl;
cout << "提示：只听模式，不会影响总线通信" << endl;
// 统计信息
map<UINT, int> id_count; // 每个 ID 的帧数统计
int total_frames = 0;
auto start = chrono::steady_clock::now();
while (true) {
auto now = chrono::steady_clock::now();
auto elapsed = chrono::duration_cast<chrono::seconds>(now - start).count();
if (elapsed >= 10) break;
vector<VCI_CAN_OBJ> frames;
DWORD count = can.Receive(frames, 100);
if (count > 0) {
for (const auto& frame : frames) {
PrintCANFrame(frame);
id_count[frame.ID]++;
total_frames++;
}
}
this_thread::sleep_for(chrono::milliseconds(50));
}
// 显示统计信息
cout << "\n========== 统计信息 ==========" << endl;
cout << "总帧数: " << total_frames << endl;
cout << "不同 ID 数量: " << id_count.size() << endl;
cout << "\nID 分布：" << endl;
for (const auto& pair : id_count) {
cout << " ID 0x" << hex << setw(3) << setfill('0') << pair.first
<< dec << ": " << pair.second << " 帧" << endl;
}
}
/**
* 示例4：周期性发送和接收
*/
void Example4_PeriodicTransmit() {
cout << "\n========== 示例4：周期性发送和接收 ==========" << endl;
CANController can;
if (!can.Initialize(0x00, 0x1C, 0)) {
return;
}
cout << "开始周期性通信（10秒）..." << endl;
bool running = true;
// 发送线程：每100ms发送一次心跳
thread send_thread([&can, &running]() {
int counter = 0;
while (running) {
BYTE data[8] = {0};
data[0] = 0xAA; // 心跳标识
data[1] = (counter >> 8) & 0xFF;
data[2] = counter & 0xFF;
data[3] = 0x55;
can.SendStandardFrame(0x100, data, 4);
counter++;
this_thread::sleep_for(chrono::milliseconds(100));
}
});
// 接收线程
thread recv_thread([&can, &running]() {
while (running) {
vector<VCI_CAN_OBJ> frames;
DWORD count = can.Receive(frames, 100);
if (count > 0) {
for (const auto& frame : frames) {
PrintCANFrame(frame);
}
}
this_thread::sleep_for(chrono::milliseconds(10));
}
});
// 运行10秒
this_thread::sleep_for(chrono::seconds(10));
running = false;
send_thread.join();
recv_thread.join();
cout << "周期性通信结束" << endl;
}
/**
* 主程序
*/
int main() {
cout << "================================================" << endl;
cout << " CAN 通信完整示例程序" << endl;
cout << "================================================" << endl;
int choice = 0;
cout << "\n请选择示例：" << endl;
cout << "1. 基本 CAN 通信测试" << endl;
cout << "2. AGV 速度控制" << endl;
cout << "3. CAN 总线监控（只听模式）" << endl;
cout << "4. 周期性发送和接收" << endl;
cout << "0. 运行所有示例" << endl;
cout << "\n请输入选择 (0-4): ";
cin >> choice;
switch (choice) {
case 1:
Example1_BasicCommunication();
break;
case 2:
Example2_AGVVelocityControl();
break;
case 3:
Example3_CANMonitor();
break;
case 4:
Example4_PeriodicTransmit();
break;
case 0:
Example1_BasicCommunication();
Example2_AGVVelocityControl();
Example3_CANMonitor();
Example4_PeriodicTransmit();
break;
default:
cout << "无效选择！" << endl;
}
cout << "\n程序执行完成！" << endl;
return 0;
}

262
src/can/can_example.cpp Normal file
View File

@@ -0,0 +1,262 @@
/**
* CAN 通信完整示例程序
* 功能：演示如何使用 ControlCAN 库进行 CAN 通信
*/
#include <iostream>
#include <cstring>
#include <thread>
#include <chrono>
#include "../../lib/ControlCAN.h"
using namespace std;
// CAN 设备配置参数
#define DEVICE_TYPE VCI_USBCAN2 // 设备类型：USBCAN-2
#define DEVICE_INDEX 0 // 设备索引：第一个设备
#define CAN_INDEX 0 // CAN 通道索引：CAN0
// 常用波特率配置（Timing0, Timing1）
// 250Kbps: 0x01, 0x1C
// 500Kbps: 0x00, 0x1C
// 1Mbps: 0x00, 0x14
#define BAUD_250K_T0 0x01
#define BAUD_250K_T1 0x1C
/**
* 初始化并打开 CAN 设备
* @return 成功返回 true，失败返回 false
*/
bool InitCAN() {
cout << "=== 初始化 CAN 设备 ===" << endl;
// 1. 打开设备
cout << "1. 打开设备..." << endl;
DWORD ret = VCI_OpenDevice(DEVICE_TYPE, DEVICE_INDEX, 0);
if (ret != 1) {
cerr << "错误：打开设备失败！" << endl;
return false;
}
cout << " 设备打开成功！" << endl;
// 2. 配置 CAN 参数
cout << "2. 配置 CAN 参数..." << endl;
VCI_INIT_CONFIG config;
config.AccCode = 0x00000000; // 验收码：接收所有帧
config.AccMask = 0xFFFFFFFF; // 屏蔽码：不过滤
config.Filter = 1; // 滤波方式：双滤波
config.Timing0 = BAUD_250K_T0; // 波特率：250Kbps
config.Timing1 = BAUD_250K_T1;
config.Mode = 0; // 模式：正常模式
config.Reserved = 0;
ret = VCI_InitCAN(DEVICE_TYPE, DEVICE_INDEX, CAN_INDEX, &config);
if (ret != 1) {
cerr << "错误：初始化 CAN 失败！" << endl;
VCI_CloseDevice(DEVICE_TYPE, DEVICE_INDEX);
return false;
}
cout << " CAN 初始化成功（波特率：250Kbps）！" << endl;
// 3. 启动 CAN
cout << "3. 启动 CAN..." << endl;
ret = VCI_StartCAN(DEVICE_TYPE, DEVICE_INDEX, CAN_INDEX);
if (ret != 1) {
cerr << "错误：启动 CAN 失败！" << endl;
VCI_CloseDevice(DEVICE_TYPE, DEVICE_INDEX);
return false;
}
cout << " CAN 启动成功！" << endl;
// 4. 清空缓冲区
VCI_ClearBuffer(DEVICE_TYPE, DEVICE_INDEX, CAN_INDEX);
cout << " 缓冲区已清空！" << endl;
return true;
}
/**
* 发送 CAN 报文
* @param can_id CAN ID
* @param data 数据指针
* @param len 数据长度（最大8字节）
* @param extend 是否为扩展帧
* @return 成功返回 true，失败返回 false
*/
bool SendCANFrame(UINT can_id, const BYTE* data, BYTE len, bool extend = false) {
VCI_CAN_OBJ send_frame;
memset(&send_frame, 0, sizeof(VCI_CAN_OBJ));
send_frame.ID = can_id;
send_frame.SendType = 0; // 正常发送
send_frame.RemoteFlag = 0; // 数据帧
send_frame.ExternFlag = extend ? 1 : 0; // 标准帧/扩展帧
send_frame.DataLen = len > 8 ? 8 : len;
if (data != nullptr) {
memcpy(send_frame.Data, data, send_frame.DataLen);
}
DWORD ret = VCI_Transmit(DEVICE_TYPE, DEVICE_INDEX, CAN_INDEX, &send_frame, 1);
if (ret != 1) {
cerr << "错误：发送 CAN 报文失败！" << endl;
return false;
}
return true;
}
/**
* 接收 CAN 报文
* @param timeout_ms 超时时间（毫秒）
* @return 接收到的报文数量
*/
int ReceiveCANFrames(int timeout_ms = 100) {
VCI_CAN_OBJ receive_frames[100]; // 接收缓冲区
// 获取接收缓冲区中的数据数量
DWORD num = VCI_GetReceiveNum(DEVICE_TYPE, DEVICE_INDEX, CAN_INDEX);
if (num == 0) {
return 0;
}
// 读取数据
DWORD ret = VCI_Receive(DEVICE_TYPE, DEVICE_INDEX, CAN_INDEX,
receive_frames, 100, timeout_ms);
if (ret > 0) {
cout << "接收到 " << ret << " 帧数据：" << endl;
for (DWORD i = 0; i < ret; i++) {
VCI_CAN_OBJ& frame = receive_frames[i];
cout << " [" << i + 1 << "] ";
cout << "ID=0x" << hex << frame.ID << dec;
cout << " (";
cout << (frame.ExternFlag ? "扩展帧" : "标准帧");
cout << (frame.RemoteFlag ? ", 远程帧" : ", 数据帧");
cout << ") ";
cout << "长度=" << (int)frame.DataLen << " ";
cout << "数据=[";
for (int j = 0; j < frame.DataLen; j++) {
printf("%02X", frame.Data[j]);
if (j < frame.DataLen - 1) cout << " ";
}
cout << "]" << endl;
}
return ret;
}
return 0;
}
/**
* 读取并显示设备信息
*/
void ShowDeviceInfo() {
VCI_BOARD_INFO board_info;
DWORD ret = VCI_ReadBoardInfo(DEVICE_TYPE, DEVICE_INDEX, &board_info);
if (ret == 1) {
cout << "\n=== 设备信息 ===" << endl;
cout << "硬件版本: " << board_info.hw_Version << endl;
cout << "固件版本: " << board_info.fw_Version << endl;
cout << "驱动版本: " << board_info.dr_Version << endl;
cout << "接口库版本: " << board_info.in_Version << endl;
cout << "CAN 通道数: " << (int)board_info.can_Num << endl;
cout << "序列号: " << board_info.str_Serial_Num << endl;
cout << "硬件类型: " << board_info.str_hw_Type << endl;
} else {
cerr << "读取设备信息失败！" << endl;
}
}
/**
* 关闭 CAN 设备
*/
void CloseCAN() {
cout << "\n=== 关闭 CAN 设备 ===" << endl;
// 复位 CAN
VCI_ResetCAN(DEVICE_TYPE, DEVICE_INDEX, CAN_INDEX);
cout << "CAN 已复位" << endl;
// 关闭设备
VCI_CloseDevice(DEVICE_TYPE, DEVICE_INDEX);
cout << "设备已关闭" << endl;
}
/**
* 主程序 - 完整的 CAN 通信流程示例
*/
int main() {
cout << "================================================" << endl;
cout << " CAN 通信完整示例程序" << endl;
cout << "================================================" << endl;
// ===== 步骤 1: 初始化设备 =====
if (!InitCAN()) {
cerr << "初始化失败，程序退出！" << endl;
return -1;
}
// ===== 步骤 2: 读取设备信息 =====
ShowDeviceInfo();
// ===== 步骤 3: 发送 CAN 报文 =====
cout << "\n=== 发送 CAN 报文 ===" << endl;
// 示例 1: 发送标准帧
BYTE data1[] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08};
cout << "发送标准帧 (ID=0x123)..." << endl;
if (SendCANFrame(0x123, data1, 8, false)) {
cout << " 发送成功！数据: [01 02 03 04 05 06 07 08]" << endl;
}
this_thread::sleep_for(chrono::milliseconds(10));
// 示例 2: 发送扩展帧
BYTE data2[] = {0xAA, 0xBB, 0xCC, 0xDD};
cout << "发送扩展帧 (ID=0x12345678)..." << endl;
if (SendCANFrame(0x12345678, data2, 4, true)) {
cout << " 发送成功！数据: [AA BB CC DD]" << endl;
}
this_thread::sleep_for(chrono::milliseconds(10));
// 示例 3: 发送控制命令（模拟 AGV 速度控制）
BYTE agv_cmd[] = {0x10, 0x00, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00};
// 假设: 字节0=命令类型(0x10=速度控制), 字节2-3=速度值(50=0x32)
cout << "发送 AGV 速度控制命令 (ID=0x200)..." << endl;
if (SendCANFrame(0x200, agv_cmd, 8, false)) {
cout << " 发送成功！速度设置为 50" << endl;
}
// ===== 步骤 4: 接收 CAN 报文 =====
cout << "\n=== 接收 CAN 报文 ===" << endl;
cout << "开始监听 CAN 总线（持续 5 秒）..." << endl;
auto start_time = chrono::steady_clock::now();
int total_received = 0;
while (true) {
auto current_time = chrono::steady_clock::now();
auto elapsed = chrono::duration_cast<chrono::seconds>(current_time - start_time).count();
if (elapsed >= 5) {
break; // 5秒后退出
}
int count = ReceiveCANFrames(100);
total_received += count;
this_thread::sleep_for(chrono::milliseconds(100));
}
cout << "监听结束，共接收 " << total_received << " 帧数据。" << endl;
// ===== 步骤 5: 关闭设备 =====
CloseCAN();
cout << "\n程序执行完成！" << endl;
return 0;
}

217
src/control_generator.cpp Normal file
View File

@@ -0,0 +1,217 @@
#include "control_generator.h"
#define _USE_MATH_DEFINES
#include <cmath>
#include <algorithm>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
ControlGenerator::ControlGenerator(const AGVModel& model)
: model_(model) {
}
ControlSequence ControlGenerator::generate(const PathCurve& path,
const AGVModel::State& initial_state,
double dt, double horizon) {
// 使用Pure Pursuit作为默认算法
return generatePurePursuit(path, initial_state, dt, 1.5, 1.0, horizon);
}
ControlSequence ControlGenerator::generatePurePursuit(const PathCurve& path,
const AGVModel::State& initial_state,
double dt,
double lookahead_distance,
double desired_velocity,
double horizon) {
ControlSequence sequence;
sequence.clear();
AGVModel::State current_state = initial_state;
double current_time = 0.0;
while (current_time < horizon) {
// 找到前视点
PathPoint target = findLookaheadPoint(path, current_state, lookahead_distance);
// 计算控制量
AGVModel::Control control = computePurePursuitControl(
current_state, target, desired_velocity);
// 保存控制量和状态
sequence.controls.push_back(control);
sequence.timestamps.push_back(current_time);
sequence.predicted_states.push_back(current_state);
// 更新状态
current_state = model_.update(current_state, control, dt);
current_time += dt;
// 修复: 检查是否接近路径终点（阈值放宽以确保完整追踪）
const auto& path_points = path.getPathPoints();
if (!path_points.empty()) {
const PathPoint& end_point = path_points.back();
double dx = current_state.x - end_point.x;
double dy = current_state.y - end_point.y;
double distance_to_end = std::sqrt(dx * dx + dy * dy);
if (distance_to_end < 0.5) {
break; // 已到达终点附近
}
}
}
return sequence;
}
ControlSequence ControlGenerator::generateStanley(const PathCurve& path,
const AGVModel::State& initial_state,
double dt,
double k_gain,
double desired_velocity,
double horizon) {
ControlSequence sequence;
sequence.clear();
AGVModel::State current_state = initial_state;
double current_time = 0.0;
const auto& path_points = path.getPathPoints();
if (path_points.empty()) {
return sequence;
}
while (current_time < horizon) {
// 找到最近的路径点
int nearest_idx = path.findNearestPoint(current_state.x, current_state.y);
// 如果找不到最近点（不应该发生），使用第一个点
if (nearest_idx < 0) {
nearest_idx = 0;
}
PathPoint nearest_point = path_points[nearest_idx];
// 计算控制量
AGVModel::Control control = computeStanleyControl(
current_state, nearest_point, k_gain, desired_velocity);
// 保存控制量和状态
sequence.controls.push_back(control);
sequence.timestamps.push_back(current_time);
sequence.predicted_states.push_back(current_state);
// 更新状态
current_state = model_.update(current_state, control, dt);
current_time += dt;
// 修复: 检查是否接近路径终点（阈值放宽以确保完整追踪）
const PathPoint& end_point = path_points.back();
double dx = current_state.x - end_point.x;
double dy = current_state.y - end_point.y;
double distance_to_end = std::sqrt(dx * dx + dy * dy);
if (distance_to_end < 0.5) {
break;
}
}
return sequence;
}
AGVModel::Control ControlGenerator::computePurePursuitControl(
const AGVModel::State& state,
const PathPoint& target_point,
double desired_velocity) {
AGVModel::Control control;
control.v = desired_velocity;
// 计算目标点在车辆坐标系中的位置
double dx = target_point.x - state.x;
double dy = target_point.y - state.y;
// 转换到车辆坐标系
double cos_theta = std::cos(state.theta);
double sin_theta = std::sin(state.theta);
double target_x = cos_theta * dx + sin_theta * dy;
double target_y = -sin_theta * dx + cos_theta * dy;
// Pure Pursuit公式计算转向角
double ld = std::sqrt(target_x * target_x + target_y * target_y);
if (ld < 1e-3) {
control.delta = 0.0;
} else {
// delta = atan(2 * L * sin(alpha) / ld)
// 其中 alpha 是前视点相对于车辆航向的角度
double alpha = std::atan2(target_y, target_x);
double wheelbase = model_.getWheelbase();
control.delta = std::atan2(2.0 * wheelbase * std::sin(alpha), ld);
}
return control;
}
AGVModel::Control ControlGenerator::computeStanleyControl(
const AGVModel::State& state,
const PathPoint& nearest_point,
double k_gain,
double desired_velocity) {
AGVModel::Control control;
control.v = desired_velocity;
// 航向误差
double heading_error = normalizeAngle(nearest_point.theta - state.theta);
// 横向误差
double dx = state.x - nearest_point.x;
double dy = state.y - nearest_point.y;
double cross_track_error = -std::sin(nearest_point.theta) * dx +
std::cos(nearest_point.theta) * dy;
// Stanley控制律
// delta = heading_error + atan(k * cross_track_error / v)
double crosstrack_term = std::atan2(k_gain * cross_track_error,
std::abs(control.v) + 0.1);
control.delta = heading_error + crosstrack_term;
return control;
}
PathPoint ControlGenerator::findLookaheadPoint(const PathCurve& path,
const AGVModel::State& state,
double lookahead_distance) const {
const auto& path_points = path.getPathPoints();
if (path_points.empty()) {
return PathPoint(state.x, state.y, state.theta, 0.0);
}
// 找到最近的路径点
int nearest_idx = path.findNearestPoint(state.x, state.y);
// 如果找不到最近点，返回起点或状态点
if (nearest_idx < 0) {
return path_points[0];
}
// 从最近点开始向前搜索前视点
for (size_t i = static_cast<size_t>(nearest_idx); i < path_points.size(); ++i) {
double dx = path_points[i].x - state.x;
double dy = path_points[i].y - state.y;
double distance = std::sqrt(dx * dx + dy * dy);
if (distance >= lookahead_distance) {
return path_points[i];
}
}
// 如果没有找到，返回路径的最后一个点
return path_points.back();
}
double ControlGenerator::normalizeAngle(double angle) const {
while (angle > M_PI) angle -= 2.0 * M_PI;
while (angle < -M_PI) angle += 2.0 * M_PI;
return angle;
}

217
src/control_generator.cpp.backup2 Normal file
View File

@@ -0,0 +1,217 @@
#include "control_generator.h"
#define _USE_MATH_DEFINES
#include <cmath>
#include <algorithm>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
ControlGenerator::ControlGenerator(const AGVModel& model)
: model_(model) {
}
ControlSequence ControlGenerator::generate(const PathCurve& path,
const AGVModel::State& initial_state,
double dt, double horizon) {
// 使用Pure Pursuit作为默认算法
return generatePurePursuit(path, initial_state, dt, 1.5, 1.0, horizon);
}
ControlSequence ControlGenerator::generatePurePursuit(const PathCurve& path,
const AGVModel::State& initial_state,
double dt,
double lookahead_distance,
double desired_velocity,
double horizon) {
ControlSequence sequence;
sequence.clear();
AGVModel::State current_state = initial_state;
double current_time = 0.0;
while (current_time < horizon) {
// 找到前视点
PathPoint target = findLookaheadPoint(path, current_state, lookahead_distance);
// 计算控制量
AGVModel::Control control = computePurePursuitControl(
current_state, target, desired_velocity);
// 保存控制量和状态
sequence.controls.push_back(control);
sequence.timestamps.push_back(current_time);
sequence.predicted_states.push_back(current_state);
// 更新状态
current_state = model_.update(current_state, control, dt);
current_time += dt;
// 检查是否接近路径终点
const auto& path_points = path.getPathPoints();
if (!path_points.empty()) {
const PathPoint& end_point = path_points.back();
double dx = current_state.x - end_point.x;
double dy = current_state.y - end_point.y;
double distance_to_end = std::sqrt(dx * dx + dy * dy);
if (distance_to_end < 0.1) {
break; // 已到达终点附近
}
}
}
return sequence;
}
ControlSequence ControlGenerator::generateStanley(const PathCurve& path,
const AGVModel::State& initial_state,
double dt,
double k_gain,
double desired_velocity,
double horizon) {
ControlSequence sequence;
sequence.clear();
AGVModel::State current_state = initial_state;
double current_time = 0.0;
const auto& path_points = path.getPathPoints();
if (path_points.empty()) {
return sequence;
}
while (current_time < horizon) {
// 找到最近的路径点
int nearest_idx = path.findNearestPoint(current_state.x, current_state.y);
// 如果找不到最近点（不应该发生），使用第一个点
if (nearest_idx < 0) {
nearest_idx = 0;
}
PathPoint nearest_point = path_points[nearest_idx];
// 计算控制量
AGVModel::Control control = computeStanleyControl(
current_state, nearest_point, k_gain, desired_velocity);
// 保存控制量和状态
sequence.controls.push_back(control);
sequence.timestamps.push_back(current_time);
sequence.predicted_states.push_back(current_state);
// 更新状态
current_state = model_.update(current_state, control, dt);
current_time += dt;
// 检查是否接近路径终点
const PathPoint& end_point = path_points.back();
double dx = current_state.x - end_point.x;
double dy = current_state.y - end_point.y;
double distance_to_end = std::sqrt(dx * dx + dy * dy);
if (distance_to_end < 0.1) {
break;
}
}
return sequence;
}
AGVModel::Control ControlGenerator::computePurePursuitControl(
const AGVModel::State& state,
const PathPoint& target_point,
double desired_velocity) {
AGVModel::Control control;
control.v = desired_velocity;
// 计算目标点在车辆坐标系中的位置
double dx = target_point.x - state.x;
double dy = target_point.y - state.y;
// 转换到车辆坐标系
double cos_theta = std::cos(state.theta);
double sin_theta = std::sin(state.theta);
double target_x = cos_theta * dx + sin_theta * dy;
double target_y = -sin_theta * dx + cos_theta * dy;
// Pure Pursuit公式计算转向角
double ld = std::sqrt(target_x * target_x + target_y * target_y);
if (ld < 1e-3) {
control.delta = 0.0;
} else {
// delta = atan(2 * L * sin(alpha) / ld)
// 其中 alpha 是前视点相对于车辆航向的角度
double alpha = std::atan2(target_y, target_x);
double wheelbase = model_.getWheelbase();
control.delta = std::atan2(2.0 * wheelbase * std::sin(alpha), ld);
}
return control;
}
AGVModel::Control ControlGenerator::computeStanleyControl(
const AGVModel::State& state,
const PathPoint& nearest_point,
double k_gain,
double desired_velocity) {
AGVModel::Control control;
control.v = desired_velocity;
// 航向误差
double heading_error = normalizeAngle(nearest_point.theta - state.theta);
// 横向误差
double dx = state.x - nearest_point.x;
double dy = state.y - nearest_point.y;
double cross_track_error = -std::sin(nearest_point.theta) * dx +
std::cos(nearest_point.theta) * dy;
// Stanley控制律
// delta = heading_error + atan(k * cross_track_error / v)
double crosstrack_term = std::atan2(k_gain * cross_track_error,
std::abs(control.v) + 0.1);
control.delta = heading_error + crosstrack_term;
return control;
}
PathPoint ControlGenerator::findLookaheadPoint(const PathCurve& path,
const AGVModel::State& state,
double lookahead_distance) const {
const auto& path_points = path.getPathPoints();
if (path_points.empty()) {
return PathPoint(state.x, state.y, state.theta, 0.0);
}
// 找到最近的路径点
int nearest_idx = path.findNearestPoint(state.x, state.y);
// 如果找不到最近点，返回起点或状态点
if (nearest_idx < 0) {
return path_points[0];
}
// 从最近点开始向前搜索前视点
for (size_t i = static_cast<size_t>(nearest_idx); i < path_points.size(); ++i) {
double dx = path_points[i].x - state.x;
double dy = path_points[i].y - state.y;
double distance = std::sqrt(dx * dx + dy * dy);
if (distance >= lookahead_distance) {
return path_points[i];
}
}
// 如果没有找到，返回路径的最后一个点
return path_points.back();
}
double ControlGenerator::normalizeAngle(double angle) const {
while (angle > M_PI) angle -= 2.0 * M_PI;
while (angle < -M_PI) angle += 2.0 * M_PI;
return angle;
}

227
src/path_curve.cpp Normal file
View File

@@ -0,0 +1,227 @@
#include "path_curve.h"
#include <limits>
#include <algorithm>
#define _USE_MATH_DEFINES
#include <cmath>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
void PathCurve::generateLine(const PathPoint& start, const PathPoint& end, int num_points) {
path_points_.clear();
path_points_.reserve(num_points);
for (int i = 0; i < num_points; ++i) {
double t = static_cast<double>(i) / (num_points - 1);
PathPoint p;
p.x = start.x + t * (end.x - start.x);
p.y = start.y + t * (end.y - start.y);
p.theta = std::atan2(end.y - start.y, end.x - start.x);
p.kappa = 0.0; // 直线曲率为0
path_points_.push_back(p);
}
}
void PathCurve::generateCircleArc(double center_x, double center_y, double radius,
double start_angle, double end_angle, int num_points) {
path_points_.clear();
path_points_.reserve(num_points);
double angle_diff = end_angle - start_angle;
// 归一化角度差
while (angle_diff > M_PI) angle_diff -= 2.0 * M_PI;
while (angle_diff < -M_PI) angle_diff += 2.0 * M_PI;
for (int i = 0; i < num_points; ++i) {
double t = static_cast<double>(i) / (num_points - 1);
double angle = start_angle + t * angle_diff;
PathPoint p;
p.x = center_x + radius * std::cos(angle);
p.y = center_y + radius * std::sin(angle);
// 切线方向垂直于半径方向
if (angle_diff > 0) {
p.theta = angle + M_PI / 2.0;
} else {
p.theta = angle - M_PI / 2.0;
}
// 圆的曲率是半径的倒数
p.kappa = 1.0 / radius;
if (angle_diff < 0) p.kappa = -p.kappa;
path_points_.push_back(p);
}
}
void PathCurve::generateCubicBezier(const PathPoint& p0, const PathPoint& p1,
const PathPoint& p2, const PathPoint& p3,
int num_points) {
path_points_.clear();
path_points_.reserve(num_points);
for (int i = 0; i < num_points; ++i) {
double t = static_cast<double>(i) / (num_points - 1);
double t2 = t * t;
double t3 = t2 * t;
double mt = 1.0 - t;
double mt2 = mt * mt;
double mt3 = mt2 * mt;
// 贝塞尔曲线公式
PathPoint p;
p.x = mt3 * p0.x + 3.0 * mt2 * t * p1.x + 3.0 * mt * t2 * p2.x + t3 * p3.x;
p.y = mt3 * p0.y + 3.0 * mt2 * t * p1.y + 3.0 * mt * t2 * p2.y + t3 * p3.y;
// 一阶导数（切线方向）
double dx = 3.0 * mt2 * (p1.x - p0.x) + 6.0 * mt * t * (p2.x - p1.x) +
3.0 * t2 * (p3.x - p2.x);
double dy = 3.0 * mt2 * (p1.y - p0.y) + 6.0 * mt * t * (p2.y - p1.y) +
3.0 * t2 * (p3.y - p2.y);
p.theta = std::atan2(dy, dx);
// 二阶导数（用于计算曲率）
double ddx = 6.0 * mt * (p2.x - 2.0 * p1.x + p0.x) +
6.0 * t * (p3.x - 2.0 * p2.x + p1.x);
double ddy = 6.0 * mt * (p2.y - 2.0 * p1.y + p0.y) +
6.0 * t * (p3.y - 2.0 * p2.y + p1.y);
// 曲率公式 κ = (x'y'' - y'x'') / (x'^2 + y'^2)^(3/2)
double velocity_squared = dx * dx + dy * dy;
if (velocity_squared > 1e-6) {
p.kappa = (dx * ddy - dy * ddx) / std::pow(velocity_squared, 1.5);
} else {
p.kappa = 0.0;
}
path_points_.push_back(p);
}
}
void PathCurve::setPathPoints(const std::vector<PathPoint>& points) {
path_points_ = points;
// 计算每个点的切线方向和曲率
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]);
}
}
// 修复: 单点情况保持原有的theta和kappa值（通常为0），避免越界访问
}
}
PathPoint PathCurve::getPointAt(double t) const {
if (path_points_.empty()) {
return PathPoint();
}
if (path_points_.size() == 1) {
return path_points_[0];
}
// 限制t在[0, 1]范围内
t = std::max(0.0, std::min(1.0, t));
double index_float = t * (path_points_.size() - 1);
size_t index = static_cast<size_t>(index_float);
if (index >= path_points_.size() - 1) {
return path_points_.back();
}
// 线性插值
double alpha = index_float - index;
const PathPoint& p1 = path_points_[index];
const PathPoint& p2 = path_points_[index + 1];
PathPoint result;
result.x = p1.x + alpha * (p2.x - p1.x);
result.y = p1.y + alpha * (p2.y - p1.y);
result.theta = p1.theta + alpha * (p2.theta - p1.theta);
result.kappa = p1.kappa + alpha * (p2.kappa - p1.kappa);
return result;
}
double PathCurve::getPathLength() const {
double length = 0.0;
for (size_t i = 1; i < path_points_.size(); ++i) {
double dx = path_points_[i].x - path_points_[i-1].x;
double dy = path_points_[i].y - path_points_[i-1].y;
length += std::sqrt(dx * dx + dy * dy);
}
return length;
}
int PathCurve::findNearestPoint(double x, double y) const {
if (path_points_.empty()) {
return -1;
}
int nearest_index = 0;
double min_distance = std::numeric_limits<double>::max();
for (size_t i = 0; i < path_points_.size(); ++i) {
double dx = x - path_points_[i].x;
double dy = y - path_points_[i].y;
double distance = std::sqrt(dx * dx + dy * dy);
if (distance < min_distance) {
min_distance = distance;
nearest_index = static_cast<int>(i);
}
}
return nearest_index;
}
double PathCurve::computeCurvature(const PathPoint& p1, const PathPoint& p2,
const PathPoint& p3) const {
// 使用三点计算曲率
double dx1 = p2.x - p1.x;
double dy1 = p2.y - p1.y;
double dx2 = p3.x - p2.x;
double dy2 = p3.y - p2.y;
double cross = dx1 * dy2 - dy1 * dx2;
double d1 = std::sqrt(dx1 * dx1 + dy1 * dy1);
double d2 = std::sqrt(dx2 * dx2 + dy2 * dy2);
if (d1 < 1e-6 || d2 < 1e-6) {
return 0.0;
}
// Menger曲率公式
double area = std::abs(cross) / 2.0;
double d3_sq = (p3.x - p1.x) * (p3.x - p1.x) + (p3.y - p1.y) * (p3.y - p1.y);
double d3 = std::sqrt(d3_sq);
if (d3 < 1e-6) {
return 0.0;
}
return 4.0 * area / (d1 * d2 * d3);
}

226
src/path_curve.cpp.backup Normal file
View File

@@ -0,0 +1,226 @@
#include "path_curve.h"
#include <limits>
#include <algorithm>
#define _USE_MATH_DEFINES
#include <cmath>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
void PathCurve::generateLine(const PathPoint& start, const PathPoint& end, int num_points) {
path_points_.clear();
path_points_.reserve(num_points);
for (int i = 0; i < num_points; ++i) {
double t = static_cast<double>(i) / (num_points - 1);
PathPoint p;
p.x = start.x + t * (end.x - start.x);
p.y = start.y + t * (end.y - start.y);
p.theta = std::atan2(end.y - start.y, end.x - start.x);
p.kappa = 0.0; // 直线曲率为0
path_points_.push_back(p);
}
}
void PathCurve::generateCircleArc(double center_x, double center_y, double radius,
double start_angle, double end_angle, int num_points) {
path_points_.clear();
path_points_.reserve(num_points);
double angle_diff = end_angle - start_angle;
// 归一化角度差
while (angle_diff > M_PI) angle_diff -= 2.0 * M_PI;
while (angle_diff < -M_PI) angle_diff += 2.0 * M_PI;
for (int i = 0; i < num_points; ++i) {
double t = static_cast<double>(i) / (num_points - 1);
double angle = start_angle + t * angle_diff;
PathPoint p;
p.x = center_x + radius * std::cos(angle);
p.y = center_y + radius * std::sin(angle);
// 切线方向垂直于半径方向
if (angle_diff > 0) {
p.theta = angle + M_PI / 2.0;
} else {
p.theta = angle - M_PI / 2.0;
}
// 圆的曲率是半径的倒数
p.kappa = 1.0 / radius;
if (angle_diff < 0) p.kappa = -p.kappa;
path_points_.push_back(p);
}
}
void PathCurve::generateCubicBezier(const PathPoint& p0, const PathPoint& p1,
const PathPoint& p2, const PathPoint& p3,
int num_points) {
path_points_.clear();
path_points_.reserve(num_points);
for (int i = 0; i < num_points; ++i) {
double t = static_cast<double>(i) / (num_points - 1);
double t2 = t * t;
double t3 = t2 * t;
double mt = 1.0 - t;
double mt2 = mt * mt;
double mt3 = mt2 * mt;
// 贝塞尔曲线公式
PathPoint p;
p.x = mt3 * p0.x + 3.0 * mt2 * t * p1.x + 3.0 * mt * t2 * p2.x + t3 * p3.x;
p.y = mt3 * p0.y + 3.0 * mt2 * t * p1.y + 3.0 * mt * t2 * p2.y + t3 * p3.y;
// 一阶导数（切线方向）
double dx = 3.0 * mt2 * (p1.x - p0.x) + 6.0 * mt * t * (p2.x - p1.x) +
3.0 * t2 * (p3.x - p2.x);
double dy = 3.0 * mt2 * (p1.y - p0.y) + 6.0 * mt * t * (p2.y - p1.y) +
3.0 * t2 * (p3.y - p2.y);
p.theta = std::atan2(dy, dx);
// 二阶导数（用于计算曲率）
double ddx = 6.0 * mt * (p2.x - 2.0 * p1.x + p0.x) +
6.0 * t * (p3.x - 2.0 * p2.x + p1.x);
double ddy = 6.0 * mt * (p2.y - 2.0 * p1.y + p0.y) +
6.0 * t * (p3.y - 2.0 * p2.y + p1.y);
// 曲率公式 κ = (x'y'' - y'x'') / (x'^2 + y'^2)^(3/2)
double velocity_squared = dx * dx + dy * dy;
if (velocity_squared > 1e-6) {
p.kappa = (dx * ddy - dy * ddx) / std::pow(velocity_squared, 1.5);
} else {
p.kappa = 0.0;
}
path_points_.push_back(p);
}
}
void PathCurve::setPathPoints(const std::vector<PathPoint>& points) {
path_points_ = points;
// 计算每个点的切线方向和曲率
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]);
}
}
}
}
PathPoint PathCurve::getPointAt(double t) const {
if (path_points_.empty()) {
return PathPoint();
}
if (path_points_.size() == 1) {
return path_points_[0];
}
// 限制t在[0, 1]范围内
t = std::max(0.0, std::min(1.0, t));
double index_float = t * (path_points_.size() - 1);
size_t index = static_cast<size_t>(index_float);
if (index >= path_points_.size() - 1) {
return path_points_.back();
}
// 线性插值
double alpha = index_float - index;
const PathPoint& p1 = path_points_[index];
const PathPoint& p2 = path_points_[index + 1];
PathPoint result;
result.x = p1.x + alpha * (p2.x - p1.x);
result.y = p1.y + alpha * (p2.y - p1.y);
result.theta = p1.theta + alpha * (p2.theta - p1.theta);
result.kappa = p1.kappa + alpha * (p2.kappa - p1.kappa);
return result;
}
double PathCurve::getPathLength() const {
double length = 0.0;
for (size_t i = 1; i < path_points_.size(); ++i) {
double dx = path_points_[i].x - path_points_[i-1].x;
double dy = path_points_[i].y - path_points_[i-1].y;
length += std::sqrt(dx * dx + dy * dy);
}
return length;
}
int PathCurve::findNearestPoint(double x, double y) const {
if (path_points_.empty()) {
return -1;
}
int nearest_index = 0;
double min_distance = std::numeric_limits<double>::max();
for (size_t i = 0; i < path_points_.size(); ++i) {
double dx = x - path_points_[i].x;
double dy = y - path_points_[i].y;
double distance = std::sqrt(dx * dx + dy * dy);
if (distance < min_distance) {
min_distance = distance;
nearest_index = static_cast<int>(i);
}
}
return nearest_index;
}
double PathCurve::computeCurvature(const PathPoint& p1, const PathPoint& p2,
const PathPoint& p3) const {
// 使用三点计算曲率
double dx1 = p2.x - p1.x;
double dy1 = p2.y - p1.y;
double dx2 = p3.x - p2.x;
double dy2 = p3.y - p2.y;
double cross = dx1 * dy2 - dy1 * dx2;
double d1 = std::sqrt(dx1 * dx1 + dy1 * dy1);
double d2 = std::sqrt(dx2 * dx2 + dy2 * dy2);
if (d1 < 1e-6 || d2 < 1e-6) {
return 0.0;
}
// Menger曲率公式
double area = std::abs(cross) / 2.0;
double d3_sq = (p3.x - p1.x) * (p3.x - p1.x) + (p3.y - p1.y) * (p3.y - p1.y);
double d3 = std::sqrt(d3_sq);
if (d3 < 1e-6) {
return 0.0;
}
return 4.0 * area / (d1 * d2 * d3);
}

191
src/path_curve_custom.cpp Normal file
View File

@@ -0,0 +1,191 @@
#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& e) {
std::cerr << "Error parsing line " << line_num << ": " << line << " (" << e.what() << ")" << 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;
}

190
src/path_curve_custom.cpp.backup Normal file
View File

@@ -0,0 +1,190 @@
#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;
}

133
src/path_tracker.cpp Normal file
View File

@@ -0,0 +1,133 @@
#include "path_tracker.h"
#include <iostream>
#include <iomanip>
#define _USE_MATH_DEFINES
#include <cmath>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
PathTracker::PathTracker(const AGVModel& model)
: model_(model)
, control_generator_(model)
, initial_state_(0.0, 0.0, 0.0) {
}
void PathTracker::setReferencePath(const PathCurve& path) {
reference_path_ = path;
}
void PathTracker::setInitialState(const AGVModel::State& state) {
initial_state_ = state;
}
bool PathTracker::generateControlSequence(const std::string& algorithm,
double dt, double horizon,
double desired_velocity) {
if (reference_path_.getPathPoints().empty()) {
std::cerr << "Error: Reference path is empty!" << std::endl;
return false;
}
if (algorithm == "pure_pursuit") {
// 修复: 自适应前视距离 = 速度 × 2.0，最小1.0米
double lookahead = std::max(1.0, desired_velocity * 2.0);
control_sequence_ = control_generator_.generatePurePursuit(
reference_path_, initial_state_, dt, lookahead, desired_velocity, horizon);
} else if (algorithm == "stanley") {
// 修复: 增加k_gain到2.0以提高响应性
control_sequence_ = control_generator_.generateStanley(
reference_path_, initial_state_, dt, 2.0, desired_velocity, horizon);
} else {
std::cerr << "Error: Unknown algorithm type \"" << algorithm << "\"" << std::endl;
return false;
}
return true;
}
void PathTracker::printControlSequence() const {
if (control_sequence_.size() == 0) {
std::cout << "Control sequence is empty!" << std::endl;
return;
}
std::cout << "\n========== Control Sequence ==========" << std::endl;
std::cout << std::fixed << std::setprecision(4);
std::cout << std::setw(8) << "Time(s)"
<< std::setw(12) << "Velocity(m/s)"
<< std::setw(15) << "Steering(rad)"
<< std::setw(15) << "Steering(deg)" << std::endl;
std::cout << std::string(50, '-') << std::endl;
for (size_t i = 0; i < control_sequence_.size(); ++i) {
double time = control_sequence_.timestamps[i];
double velocity = control_sequence_.controls[i].v;
double steering_rad = control_sequence_.controls[i].delta;
double steering_deg = steering_rad * 180.0 / M_PI;
std::cout << std::setw(8) << time
<< std::setw(12) << velocity
<< std::setw(15) << steering_rad
<< std::setw(15) << steering_deg << std::endl;
}
std::cout << "=============================" << std::endl;
std::cout << "Total control steps: " << control_sequence_.size() << std::endl;
}
bool PathTracker::saveControlSequence(const std::string& filename) const {
std::ofstream file(filename);
if (!file.is_open()) {
std::cerr << "Unable to open file: " << filename << std::endl;
return false;
}
file << "# AGV Control Sequence" << std::endl;
file << "# Time(s), Velocity(m/s), Steering(rad), Steering(deg)" << std::endl;
file << std::fixed << std::setprecision(6);
for (size_t i = 0; i < control_sequence_.size(); ++i) {
double time = control_sequence_.timestamps[i];
double velocity = control_sequence_.controls[i].v;
double steering_rad = control_sequence_.controls[i].delta;
double steering_deg = steering_rad * 180.0 / M_PI;
file << time << ", "
<< velocity << ", "
<< steering_rad << ", "
<< steering_deg << std::endl;
}
file.close();
std::cout << "Control sequence saved to: " << filename << std::endl;
return true;
}
bool PathTracker::saveTrajectory(const std::string& filename) const {
std::ofstream file(filename);
if (!file.is_open()) {
std::cerr << "Unable to open file: " << filename << std::endl;
return false;
}
file << "# AGV Predicted Trajectory" << std::endl;
file << "# x(m), y(m), theta(rad), theta(deg)" << std::endl;
file << std::fixed << std::setprecision(6);
for (size_t i = 0; i < control_sequence_.predicted_states.size(); ++i) {
const auto& state = control_sequence_.predicted_states[i];
double theta_deg = state.theta * 180.0 / M_PI;
file << state.x << ", "
<< state.y << ", "
<< state.theta << ", "
<< theta_deg << std::endl;
}
file.close();
std::cout << "Trajectory saved to: " << filename << std::endl;
return true;
}

129
src/path_tracker.cpp.backup3 Normal file
View File

@@ -0,0 +1,129 @@
#include "path_tracker.h"
#include <iostream>
#include <iomanip>
#define _USE_MATH_DEFINES
#include <cmath>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
PathTracker::PathTracker(const AGVModel& model)
: model_(model)
, control_generator_(model)
, initial_state_(0.0, 0.0, 0.0) {
}
void PathTracker::setReferencePath(const PathCurve& path) {
reference_path_ = path;
}
void PathTracker::setInitialState(const AGVModel::State& state) {
initial_state_ = state;
}
bool PathTracker::generateControlSequence(const std::string& algorithm,
double dt, double horizon) {
if (reference_path_.getPathPoints().empty()) {
std::cerr << "Error: Reference path is empty!" << std::endl;
return false;
}
if (algorithm == "pure_pursuit") {
control_sequence_ = control_generator_.generatePurePursuit(
reference_path_, initial_state_, dt, 1.5, 1.0, horizon);
} else if (algorithm == "stanley") {
control_sequence_ = control_generator_.generateStanley(
reference_path_, initial_state_, dt, 1.0, 1.0, horizon);
} else {
std::cerr << "Error: Unknown algorithm type \"" << algorithm << "\"" << std::endl;
return false;
}
return true;
}
void PathTracker::printControlSequence() const {
if (control_sequence_.size() == 0) {
std::cout << "Control sequence is empty!" << std::endl;
return;
}
std::cout << "\n========== Control Sequence ==========" << std::endl;
std::cout << std::fixed << std::setprecision(4);
std::cout << std::setw(8) << "Time(s)"
<< std::setw(12) << "Velocity(m/s)"
<< std::setw(15) << "Steering(rad)"
<< std::setw(15) << "Steering(deg)" << std::endl;
std::cout << std::string(50, '-') << std::endl;
for (size_t i = 0; i < control_sequence_.size(); ++i) {
double time = control_sequence_.timestamps[i];
double velocity = control_sequence_.controls[i].v;
double steering_rad = control_sequence_.controls[i].delta;
double steering_deg = steering_rad * 180.0 / M_PI;
std::cout << std::setw(8) << time
<< std::setw(12) << velocity
<< std::setw(15) << steering_rad
<< std::setw(15) << steering_deg << std::endl;
}
std::cout << "=============================" << std::endl;
std::cout << "Total control steps: " << control_sequence_.size() << std::endl;
}
bool PathTracker::saveControlSequence(const std::string& filename) const {
std::ofstream file(filename);
if (!file.is_open()) {
std::cerr << "Unable to open file: " << filename << std::endl;
return false;
}
file << "# AGV Control Sequence" << std::endl;
file << "# Time(s), Velocity(m/s), Steering(rad), Steering(deg)" << std::endl;
file << std::fixed << std::setprecision(6);
for (size_t i = 0; i < control_sequence_.size(); ++i) {
double time = control_sequence_.timestamps[i];
double velocity = control_sequence_.controls[i].v;
double steering_rad = control_sequence_.controls[i].delta;
double steering_deg = steering_rad * 180.0 / M_PI;
file << time << ", "
<< velocity << ", "
<< steering_rad << ", "
<< steering_deg << std::endl;
}
file.close();
std::cout << "Control sequence saved to: " << filename << std::endl;
return true;
}
bool PathTracker::saveTrajectory(const std::string& filename) const {
std::ofstream file(filename);
if (!file.is_open()) {
std::cerr << "Unable to open file: " << filename << std::endl;
return false;
}
file << "# AGV Predicted Trajectory" << std::endl;
file << "# x(m), y(m), theta(rad), theta(deg)" << std::endl;
file << std::fixed << std::setprecision(6);
for (size_t i = 0; i < control_sequence_.predicted_states.size(); ++i) {
const auto& state = control_sequence_.predicted_states[i];
double theta_deg = state.theta * 180.0 / M_PI;
file << state.x << ", "
<< state.y << ", "
<< state.theta << ", "
<< theta_deg << std::endl;
}
file.close();
std::cout << "Trajectory saved to: " << filename << std::endl;
return true;
}

29
src/tests/test_csv_load.cpp Normal file
View File

@@ -0,0 +1,29 @@
#include "include/path_curve.h"
#include <iostream>
int main() {
std::cout << "Testing CSV loading..." << std::endl;
PathCurve path;
std::cout << "Attempting to load smooth_path_arc.csv..." << std::endl;
bool success = path.loadFromCSV("smooth_path_arc.csv", true);
if (success) {
std::cout << "CSV loaded successfully!" << std::endl;
const auto& points = path.getPathPoints();
std::cout << "Total points: " << points.size() << std::endl;
if (!points.empty()) {
std::cout << "First point: (" << points[0].x << ", " << points[0].y
<< ", " << points[0].theta << ", " << points[0].kappa << ")" << std::endl;
std::cout << "Last point: (" << points.back().x << ", " << points.back().y
<< ", " << points.back().theta << ", " << points.back().kappa << ")" << std::endl;
}
} else {
std::cout << "Failed to load CSV!" << std::endl;
return 1;
}
return 0;
}