diff --git a/src/rm_hardware_driver/ros2_hik_camera/src/hik_camera_node.cpp b/src/rm_hardware_driver/ros2_hik_camera/src/hik_camera_node.cpp index 0588257..0fddf93 100644 --- a/src/rm_hardware_driver/ros2_hik_camera/src/hik_camera_node.cpp +++ b/src/rm_hardware_driver/ros2_hik_camera/src/hik_camera_node.cpp @@ -377,37 +377,69 @@ private: continue; } - // Ensure output buffer - const size_t need_size = - static_cast(out_frame.stFrameInfo.nWidth) * out_frame.stFrameInfo.nHeight * 3; - if (image_msg_.data.size() != need_size) { - image_msg_.data.resize(need_size); - } + // Handle different pixel formats + // Mono8 (0x1080009) needs to be converted to RGB8 by duplicating the channel + // RGB8 (PixelType_Gvsp_RGB8_Packed = 0x02100014) can be used directly + const bool is_mono8 = (out_frame.stFrameInfo.enPixelType == 0x1080009); + const bool is_rgb8 = (out_frame.stFrameInfo.enPixelType == 0x02100014); - convert_param_.pDstBuffer = image_msg_.data.data(); - convert_param_.nDstBufferSize = image_msg_.data.size(); - convert_param_.pSrcData = out_frame.pBufAddr; - convert_param_.nSrcDataLen = out_frame.stFrameInfo.nFrameLen; - convert_param_.enSrcPixelType = out_frame.stFrameInfo.enPixelType; - - { - std::lock_guard lock(camera_mutex_); - if (!is_open_ || camera_handle_ == nullptr) { - continue; + if (is_mono8) { + // Convert Mono8 to RGB8 by duplicating the single channel + const size_t mono_size = static_cast(out_frame.stFrameInfo.nWidth) * out_frame.stFrameInfo.nHeight; + const size_t rgb_size = mono_size * 3; + if (image_msg_.data.size() != rgb_size) { + image_msg_.data.resize(rgb_size); } - ret = MV_CC_ConvertPixelType(camera_handle_, &convert_param_); - } - if (ret != MV_OK) { - FYT_WARN("camera_driver", "ConvertPixelType failed: [0x{:x}], src_pixel_type=0x{:x}, width={}, height={}", - ret, out_frame.stFrameInfo.enPixelType, out_frame.stFrameInfo.nWidth, out_frame.stFrameInfo.nHeight); + uint8_t* src = static_cast(out_frame.pBufAddr); + uint8_t* dst = image_msg_.data.data(); + for (size_t i = 0; i < mono_size; i++) { + dst[i * 3 + 0] = src[i]; // B + dst[i * 3 + 1] = src[i]; // G + dst[i * 3 + 2] = src[i]; // R + } + image_msg_.encoding = "rgb8"; + } else if (is_rgb8) { + // Already RGB8, just copy directly + const size_t rgb_size = static_cast(out_frame.stFrameInfo.nWidth) * out_frame.stFrameInfo.nHeight * 3; + if (image_msg_.data.size() != rgb_size) { + image_msg_.data.resize(rgb_size); + } + memcpy(image_msg_.data.data(), out_frame.pBufAddr, rgb_size); + image_msg_.encoding = "rgb8"; + } else { + // Try SDK conversion for other formats + const size_t need_size = + static_cast(out_frame.stFrameInfo.nWidth) * out_frame.stFrameInfo.nHeight * 3; + if (image_msg_.data.size() != need_size) { + image_msg_.data.resize(need_size); + } + + convert_param_.pDstBuffer = image_msg_.data.data(); + convert_param_.nDstBufferSize = image_msg_.data.size(); + convert_param_.pSrcData = out_frame.pBufAddr; + convert_param_.nSrcDataLen = out_frame.stFrameInfo.nFrameLen; + convert_param_.enSrcPixelType = out_frame.stFrameInfo.enPixelType; + { std::lock_guard lock(camera_mutex_); - if (camera_handle_ != nullptr) { - (void)MV_CC_FreeImageBuffer(camera_handle_, &out_frame); + if (!is_open_ || camera_handle_ == nullptr) { + continue; } + ret = MV_CC_ConvertPixelType(camera_handle_, &convert_param_); } - fail_count_++; - continue; + if (ret != MV_OK) { + FYT_WARN("camera_driver", "ConvertPixelType failed: [0x{:x}], src_pixel_type=0x{:x}, width={}, height={}", + ret, out_frame.stFrameInfo.enPixelType, out_frame.stFrameInfo.nWidth, out_frame.stFrameInfo.nHeight); + { + std::lock_guard lock(camera_mutex_); + if (camera_handle_ != nullptr) { + (void)MV_CC_FreeImageBuffer(camera_handle_, &out_frame); + } + } + fail_count_++; + continue; + } + image_msg_.encoding = "rgb8"; } auto stamp = this->now();