端到端走读：一帧图像的旅程¶

// 相机驱动核心循环
while (rclcpp::ok()) {
    MV_FRAME_OUT frame;
    int ret = MV_CC_GetImageBuffer(handle, &frame, 1000);
    if (ret != MV_OK) continue;

    auto msg = std::make_unique<sensor_msgs::msg::Image>();
    msg->header.stamp = this->now();
    msg->header.frame_id = "camera_optical_frame";
    msg->width = frame.stFrameInfo.nWidth;       // 1440
    msg->height = frame.stFrameInfo.nHeight;      // 1080
    msg->encoding = "rgb8";
    msg->step = frame.stFrameInfo.nWidth * 3;
    msg->data.assign(frame.pBufAddr,
                     frame.stFrameInfo.nFrameLen);

    image_pub_->publish(std::move(msg));
    MV_CC_FreeImageBuffer(handle, &frame);
}

void DetectorNode::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) {
    // ROS Image → OpenCV Mat
    cv::Mat rgb = cv_bridge::toCvCopy(msg, "rgb8")->image;
    cv::Mat gray, binary;

    // 转灰度
    cv::cvtColor(rgb, gray, cv::COLOR_RGB2GRAY);

    // 固定阈值二值化
    cv::threshold(gray, binary, 80, 255, cv::THRESH_BINARY);

    // 后续灯条检测使用 binary
    detect_lights(binary, gray, lights);
}

void DetectorNode::detect_lights(const cv::Mat & binary,
                                  const cv::Mat & gray,
                                  std::vector<LightBar> & lights) {
    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(binary, contours, cv::RETR_EXTERNAL,
                     cv::CHAIN_APPROX_NONE);

    for (const auto & contour : contours) {
        if (contour.size() < 6) continue;  // 面积太小跳过

        cv::RotatedRect rect = cv::minAreaRect(contour);

        // 灯条长宽比过滤：灯条是细长的，width/height 应在 0.1~0.4
        float ratio = std::min(rect.size.width, rect.size.height)
                    / std::max(rect.size.width, rect.size.height);
        if (ratio < 0.1 || ratio > 0.4) continue;

        // 角度过滤：灯条大致垂直，倾斜不超过 40°
        float angle = rect.angle;
        if (rect.size.width > rect.size.height) {
            angle = angle + 90;
        }
        if (std::abs(angle) > 40.0) continue;

        // 颜色判断：比较 R 通道和 B 通道
        LightBar light;
        light.center = rect.center;
        light.length = std::max(rect.size.width, rect.size.height);
        light.width  = std::min(rect.size.width, rect.size.height);
        light.angle  = angle;
        light.color  = get_color(gray, rect);

        lights.push_back(light);
    }
}

int DetectorNode::get_color(const cv::Mat & gray,
                             const cv::RotatedRect & rect) {
    // 在灰度图上截取灯条区域
    cv::Mat roi;
    cv::getRectSubPix(gray, rect.size, rect.center, roi);
    float mean_val = cv::mean(roi)[0];

    // 更精确的做法：在原 RGB 图上比较 R/B 通道
    // 这里简化：高亮区域偏红还是偏蓝
    // 实际代码中会对 rgb 图做 R-B 通道比较
    return (mean_val > 128) ? RED : BLUE;
}

void DetectorNode::match_armors(const std::vector<LightBar> & lights,
                                 std::vector<Armor> & armors) {
    for (size_t i = 0; i < lights.size(); i++) {
        for (size_t j = i + 1; j < lights.size(); j++) {
            // 必须同色
            if (lights[i].color != lights[j].color) continue;

            // 长度比过滤：两个灯条长度应接近
            float len_ratio = std::min(lights[i].length, lights[j].length)
                            / std::max(lights[i].length, lights[j].length);
            if (len_ratio < 0.8) continue;

            // 两灯条中心距 vs 平均长度 的比值判断类型
            float dist = cv::norm(lights[i].center - lights[j].center);
            float avg_len = (lights[i].length + lights[j].length) / 2.0f;
            float dist_ratio = dist / avg_len;

            Armor armor;
            armor.left = lights[i];
            armor.right = lights[j];
            armor.type = (dist_ratio < 3.2) ? SMALL : LARGE;

            // 角度差：两个灯条倾斜方向应大致一致
            float angle_diff = std::abs(lights[i].angle - lights[j].angle);
            if (angle_diff > 15.0) continue;

            armors.push_back(armor);
        }
    }
}

