fu 优化绕障和文件结构

src/autonomy/fu/config/fu_params.yaml

@@ -10,7 +10,7 @@ FU:
     enable_visualize_grid: False  
 
     # 车前必停区行数 int
-    FRONT_STOP_ZONE_ROWS: 3  
+    FRONT_STOP_ZONE_ROWS: 2  
 
     # 车后必停区行数 int
     REAR_STOP_ZONE_ROWS: 3  

src/autonomy/fu/include/fu/fu_params.h

@@ -0,0 +1,98 @@
+#ifndef FU_PARAMS_H
+#define FU_PARAMS_H
+
+#include "rclcpp/rclcpp.hpp"
+
+// ================== FU 参数配置 ==================
+struct FuParams
+{
+    // 功能开关
+    bool enable_obstacle_stop = true;
+    bool enable_obstacle_avoid = true;
+    bool enable_visualize_grid = true;
+
+    // 安全区尺寸(栅格数)
+    int front_stop_zone_rows = 3;
+    int rear_stop_zone_rows = 1;
+    int left_stop_zone_cols = 3;
+    int right_stop_zone_cols = 3;
+
+    // 扩展检测区域(栅格数)
+    int front_area_extend = 7;
+    int left_area_extend = 2;
+    int right_area_extend = 2;
+
+    // 绕障参数
+    double waypoint_tolerance = 2.0;
+    int lateral_offset = 2;
+    int parallel_distance = 3;
+    int avoid_able_width = 6;
+    int min_free_space = 6;
+    int uss_stop_distance = 200;
+
+    // ======== 从 ROS 节点声明并读取参数 ========
+    void declareAndLoad(rclcpp::Node* node)
+    {
+        node->declare_parameter("enable_obstacle_stop", enable_obstacle_stop);
+        node->declare_parameter("enable_obstacle_avoid", enable_obstacle_avoid);
+        node->declare_parameter("enable_visualize_grid", enable_visualize_grid);
+        node->declare_parameter("FRONT_STOP_ZONE_ROWS", front_stop_zone_rows);
+        node->declare_parameter("REAR_STOP_ZONE_ROWS", rear_stop_zone_rows);
+        node->declare_parameter("LEFT_STOP_ZONE_COLS", left_stop_zone_cols);
+        node->declare_parameter("RIGHT_STOP_ZONE_COLS", right_stop_zone_cols);
+        node->declare_parameter("front_area_extend", front_area_extend);
+        node->declare_parameter("left_area_extend", left_area_extend);
+        node->declare_parameter("right_area_extend", right_area_extend);
+        node->declare_parameter("waypoint_tolerance", waypoint_tolerance);
+        node->declare_parameter("lateral_offset", lateral_offset);
+        node->declare_parameter("parallel_distance", parallel_distance);
+        node->declare_parameter("avoid_able_width", avoid_able_width);
+        node->declare_parameter("min_free_space", min_free_space);
+        node->declare_parameter("uss_stop_distance", uss_stop_distance);
+
+        node->get_parameter("enable_obstacle_stop", enable_obstacle_stop);
+        node->get_parameter("enable_obstacle_avoid", enable_obstacle_avoid);
+        node->get_parameter("enable_visualize_grid", enable_visualize_grid);
+        node->get_parameter("FRONT_STOP_ZONE_ROWS", front_stop_zone_rows);
+        node->get_parameter("REAR_STOP_ZONE_ROWS", rear_stop_zone_rows);
+        node->get_parameter("LEFT_STOP_ZONE_COLS", left_stop_zone_cols);
+        node->get_parameter("RIGHT_STOP_ZONE_COLS", right_stop_zone_cols);
+        node->get_parameter("front_area_extend", front_area_extend);
+        node->get_parameter("left_area_extend", left_area_extend);
+        node->get_parameter("right_area_extend", right_area_extend);
+        node->get_parameter("waypoint_tolerance", waypoint_tolerance);
+        node->get_parameter("lateral_offset", lateral_offset);
+        node->get_parameter("parallel_distance", parallel_distance);
+        node->get_parameter("avoid_able_width", avoid_able_width);
+        node->get_parameter("min_free_space", min_free_space);
+        node->get_parameter("uss_stop_distance", uss_stop_distance);
+    }
+
+    // ======== 处理参数动态更新 ========
+    bool handleUpdate(const rclcpp::Parameter& param)
+    {
+        if (param.get_name() == "enable_obstacle_stop")
+            enable_obstacle_stop = param.as_bool();
+        else if (param.get_name() == "enable_obstacle_avoid")
+            enable_obstacle_avoid = param.as_bool();
+        else if (param.get_name() == "enable_visualize_grid")
+            enable_visualize_grid = param.as_bool();
+        else if (param.get_name() == "waypoint_tolerance")
+            waypoint_tolerance = param.as_double();
+        else if (param.get_name() == "lateral_offset")
+            lateral_offset = param.as_int();
+        else if (param.get_name() == "parallel_distance")
+            parallel_distance = param.as_int();
+        else if (param.get_name() == "avoid_able_width")
+            avoid_able_width = param.as_int();
+        else if (param.get_name() == "min_free_space")
+            min_free_space = param.as_int();
+        else if (param.get_name() == "uss_stop_distance")
+            uss_stop_distance = param.as_int();
+        else
+            return false;  // 未知参数
+        return true;
+    }
+};
+
+#endif  // FU_PARAMS_H

