recharge/src3/wheelchair_state_machine/include/wheelchair_state_machine/lidascan_ctrl.hpp

// lidascan_ctrl.hpp
#ifndef LIDAR_SCAN_CONTROLLER_HPP
#define LIDAR_SCAN_CONTROLLER_HPP

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include "wheelchair_types.hpp"
#include "recharge_tool.hpp"
#include <map>
#include <memory>
#include <tuple>

class LidarScanController
{
public:
    using LaserScan = sensor_msgs::msg::LaserScan;
    using LaserScanSharedPtr = sensor_msgs::msg::LaserScan::SharedPtr;

    LidarScanController(rclcpp::Node *node);
    ~LidarScanController();

    // 初始化
    void initialize();

    // 导航控制接口（外部调用）
    void startNavigation();
    void stopNavigation();
    void stopMotion();

    // 充电电压检测接口
    void setChargingVoltage(bool value);
    bool getChargingVoltage() const;

    // 状态查询
    bool isActive() const { return active_; }
    ChargingState getChargingState() const { return charging_state_; }

private:
    rclcpp::Node *node_;

    // ROS2 订阅者和发布者
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_ctrl_;
    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_rviz_;
    rclcpp::TimerBase::SharedPtr navigation_timer_;
    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_filtered_;
    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_line_;

    // 状态变量
    bool active_;
    ChargingState charging_state_;
    geometry_msgs::msg::Twist ctrl_twist_;
    rclcpp::Time ctrl_twist_time_;
    rclcpp::Time detect_station_time_;

    // 参数
    ScanFilterParam param_;
    double distance_threshold_;
    int fit_window_size_;
    double fit_residual_threshold_;
    int frame_index_;

    // 充电检测
    bool detect_charging_voltage_;
    rclcpp::Time detect_charging_voltage_time_;

    // ========== 核心算法函数 ==========

    // 激光数据处理
    void processLaserScan(const LaserScanSharedPtr &msg);
    bool processSegmentData(const LaserScanSharedPtr &msg, int begin_index, int end_index);
    void procData(RechargeResult &result);

    // 运动控制算法
    void controlRechargeMotion(const RechargeResult &result);

    // 过滤算法
    void filterScanModify0(LaserScanSharedPtr msg);
    void filterScanModify1(LaserScanSharedPtr msg);
    void filterScanModify2(LaserScanSharedPtr msg);
    void filterScanModify3(LaserScanSharedPtr msg);
    std::pair<int, int> filterScanModify4(LaserScanSharedPtr msg);

    // 坐标转换
    std::map<int, std::pair<double, double>> laserScanToXY(const LaserScanSharedPtr &scan_msg);
    std::map<int, std::pair<double, double>> laserScanToXY1(int begin_index, int end_index,
                                                            const LaserScanSharedPtr &scan_msg);

    // 分段处理算法
    std::vector<std::pair<int, int>> getSections(
        int begin_index, int end_index,
        const LaserScanSharedPtr &msg,
        const std::map<int, std::pair<double, double>> &xy_list);

    std::map<int, std::tuple<int, int, double, double, double, double, double, int>>
    getSegments(const std::map<int, std::pair<double, double>> &xy_points_map);

    std::map<int, std::tuple<int, int, double, double, double, double, double, int>>
    filterSegments(const std::map<int, std::tuple<int, int, double, double, double, double, double, int>> &turn_segments);

    // 可视化
    void publishRechargeVisualization(const RechargeResult &result);

    // ========== 辅助函数 ==========
    void publishFilteredScan(const LaserScanSharedPtr &original_msg,
                             const std::vector<float> &filtered_ranges);
    LaserScanSharedPtr resetLaserData(LaserScanSharedPtr msg, int b_index, int e_index);
    bool isValid(float value) const;
    int getFixedIndex(const LaserScanSharedPtr &msg, int i) const;
    int getFixed() const { return 0; }
    Eigen::MatrixXd getPointList(const std::vector<IndexPoint> &points);

    // 定时器回调
    void navigationTimerCallback();

    // 控制发布
    void publishControl();
};

#endif // LIDAR_SCAN_CONTROLLER_HPP