void DetectorNode::classify_armors(cv::Mat & src,
                                    std::vector<Armor> & armors) {
    for (auto & armor : armors) {
        // 1. 构造 ROI 四个角点（灯条两端扩展）
        std::vector<cv::Point2f> roi_pts = get_armor_roi(armor);

        // 2. 透视变换校正
        std::vector<cv::Point2f> dst_pts = {
            {0, 0}, {20, 0}, {20, 28}, {0, 28}
        };
        cv::Mat M = cv::getPerspectiveTransform(roi_pts, dst_pts);
        cv::Mat roi;
        cv::warpPerspective(src, roi, M, cv::Size(20, 28));

        // 3. 二值化
        cv::cvtColor(roi, roi, cv::COLOR_RGB2GRAY);
        cv::threshold(roi, roi, 0, 255,
                      cv::THRESH_BINARY | cv::THRESH_OTSU);

        // 4. MLP 推理
        cv::Mat input = roi.reshape(1, 1);  // 展平为 1×560
        input.convertTo(input, CV_32F, 1.0 / 255.0);

        cv::Mat output;
        mlp_->predict(input, output);       // 输出 1×9

        // 5. 取最大置信度
        double max_val;
        cv::Point max_loc;
        cv::minMaxLoc(output, nullptr, &max_val, nullptr, &max_loc);

        if (max_val > 0.8) {
            armor.number = max_loc.x + 1;   // 数字 1~9
            armor.confidence = max_val;
        } else {
            armor.number = 0;               // 未识别
        }
    }
}

// 加载预训练的 MLP 模型
cv::Ptr<cv::ml::ANN_MLP> mlp_ = cv::ml::ANN_MLP::load("mlp.xml");

// 网络结构: 560(输入) → 256(隐藏层) → 9(输出层)
// 560 = 20 × 28（ROI图像展平）
// 9 个输出对应数字 1~9

std::vector<cv::Point2f> DetectorNode::get_armor_roi(const Armor & armor) {
    // 从两个灯条的四个端点提取装甲板区域
    auto tl = armor.left.center  + cv::Point2f(-armor.left.width * 0.5, 0);
    auto bl = armor.left.center  + cv::Point2f( armor.left.width * 0.5, 0);
    auto tr = armor.right.center + cv::Point2f(-armor.right.width * 0.5, 0);
    auto br = armor.right.center + cv::Point2f( armor.right.width * 0.5, 0);
    return {tl, tr, br, bl};
}

void DetectorNode::solve_pnp(Armor & armor) {
    // 装甲板 3D 模型点（单位：米）
    // 小型装甲板: 135mm × 55mm
    std::vector<cv::Point3f> object_points;
    if (armor.type == SMALL) {
        float h = 0.0675f, w = 0.0275f;
        object_points = {
            {-h, -w, 0}, { h, -w, 0},
            { h,  w, 0}, {-h,  w, 0}
        };
    } else {  // LARGE: 225mm × 55mm
        float h = 0.1125f, w = 0.0275f;
        object_points = {
            {-h, -w, 0}, { h, -w, 0},
            { h,  w, 0}, {-h,  w, 0}
        };
    }

    // 装甲板 4 个 2D 像素点
    std::vector<cv::Point2f> image_points = {
        armor.left.center,
        armor.right.center,
        // ... 四个角点
    };

    // PnP 求解
    cv::Mat rvec, tvec;
    cv::solvePnP(object_points, image_points,
                 camera_matrix_, dist_coeffs_,
                 rvec, tvec, false,
                 cv::SOLVEPNP_IPPE);

    // 旋转向量 → 四元数
    cv::Mat R;
    cv::Rodrigues(rvec, R);
    Eigen::Matrix3d eigen_R;
    cv::cv2eigen(R, eigen_R);
    Eigen::Quaterniond q(eigen_R);

    armor.pose.position = {tvec.at<double>(0),
                           tvec.at<double>(1),
                           tvec.at<double>(2)};
    armor.pose.orientation = {q.w(), q.x(), q.y(), q.z()};
}