src/autonomy/fu/include/fu/fu_types.h

@@ -0,0 +1,137 @@
+#ifndef FU_TYPES_H
+#define FU_TYPES_H
+
+#include <algorithm>
+#include <cmath>
+#include <string>
+#include <vector>
+
+// ================== 栅格单元类型 ==================
+constexpr int EMPTY = 0;
+constexpr int OBSTACLE = 1;
+constexpr int VEHICLE = 2;
+
+// ================== GPS坐标系常数 ==================
+constexpr double LAT_ORIGIN = 32.045062;
+constexpr double LON_ORIGIN = 118.845200366;
+constexpr double DEG_RAD = 0.01745329252;
+constexpr double R_LATI = 6378137.0;
+constexpr double R_LONT = 5407872.0;
+
+// ================== RTK 数据 ==================
+struct RtkData
+{
+    double lon = 0.0;
+    double lat = 0.0;
+    double direction = 0.0;
+    double speed = 0.0;
+    int reliability = 0;
+};
+
+// ================== 路点结构体 ==================
+struct Waypoint
+{
+    double x = 0.0;
+    double y = 0.0;
+    int speed = 300;
+    double lon = 0.0;
+    double lat = 0.0;
+
+    Waypoint() = default;
+    Waypoint(double px, double py, int s = 300, double plon = 0, double plat = 0)
+        : x(px), y(py), speed(s), lon(plon), lat(plat)
+    {
+    }
+};
+
+// ================== 安全区结构体 ==================
+struct SafetyZone
+{
+    int min_x = 0, max_x = 0, min_y = 0, max_y = 0;
+};
+
+// ================== 障碍物分析结果 ==================
+struct ObstacleAnalysis
+{
+    bool has_front_critical = false;
+    bool has_front_area = false;
+    bool has_left_critical = false;
+    bool has_right_critical = false;
+
+    int obstacle_distance = -1;
+    int obstacle_width = 0;
+    int left_edge = -1, right_edge = -1;
+    int free_space_left = 0;
+    int free_space_right = 0;
+
+    int calculateSafeDistance(int speed) const { return std::max(3, speed / 400 + 2); }
+
+    bool needEmergencyBrake(int speed) const
+    {
+        if (!has_front_critical) return false;
+        return obstacle_distance <= calculateSafeDistance(speed);
+    }
+};
+
+// ================== GPS工具函数 ==================
+
+// 计算两点间GPS距离(米)
+inline double gps_distance(double lon1, double lat1, double lon2, double lat2)
+{
+    double x = R_LONT * (lon2 - lon1) * DEG_RAD;
+    double y = R_LATI * (lat2 - lat1) * DEG_RAD;
+    return std::sqrt(x * x + y * y);
+}
+
+// GPS转相对坐标(米)
+// cur_heading: 当前航向角(度)
+// cur_lon, cur_lat: 当前位置
+// dest_lon, dest_lat: 目标位置
+// out_x, out_y: 输出相对坐标(车体坐标系, x=横向, y=纵向)
+inline void gps_to_relative(double cur_heading, double cur_lon, double cur_lat, double dest_lon, double dest_lat,
+                            double* out_x, double* out_y)
+{
+    double nav_rad = cur_heading * DEG_RAD;
+    double dx = (dest_lon - cur_lon) * DEG_RAD * R_LONT;
+    double dy = (dest_lat - cur_lat) * DEG_RAD * R_LATI;
+
+    double cos_h = std::cos(nav_rad);
+    double sin_h = std::sin(nav_rad);
+
+    *out_x = dx * cos_h + dy * (-sin_h);
+    *out_y = dx * sin_h + dy * cos_h;
+}
+
+// 相对坐标转GPS绝对坐标
+// rel_x, rel_y: 车体坐标系下的偏移(米)
+// cur_heading, cur_lon, cur_lat: 当前位姿
+// out_lon, out_lat: 输出GPS坐标
+inline void relative_to_gps(double rel_x, double rel_y, double cur_heading, double cur_lon, double cur_lat,
+                            double* out_lon, double* out_lat)
+{
+    double nav_rad = cur_heading * DEG_RAD;
+    double cos_h = std::cos(nav_rad);
+    double sin_h = std::sin(nav_rad);
+
+    // 逆向旋转：车体坐标系 -> 地理坐标系
+    double delta_x = rel_x * cos_h + rel_y * (-sin_h);
+    double delta_y = rel_x * sin_h + rel_y * cos_h;
+
+    *out_lon = cur_lon + delta_x / (DEG_RAD * R_LONT);
+    *out_lat = cur_lat + delta_y / (DEG_RAD * R_LATI);
+}
+
+// ================== 角度计算工具 ==================
+
+// 计算转向角：将目标相对坐标转为方向盘角度
+// target_x: 横向偏移(正=右), target_y: 纵向距离(正=前)
+// 返回: 方向盘角度 [-50, 50] 度, 90度为正前方
+inline float calculate_steering_angle(double target_x, double target_y)
+{
+    if (target_x == 0) return 90.0f;
+    float angle_f = std::atan2(target_y, target_x) * 180.0f / M_PI;
+    float steering = (angle_f <= 90.0f) ? 90.0f - angle_f : -(angle_f - 90.0f);
+    return std::clamp(steering, -50.0f, 50.0f);
+}
+
+#endif  // FU_TYPES_H

