diff --git a/src/autonomy/fu/config/fu_params.yaml b/src/autonomy/fu/config/fu_params.yaml index d06d530..34f4013 100644 --- a/src/autonomy/fu/config/fu_params.yaml +++ b/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 diff --git a/src/autonomy/fu/include/fu/fu_params.h b/src/autonomy/fu/include/fu/fu_params.h new file mode 100644 index 0000000..c4bd051 --- /dev/null +++ b/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 diff --git a/src/autonomy/fu/include/fu/fu_types.h b/src/autonomy/fu/include/fu/fu_types.h new file mode 100644 index 0000000..e821af5 --- /dev/null +++ b/src/autonomy/fu/include/fu/fu_types.h @@ -0,0 +1,137 @@ +#ifndef FU_TYPES_H +#define FU_TYPES_H + +#include +#include +#include +#include + +// ================== 栅格单元类型 ================== +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 diff --git a/src/autonomy/fu/include/fu/state_machine.h b/src/autonomy/fu/include/fu/state_machine.h new file mode 100644 index 0000000..7a03c2d --- /dev/null +++ b/src/autonomy/fu/include/fu/state_machine.h @@ -0,0 +1,90 @@ +#ifndef STATE_MACHINE_H +#define STATE_MACHINE_H + +#include + +#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 diff --git a/src/autonomy/fu/src/fu_node.cpp b/src/autonomy/fu/src/fu_node.cpp index 3aa461e..da42e58 100644 --- a/src/autonomy/fu/src/fu_node.cpp +++ b/src/autonomy/fu/src/fu_node.cpp @@ -1,217 +1,31 @@ -#include -#include - #include -#include #include -#include -#include #include -#include -#include #include #include +#include "fu/fu_params.h" +#include "fu/fu_types.h" +#include "fu/state_machine.h" #include "logger/logger.h" #include "rclcpp/rclcpp.hpp" -#include "sweeper_interfaces/msg/can_frame.hpp" +#include "std_msgs/msg/int32_multi_array.hpp" +#include "std_msgs/msg/u_int16_multi_array.hpp" #include "sweeper_interfaces/msg/fu.hpp" #include "sweeper_interfaces/msg/mc_ctrl.hpp" #include "sweeper_interfaces/msg/pl.hpp" #include "sweeper_interfaces/msg/rtk.hpp" -#include "sweeper_interfaces/msg/sub.hpp" #include "sweeper_interfaces/msg/task.hpp" using namespace std; using namespace std::chrono; -// ================== RTK全局变量 ================== +// ================== 全局变量 ================== system_clock::time_point last_rtk_time; -const duration rtk_timeout = duration(0.5); - -struct struct_rtk -{ - double lon; - double lat; - double direction; - double speed; - int reliability; -}; -struct_rtk g_rtk; - -// ================== GPS转相对坐标的常数 ================== -#define Lat_Origin 32.045062 -#define Lon_Origin 118.845200366 -#define deg_rad (0.01745329252) -#define R_LATI (6378137) -#define R_LONT (5407872) - -// ================== 枚举定义 ================== -const int EMPTY = 0; -const int OBSTACLE = 1; -const int VEHICLE = 2; +const duration rtk_timeout(0.5); +RtkData g_rtk; int is_start = 0; - -// 当前任务信息 -int current_task_mode = 0; // 清扫模式: 0=标准模式(刷子), 1=混合模式(刷子+洒水) - -// ================== 路点结构体 ================== -struct Waypoint -{ - double x, y; - int speed; - double lon, lat; // 路点的绝对坐标（GPS） - Waypoint() : x(0), y(0), speed(300), lon(0), lat(0) {} - 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 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坐标转换函数 ================== -// 计算两点间距离(米) -double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2) -{ - double x, y, length; - x = (R_LONT) * (lonti2 - lonti1) * deg_rad; - y = (R_LATI) * (lati2 - lati1) * deg_rad; - length = sqrt(x * x + y * y); - return length; -} - -// GPS转相对坐标(米) -// Cur_navAngle: 当前航向角(度) -// Cur_lonti, Cur_lati: 当前位置GPS坐标 -// Dest_lonti, Dest_lati: 目标位置GPS坐标 -// x_diff, y_diff: 输出的相对坐标(米) -int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, double Dest_lonti, double Dest_lati, - double* x_diff, double* y_diff) -{ - double Navi_rad, x, y; - float k1, k2, k3, k4; - Navi_rad = Cur_navAngle * deg_rad; - - x = (Dest_lonti - Cur_lonti) * deg_rad * R_LONT; - y = (Dest_lati - Cur_lati) * deg_rad * R_LATI; - - k1 = cos(Navi_rad); - k2 = (-1) * sin(Navi_rad); - k3 = (-1) * k2; - k4 = k1; - - *x_diff = x * k1 + y * k2; - *y_diff = x * k3 + y * k4; - - return 0; -} - -// ================== 状态机核心类 ================== -class StateMachine -{ - public: - enum State - { - NORMAL, - WAITING, - LATERAL_SHIFT, - PARALLEL_MOVE - }; - - StateMachine() { current_state_ = NORMAL; } - - void setState(State new_state) - { - if (new_state != current_state_) - { - exit(current_state_); - current_state_ = new_state; - enter(new_state); - } - } - - void execute() {} - - State getCurrentState() const { return current_state_; } - - std::string getStateString() const - { - switch (current_state_) - { - case NORMAL: - return "正常行驶"; - case WAITING: - return "等待决策"; - case LATERAL_SHIFT: - return "横向偏移"; - case PARALLEL_MOVE: - return "平行前进"; - default: - return "未知状态"; - } - } - - ObstacleAnalysis obstacle_analysis; - int publish_speed = 0; - float publish_angle = 0.0f; - int counter = 0; - int direction = 0; - std::vector angle_history; - rclcpp::Time start_time; - bool start_time_set = false; - - private: - State current_state_; - - void enter(State state) - { - switch (state) - { - case NORMAL: - counter = 0; - direction = 0; - break; - case WAITING: - publish_speed = 0; - publish_angle = 0.0f; - counter = 0; - break; - case LATERAL_SHIFT: - publish_speed = 300; - counter = 0; - break; - case PARALLEL_MOVE: - publish_speed = 300; - counter = 0; - break; - default: - break; - } - } - - void exit(State state) {} -}; +int current_task_mode = 0; // ================== FU节点类 ================== class fu_node : public rclcpp::Node @@ -220,53 +34,15 @@ class fu_node : public rclcpp::Node fu_node(std::string name) : Node(name), state_machine_(std::make_unique()) { LOG_INFO("%s节点已经启动.", name.c_str()); - // 初始化最后接收时间为启动时间（避免初始状态直接超时） last_rtk_time = system_clock::now(); + node_clock_ = this->get_clock(); - // 订阅任务消息以获取清扫模式等信息 - task_subscribe_ = this->create_subscription( - "task_message/download", 10, std::bind(&fu_node::task_callback, this, std::placeholders::_1)); - - // ======== 参数声明 ======== - this->declare_parameter("enable_obstacle_stop", true); - this->declare_parameter("enable_obstacle_avoid", true); - this->declare_parameter("enable_visualize_grid", true); - this->declare_parameter("FRONT_STOP_ZONE_ROWS", 3); - this->declare_parameter("REAR_STOP_ZONE_ROWS", 1); - this->declare_parameter("LEFT_STOP_ZONE_COLS", 3); - this->declare_parameter("RIGHT_STOP_ZONE_COLS", 3); - this->declare_parameter("front_area_extend", 7); - this->declare_parameter("left_area_extend", 2); - this->declare_parameter("right_area_extend", 2); - this->declare_parameter("waypoint_tolerance", 2.0); - this->declare_parameter("lateral_offset", 2); - this->declare_parameter("parallel_distance", 3); - this->declare_parameter("avoid_able_width", 6); - this->declare_parameter("min_free_space", 6); - this->declare_parameter("uss_stop_distance", 200); - - // ======== 参数读取 ======== - this->get_parameter("enable_obstacle_stop", enable_obstacle_stop_); - this->get_parameter("enable_obstacle_avoid", enable_obstacle_avoid_); - this->get_parameter("enable_visualize_grid", enable_visualize_grid_); - this->get_parameter("FRONT_STOP_ZONE_ROWS", FRONT_STOP_ZONE_ROWS_); - this->get_parameter("REAR_STOP_ZONE_ROWS", REAR_STOP_ZONE_ROWS_); - this->get_parameter("LEFT_STOP_ZONE_COLS", LEFT_STOP_ZONE_COLS_); - this->get_parameter("RIGHT_STOP_ZONE_COLS", RIGHT_STOP_ZONE_COLS_); - this->get_parameter("front_area_extend", front_area_extend_); - this->get_parameter("left_area_extend", left_area_extend_); - this->get_parameter("right_area_extend", right_area_extend_); - this->get_parameter("waypoint_tolerance", waypoint_tolerance_); - this->get_parameter("lateral_offset", lateral_offset_); - this->get_parameter("parallel_distance", parallel_distance_); - this->get_parameter("avoid_able_width", avoid_able_width_); - this->get_parameter("min_free_space", min_free_space_); - this->get_parameter("uss_stop_distance", uss_stop_distance_); - + // ======== 参数加载 ======== + params_.declareAndLoad(this); parameter_callback_handle_ = this->add_on_set_parameters_callback(std::bind(&fu_node::parameter_callback, this, std::placeholders::_1)); - // ======== 订阅者初始化 ======== + // ======== 订阅者 ======== subscription_grid = this->create_subscription( "grid_raw", 10, std::bind(&fu_node::gridCallback, this, std::placeholders::_1)); msg_subscribe_pl = this->create_subscription( @@ -278,61 +54,41 @@ class fu_node : public rclcpp::Node uss_subscriber_ = this->create_subscription( "USS", 10, std::bind(&fu_node::uss_callback, this, std::placeholders::_1)); - // ======== 发布者初始化 ======== + // ======== 发布者 ======== pub_mc = this->create_publisher("auto_mc_ctrl", 10); pub_fu = this->create_publisher("fu_message", 10); - // ======== 定时器初始化 ======== + // ======== 定时器 20ms ======== timer_ = this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&fu_node::timer_callback, this)); // ======== 初始化 ======== - avoiding_obstacle_ = false; - current_waypoint_idx_ = 0; - vehicle_min_x_ = -1; - vehicle_max_x_ = -1; - vehicle_min_y_ = -1; - vehicle_max_y_ = -1; - node_clock_ = this->get_clock(); - - // 初始化栅格时间戳（使用 node_clock_ 确保时钟源一致） + vehicle_min_x_ = vehicle_max_x_ = vehicle_min_y_ = vehicle_max_y_ = -1; { std::lock_guard lock(grid_mutex_); last_grid_time_ = node_clock_->now(); } last_uss_time_ = node_clock_->now(); - front_stop_zone_ = {0, 0, 0, 0}; - rear_stop_zone_ = {0, 0, 0, 0}; - left_stop_zone_ = {0, 0, 0, 0}; - right_stop_zone_ = {0, 0, 0, 0}; - LOG_INFO( "\n---------- FU节点参数配置 ----------\n 遇障停车: %s\n 绕障: %s\n 栅格可视化: %s\n" - " 路点容差: %.2f米\n 横向偏移: %d栅格\n 平行距离: %d栅格\n" - " 可绕宽度: %d栅格\n 最小空闲: %d栅格\n 超声波停车距离: %dcm\n--------------------------------------------\n", - (enable_obstacle_stop_ ? "开启" : "关闭"), (enable_obstacle_avoid_ ? "开启" : "关闭"), - (enable_visualize_grid_ ? "是" : "否"), waypoint_tolerance_, lateral_offset_, parallel_distance_, - avoid_able_width_, min_free_space_, uss_stop_distance_); + " 路点容差: %.2f米\n 可绕宽度: %d栅格\n 最小空闲: %d栅格\n" + " 超声波停车距离: %dcm\n--------------------------------------------\n", + (params_.enable_obstacle_stop ? "开启" : "关闭"), (params_.enable_obstacle_avoid ? "开启" : "关闭"), + (params_.enable_visualize_grid ? "是" : "否"), params_.waypoint_tolerance, params_.avoid_able_width, + params_.min_free_space, params_.uss_stop_distance); } private: + // ======== 核心成员 ======== std::unique_ptr state_machine_; rclcpp::Clock::SharedPtr node_clock_; + FuParams params_; - // ======== 配置参数 ======== - bool enable_obstacle_stop_, enable_obstacle_avoid_, enable_visualize_grid_; - int FRONT_STOP_ZONE_ROWS_, REAR_STOP_ZONE_ROWS_; - int LEFT_STOP_ZONE_COLS_, RIGHT_STOP_ZONE_COLS_; - int front_area_extend_, left_area_extend_, right_area_extend_; - double waypoint_tolerance_; - int lateral_offset_, parallel_distance_, avoid_able_width_, min_free_space_; + // ======== 线程安全 ======== + std::mutex grid_mutex_; + std::mutex uss_mutex_; - rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; - - // ======== 线程安全锁 ======== - std::mutex grid_mutex_; // 保护栅格数据相关变量 - - // ======== 规划层消息缓存 ======== + // ======== PL数据 ======== double pl_x_ = 0.0, pl_y_ = 0.0; int pl_speed_ = 0; @@ -346,30 +102,36 @@ class fu_node : public rclcpp::Node // ======== 自车位置 ======== int vehicle_min_x_, vehicle_max_x_, vehicle_min_y_, vehicle_max_y_; - - // ======== 安全区结构体 ======== - struct SafetyZone - { - int min_x, max_x, min_y, max_y; - }; SafetyZone front_stop_zone_, rear_stop_zone_, left_stop_zone_, right_stop_zone_; - // ======== 避障相关 ======== - bool avoiding_obstacle_; + // ======== 避障 ======== + bool avoiding_obstacle_ = false; std::vector avoidance_waypoints_; - int current_waypoint_idx_; + int current_waypoint_idx_ = 0; std::vector> cached_grid_; bool grid_data_valid_ = false; rclcpp::Time last_grid_time_; - // ======== USS超声波传感器数据 ======== + // ======== USS ======== uint16_t uss_data_[8] = {0}; bool uss_data_valid_ = false; - int uss_stop_distance_ = 200; rclcpp::Time last_uss_time_; - std::mutex uss_mutex_; - // ======== 任务消息回调 ======== + // ======== ROS接口 ======== + rclcpp::Publisher::SharedPtr pub_mc; + rclcpp::Publisher::SharedPtr pub_fu; + rclcpp::Subscription::SharedPtr subscription_grid; + rclcpp::Subscription::SharedPtr msg_subscribe_pl; + rclcpp::Subscription::SharedPtr msg_subscribe_rtk; + rclcpp::Subscription::SharedPtr task_subscribe_; + rclcpp::Subscription::SharedPtr uss_subscriber_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; + + // ===================================================================== + // 回调函数 + // ===================================================================== + void task_callback(const sweeper_interfaces::msg::Task::SharedPtr msg) { if (msg->task_id > 0) @@ -379,127 +141,56 @@ class fu_node : public rclcpp::Node } } - // ======== USS超声波传感器回调 ======== void uss_callback(const std_msgs::msg::UInt16MultiArray::SharedPtr msg) { if (msg->data.size() < 2) return; - std::lock_guard lock(uss_mutex_); - for (size_t i = 0; i < msg->data.size() && i < 8; i++) - { - uss_data_[i] = msg->data[i]; - } + for (size_t i = 0; i < msg->data.size() && i < 8; i++) uss_data_[i] = msg->data[i]; uss_data_valid_ = true; last_uss_time_ = node_clock_->now(); } - // ======== 发布者和订阅者 ======== - rclcpp::Publisher::SharedPtr pub_mc; - rclcpp::Publisher::SharedPtr pub_fu; - rclcpp::Subscription::SharedPtr subscription_grid; - rclcpp::Subscription::SharedPtr msg_subscribe_pl; - rclcpp::Subscription::SharedPtr msg_subscribe_rtk; - rclcpp::Subscription::SharedPtr task_subscribe_; - rclcpp::Subscription::SharedPtr uss_subscriber_; - rclcpp::TimerBase::SharedPtr timer_; - - // ======== 参数回调 ======== - rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector& parameters) - { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - for (const auto& param : parameters) - { - if (param.get_name() == "enable_obstacle_stop") - { - enable_obstacle_stop_ = param.as_bool(); - LOG_INFO("[参数更新] 遇障停车: %s", enable_obstacle_stop_ ? "开启" : "关闭"); - } - else if (param.get_name() == "enable_obstacle_avoid") - { - enable_obstacle_avoid_ = param.as_bool(); - LOG_INFO("[参数更新] 自主绕障: %s", enable_obstacle_avoid_ ? "开启" : "关闭"); - if (!enable_obstacle_avoid_) - { - state_machine_->setState(StateMachine::NORMAL); - avoiding_obstacle_ = false; - avoidance_waypoints_.clear(); - } - } - 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(); - } - return result; - } - - // ======== RTK回调 ======== void msg_callback_rtk(const sweeper_interfaces::msg::Rtk::SharedPtr msg) { last_rtk_time = system_clock::now(); - g_rtk.reliability = (msg->p_quality == 4 && msg->h_quality == 4) ? 1 : 0; g_rtk.lat = msg->lat; g_rtk.lon = msg->lon; g_rtk.direction = msg->head; } - // ======== 角度计算 ======== - float calculate_the_angle(double target_x, double target_y) + void msg_callback_pl(const sweeper_interfaces::msg::Pl::SharedPtr msg) { - if (target_x == 0) return 90.0f; - float angle_f = atan2(target_y, target_x) * 180 / M_PI; - float steering_angle = (angle_f <= 90.0f) ? 90.0f - angle_f : -(angle_f - 90.0f); - // 计算阶段就限制角度范围 - return std::clamp(steering_angle, -50.0f, 50.0f); + pl_x_ = msg->x; + pl_y_ = msg->y; + pl_speed_ = (int)msg->speed; + is_start = msg->is_start; + LOG_INFO("[PL回调] 目标(%.2f, %.2f) | speed=%d", pl_x_, pl_y_, pl_speed_); } - // ======== 自车位置检测 ======== - void findVehicleRectangle(const std::vector>& grid) + rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector& parameters) { - vehicle_min_x_ = vehicle_max_x_ = vehicle_min_y_ = vehicle_max_y_ = -1; - for (size_t i = 0; i < grid.size(); ++i) + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto& param : parameters) { - for (size_t j = 0; j < grid[i].size(); ++j) + if (!params_.handleUpdate(param)) { - if (grid[i][j] == VEHICLE) - { - if (vehicle_min_x_ == -1 || (int)i < vehicle_min_x_) vehicle_min_x_ = i; - if (vehicle_max_x_ == -1 || (int)i > vehicle_max_x_) vehicle_max_x_ = i; - if (vehicle_min_y_ == -1 || (int)j < vehicle_min_y_) vehicle_min_y_ = j; - if (vehicle_max_y_ == -1 || (int)j > vehicle_max_y_) vehicle_max_y_ = j; - } + result.successful = false; + result.reason = "未知参数: " + param.get_name(); + continue; + } + if (param.get_name() == "enable_obstacle_avoid" && !params_.enable_obstacle_avoid) + { + state_machine_->setState(StateMachine::NORMAL); + avoiding_obstacle_ = false; + avoidance_waypoints_.clear(); + LOG_INFO("[参数更新] 绕障已关闭，恢复正常状态"); } } - if (vehicle_min_x_ != -1 && vehicle_max_x_ != -1 && vehicle_min_y_ != -1 && vehicle_max_y_ != -1) - { - calculateSafetyZones(grid.size(), grid[0].size()); - } + return result; } - // ======== 安全区计算 ======== - void calculateSafetyZones(int grid_height, int grid_width) - { - front_stop_zone_ = {max(0, vehicle_min_x_ - FRONT_STOP_ZONE_ROWS_), vehicle_min_x_ - 1, vehicle_min_y_, - vehicle_max_y_}; - rear_stop_zone_ = {vehicle_max_x_ + 1, min(grid_height - 1, vehicle_max_x_ + REAR_STOP_ZONE_ROWS_), - vehicle_min_y_, vehicle_max_y_}; - left_stop_zone_ = {vehicle_min_x_, vehicle_max_x_, max(0, vehicle_min_y_ - LEFT_STOP_ZONE_COLS_), - vehicle_min_y_ - 1}; - right_stop_zone_ = {vehicle_min_x_, vehicle_max_x_, vehicle_max_y_ + 1, - min(grid_width - 1, vehicle_max_y_ + RIGHT_STOP_ZONE_COLS_)}; - } - - // ======== 栅格回调 ======== void gridCallback(const std_msgs::msg::Int32MultiArray::SharedPtr msg) { if (msg->layout.dim.size() != 2) @@ -507,10 +198,8 @@ class fu_node : public rclcpp::Node LOG_ERROR("栅格地图消息维度无效！"); return; } - size_t height = msg->layout.dim[0].size; size_t width = msg->layout.dim[1].size; - if (msg->data.size() != height * width) { LOG_ERROR("栅格地图数据长度不匹配！期望%zu，实际%zu", height * width, msg->data.size()); @@ -523,38 +212,57 @@ class fu_node : public rclcpp::Node findVehicleRectangle(grid); analyzeObstacles(grid); - if (enable_visualize_grid_) visualizeGridInTerminal(grid); + if (params_.enable_visualize_grid) visualizeGridInTerminal(grid); - // 只在更新共享变量时加锁 { std::lock_guard lock(grid_mutex_); last_grid_time_ = node_clock_->now(); - cacheGridData(grid); + cached_grid_ = grid; + grid_data_valid_ = true; } - // 立即执行紧急停车检查（高优先级） updateObstacleAnalysis(); - if (enable_obstacle_stop_ && is_start) + if (params_.enable_obstacle_stop && is_start && state_machine_->obstacle_analysis.has_front_critical) { - if (state_machine_->obstacle_analysis.has_front_critical) - { - state_machine_->publish_speed = 0; - state_machine_->publish_angle = 0.0f; - state_machine_->setState(StateMachine::WAITING); - publishControlCommand(true, true); // 立即发布带刹车的停车指令，关闭清扫 - LOG_WARN("[紧急停车] 栅格回调中检测到前方障碍物，立即停车！"); - } + state_machine_->publish_speed = 0; + state_machine_->publish_angle = 0.0f; + state_machine_->setState(StateMachine::WAITING); + publishControlCommand(true, true); + LOG_WARN("[紧急停车] 栅格回调中检测到前方障碍物，立即停车！"); } } - // ======== 栅格缓存 ======== - void cacheGridData(const std::vector>& grid) + // ===================================================================== + // 栅格分析 + // ===================================================================== + + void findVehicleRectangle(const std::vector>& grid) { - cached_grid_ = grid; - grid_data_valid_ = true; + vehicle_min_x_ = vehicle_max_x_ = vehicle_min_y_ = vehicle_max_y_ = -1; + for (size_t i = 0; i < grid.size(); ++i) + for (size_t j = 0; j < grid[i].size(); ++j) + if (grid[i][j] == VEHICLE) + { + if (vehicle_min_x_ == -1 || (int)i < vehicle_min_x_) vehicle_min_x_ = i; + if (vehicle_max_x_ == -1 || (int)i > vehicle_max_x_) vehicle_max_x_ = i; + if (vehicle_min_y_ == -1 || (int)j < vehicle_min_y_) vehicle_min_y_ = j; + if (vehicle_max_y_ == -1 || (int)j > vehicle_max_y_) vehicle_max_y_ = j; + } + if (vehicle_min_x_ != -1) calculateSafetyZones(grid.size(), grid[0].size()); + } + + void calculateSafetyZones(int grid_height, int grid_width) + { + front_stop_zone_ = {max(0, vehicle_min_x_ - params_.front_stop_zone_rows), vehicle_min_x_ - 1, vehicle_min_y_, + vehicle_max_y_}; + rear_stop_zone_ = {vehicle_max_x_ + 1, min(grid_height - 1, vehicle_max_x_ + params_.rear_stop_zone_rows), + vehicle_min_y_, vehicle_max_y_}; + left_stop_zone_ = {vehicle_min_x_, vehicle_max_x_, max(0, vehicle_min_y_ - params_.left_stop_zone_cols), + vehicle_min_y_ - 1}; + right_stop_zone_ = {vehicle_min_x_, vehicle_max_x_, vehicle_max_y_ + 1, + min(grid_width - 1, vehicle_max_y_ + params_.right_stop_zone_cols)}; } - // ======== 障碍物分析 ======== void analyzeObstacles(const std::vector>& grid) { obstacle_in_front_ = obstacle_in_rear_ = obstacle_in_left_ = obstacle_in_right_ = false; @@ -564,18 +272,21 @@ class fu_node : public rclcpp::Node obstacle_max_x_ = 100; if (vehicle_min_x_ == -1) return; - // 检测前方必停区障碍物 - bool found_front_obstacle = false; - for (int i = front_stop_zone_.max_x; i >= front_stop_zone_.min_x && !found_front_obstacle; --i) + // 前方必停区 + for (int i = front_stop_zone_.max_x; i >= front_stop_zone_.min_x; --i) + { + bool found = false; for (int j = front_stop_zone_.min_y; j <= front_stop_zone_.max_y; ++j) if ((size_t)i < grid.size() && (size_t)j < grid[0].size() && grid[i][j] == OBSTACLE) { obstacle_in_front_ = true; - found_front_obstacle = true; + found = true; break; } + if (found) break; + } - // 检测后方必停区障碍物 + // 后方必停区 for (int i = rear_stop_zone_.min_x; i <= rear_stop_zone_.max_x; ++i) for (int j = rear_stop_zone_.min_y; j <= rear_stop_zone_.max_y; ++j) if ((size_t)i < grid.size() && (size_t)j < grid[0].size() && grid[i][j] == OBSTACLE) @@ -584,7 +295,7 @@ class fu_node : public rclcpp::Node break; } - // 检测左侧必停区障碍物 + // 左侧必停区 for (int i = left_stop_zone_.min_x; i <= left_stop_zone_.max_x; ++i) for (int j = left_stop_zone_.min_y; j <= left_stop_zone_.max_y; ++j) if ((size_t)i < grid.size() && (size_t)j < grid[0].size() && grid[i][j] == OBSTACLE) @@ -593,7 +304,7 @@ class fu_node : public rclcpp::Node break; } - // 检测右侧必停区障碍物 + // 右侧必停区 for (int i = right_stop_zone_.min_x; i <= right_stop_zone_.max_x; ++i) for (int j = right_stop_zone_.min_y; j <= right_stop_zone_.max_y; ++j) if ((size_t)i < grid.size() && (size_t)j < grid[0].size() && grid[i][j] == OBSTACLE) @@ -602,95 +313,78 @@ class fu_node : public rclcpp::Node break; } - // 检测前方扩展区域障碍物 - int front_area_min_x = max(0, vehicle_min_x_ - front_area_extend_); - int front_area_max_x = vehicle_min_x_ - 1; - int front_area_min_y = max(0, vehicle_min_y_ - left_area_extend_); - int front_area_max_y = min((int)grid[0].size() - 1, vehicle_max_y_ + right_area_extend_); - int leftmost_obstacle = -1, rightmost_obstacle = -1; + // 前方扩展区域 + int front_min_x = max(0, vehicle_min_x_ - params_.front_area_extend); + int front_max_x = vehicle_min_x_ - 1; + int front_min_y = max(0, vehicle_min_y_ - params_.left_area_extend); + int front_max_y = min((int)grid[0].size() - 1, vehicle_max_y_ + params_.right_area_extend); + int leftmost = -1, rightmost = -1; - for (size_t i = front_area_min_x; i <= (size_t)front_area_max_x; ++i) - { - for (size_t j = front_area_min_y; j <= (size_t)front_area_max_y; ++j) - { + for (int i = front_min_x; i <= front_max_x; ++i) + for (int j = front_min_y; j <= front_max_y; ++j) if (grid[i][j] == OBSTACLE) { obstacle_in_front_area_ = true; - if (leftmost_obstacle == -1 || (int)j < leftmost_obstacle) leftmost_obstacle = j; - if (rightmost_obstacle == -1 || (int)j > rightmost_obstacle) rightmost_obstacle = j; - int distance = vehicle_min_x_ - (int)i; - if (distance < obstacle_max_x_) obstacle_max_x_ = distance; + if (leftmost == -1 || j < leftmost) leftmost = j; + if (rightmost == -1 || j > rightmost) rightmost = j; + int dist = vehicle_min_x_ - i; + if (dist < obstacle_max_x_) obstacle_max_x_ = dist; } - } - } - // 计算绕障空间 - calculateAvoidanceSpace(grid, leftmost_obstacle, rightmost_obstacle); + calculateAvoidanceSpace(grid, leftmost, rightmost); } - // ======== 绕障空间计算 ======== - void calculateAvoidanceSpace(const std::vector>& grid, int core_left_obstacle, - int core_right_obstacle) + void calculateAvoidanceSpace(const std::vector>& grid, int core_left, int core_right) { if (!obstacle_in_front_area_) { - free_space_left_ = left_area_extend_ + (vehicle_max_y_ - vehicle_min_y_ + 1); - free_space_right_ = right_area_extend_ + (vehicle_max_y_ - vehicle_min_y_ + 1); + free_space_left_ = params_.left_area_extend + (vehicle_max_y_ - vehicle_min_y_ + 1); + free_space_right_ = params_.right_area_extend + (vehicle_max_y_ - vehicle_min_y_ + 1); obstacle_left_edge_ = obstacle_right_edge_ = -1; return; } - obstacle_left_edge_ = core_left_obstacle; - obstacle_right_edge_ = core_right_obstacle; - size_t avoidance_min_x = (size_t)max(0, vehicle_min_x_ - front_area_extend_); - size_t avoidance_max_x = (size_t)vehicle_min_x_ - 1; + obstacle_left_edge_ = core_left; + obstacle_right_edge_ = core_right; + size_t min_x = (size_t)max(0, vehicle_min_x_ - params_.front_area_extend); + size_t max_x = (size_t)vehicle_min_x_ - 1; - // 计算左侧空闲空间 - int left_check_start = max(0, vehicle_min_y_ - left_area_extend_); - int left_check_end = obstacle_left_edge_ - 1; - free_space_left_ = - calculateSpaceInDirection(grid, avoidance_min_x, avoidance_max_x, left_check_start, left_check_end); + int left_start = max(0, vehicle_min_y_ - params_.left_area_extend); + int left_end = obstacle_left_edge_ - 1; + free_space_left_ = countClearColumns(grid, min_x, max_x, left_start, left_end); - // 计算右侧空闲空间 - int right_check_start = obstacle_right_edge_ + 1; - int right_check_end = min((int)grid[0].size() - 1, vehicle_max_y_ + right_area_extend_); - free_space_right_ = - calculateSpaceInDirection(grid, avoidance_min_x, avoidance_max_x, right_check_start, right_check_end); + int right_start = obstacle_right_edge_ + 1; + int right_end = min((int)grid[0].size() - 1, vehicle_max_y_ + params_.right_area_extend); + free_space_right_ = countClearColumns(grid, min_x, max_x, right_start, right_end); } - // ======== 方向空闲空间计算 ======== - int calculateSpaceInDirection(const std::vector>& grid, size_t min_x, size_t max_x, - int check_start_y, int check_end_y) + int countClearColumns(const std::vector>& grid, size_t min_x, size_t max_x, int start_y, int end_y) { - if (check_start_y > check_end_y) return 0; - int available_space = 0; - for (int j = check_start_y; j <= check_end_y; ++j) + if (start_y > end_y) return 0; + int count = 0; + for (int j = start_y; j <= end_y; ++j) { if (j < 0 || j >= (int)grid[0].size()) break; - bool column_clear = true; + bool clear = true; for (size_t i = min_x; i <= max_x; ++i) - { if (i >= grid.size() || grid[i][j] == OBSTACLE) { - column_clear = false; + clear = false; break; } - } - if (column_clear) - available_space++; + if (clear) + count++; else break; } - return available_space; + return count; } - // ======== 栅格可视化 ======== void visualizeGridInTerminal(const std::vector>& grid) { std::stringstream ss; ss << " ----------栅格地图可视化---------- " << endl; - ss << " " << string(grid[0].size(), '-') << endl; - ss << " "; + ss << " " << string(grid[0].size(), '-') << endl << " "; for (size_t i = 0; i < grid[0].size(); i++) ss << (i % 10); ss << endl; for (size_t i = 0; i < grid.size(); i++) @@ -704,7 +398,6 @@ class fu_node : public rclcpp::Node j >= left_stop_zone_.min_y && j <= left_stop_zone_.max_y); bool in_right = (i >= right_stop_zone_.min_x && i <= right_stop_zone_.max_x && j >= right_stop_zone_.min_y && j <= right_stop_zone_.max_y); - if (grid[i][j] == OBSTACLE) ss << "@"; else if (grid[i][j] == VEHICLE) @@ -724,134 +417,171 @@ class fu_node : public rclcpp::Node LOG_INFO("%s", ss.str().c_str()); } - // ======== 规划层消息回调 ======== - void msg_callback_pl(const sweeper_interfaces::msg::Pl::SharedPtr msg) - { - pl_x_ = msg->x; - pl_y_ = msg->y; - pl_speed_ = (int)msg->speed; - is_start = msg->is_start; + // ===================================================================== + // 障碍物分析更新 + // ===================================================================== - LOG_INFO("[规划层消息回调] 目标点位置： x -- %.2f , y -- %.2f", pl_x_, pl_y_); - LOG_INFO("[规划层消息回调] pl_speed_： %d ", pl_speed_); + void updateObstacleAnalysis() + { + auto& oa = state_machine_->obstacle_analysis; + oa.has_front_critical = obstacle_in_front_; + oa.has_front_area = obstacle_in_front_area_; + oa.has_left_critical = obstacle_in_left_; + oa.has_right_critical = obstacle_in_right_; + oa.obstacle_distance = (obstacle_max_x_ > 0) ? obstacle_max_x_ : -1; + oa.left_edge = obstacle_left_edge_; + oa.right_edge = obstacle_right_edge_; + oa.free_space_left = free_space_left_; + oa.free_space_right = free_space_right_; + if (obstacle_left_edge_ != -1 && obstacle_right_edge_ != -1) + oa.obstacle_width = obstacle_right_edge_ - obstacle_left_edge_ + 1; + + LOG_INFO_THROTTLE(1000, "[障碍物] 距离:%d 宽度:%d 左边缘:%d 右边缘:%d", oa.obstacle_distance, oa.obstacle_width, + oa.left_edge, oa.right_edge); } - // ======== 绕障路点规划 ======== - void planAvoidanceWaypoints(int avoid_direction) + // ===================================================================== + // 绕障逻辑 + // ===================================================================== + + bool canAvoidObstacle() const + { + auto& oa = state_machine_->obstacle_analysis; + if (oa.obstacle_width <= 0 || oa.obstacle_width > params_.avoid_able_width) + { + LOG_INFO("[绕障判断] 宽度%d栅格，超过上限%d，不可绕", oa.obstacle_width, params_.avoid_able_width); + return false; + } + bool left_ok = oa.free_space_left >= params_.min_free_space; + bool right_ok = oa.free_space_right >= params_.min_free_space; + LOG_INFO("[绕障判断] 左空闲%d 右空闲%d 需求%d | 左:%s 右:%s", oa.free_space_left, oa.free_space_right, + params_.min_free_space, left_ok ? "可" : "不可", right_ok ? "可" : "不可"); + return left_ok || right_ok; + } + + int selectOptimalAvoidDirection() const + { + auto& oa = state_machine_->obstacle_analysis; + if (oa.free_space_left > oa.free_space_right) return -1; + if (oa.free_space_right > oa.free_space_left) return 1; + return (pl_x_ > 0) ? 1 : -1; + } + + // ======== 侧移航点生成（每次只生成1个） ======== + void planSideShift() { avoidance_waypoints_.clear(); + const double GRID_RES = 0.3; + const int SAFE_GAP = 2; + const int SPEED = 300; - const double GRID_RESOLUTION = 0.3; - const int SAFE_GRID_GAP = 1; - const int AVOID_SPEED = 300; + int dir = selectOptimalAvoidDirection(); + state_machine_->avoid_direction = dir; - Waypoint point_lateral_shift; - Waypoint point_parallel_forward; - double actual_lateral_offset_m = 0.0; - int lateral_offset_grid_num = 0; - - const int stage1_forward_grid = state_machine_->obstacle_analysis.obstacle_distance; - const int stage2_forward_grid = parallel_distance_; - const double stage1_forward_m = stage1_forward_grid * GRID_RESOLUTION; - const double stage2_forward_m = stage2_forward_grid * GRID_RESOLUTION; - - // 计算横向偏移量 - if (avoid_direction == -1) + double lateral = 0.0; + if (dir == -1) { - lateral_offset_grid_num = (vehicle_max_y_ - state_machine_->obstacle_analysis.left_edge) + SAFE_GRID_GAP; - actual_lateral_offset_m = -1 * lateral_offset_grid_num * GRID_RESOLUTION; - } - else if (avoid_direction == 1) - { - lateral_offset_grid_num = (state_machine_->obstacle_analysis.right_edge - vehicle_min_y_) + SAFE_GRID_GAP; - actual_lateral_offset_m = 1 * lateral_offset_grid_num * GRID_RESOLUTION; - } - - // 相对坐标 - double relative_x_1 = actual_lateral_offset_m; - double relative_y_1 = stage1_forward_m; - double relative_x_2 = actual_lateral_offset_m; - double relative_y_2 = stage2_forward_m; - - // 转换为绝对GPS坐标 - double abs_lon_1, abs_lat_1, abs_lon_2, abs_lat_2; - rtk_relative_to_gps(relative_x_1, relative_y_1, g_rtk.direction, g_rtk.lon, g_rtk.lat, &abs_lon_1, &abs_lat_1); - rtk_relative_to_gps(relative_x_2, relative_y_2, g_rtk.direction, g_rtk.lon, g_rtk.lat, &abs_lon_2, &abs_lat_2); - - // 构建路点 - point_lateral_shift = Waypoint(relative_x_1, relative_y_1, AVOID_SPEED, abs_lon_1, abs_lat_1); - point_parallel_forward = Waypoint(relative_x_2, relative_y_2, AVOID_SPEED, abs_lon_2, abs_lat_2); - - avoidance_waypoints_.push_back(point_lateral_shift); - avoidance_waypoints_.push_back(point_parallel_forward); - - current_waypoint_idx_ = 0; - avoiding_obstacle_ = true; - state_machine_->setState(StateMachine::LATERAL_SHIFT); - - LOG_INFO("[绕障规划] 方向：%s | 路点1 GPS(%.6f,%.6f) | 路点2 GPS(%.6f,%.6f)", - avoid_direction == -1 ? "左" : "右", abs_lon_1, abs_lat_1, abs_lon_2, abs_lat_2); - } - - // ======== 相对坐标转GPS绝对坐标 ======== - void rtk_relative_to_gps(double rel_x, double rel_y, double cur_heading, double cur_lon, double cur_lat, - double* dest_lon, double* dest_lat) - { - double Navi_rad = cur_heading * deg_rad; - - // 逆向旋转矩阵：从车体坐标系转回地理坐标系 - float k1 = cos(Navi_rad); - float k2 = sin(Navi_rad); - float k3 = -sin(Navi_rad); - float k4 = cos(Navi_rad); - - // 地理坐标增量 - double delta_x = rel_x * k1 + rel_y * k3; // 东向增量 - double delta_y = rel_x * k2 + rel_y * k4; // 北向增量 - - // 转换为GPS坐标增量 - double delta_lon = delta_x / (deg_rad * R_LONT); - double delta_lat = delta_y / (deg_rad * R_LATI); - - *dest_lon = cur_lon + delta_lon; - *dest_lat = cur_lat + delta_lat; - - LOG_INFO("[相对转GPS] 相对(%.2f,%.2f) -> GPS增量(%.8f,%.8f) -> 目标(%.6f,%.6f)", rel_x, rel_y, delta_lon, - delta_lat, *dest_lon, *dest_lat); - } - - // ======== 路点到达判断 ======== - // 返回值：1=到达 0=未到达 -1=RTK信号丢失(需要停车) - int waypointReached(const Waypoint& target) - { - // 验证RTK信号质量 - auto now = system_clock::now(); - duration time_since_last = now - last_rtk_time; - - if (g_rtk.reliability == 1 && time_since_last <= rtk_timeout) - { - // RTK信号良好：使用绝对坐标判断 - double distance = ntzx_GPS_length(g_rtk.lon, g_rtk.lat, target.lon, target.lat); - - LOG_INFO("[路点判断] RTK信号良好 | 当前GPS(%.6f,%.6f) | 目标GPS(%.6f,%.6f) | 距离: %.2f m | 容差: %.2f m", - g_rtk.lon, g_rtk.lat, target.lon, target.lat, distance, waypoint_tolerance_); - - return distance < waypoint_tolerance_ ? 1 : 0; + int grids = (vehicle_max_y_ - state_machine_->obstacle_analysis.left_edge) + SAFE_GAP; + lateral = -grids * GRID_RES; } else { - // RTK信号丢失：无法安全判断 - if (time_since_last > rtk_timeout) - { - LOG_ERROR("[路点判断] RTK超时(%.2fs)，信号丢失，停车退出绕障！", time_since_last.count()); - } - else - { - LOG_ERROR("[路点判断] RTK信号差(可靠性=%d)，停车退出绕障！", g_rtk.reliability); - } + int grids = (state_machine_->obstacle_analysis.right_edge - vehicle_min_y_) + SAFE_GAP; + lateral = grids * GRID_RES; + } + double abs_lon, abs_lat; + relative_to_gps(lateral, 0.0, g_rtk.direction, g_rtk.lon, g_rtk.lat, &abs_lon, &abs_lat); + + avoidance_waypoints_.push_back(Waypoint(lateral, 0.0, SPEED, abs_lon, abs_lat)); + current_waypoint_idx_ = 0; + avoiding_obstacle_ = true; + state_machine_->setState(StateMachine::SHIFT_SIDE); + + LOG_INFO("[侧移规划] 方向:%s 横移:%.2fm GPS(%.6f,%.6f)", dir == -1 ? "左" : "右", lateral, abs_lon, abs_lat); + } + + // ======== 前进航点生成（越过障碍物 / 回切） ======== + void planAdvance(bool return_mode = false) + { + avoidance_waypoints_.clear(); + const int SPEED = return_mode ? 500 : 500; + + double lateral = 0.0; + double forward = 0.0; + + if (return_mode) + { + // 回切: 直接用PL目标 + lateral = pl_x_; + forward = pl_y_; + } + else + { + // 越过障碍物: 前进固定距离 + forward = 8 * 0.3; // 8格 × 0.3m = 2.4m + } + + double abs_lon, abs_lat; + relative_to_gps(lateral, forward, g_rtk.direction, g_rtk.lon, g_rtk.lat, &abs_lon, &abs_lat); + + avoidance_waypoints_.push_back(Waypoint(lateral, forward, SPEED, abs_lon, abs_lat)); + current_waypoint_idx_ = 0; + avoiding_obstacle_ = true; + state_machine_->setState(StateMachine::ADVANCE); + + LOG_INFO("[前进规划] 模式:%s 横移:%.2f 前进:%.2fm GPS(%.6f,%.6f)", return_mode ? "回切" : "越过", lateral, + forward, abs_lon, abs_lat); + } + + // ======== 侧方清空判断（障碍物是否结束） ======== + bool isSideClear(const std::vector>& grid) const + { + int dir = state_machine_->avoid_direction; + if (dir == 0) return false; + + int col_start, col_end; + int safe_margin = 2; + if (dir == -1) + { + col_start = 0; + col_end = vehicle_min_y_ - safe_margin; + } + else + { + col_start = vehicle_max_y_ + safe_margin; + col_end = (int)grid[0].size() - 1; + } + + if (col_start > col_end) return true; + + int row_min = max(0, vehicle_min_x_ - params_.front_area_extend); + int row_max = vehicle_max_x_; + + for (int row = row_min; row <= row_max; ++row) + for (int col = col_start; col <= col_end; ++col) + if (grid[row][col] == OBSTACLE) return false; + + return true; + } + + // ======== 路点到达判断 ======== + // 返回: 1=到达, 0=未到达, -1=RTK丢失 + int waypointReached(const Waypoint& target) + { + auto now = system_clock::now(); + duration elapsed = now - last_rtk_time; + + if (g_rtk.reliability != 1 || elapsed > rtk_timeout) + { + LOG_ERROR("[路点判断] RTK信号丢失(%.2fs)，停车退出绕障！", elapsed.count()); return -1; } + + double dist = gps_distance(g_rtk.lon, g_rtk.lat, target.lon, target.lat); + LOG_INFO("[路点判断] 当前(%.6f,%.6f) 目标(%.6f,%.6f) 距离:%.2fm 容差:%.2fm", g_rtk.lon, g_rtk.lat, target.lon, + target.lat, dist, params_.waypoint_tolerance); + return dist < params_.waypoint_tolerance ? 1 : 0; } // ======== 路点跟踪执行 ======== @@ -859,147 +589,125 @@ class fu_node : public rclcpp::Node { if (current_waypoint_idx_ >= (int)avoidance_waypoints_.size()) { - avoiding_obstacle_ = false; - current_waypoint_idx_ = 0; - avoidance_waypoints_.clear(); - state_machine_->setState(StateMachine::NORMAL); - LOG_INFO("[绕障完成] 所有路点跟踪完毕，恢复全局规划控制"); + finishAvoidance("所有路点跟踪完毕"); return; } Waypoint& target = avoidance_waypoints_[current_waypoint_idx_]; - - // 计算目标方向角： - float target_angle = calculate_the_angle(target.x, target.y); - state_machine_->publish_angle = target_angle; + float angle = calculate_steering_angle(target.x, target.y); + state_machine_->publish_angle = angle; state_machine_->publish_speed = target.speed; - LOG_INFO("[路点跟踪] 路点%d/%d | 相对坐标(%.2f,%.2f) | 目标方向: %.2f度", current_waypoint_idx_ + 1, - (int)avoidance_waypoints_.size(), target.x, target.y, target_angle); + LOG_INFO("[路点跟踪] %d/%d | 目标(%.2f,%.2f) | 角度:%.1f", current_waypoint_idx_ + 1, + (int)avoidance_waypoints_.size(), target.x, target.y, angle); - // 检查路点到达情况 int reached = waypointReached(target); if (reached == -1) { - // RTK信号丢失，立即停车并退出绕障 + // RTK丢失 state_machine_->publish_speed = 0; state_machine_->publish_angle = 0.0f; state_machine_->setState(StateMachine::WAITING); - avoiding_obstacle_ = false; current_waypoint_idx_ = 0; avoidance_waypoints_.clear(); - - LOG_ERROR("[绕障中止] RTK信号丢失，停车并退出绕障！"); + LOG_ERROR("[绕障中止] RTK信号丢失！"); return; } - else if (reached == 1) - { - // 已到达路点，切换到下一个 - current_waypoint_idx_++; - if (current_waypoint_idx_ < (int)avoidance_waypoints_.size()) + if (reached == 1) + { + current_waypoint_idx_++; + if (current_waypoint_idx_ >= (int)avoidance_waypoints_.size()) { - state_machine_->setState(StateMachine::PARALLEL_MOVE); - LOG_INFO("[路点跟踪] 已到达路点%d，切换到路点%d", current_waypoint_idx_, current_waypoint_idx_ + 1); - } - else - { - avoiding_obstacle_ = false; - current_waypoint_idx_ = 0; - avoidance_waypoints_.clear(); - state_machine_->setState(StateMachine::NORMAL); - LOG_INFO("[绕障完成] 所有路点跟踪完毕，恢复全局规划控制"); + // 所有航点完成, 检查是否需要继续绕障 + onWaypointSequenceComplete(); return; } - } - else - { - // 未到达路点，继续前往 - state_machine_->setState(StateMachine::LATERAL_SHIFT); + LOG_INFO("[路点跟踪] 到达路点%d，切换到下一个", current_waypoint_idx_); } - // 安全检查：前方必停区有障碍物则立即停车 + // 安全检查 if (state_machine_->obstacle_analysis.has_front_critical) { state_machine_->publish_speed = 0; state_machine_->publish_angle = 0.0f; state_machine_->setState(StateMachine::WAITING); - LOG_WARN("[绕障安全] 路点跟踪中检测到前方必停区障碍物，立即停车"); - - // 立即发布停车指令（带刹车） publishControlCommand(true); avoiding_obstacle_ = false; current_waypoint_idx_ = 0; avoidance_waypoints_.clear(); - return; + LOG_WARN("[绕障安全] 前方必停区障碍物，停车"); } } - // ======== 可绕障判断 ======== - bool canAvoidObstacle() const + // ======== 一轮航点序列完成后 ======== + void onWaypointSequenceComplete() { - if (state_machine_->obstacle_analysis.obstacle_width <= 0 || - state_machine_->obstacle_analysis.obstacle_width > avoid_able_width_) + StateMachine::State current = state_machine_->getCurrentState(); + + if (current == StateMachine::SHIFT_SIDE) { - LOG_INFO("[绕障判断] 障碍物宽度%d栅格，超过可绕最大宽度%d栅格，不可绕", - state_machine_->obstacle_analysis.obstacle_width, avoid_able_width_); - return false; - } - bool left_ok = state_machine_->obstacle_analysis.free_space_left >= min_free_space_; - bool right_ok = state_machine_->obstacle_analysis.free_space_right >= min_free_space_; - LOG_INFO("[绕障判断] 左侧空闲%d栅格，右侧空闲%d栅格，最小需求%d栅格 | 左可绕：%s，右可绕：%s", - state_machine_->obstacle_analysis.free_space_left, state_machine_->obstacle_analysis.free_space_right, - min_free_space_, left_ok ? "是" : "否", right_ok ? "是" : "否"); - return left_ok || right_ok; - } + // 侧移到位, 重新感知栅格 + // 获取最新栅格判断侧方 + std::vector> grid; + { + std::lock_guard lock(grid_mutex_); + grid = cached_grid_; + } - // ======== 绕障方向选择 ======== - int selectOptimalAvoidDirection() const - { - if (state_machine_->obstacle_analysis.free_space_left > state_machine_->obstacle_analysis.free_space_right) - return -1; - else if (state_machine_->obstacle_analysis.free_space_right > state_machine_->obstacle_analysis.free_space_left) - return 1; + if (!grid.empty() && isSideClear(grid)) + { + // 侧方清空 = 障碍物结束, 前进越过 + LOG_INFO("[绕障] 侧方清空，障碍物已结束，开始前进越过"); + planAdvance(false); + } + else + { + // 侧方还有障碍物, 继续侧移 + LOG_INFO("[绕障] 侧方仍有障碍物，继续侧移"); + planSideShift(); + } + } + else if (current == StateMachine::ADVANCE) + { + // 前进到位, 检查PL偏差 + if (std::abs(pl_x_) < 0.5) + { + // 已回到路径附近 + finishAvoidance("前进完成，已回到路径附近"); + } + else + { + // 还离路径远，继续回切 + LOG_INFO("[绕障] PL偏差%.2fm，继续回切", pl_x_); + planAdvance(true); + } + } else - return (pl_x_ > 0) ? 1 : -1; - } - - // ======== 更新障碍物分析 ======== - void updateObstacleAnalysis() - { - state_machine_->obstacle_analysis.has_front_critical = obstacle_in_front_; - state_machine_->obstacle_analysis.has_front_area = obstacle_in_front_area_; - state_machine_->obstacle_analysis.has_left_critical = obstacle_in_left_; - state_machine_->obstacle_analysis.has_right_critical = obstacle_in_right_; - state_machine_->obstacle_analysis.obstacle_distance = (obstacle_max_x_ > 0) ? obstacle_max_x_ : -1; - state_machine_->obstacle_analysis.left_edge = obstacle_left_edge_; - state_machine_->obstacle_analysis.right_edge = obstacle_right_edge_; - state_machine_->obstacle_analysis.free_space_left = free_space_left_; - state_machine_->obstacle_analysis.free_space_right = free_space_right_; - - if (obstacle_left_edge_ != -1 && obstacle_right_edge_ != -1) { - state_machine_->obstacle_analysis.obstacle_width = obstacle_right_edge_ - obstacle_left_edge_ + 1; + finishAvoidance("航点序列完成"); } - - LOG_INFO_THROTTLE(1000, "[障碍物分析] 距离：%d栅格 | 宽度：%d栅格 | 左边缘：%d | 右边缘：%d", - state_machine_->obstacle_analysis.obstacle_distance, - state_machine_->obstacle_analysis.obstacle_width, state_machine_->obstacle_analysis.left_edge, - state_machine_->obstacle_analysis.right_edge); } - // ======== 计算行驶状态 ======== + void finishAvoidance(const char* reason) + { + avoiding_obstacle_ = false; + current_waypoint_idx_ = 0; + avoidance_waypoints_.clear(); + state_machine_->setState(StateMachine::NORMAL); + LOG_INFO("[绕障完成] %s，恢复正常行驶", reason); + } + + // ===================================================================== + // 行驶状态计算 + // ===================================================================== + int16_t calculateDrivingStatus() { - // 状态6: 驻车（任务开始前） - if (!is_start) - { - return 6; - } + if (!is_start) return 6; - // 检查栅格数据新鲜度（超过3秒认为数据过期） auto now = node_clock_->now(); double grid_age = 0.0; { @@ -1007,70 +715,36 @@ class fu_node : public rclcpp::Node grid_age = (now - last_grid_time_).seconds(); } - // 检查RTK信号 auto rtk_now = system_clock::now(); - duration time_since_last_rtk = rtk_now - last_rtk_time; - bool rtk_signal_good = (g_rtk.reliability == 1 && time_since_last_rtk <= rtk_timeout); - // LOG_INFO("rtk_signal_good ? %d",rtk_signal_good); + bool rtk_ok = (g_rtk.reliability == 1 && (rtk_now - last_rtk_time) <= rtk_timeout); - // 状态3: 雷达数据超时，停车 - if (grid_age > 3.0) - { - return 3; - } + if (grid_age > 3.0) return 3; + if (!rtk_ok) return 4; + if (pl_speed_ == 0 && is_start) return 5; - // 状态4: RTK没信号，停车 - if (!rtk_signal_good) - { - return 4; - } + StateMachine::State st = state_machine_->getCurrentState(); + if (st == StateMachine::SHIFT_SIDE || st == StateMachine::ADVANCE || st == StateMachine::RETURN_PATH) return 2; + if (st == StateMachine::WAITING && state_machine_->obstacle_analysis.has_front_critical) return 1; - // 状态5: 到达终点，停车 (PL速度为0) - if (pl_speed_ == 0 && is_start) - { - return 5; - } - - // 状态2: 绕障中 (LATERAL_SHIFT 或 PARALLEL_MOVE) - StateMachine::State current_state = state_machine_->getCurrentState(); - if (current_state == StateMachine::LATERAL_SHIFT || current_state == StateMachine::PARALLEL_MOVE) - { - return 2; - } - - // 状态1: 遇障停车，停车 (WAITING状态且有障碍物) - if (current_state == StateMachine::WAITING && state_machine_->obstacle_analysis.has_front_critical) - { - return 1; - } - - // 状态0: 默认值，正常行驶 return 0; } - // ======== 紧急停车执行 ======== bool executeEmergencyStop() { - if (!enable_obstacle_stop_) return false; + if (!params_.enable_obstacle_stop) return false; - // 检查栅格数据新鲜度（超过3秒认为数据过期） auto now = node_clock_->now(); - // 如果有有效栅格数据，即使稍微过期也继续使用（只要数据不是太旧） double grid_age = 0.0; - bool is_grid_valid = false; + bool grid_valid = false; { std::lock_guard lock(grid_mutex_); grid_age = (now - last_grid_time_).seconds(); - is_grid_valid = grid_data_valid_; + grid_valid = grid_data_valid_; } - if (is_grid_valid && grid_age <= 3.0) + if (grid_age > 3.0) { - // 数据在3秒内，继续使用现有数据进行障碍物检测 - } - else if (grid_age > 3.0) - { - LOG_WARN("[紧急停车] 栅格数据过期(%.3fs)，执行安全停车！", grid_age); + LOG_WARN("[紧急停车] 栅格过期(%.3fs)", grid_age); state_machine_->publish_speed = 0; state_machine_->publish_angle = 0.0f; state_machine_->setState(StateMachine::WAITING); @@ -1082,161 +756,142 @@ class fu_node : public rclcpp::Node state_machine_->publish_speed = 0; state_machine_->publish_angle = 0.0f; state_machine_->setState(StateMachine::WAITING); - LOG_WARN("[紧急停车] 前方必停区检测到障碍物，执行紧急停车"); + LOG_WARN("[紧急停车] 前方必停区障碍物"); return true; } return false; } - // ======== 定时器回调（主逻辑） ======== + // ===================================================================== + // 定时器主逻辑 + // ===================================================================== + void timer_callback() { - // 更新障碍物分析 updateObstacleAnalysis(); - // 检查栅格数据超时（不依赖任务是否开始） auto now = node_clock_->now(); double grid_age = 0.0; - bool is_grid_valid = false; { std::lock_guard lock(grid_mutex_); grid_age = (now - last_grid_time_).seconds(); - is_grid_valid = grid_data_valid_; } + if (grid_age > 3.0) LOG_WARN("[栅格超时] %.3fs", grid_age); - if (grid_age > 3.0) LOG_WARN("[栅格超时检测] 栅格数据已过期(%.3fs)！", grid_age); - // else - // LOG_INFO_THROTTLE(1000,"[栅格超时检测] 栅格数据正常！"); - - // 开始清扫任务 if (is_start) { - // 超声波传感器近距离障碍物检测 - if (enable_obstacle_stop_) + // 超声波检测 + if (params_.enable_obstacle_stop) { auto uss_now = node_clock_->now(); bool uss_valid = false; - uint16_t front_left = 0, front_right = 0; + uint16_t fl = 0, fr = 0; double uss_age = 0.0; { std::lock_guard lock(uss_mutex_); uss_age = (uss_now - last_uss_time_).seconds(); uss_valid = uss_data_valid_; - front_left = uss_data_[0]; - front_right = uss_data_[1]; + fl = uss_data_[0]; + fr = uss_data_[1]; } - if (uss_valid && uss_age < 1.0) { - if ((front_left > 0 && front_left < (uint16_t)uss_stop_distance_) || - (front_right > 0 && front_right < (uint16_t)uss_stop_distance_)) + if ((fl > 0 && fl < (uint16_t)params_.uss_stop_distance) || + (fr > 0 && fr < (uint16_t)params_.uss_stop_distance)) { state_machine_->publish_speed = 0; state_machine_->publish_angle = 0.0f; state_machine_->setState(StateMachine::WAITING); - LOG_WARN("[超声波停车] 前方检测到障碍物！左:%ucm 右:%ucm 阈值:%dcm，立即停车！", - front_left, front_right, uss_stop_distance_); - publishControlCommand(true, true); // 紧急停车并关闭清扫 + LOG_WARN("[超声波停车] 左:%u 右:%u 阈值:%d", fl, fr, params_.uss_stop_distance); + publishControlCommand(true, true); return; } } } - // 紧急停车优先级最高 + // 紧急停车 if (executeEmergencyStop()) { - publishControlCommand(true, true); // 紧急停车使用刹车并关闭清扫 + publishControlCommand(true, true); return; } - // PL速度为0，停车 + // PL停车 if (pl_speed_ == 0) { state_machine_->publish_speed = 0; state_machine_->publish_angle = 0; - LOG_INFO("PL速度为0，已停车"); - publishControlCommand(false, true); // 停车时关闭清扫部件 + publishControlCommand(false, true); return; } - // 若之前处于WAITING状态，且现在没有障碍物且RTK信号良好，恢复到NORMAL状态 - auto now = system_clock::now(); - duration time_since_last_rtk = now - last_rtk_time; - bool rtk_signal_good = (g_rtk.reliability == 1 && time_since_last_rtk <= rtk_timeout); - + // WAITING恢复 + auto rtk_now = system_clock::now(); + bool rtk_ok = (g_rtk.reliability == 1 && (rtk_now - last_rtk_time) <= rtk_timeout); if (state_machine_->getCurrentState() == StateMachine::WAITING && - !state_machine_->obstacle_analysis.has_front_critical && rtk_signal_good) + !state_machine_->obstacle_analysis.has_front_critical && rtk_ok) { state_machine_->setState(StateMachine::NORMAL); - LOG_INFO("[状态恢复] 障碍物已消失且RTK信号良好，恢复正常行驶状态"); + LOG_INFO("[状态恢复] 障碍物消失+RTK恢复，正常行驶"); } // 绕障逻辑 - if (enable_obstacle_avoid_) + if (params_.enable_obstacle_avoid) { if (avoiding_obstacle_) { - // 正在绕障，执行路点跟踪 executeWaypointTracking(); } else if (state_machine_->obstacle_analysis.has_front_area && canAvoidObstacle()) { - // 需要开始绕障 - int avoid_dir = selectOptimalAvoidDirection(); - planAvoidanceWaypoints(avoid_dir); - LOG_INFO("[启动绕障] 开始%s侧绕障", avoid_dir == -1 ? "左" : "右"); + planSideShift(); + LOG_INFO("[启动绕障] 开始%s侧绕障", state_machine_->avoid_direction == -1 ? "左" : "右"); } else if (state_machine_->getCurrentState() != StateMachine::NORMAL) { - // 无需绕障，恢复正常状态 state_machine_->setState(StateMachine::NORMAL); } } - // 正常行驶逻辑 + // 正常行驶 if (state_machine_->getCurrentState() == StateMachine::NORMAL) { state_machine_->publish_speed = 800; - state_machine_->publish_angle = calculate_the_angle(pl_x_, pl_y_); + state_machine_->publish_angle = calculate_steering_angle(pl_x_, pl_y_); } - // 发布控制指令 publishControlCommand(); } else { - // 任务未运行时，发送停止命令，确保车辆和清扫部件都停止 state_machine_->publish_speed = 0; state_machine_->publish_angle = 0; - publishControlCommand(false, true); // emergency_brake=false, disable_sweep=true + publishControlCommand(false, true); } } - // ======== 发布控制指令 ======== + // ===================================================================== + // 控制发布 + // ===================================================================== + void publishControlCommand(bool emergency_brake = false, bool disable_sweep = false) { - auto message = sweeper_interfaces::msg::McCtrl(); + auto msg = sweeper_interfaces::msg::McCtrl(); + msg.enable_main_brush = !disable_sweep; + msg.enable_vacuum = !disable_sweep; + msg.enable_dust_shake = false; + msg.enable_main_brush_pole = !disable_sweep; + msg.enable_flap_pole = !disable_sweep; + msg.enable_side_brush = !disable_sweep; + msg.sweep_mode = current_task_mode; + msg.enable_water_pump = (!disable_sweep && msg.sweep_mode == 1); + msg.brake = emergency_brake ? 1 : 0; + msg.gear = 2; + msg.angle_speed = 800; + msg.rpm = std::clamp(state_machine_->publish_speed, 0, 800); + msg.angle = std::clamp(state_machine_->publish_angle, -50.0f, 50.0f); + pub_mc->publish(msg); - message.enable_main_brush = !disable_sweep; - message.enable_vacuum = !disable_sweep; - message.enable_dust_shake = false; - message.enable_main_brush_pole = !disable_sweep; - message.enable_flap_pole = !disable_sweep; - message.enable_side_brush = !disable_sweep; - message.sweep_mode = current_task_mode; // 清扫模式: 0=标准模式(刷子), 1=混合模式(刷子+洒水) - message.enable_water_pump = (!disable_sweep && message.sweep_mode == 1); - - message.brake = emergency_brake ? 1 : 0; // 紧急停车时启用刹车 - message.gear = 2; - message.angle_speed = 800; - - // 转速限制 - message.rpm = std::clamp(state_machine_->publish_speed, 0, 800); - message.angle = std::clamp(state_machine_->publish_angle, -50.0f, 50.0f); - - pub_mc->publish(message); - - // 发布 Fu 消息，包含行驶状态 auto fu_msg = sweeper_interfaces::msg::Fu(); fu_msg.angle = state_machine_->publish_angle; fu_msg.speed = state_machine_->publish_speed; @@ -1246,30 +901,22 @@ class fu_node : public rclcpp::Node pub_fu->publish(fu_msg); if (emergency_brake) - { - LOG_WARN("[控制发布] 紧急刹车！状态：%s | 转速：%d | 转向角度：%.1f° | 刹车：%d | 行驶状态：%d", - state_machine_->getStateString().c_str(), message.rpm, message.angle, message.brake, - fu_msg.driving_status); - } + LOG_WARN("[控制] 紧急刹车! %s | rpm=%d angle=%.1f brake=%d status=%d", + state_machine_->getStateString().c_str(), msg.rpm, msg.angle, msg.brake, fu_msg.driving_status); else - { - LOG_INFO("[控制发布] 状态：%s | 转速：%d | 转向角度：%.1f° | 行驶状态：%d", - state_machine_->getStateString().c_str(), message.rpm, message.angle, fu_msg.driving_status); - } + LOG_INFO("[控制] %s | rpm=%d angle=%.1f status=%d", state_machine_->getStateString().c_str(), msg.rpm, + msg.angle, fu_msg.driving_status); } }; -// ======== 主函数 ======== +// ================== 主函数 ================== int main(int argc, char** argv) { logger::Logger::Init("fu", "./nodes_log"); - rclcpp::init(argc, argv); auto node = std::make_shared("fu_node"); rclcpp::spin(node); rclcpp::shutdown(); - logger::Logger::Shutdown(); - return 0; -} \ No newline at end of file +}