void DetectorNode::publish_armors(const std::vector<Armor> & armors) {
    auto msg = std::make_unique<rm_interfaces::msg::ArmorArray>();
    msg->header.stamp = this->now();
    msg->header.frame_id = "camera_optical_frame";

    for (const auto & armor : armors) {
        rm_interfaces::msg::Armor armor_msg;
        armor_msg.number = armor.number;
        armor_msg.type = (armor.type == SMALL) ? "small" : "large";
        armor_msg.pose.position.x = armor.pose.position.x();
        armor_msg.pose.position.y = armor.pose.position.y();
        armor_msg.pose.position.z = armor.pose.position.z();
        armor_msg.pose.orientation.w = armor.pose.orientation.w();
        armor_msg.pose.orientation.x = armor.pose.orientation.x();
        armor_msg.pose.orientation.y = armor.pose.orientation.y();
        armor_msg.pose.orientation.z = armor.pose.orientation.z();
        msg->armors.push_back(armor_msg);
    }

    armors_pub_->publish(std::move(msg));
}

void TrackerNode::armors_callback(
    const rm_interfaces::msg::ArmorArray::SharedPtr msg) {
    // 查询 TF: camera_optical_frame → odom
    geometry_msgs::msg::TransformStamped tf;
    try {
        tf = tf_buffer_->lookupTransform(
            "odom", "camera_optical_frame",
            msg->header.stamp,  // 使用消息时间戳查询
            rclcpp::Duration::from_seconds(0.05));
    } catch (tf2::TransformException & ex) {
        RCLCPP_WARN(this->get_logger(), "TF failed: %s", ex.what());
        return;
    }

    // 将每个装甲板位姿从相机坐标系变换到 odom 坐标系
    for (auto & armor : msg->armors) {
        geometry_msgs::msg::PoseStamped pose_in, pose_out;
        pose_in.header = msg->header;
        pose_in.pose = armor.pose;
        tf2::doTransform(pose_in, pose_out, tf);

        // pose_out 现在在 odom 坐标系下
        process_armor(armor, pose_out.pose);
    }
}

void Tracker::predict(double dt) {
    // 状态转移矩阵 F
    // x' = x + vx * dt  (位置 = 位置 + 速度×时间)
    // vx' = vx          (假设匀速模型)
    F_ = Eigen::Matrix<double, 11, 11>::Identity();
    F_(0, 1) = dt;   // xc += vxc * dt
    F_(2, 3) = dt;   // yc += vyc * dt
    F_(4, 5) = dt;   // zc += vzc * dt
    F_(6, 7) = dt;   // yaw += vyaw * dt

    // 预测状态
    x_ = F_ * x_;

    // 预测协方差
    P_ = F_ * P_ * F_.transpose() + Q_;
}

void Tracker::assign(const std::vector<Armor> & detections) {
    // 从 EKF 状态预测装甲板在球坐标中的位置
    double h_armor_x = x_(0);  // 预测的 x
    double h_armor_y = x_(2);  // 预测的 y
    double h_armor_z = x_(4);  // 预测的 z

    // 计算每个检测与预测位置的角度误差
    std::vector<double> costs;
    for (const auto & det : detections) {
        double dx = det.pose.position.x - h_armor_x;
        double dy = det.pose.position.y - h_armor_y;
        double dz = det.pose.position.z - h_armor_z;
        double dist = std::sqrt(dx*dx + dy*dy + dz*dz);

        // 球坐标角度误差
        double cost = std::atan2(dist,
            std::sqrt(h_armor_x*h_armor_x +
                      h_armor_y*h_armor_y +
                      h_armor_z*h_armor_z));
        costs.push_back(cost);
    }

    // 取角度误差最小的检测作为匹配
    auto min_it = std::min_element(costs.begin(), costs.end());
    if (min_it != costs.end() && *min_it < max_match_angle_) {
        matched_idx_ = std::distance(costs.begin(), min_it);
    } else {
        matched_idx_ = -1;  // 无匹配
    }
}