src/autonomy/fu/include/fu/state_machine.h

@@ -0,0 +1,90 @@
+#ifndef STATE_MACHINE_H
+#define STATE_MACHINE_H
+
+#include <string>
+
+#include "fu/fu_types.h"
+
+class StateMachine
+{
+   public:
+    enum State
+    {
+        NORMAL,      // 正常行驶, 跟随PL路径
+        WAITING,     // 停车等待, 障碍物消失或RTK恢复后恢复
+        SHIFT_SIDE,  // 侧移避障, 每次只生成1个航点
+        ADVANCE,     // 前进穿过障碍物 / 回切向路径靠拢
+        RETURN_PATH  // 已确认越过障碍物, 渐进回正到PL路径
+    };
+
+    StateMachine() : current_state_(NORMAL) {}
+
+    void setState(State new_state)
+    {
+        if (new_state != current_state_)
+        {
+            onExit(current_state_);
+            current_state_ = new_state;
+            onEnter(new_state);
+        }
+    }
+
+    State getCurrentState() const { return current_state_; }
+
+    std::string getStateString() const
+    {
+        switch (current_state_)
+        {
+            case NORMAL:
+                return "正常行驶";
+            case WAITING:
+                return "等待决策";
+            case SHIFT_SIDE:
+                return "侧移避障";
+            case ADVANCE:
+                return "前进通过";
+            case RETURN_PATH:
+                return "回正路径";
+            default:
+                return "未知状态";
+        }
+    }
+
+    // ======== 运行时数据 ========
+    ObstacleAnalysis obstacle_analysis;
+    int publish_speed = 0;
+    float publish_angle = 0.0f;
+    int avoid_direction = 0;  // 绕障方向: -1=左, +1=右
+
+   private:
+    State current_state_;
+
+    void onEnter(State state)
+    {
+        switch (state)
+        {
+            case NORMAL:
+                avoid_direction = 0;
+                break;
+            case WAITING:
+                publish_speed = 0;
+                publish_angle = 0.0f;
+                break;
+            case SHIFT_SIDE:
+                publish_speed = 300;
+                break;
+            case ADVANCE:
+                publish_speed = 500;
+                break;
+            case RETURN_PATH:
+                publish_speed = 500;
+                break;
+            default:
+                break;
+        }
+    }
+
+    void onExit(State state) {}
+};
+
+#endif  // STATE_MACHINE_H