diff --git a/doc/2026-08-16_ae_ag_roi_sync_feature.md b/doc/2026-08-16_ae_ag_roi_sync_feature.md new file mode 100644 index 0000000..b889f12 --- /dev/null +++ b/doc/2026-08-16_ae_ag_roi_sync_feature.md @@ -0,0 +1,94 @@ +# 2026-08-16 코드 변경 — ROI 측광 / 자동 게인 / 우선순위 / 마스터-슬레이브 동기화 + +> `hik_camera_node.cpp`에 실제 기능을 추가했다. 아래는 무엇을, 왜, 어떻게 바꿨는지와 사용법. +> 관련 배경: `2026-08-15_test1_notes.md`, `camera_exposure_design_notes.md` 4절/5.4절. + +--- + +## 1. 요청 사항 4가지와 대응 + +| 요청 | 결론 | +|---|---| +| 자동 게인 도입 | `gain_auto` 파라미터 신규 추가 (기존엔 `GainAuto`가 항상 강제 Off였음) | +| AE 측광 ROI를 화면 중간~맨 아래로 제한 | **가능. 단, 카메라 온보드 기능이 아니라 소프트웨어로 구현** (아래 2절 이유 참조) | +| 노출/게인 자동 조절 우선순위 지정 | `use_software_ae` 루프에서 명시적으로 구현 (노출 먼저, 게인은 최후 수단) | +| 가운데 카메라가 나머지 노출/게인을 결정 | `sync_role`(master/slave) + 토픽 2개로 구현 | + +--- + +## 2. 왜 ROI 측광이 카메라 온보드로는 안 되는가 + +`hikSDK/include/MvCameraControl.h`, `CameraParams.h`를 다시 검색했다. AE 전용 측광 ROI(흔히 "AutoFunctionAOI"류)에 해당하는 노드/함수는 없고, 있는 건: + +``` +MV_CC_GetAOIoffsetX/Y, MV_CC_SetAOIoffsetX/Y ← 캡처 자체를 크롭하는 AOI (측광 전용 아님) +GainAuto 관련 전용 C 함수는 아예 없음 (ExposureAuto와 동일하게 문자열 노드로만 설정 가능) +``` + +즉 이미지를 자르지 않고 "측광 계산에만" 특정 영역을 쓰는 카메라 자체 기능이 없다. 그래서 온보드 `ExposureAuto`/`GainAuto`를 끄고, 소프트웨어에서 직접 퍼센타일을 계산해 `ExposureTime`/`Gain`을 쓰는 방식(`use_software_ae`)으로 구현했다. + +--- + +## 3. 새 파라미터 + +### 게인 (기존 `gain`은 유지, 아래 신규) + +| 파라미터 | 기본값 | 의미 | +|---|---|---| +| `gain_auto` | `false` | true면 게인 자동 조절 (온보드 Continuous 또는 소프트웨어 루프) | +| `gain_auto_max_db` | `12.0` | 자동 게인 상한. 하드웨어 실측 상한(~16.9dB, `answer_1.md` 2절)보다 낮게 잡아 노이즈 제한 | + +### 소프트웨어 AE/AG (ROI 측광 + 우선순위) + +| 파라미터 | 기본값 | 의미 | +|---|---|---| +| `use_software_ae` | `false` | true면 온보드 auto 대신 아래 방식으로 직접 계산 | +| `ae_roi_top_ratio` | `0.0` | 측광에서 제외할 상단 비율. `0.5` = 하단 절반만 측광(하늘 제외) | +| `ae_target_percentile` / `ae_target_dn` | `70.0` / `130` | ROI 내 목표 퍼센타일이 이 DN이 되도록 조절 (설계노트 4.2절과 동일 사상) | +| `ae_saturation_percentile` / `ae_saturation_dn` | `98.0` / `245` | 이 퍼센타일이 이 DN을 넘지 않도록 하는 하드 제약 — 목표 미달이어도 이게 우선 | +| `ae_step_gain` | `0.5` | 프레임당 보정 비율(댐핑). 크면 빨리 수렴하되 진동 위험 | + +**우선순위 로직** (`runSoftwareAeStep()`): 밝게 해야 할 때는 **노출을 `exposure_auto_max`까지 먼저** 늘리고, 그래도 모자라면 게인을 `gain_auto_max_db`까지 씀. 어둡게 해야 할 때는 반대로 **게인을 먼저** 줄이고, 그래도 남으면 노출을 줄임. 게인은 노이즈 비용이 있으므로 항상 마지막 수단. + +### 마스터-슬레이브 동기화 + +| 파라미터 | 기본값 | 의미 | +|---|---|---| +| `sync_role` | `"independent"` | `independent`(기존 동작) / `master` / `slave` | +| `sync_master_camera_ns` | `""` | `sync_role=slave`일 때 구독할 마스터의 `camera_name` (예: `"cam2"`) | + +동작 방식: 모든 카메라가 `/ae_exposure_time_us`, `/ae_gain_db` 토픽을 항상 발행한다(관찰용, role 무관). `master`는 이 값을 SDK가 매 프레임 주는 **실제 적용값**(`MV_FRAME_OUT_INFO_EX::fExposureTime`/`fGain` — 이미 `answer_1.md` 3절에서 "이미 있는데 안 쓰고 있다"고 지적했던 그 필드)으로 채워 발행하므로, 온보드 auto든 소프트웨어 AE든 수동 고정이든 어떤 모드든 그대로 방송된다. `slave`는 자기 auto 로직을 전부 끄고, 구독한 마스터 값을 그대로 `ExposureTime`/`Gain`에 적용한다. + +**사용 예 (cam2가 마스터, cam1/cam3가 슬레이브):** +```yaml +# camera_params_cam2.yaml +sync_role: "master" + +# camera_params_cam1.yaml, camera_params_cam3.yaml +sync_role: "slave" +sync_master_camera_ns: "cam2" +``` + +--- + +## 4. 알아두어야 할 제약 + +1. **`AutoGainLowerLimit`/`AutoGainUpperLimit` 노드명은 검증되지 않음.** `AutoExposureTimeLowerLimit`/`UpperLimit`과 같은 명명 규칙일 거라 가정하고 그대로 썼다 (`applyGainMode()`). 실패하면 `RCLCPP_WARN`으로 바로 로그에 뜨니, 뜨면 MVS Feature Tree에서 실제 노드명을 확인해서 고쳐야 한다. (`gain_auto`+`use_software_ae=false` 조합, 즉 온보드 auto gain을 쓸 때만 해당 — `use_software_ae=true`면 이 노드를 안 쓰므로 무관.) +2. **기본값은 전부 기존 동작을 그대로 유지한다.** `gain_auto:false`, `use_software_ae:false`, `sync_role:"independent"` — 기존 3대 yaml(`camera_params_cam1/2/3.yaml`)은 코드 수정 후에도 아무 변경 없이 그대로 동작한다. 새 기능을 쓰려면 yaml에 위 파라미터를 명시적으로 추가해야 한다 (이번에 yaml 파일 자체는 건드리지 않았다 — 실제 배포 설정은 테스트해보고 정할 문제라 판단). +3. **소프트웨어 AE 루프는 매 프레임 이미지에서 4픽셀 간격으로 서브샘플링해 밝기 퍼센타일을 계산**한다 (`runSoftwareAeStep()`). 1440×1080 기준 약 9.7만 샘플, 정렬 비용은 10Hz에서 무시할 만한 수준. +4. **동기화는 ROS 파라미터/토픽 레이어를 거치므로 완전한 프레임 단위 lockstep은 아니다.** 마스터가 결정한 값이 슬레이브에 반영되기까지 최소 한 토픽 왕복(수 ms~한 프레임 이내, 10Hz 트리거 기준) 지연이 있다. 카메라 3대가 완전히 다른 방향(예: 하나는 역광, 하나는 그늘)을 보고 있으면 슬레이브가 부적절하게 노출될 수 있음 — 실측으로 확인 필요 (설계노트 5.4절의 Master-Slave 단점과 동일한 근본 제약). + +--- + +## 5. 별도로 코드 변경이 필요 없던 것 — 화이트밸런스 + +같은 대화에서 논의된 화이트밸런스 "끄기"는 코드 변경 없이 이미 가능하다: `balance_white_auto:false` + `balance_ratio_red/green/blue: 1024/1024/1024`(중립/1x 게인)로 설정하면 됨. cam2-cam3 겹침 매칭은 아직 실측/적용 전 — cam1-cam2와 같은 방식으로 겹침 장면에서 Continuous AWB 수렴값을 읽어(`captureLoop()`가 3초마다 로그로 이미 출력) 3대 평균을 내는 절차가 남아 있음 (코드 변경 사항 아님, 데이터 수집 후 yaml 값만 갱신하면 됨). + +--- + +## 6. 빌드 검증에 대한 한계 + +이 변경은 개발 PC(macOS, ROS 2/colcon 미설치)에서 작성되어 **실제 `colcon build`로 컴파일 검증을 하지 못했다.** 코드를 전체 재검토해 문법·타입·초기화 순서 오류를 잡았지만(초기 구현에서 `soft_ae_exposure_us_`/`soft_ae_gain_db_` 초기화 순서 버그 1건 발견해 수정함), 로봇 PC에서 `colcon build --symlink-install` 후 다음을 반드시 확인할 것: +- 빌드 자체가 통과하는지 (특히 `AutoGainLowerLimit`/`AutoGainUpperLimit` 같은 문자열 노드는 컴파일 타임에 검증 안 됨 — 런타임 WARN 로그로만 확인 가능) +- `use_software_ae:true`로 켰을 때 `ros2 topic echo /cam2/ae_exposure_time_us` 등으로 실제 값이 그럴듯하게 움직이는지 +- `sync_role` master/slave 조합에서 슬레이브가 마스터 값을 잘 따라가는지 diff --git a/src/hik_camera_ros2_driver/README.md b/src/hik_camera_ros2_driver/README.md index 24aad65..c8b37a4 100644 --- a/src/hik_camera_ros2_driver/README.md +++ b/src/hik_camera_ros2_driver/README.md @@ -44,11 +44,56 @@ None. - Auto exposure upper limit in microseconds. Active only when `exposure_auto` is `true`. Can be changed at runtime. - `gain` (double) - - Manual gain. Auto gain is always disabled. Can be changed at runtime: + - Manual gain, used when `gain_auto` is `false`. Can be changed at runtime: ```bash ros2 param set /hik_camera_ros2_driver gain 2.0 ``` +- `gain_auto` (bool, default: `false`) + - Enable auto gain. Onboard `Continuous` when `use_software_ae` is `false`; controlled by the + software AE/AG loop (see below) when `use_software_ae` is `true`. Can be changed at runtime. + +- `gain_auto_max_db` (double, default: `12.0`) + - Ceiling for auto gain, in dB. Independent from `gain`'s own hardware range — lets you cap + auto gain below the hardware maximum (e.g. this camera's real limit is ~16.9 dB) to bound + noise. + +- `use_software_ae` (bool, default: `false`) + - The camera's onboard `ExposureAuto`/`GainAuto` always meter the *full* captured frame — this + SDK has no GenICam node for a metering-only ROI distinct from the capture AOI. When `true`, + onboard auto is disabled and exposure/gain are instead computed each frame in software from a + percentile of the ROI defined by `ae_roi_top_ratio`, then written directly via + `ExposureTime`/`Gain`. Requires `exposure_auto` and/or `gain_auto` to also be `true` to + actually adjust anything (otherwise held fixed at `exposure_time`/`gain`). Can be changed at + runtime. + +- `ae_roi_top_ratio` (double, default: `0.0`) + - Fraction of frame height excluded from the top when metering under `use_software_ae`. `0.5` + meters only the bottom half (e.g. to exclude sky). + +- `ae_target_percentile` / `ae_target_dn` (default: `70.0` / `130`) + - Software AE/AG target: adjust exposure/gain so this percentile of the ROI reaches this DN. + +- `ae_saturation_percentile` / `ae_saturation_dn` (default: `98.0` / `245`) + - Hard ceiling: never brighten past the point where this percentile would exceed this DN, even + if the target above hasn't been reached. + +- `ae_step_gain` (double, default: `0.5`) + - Per-frame correction damping (0-1) for the software AE/AG loop. + +- `sync_role` (string, default: `"independent"`) + - `"independent"`: this camera decides its own exposure/gain (default, unchanged behavior). + - `"master"`: publishes the actually-applied exposure/gain (from the SDK's per-frame frame info, + valid regardless of AE mode) on `/ae_exposure_time_us` and + `/ae_gain_db`. + - `"slave"`: ignores its own auto exposure/gain and instead applies whatever `sync_master_camera_ns` + publishes, directly. Use this to make one camera (e.g. the center one) drive exposure/gain for + the others. + +- `sync_master_camera_ns` (string, default: `""`) + - When `sync_role` is `"slave"`, the `camera_name` of the master camera to subscribe to (e.g. + `"cam2"`). + - `acquisition_frame_rate` (double, default: `165`) - The acquisition frame rate in hz for the camera. diff --git a/src/hik_camera_ros2_driver/package.xml b/src/hik_camera_ros2_driver/package.xml index cb20247..3c79192 100644 --- a/src/hik_camera_ros2_driver/package.xml +++ b/src/hik_camera_ros2_driver/package.xml @@ -13,6 +13,7 @@ rclcpp rclcpp_components sensor_msgs + std_msgs image_transport image_transport_plugins camera_info_manager diff --git a/src/hik_camera_ros2_driver/src/hik_camera_node.cpp b/src/hik_camera_ros2_driver/src/hik_camera_node.cpp index 9ac790f..33a8f39 100644 --- a/src/hik_camera_ros2_driver/src/hik_camera_node.cpp +++ b/src/hik_camera_ros2_driver/src/hik_camera_node.cpp @@ -1,7 +1,9 @@ +#include #include #include #include #include +#include #include #include #include @@ -11,6 +13,7 @@ #include "image_transport/image_transport.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/utilities.hpp" +#include "std_msgs/msg/float64.hpp" namespace hik_camera_ros2_driver { @@ -32,6 +35,7 @@ public: declareParameters(); startCamera(); initSharedMemory(); + initSync(); params_callback_handle_ = this->add_on_set_parameters_callback( std::bind(&HikCameraRos2DriverNode::dynamicParametersCallback, this, std::placeholders::_1)); @@ -172,6 +176,16 @@ private: RCLCPP_INFO(this->get_logger(), "Acquisition frame rate: %f", acquisition_frame_rate); } + // 노출/게인 동기화 (마스터-슬레이브) — exposure/gain 블록보다 먼저 선언해야 + // applyExposureMode()/applyGainMode()가 이 값을 보고 분기할 수 있음. + // independent: 이 카메라 단독으로 노출/게인 결정 (기존 동작, 기본값) + // master: 실제 적용된 노출/게인을 ae_exposure_time_us / ae_gain_db 토픽으로 발행 + // slave: sync_master_camera_ns 의 master 토픽을 구독해 그 값을 그대로 적용 + param_desc.description = "노출/게인 동기화 역할: independent / master / slave"; + sync_role_ = this->declare_parameter("sync_role", std::string("independent")); + param_desc.description = "sync_role=slave일 때 구독할 마스터 카메라의 camera_name (예: \"cam2\")"; + sync_master_camera_ns_ = this->declare_parameter("sync_master_camera_ns", std::string("")); + // Exposure — 모든 파라미터를 항상 선언하여 런타임 모드 전환 지원 MV_CC_GetFloatValue(camera_handle_, "ExposureTime", &f_value); exposure_auto_ = this->declare_parameter("exposure_auto", false); @@ -193,17 +207,65 @@ private: param_desc.description = "Manual exposure time in microseconds"; exposure_time_ = this->declare_parameter("exposure_time", 5000, param_desc); + // 소프트웨어 AE/AG — 카메라 온보드 ExposureAuto/GainAuto는 항상 풀프레임을 측광한다. + // 이 SDK(hikSDK/include)에는 캡처 AOI(MV_CC_Set/GetAOIoffsetX/Y, 이미지 자체를 자름)와 + // 별개로 "측광에만 쓰는 ROI"를 지정하는 GenICam 노드가 없다 (AutoFunctionAOI 계열 + // 탐색했으나 없음). 하늘을 제외한 하단부만 측광하려면(4번 질문, camera_exposure_design_notes.md + // 4절) 온보드 auto를 끄고 소프트웨어에서 직접 ExposureTime/Gain을 계산해 써야 한다. + param_desc.description = + "true면 온보드 Continuous auto 대신, ae_roi_top_ratio로 제한한 영역의 퍼센타일 밝기로 " + "노출/게인을 소프트웨어에서 직접 계산해 적용한다"; + use_software_ae_ = this->declare_parameter("use_software_ae", false); + + param_desc.description = + "측광에서 제외할 프레임 상단 비율 (0.5 = 하단 절반만 측광, 하늘 제외 용도)"; + ae_roi_top_ratio_ = this->declare_parameter("ae_roi_top_ratio", 0.0); + + param_desc.description = "측광 ROI 내 목표 퍼센타일 (0~100)"; + ae_target_percentile_ = this->declare_parameter("ae_target_percentile", 70.0); + + param_desc.description = "ae_target_percentile 지점의 목표 DN (0~255)"; + ae_target_dn_ = this->declare_parameter("ae_target_dn", 130); + + param_desc.description = "포화 방지용 퍼센타일 (0~100)"; + ae_saturation_percentile_ = this->declare_parameter("ae_saturation_percentile", 98.0); + + param_desc.description = "ae_saturation_percentile 지점이 이 DN을 넘지 않도록 제한"; + ae_saturation_dn_ = this->declare_parameter("ae_saturation_dn", 245); + + param_desc.description = "소프트웨어 AE/AG 1프레임당 보정 비율 (0~1, 클수록 빨리 수렴하되 진동 위험)"; + ae_step_gain_ = this->declare_parameter("ae_step_gain", 0.5); + + if (use_software_ae_) { + soft_ae_exposure_us_ = static_cast(exposure_time_); + } + applyExposureMode(); - // Gain (manual only — GainAuto=Off) - MV_CC_SetEnumValue(camera_handle_, "GainAuto", 0); + // Gain + param_desc.description = + "true면 게인을 자동 조절한다 (use_software_ae=false: 온보드 Continuous, " + "use_software_ae=true: 위 소프트웨어 AE/AG 루프가 함께 조절)"; + gain_auto_ = this->declare_parameter("gain_auto", false); + param_desc.description = "Gain"; MV_CC_GetFloatValue(camera_handle_, "Gain", &f_value); param_desc.integer_range[0].from_value = static_cast(f_value.fMin); param_desc.integer_range[0].to_value = static_cast(f_value.fMax); - double gain = this->declare_parameter("gain", f_value.fCurValue, param_desc); - MV_CC_SetFloatValue(camera_handle_, "Gain", gain); - RCLCPP_INFO(this->get_logger(), "Gain: %f", gain); + gain_ = this->declare_parameter("gain", static_cast(f_value.fCurValue), param_desc); + + param_desc.description = + "자동 게인 상한 [dB] — 하드웨어 실측 상한(카메라별 상이, 대략 16.9dB)보다 낮게 잡아서 " + "노이즈를 제한하는 용도. gain 파라미터 자체의 상한(위 range)과는 별개 값"; + param_desc.integer_range[0].from_value = 0; + param_desc.integer_range[0].to_value = static_cast(f_value.fMax); + gain_auto_max_db_ = this->declare_parameter("gain_auto_max_db", 12.0, param_desc); + + if (use_software_ae_) { + soft_ae_gain_db_ = gain_; + } + + applyGainMode(); // White balance — 기본은 수동(Off) + 고정 R/G/B 비율. // 이유: 카메라별로 독립 Continuous AWB를 켜두면 두 카메라가 서로 다른 화각을 보고 @@ -320,8 +382,65 @@ private: RCLCPP_INFO(this->get_logger(), "Shared memory timestamp enabled: %s", path.c_str()); } + // 노출/게인 마스터-슬레이브 동기화. 카메라별 실제 적용값(fExposureTime/fGain, + // MV_FRAME_OUT_INFO_EX에 매 프레임 이미 들어있음 — captureLoop 참조)을 퍼블리시/구독한다. + // publisher는 role과 무관하게 항상 만들어서, 굳이 master로 지정하지 않아도 다른 노드가 + // 관찰용으로 구독할 수 있게 한다. + void initSync() + { + ae_exposure_pub_ = this->create_publisher( + camera_name_ + "/ae_exposure_time_us", 10); + ae_gain_pub_ = this->create_publisher( + camera_name_ + "/ae_gain_db", 10); + + if (sync_role_ == "slave") { + if (sync_master_camera_ns_.empty()) { + RCLCPP_ERROR(this->get_logger(), + "sync_role=slave 인데 sync_master_camera_ns가 비어 있음 — 마스터를 구독할 수 없음"); + return; + } + const std::string prefix = "/" + sync_master_camera_ns_; + ae_exposure_sub_ = this->create_subscription( + prefix + "/ae_exposure_time_us", 10, + [this](const std_msgs::msg::Float64::SharedPtr msg) { + exposure_time_ = static_cast(std::lround(msg->data)); + MV_CC_SetFloatValue(camera_handle_, "ExposureTime", static_cast(msg->data)); + }); + ae_gain_sub_ = this->create_subscription( + prefix + "/ae_gain_db", 10, + [this](const std_msgs::msg::Float64::SharedPtr msg) { + gain_ = msg->data; + MV_CC_SetFloatValue(camera_handle_, "Gain", static_cast(msg->data)); + }); + RCLCPP_INFO(this->get_logger(), "Sync: slave of %s", prefix.c_str()); + } else if (sync_role_ == "master") { + RCLCPP_INFO(this->get_logger(), + "Sync: master (publishing %s/ae_exposure_time_us, %s/ae_gain_db)", + camera_name_.c_str(), camera_name_.c_str()); + } + } + void applyExposureMode() { + if (sync_role_ == "slave") { + // 슬레이브는 마스터가 보내주는 값을 그대로 적용할 뿐, 자체 auto 로직을 돌리지 않는다 + // (initSync()의 구독 콜백이 실제 적용을 담당). 여기서는 초기값만 세팅. + MV_CC_SetEnumValue(camera_handle_, "ExposureAuto", 0); + MV_CC_SetFloatValue(camera_handle_, "ExposureTime", static_cast(exposure_time_)); + RCLCPP_INFO(this->get_logger(), "Exposure: slave mode, waiting for master sync"); + return; + } + if (use_software_ae_) { + // 온보드 auto는 ROI 측광을 지원하지 않으므로 항상 Off로 두고, 실제 조절은 + // runSoftwareAeStep()이 매 프레임 ExposureTime을 직접 써서 수행한다. + MV_CC_SetEnumValue(camera_handle_, "ExposureAuto", 0); + MV_CC_SetFloatValue(camera_handle_, "ExposureTime", static_cast(soft_ae_exposure_us_)); + RCLCPP_INFO(this->get_logger(), + "Software AE: %s, ROI top-exclude=%.2f, target p%.0f=DN%d, range=[%.0f, %.0f] us", + exposure_auto_ ? "ON" : "OFF (fixed)", ae_roi_top_ratio_, + ae_target_percentile_, ae_target_dn_, exposure_auto_min_, exposure_auto_max_); + return; + } if (exposure_auto_) { MV_CC_SetEnumValue(camera_handle_, "ExposureAuto", 2); // Continuous MV_CC_SetIntValue(camera_handle_, "AutoTargetBrightness", @@ -340,6 +459,45 @@ private: } } + void applyGainMode() + { + if (sync_role_ == "slave") { + MV_CC_SetEnumValue(camera_handle_, "GainAuto", 0); + MV_CC_SetFloatValue(camera_handle_, "Gain", static_cast(gain_)); + RCLCPP_INFO(this->get_logger(), "Gain: slave mode, waiting for master sync"); + return; + } + if (use_software_ae_) { + MV_CC_SetEnumValue(camera_handle_, "GainAuto", 0); + MV_CC_SetFloatValue(camera_handle_, "Gain", static_cast(soft_ae_gain_db_)); + RCLCPP_INFO(this->get_logger(), + "Software AG: %s, range=[0, %.1f] dB", gain_auto_ ? "ON" : "OFF (fixed)", + gain_auto_max_db_); + return; + } + if (gain_auto_) { + MV_CC_SetEnumValue(camera_handle_, "GainAuto", 2); // Continuous + // AutoGainLowerLimit/UpperLimit은 AutoExposureTimeLowerLimit/UpperLimit과 같은 명명 + // 규칙을 따른다고 가정한 값이다 — 이 카메라의 실제 GenICam 노드맵에서 검증된 적은 + // 없음. 실패 시 아래 WARN 로그로 바로 드러나므로, 뜨면 MVS Feature Tree에서 + // 정확한 노드명을 확인해서 고칠 것. + int status_lo = MV_CC_SetFloatValue(camera_handle_, "AutoGainLowerLimit", 0.0f); + int status_hi = MV_CC_SetFloatValue( + camera_handle_, "AutoGainUpperLimit", static_cast(gain_auto_max_db_)); + if (status_lo != MV_OK || status_hi != MV_OK) { + RCLCPP_WARN(this->get_logger(), + "Failed to set AutoGainLowerLimit/UpperLimit (status=0x%x/0x%x) — node name may not " + "match this camera's GenICam map, verify via MVS Feature Tree", + status_lo, status_hi); + } + RCLCPP_INFO(this->get_logger(), "Auto gain: ON, range=[0, %.1f] dB", gain_auto_max_db_); + } else { + MV_CC_SetEnumValue(camera_handle_, "GainAuto", 0); // Off + MV_CC_SetFloatValue(camera_handle_, "Gain", static_cast(gain_)); + RCLCPP_INFO(this->get_logger(), "Manual gain: %.1f dB", gain_); + } + } + // 2026-07-29: nTriggerIndex는 이 카메라(MV-CS016-10UC USB3)에서 항상 0으로 고정되어 // 실사용 불가로 확인됨 (실기기 로그로 검증). 대신 nDevTimeStampHigh/Low(카메라 자체 // 자유구동 하드웨어 클럭)는 프레임마다 정상적으로 증가하는 것을 로그로 확인했다. @@ -479,6 +637,100 @@ private: return this->now(); } + // ROI 제한 퍼센타일 측광으로 노출/게인을 계산해 직접 적용한다 (설계노트 4.2절 방식). + // 밝게 해야 할 때는 노출을 exposure_auto_max_까지 먼저 늘리고, 그래도 모자라면 게인을 + // 쓴다. 어둡게 해야 할 때는 반대로 게인을 먼저 줄이고, 그래도 남으면 노출을 줄인다 + // (게인이 노이즈 비용이 있으므로 항상 마지막 수단으로 남겨두는 우선순위). + void runSoftwareAeStep() + { + const int width = static_cast(image_msg_.width); + const int height = static_cast(image_msg_.height); + const double top_ratio = std::min(0.95, std::max(0.0, ae_roi_top_ratio_)); + const int row_start = static_cast(height * top_ratio); + if (row_start >= height || width <= 0) { + return; + } + + constexpr int kStride = 4; // 매 프레임 전체 픽셀을 다 볼 필요 없음 — 서브샘플링 + std::vector luma; + luma.reserve((static_cast(width) / kStride + 1) * + (static_cast(height - row_start) / kStride + 1)); + const uint8_t * data = image_msg_.data.data(); + const size_t step = image_msg_.step; // width * 3 (rgb8) + for (int y = row_start; y < height; y += kStride) { + const uint8_t * row = data + static_cast(y) * step; + for (int x = 0; x < width; x += kStride) { + const uint8_t * px = row + static_cast(x) * 3; + luma.push_back(static_cast( + (static_cast(px[0]) + px[1] + px[2]) / 3)); + } + } + if (luma.empty()) { + return; + } + std::sort(luma.begin(), luma.end()); + auto percentile_dn = [&luma](double p) { + double clamped = std::min(100.0, std::max(0.0, p)); + size_t idx = static_cast(clamped / 100.0 * static_cast(luma.size() - 1)); + return static_cast(luma[idx]); + }; + + const double p_target = percentile_dn(ae_target_percentile_); + const double p_sat = percentile_dn(ae_saturation_percentile_); + + double delta_stop = std::log2(static_cast(ae_target_dn_) / std::max(p_target, 1.0)); + const double delta_stop_sat_limit = + std::log2(static_cast(ae_saturation_dn_) / std::max(p_sat, 1.0)); + delta_stop = std::min(delta_stop, delta_stop_sat_limit); // 포화 제약이 우선 + delta_stop *= ae_step_gain_; + + double remaining = delta_stop; + if (remaining > 0.0 && exposure_auto_) { + const double t_stop_avail = std::log2(exposure_auto_max_ / soft_ae_exposure_us_); + const double t_stop_used = std::min(remaining, std::max(0.0, t_stop_avail)); + soft_ae_exposure_us_ *= std::pow(2.0, t_stop_used); + remaining -= t_stop_used; + if (gain_auto_ && remaining > 0.0) { + const double g_stop_avail = (gain_auto_max_db_ - soft_ae_gain_db_) / 6.02; + const double g_stop_used = std::min(remaining, std::max(0.0, g_stop_avail)); + soft_ae_gain_db_ += g_stop_used * 6.02; + } + } else if (remaining < 0.0) { + double need = -remaining; + if (gain_auto_) { + const double g_stop_avail = soft_ae_gain_db_ / 6.02; + const double g_stop_used = std::min(need, std::max(0.0, g_stop_avail)); + soft_ae_gain_db_ -= g_stop_used * 6.02; + need -= g_stop_used; + } + if (need > 0.0 && exposure_auto_) { + const double t_stop_avail = std::log2(soft_ae_exposure_us_ / exposure_auto_min_); + const double t_stop_used = std::min(need, std::max(0.0, t_stop_avail)); + soft_ae_exposure_us_ /= std::pow(2.0, t_stop_used); + } + } + + soft_ae_exposure_us_ = + std::min(exposure_auto_max_, std::max(exposure_auto_min_, soft_ae_exposure_us_)); + soft_ae_gain_db_ = std::min(gain_auto_max_db_, std::max(0.0, soft_ae_gain_db_)); + + MV_CC_SetFloatValue(camera_handle_, "ExposureTime", static_cast(soft_ae_exposure_us_)); + MV_CC_SetFloatValue(camera_handle_, "Gain", static_cast(soft_ae_gain_db_)); + } + + // 실제 적용된 노출/게인(chunk 없이도 SDK가 매 프레임 넘겨주는 값)을 브로드캐스트한다. + // 온보드 auto / 소프트웨어 AE / 수동 고정 어느 모드든 상관없이 "진짜 적용값"을 그대로 보냄. + void publishAeState(const MV_FRAME_OUT_INFO_EX & frame_info) + { + std_msgs::msg::Float64 exp_msg; + exp_msg.data = static_cast(frame_info.fExposureTime); + ae_exposure_pub_->publish(exp_msg); + + std_msgs::msg::Float64 gain_msg; + gain_msg.data = static_cast(frame_info.fGain); + ae_gain_pub_->publish(gain_msg); + } + void captureLoop() { MV_FRAME_OUT out_frame; @@ -539,6 +791,13 @@ private: camera_info_msg_.header = image_msg_.header; camera_pub_.publish(image_msg_, camera_info_msg_); + if (sync_role_ != "slave" && use_software_ae_ && (exposure_auto_ || gain_auto_)) { + runSoftwareAeStep(); + } + if (sync_role_ == "master") { + publishAeState(out_frame.stFrameInfo); + } + MV_CC_FreeImageBuffer(camera_handle_, &out_frame); static auto last_log_time = std::chrono::steady_clock::now(); @@ -593,6 +852,17 @@ private: } else if (name == "exposure_auto") { exposure_auto_ = param.as_bool(); applyExposureMode(); + } else if (name == "gain_auto") { + gain_auto_ = param.as_bool(); + applyGainMode(); + } else if (name == "use_software_ae") { + use_software_ae_ = param.as_bool(); + if (use_software_ae_) { + soft_ae_exposure_us_ = static_cast(exposure_time_); + soft_ae_gain_db_ = gain_; + } + applyExposureMode(); + applyGainMode(); } else { result.successful = false; result.reason = "Unknown parameter: " + name; @@ -600,7 +870,10 @@ private: } } else if (type == rclcpp::ParameterType::PARAMETER_DOUBLE) { if (name == "gain") { - status = MV_CC_SetFloatValue(camera_handle_, "Gain", param.as_double()); + gain_ = param.as_double(); + if (!gain_auto_ && !use_software_ae_) { + status = MV_CC_SetFloatValue(camera_handle_, "Gain", static_cast(gain_)); + } } else if (name == "exposure_auto_min") { exposure_auto_min_ = param.as_double(); if (exposure_auto_) { @@ -702,6 +975,30 @@ private: double exposure_auto_max_ = 10000.0; int exposure_time_ = 5000; + // Gain + bool gain_auto_ = false; + double gain_ = 0.0; + double gain_auto_max_db_ = 12.0; + + // Software AE/AG — ROI-restricted percentile metering (runSoftwareAeStep() 참조) + bool use_software_ae_ = false; + double ae_roi_top_ratio_ = 0.0; + double ae_target_percentile_ = 70.0; + int ae_target_dn_ = 130; + double ae_saturation_percentile_ = 98.0; + int ae_saturation_dn_ = 245; + double ae_step_gain_ = 0.5; + double soft_ae_exposure_us_ = 1000.0; + double soft_ae_gain_db_ = 0.0; + + // 노출/게인 마스터-슬레이브 동기화 (initSync() 참조) + std::string sync_role_ = "independent"; + std::string sync_master_camera_ns_; + rclcpp::Publisher::SharedPtr ae_exposure_pub_; + rclcpp::Publisher::SharedPtr ae_gain_pub_; + rclcpp::Subscription::SharedPtr ae_exposure_sub_; + rclcpp::Subscription::SharedPtr ae_gain_sub_; + // White balance bool balance_white_auto_ = false;