void Tracker::update(const Armor & detection) {
    // 观测向量 z: 检测到的装甲板位置
    Eigen::Vector3d z;
    z << detection.pose.position.x,
         detection.pose.position.y,
         detection.pose.position.z;

    // 观测函数 h(x): 从状态预测观测
    // 直接观测装甲板中心坐标
    Eigen::Vector3d h_x;
    h_x << x_(0), x_(2), x_(4);

    // 观测矩阵 H (3×11)
    Eigen::Matrix<double, 3, 11> H = Eigen::Matrix<double, 3, 11>::Zero();
    H(0, 0) = 1.0;  // z_x → x_c
    H(1, 2) = 1.0;  // z_y → y_c
    H(2, 4) = 1.0;  // z_z → z_c

    // 新息（残差）
    Eigen::Vector3d y = z - h_x;

    // 新息协方差
    Eigen::Matrix3d S = H * P_ * H.transpose() + R_;

    // 卡尔曼增益: K = P * H' * S^{-1}
    Eigen::Matrix<double, 11, 3> K = P_ * H.transpose() * S.inverse();

    // 状态更新: x = x + K * y
    x_ = x_ + K * y;

    // 协方差更新 (Joseph 形式，数值更稳定)
    Eigen::Matrix<double, 11, 11> I =
        Eigen::Matrix<double, 11, 11>::Identity();
    Eigen::Matrix<double, 11, 11> IKH = I - K * H;
    P_ = IKH * P_ * IKH.transpose() + K * R_ * K.transpose();
}

enum class State { LOST, DETECTING, TRACKING, TEMP_LOST };

void Tracker::update_state(bool detected) {
    switch (state_) {
    case State::LOST:
        if (detected) {
            detect_count_++;
            if (detect_count_ >= 5) {  // 连续5帧检测到
                state_ = State::TRACKING;
                detect_count_ = 0;
            } else {
                state_ = State::DETECTING;
            }
        }
        break;

    case State::DETECTING:
        if (detected) {
            detect_count_++;
            if (detect_count_ >= 5) {
                state_ = State::TRACKING;
            }
        } else {
            detect_count_ = 0;
            state_ = State::LOST;
        }
        break;

    case State::TRACKING:
        if (!detected) {
            lost_time_ = this->now();
            state_ = State::TEMP_LOST;
        }
        break;

    case State::TEMP_LOST:
        if (detected) {
            state_ = State::TRACKING;
        } else if ((this->now() - lost_time_).seconds() > 1.0) {
            state_ = State::LOST;  // 超过1秒丢失
            reset();
        }
        break;
    }
}

void TrackerNode::publish_target() {
    if (state_ != State::TRACKING &&
        state_ != State::TEMP_LOST) return;

    auto msg = std::make_unique<rm_interfaces::msg::Target>();
    msg->header.stamp = this->now();
    msg->header.frame_id = "odom";

    // 位置和速度
    msg->position.x = x_(0);   // xc
    msg->position.y = x_(2);   // yc
    msg->position.z = x_(4);   // zc
    msg->velocity.x = x_(1);   // vxc
    msg->velocity.y = x_(3);   // vyc
    msg->velocity.z = x_(5);   // vzc

    // 朝向
    msg->yaw    = x_(6);       // yaw
    msg->v_yaw  = x_(7);       // vyaw

    // 装甲板几何参数
    msg->radius_1 = x_(8);     // r
    msg->radius_2 = x_(8) + x_(9);  // r + r_diff
    msg->dz       = x_(10);    // dz

    target_pub_->publish(std::move(msg));
}

