134 lines
4.7 KiB
C
134 lines
4.7 KiB
C
|
/**
|
|||
|
|
* @file nav_script.h
|
|||
|
|
* @brief 段脚本执行器：编排完整比赛流程
|
|||
|
|
*
|
|||
|
|
* 比赛流程模型：
|
|||
|
|
* ┌─────────────┐
|
|||
|
|
* │ ENTRY_ALIGN │ 入口对准：启动区 -> 第一垄沟入口
|
|||
|
|
* └──────┬──────┘
|
|||
|
|
* │
|
|||
|
|
* v
|
|||
|
|
* ┌─────────────┐
|
|||
|
|
* │ CORRIDOR_A │ 第一垄沟跟踪
|
|||
|
|
* └──────┬──────┘
|
|||
|
|
* │ (到端)
|
|||
|
|
* v
|
|||
|
|
* ┌─────────────┐
|
|||
|
|
* │ TURN_180 │ 原地 180 度转向
|
|||
|
|
* └──────┬──────┘
|
|||
|
|
* │
|
|||
|
|
* v
|
|||
|
|
* ┌─────────────┐
|
|||
|
|
* │ CORRIDOR_B │ 返回垄沟跟踪
|
|||
|
|
* └──────┬──────┘
|
|||
|
|
* │ (到端)
|
|||
|
|
* v
|
|||
|
|
* ┌─────────────┐
|
|||
|
|
* │ EXIT │ 退出比赛场地
|
|||
|
|
* └──────┬──────┘
|
|||
|
|
* │
|
|||
|
|
* v
|
|||
|
|
* ┌─────────────┐
|
|||
|
|
* │ FINISH │ 停在启动区
|
|||
|
|
* └─────────────┘
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
#ifndef NAV_SCRIPT_H
|
|||
|
|
#define NAV_SCRIPT_H
|
|||
|
|
|
|||
|
|
#include <stdint.h>
|
|||
|
|
#include <stdbool.h>
|
|||
|
|
#include "preproc/corridor_msgs.h"
|
|||
|
|
#include "preproc/corridor_preproc.h"
|
|||
|
|
|
|||
|
|
#ifdef __cplusplus
|
|||
|
|
extern "C" {
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
/* =========================================================
|
|||
|
|
* 段脚本阶段枚举
|
|||
|
|
* ========================================================= */
|
|||
|
|
typedef enum {
|
|||
|
|
SCRIPT_STAGE_IDLE = 0, // 待命阶段，不输出任何指令
|
|||
|
|
SCRIPT_STAGE_ENTRY_ALIGN, // 入口对准：从启动区到第一垄沟
|
|||
|
|
SCRIPT_STAGE_CORRIDOR_FORWARD, // 向前走垄沟
|
|||
|
|
SCRIPT_STAGE_TURN_AT_END, // 到端原地转向
|
|||
|
|
SCRIPT_STAGE_CORRIDOR_BACKWARD,// 向后走垄沟
|
|||
|
|
SCRIPT_STAGE_EXIT, // 退出场地
|
|||
|
|
SCRIPT_STAGE_FINISHED, // 比赛完成，安全停车
|
|||
|
|
} NavScriptStage_t;
|
|||
|
|
|
|||
|
|
/* =========================================================
|
|||
|
|
* 段脚本配置参数
|
|||
|
|
* ========================================================= */
|
|||
|
|
typedef struct {
|
|||
|
|
float turn_target_angle; // 原地转向目标角度 (rad)，默认 PI (180度)
|
|||
|
|
float turn_omega; // 原地转向角速度 (rad/s)
|
|||
|
|
float corridor_length; // 垄沟长度估计 (m)，用于触发到端（备用）
|
|||
|
|
float entry_align_timeout; // 入口对准超时 (ms)
|
|||
|
|
float d_entry_exit_front; // 出入口前向距离阈值 (m)
|
|||
|
|
float entry_align_v; // 入口对准慢速前进速度 (m/s)
|
|||
|
|
float exit_runout_m; // 离开走廊后继续冲出的距离 (m)
|
|||
|
|
float exit_v; // 退出场地直线冲出速度 (m/s)
|
|||
|
|
} NavScriptConfig_t;
|
|||
|
|
|
|||
|
|
/* =========================================================
|
|||
|
|
* 段脚本输出
|
|||
|
|
* ========================================================= */
|
|||
|
|
typedef struct {
|
|||
|
|
NavScriptStage_t stage; // 当前执行阶段
|
|||
|
|
const char *stage_name; // 阶段名称字符串
|
|||
|
|
bool active; // 脚本是否正在运行
|
|||
|
|
bool request_corridor; // 是否请求走廊控制器
|
|||
|
|
float override_v; // 覆盖输出的 v (m/s)
|
|||
|
|
float override_w; // 覆盖输出的 w (rad/s)
|
|||
|
|
bool use_override; // 是否使用覆盖值
|
|||
|
|
} NavScriptOutput_t;
|
|||
|
|
|
|||
|
|
/* =========================================================
|
|||
|
|
* API 接口
|
|||
|
|
* ========================================================= */
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* @brief 初始化段脚本执行器
|
|||
|
|
*/
|
|||
|
|
void NavScript_Init(const NavScriptConfig_t *config);
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* @brief 开始运行比赛脚本
|
|||
|
|
*/
|
|||
|
|
void NavScript_Start(void);
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* @brief 核心执行函数：每个导航周期调用一次
|
|||
|
|
* @param obs 预处理层的观测快照
|
|||
|
|
* @param state 滤波器的走廊状态
|
|||
|
|
* @param imu_yaw_continuous_deg IMU unwrap 后的连续偏航角 (度)，用于转弯判定
|
|||
|
|
* @param out 段脚本输出
|
|||
|
|
*/
|
|||
|
|
void NavScript_Update(const CorridorObs_t *obs,
|
|||
|
|
const CorridorState_t *state,
|
|||
|
|
float imu_yaw_continuous_deg,
|
|||
|
|
NavScriptOutput_t *out);
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* @brief 获取当前执行阶段
|
|||
|
|
*/
|
|||
|
|
NavScriptStage_t NavScript_GetStage(void);
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* @brief 获取阶段名称
|
|||
|
|
*/
|
|||
|
|
const char* NavScript_GetStageName(NavScriptStage_t stage);
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* @brief 强制重置脚本到 IDLE
|
|||
|
|
*/
|
|||
|
|
void NavScript_Reset(void);
|
|||
|
|
|
|||
|
|
#ifdef __cplusplus
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#endif // NAV_SCRIPT_H
|