struct AimPoint {
    double x, y, z;       // 预测命中的目标位置
    double pitch, yaw;     // 发射角度
};

AimPoint SolverNode::auto_solve_trajectory(
    const rm_interfaces::msg::Target & target) {
    // 1. 预测目标在未来子弹飞行时间内的位置
    double fly_time = estimate_fly_time(target);
    double pred_x = target.position.x + target.velocity.x * fly_time;
    double pred_y = target.position.y + target.velocity.y * fly_time;
    double pred_z = target.position.z + target.velocity.z * fly_time;

    // 2. 选择瞄准点（choose_aim_point）
    //    对于旋转目标，选择最近的装甲板位置
    double aim_x = pred_x, aim_y = pred_y, aim_z = pred_z;
    choose_aim_point(target, fly_time, aim_x, aim_y, aim_z);

    // 3. 迭代求解 pitch
    double yaw   = std::atan2(aim_y, aim_x);
    double pitch  = std::atan2(aim_z,
                     std::sqrt(aim_x*aim_x + aim_y*aim_y));

    for (int i = 0; i < 10; i++) {
        double t = estimate_arrival_time(aim_x, aim_y, aim_z, pitch, yaw);
        double z_hit = ballistic_model(aim_x, aim_y, pitch, yaw, t);
        double z_err = aim_z - z_hit;
        if (std::abs(z_err) < 0.001) break;  // 精度 1mm

        // pitch 补偿修正
        pitch += std::atan2(z_err,
                 std::sqrt(aim_x*aim_x + aim_y*aim_y));
    }

    return {aim_x, aim_y, aim_z, pitch, yaw};
}

double SolverNode::ballistic_model(double x, double y,
                                    double pitch, double yaw,
                                    double t) {
    // 无空气阻力的简化抛物线
    // v0: 弹速（约 25~30 m/s）
    // g: 重力加速度 9.81 m/s²
    double v_xy = v0_ * std::cos(pitch);
    double vz   = v0_ * std::sin(pitch);
    double dist_xy = std::sqrt(x*x + y*y);

    // 飞行时间 = 水平距离 / 水平速度
    // z = vz*t - 0.5*g*t²
    return vz * t - 0.5 * 9.81 * t * t;
}

// 串口数据包格式 (19 字节)
!!! info "简化示意"
    以下是端到端走读中的简化版协议，便于理解数据流。完整的协议格式请参考 [ch06 串口协议详解](ch06-architecture/index.md)。

struct SendPacket {
    uint8_t  header[2];    // 0xAA 0x55  帧头
    uint8_t  length;       // 0x0C = 12  数据长度
    float    pitch;        // 4 字节 IEEE 754
    float    yaw;          // 4 字节 IEEE 754
    uint8_t  shoot;        // 1 字节: 0=不射击, 1=射击
    uint8_t  padding;      // 1 字节对齐填充
    uint16_t crc16;        // 2 字节 CRC 校验
};  // 总计 2+1+4+4+1+1+2 = 15 字节（可能因对齐有差异）

void SolverNode::send_to_stm32(double pitch, double yaw, bool shoot) {
    SendPacket pkt;
    pkt.header[0] = 0xAA;
    pkt.header[1] = 0x55;
    pkt.length = 0x0C;
    pkt.pitch = static_cast<float>(pitch);
    pkt.yaw   = static_cast<float>(yaw);
    pkt.shoot = shoot ? 1 : 0;
    pkt.padding = 0;
    pkt.crc16 = crc16_calc(reinterpret_cast<uint8_t*>(&pkt),
                           sizeof(pkt) - 2);

    serial_.write(reinterpret_cast<uint8_t*>(&pkt), sizeof(pkt));
}

uint16_t crc16_calc(uint8_t *data, size_t len) {
    uint16_t crc = 0xFFFF;
    for (size_t i = 0; i < len; i++) {
        crc ^= data[i];
        for (int j = 0; j < 8; j++) {
            if (crc & 1) crc = (crc >> 1) ^ 0xA001;
            else         crc >>= 1;
        }
    }
    return crc;
}