try fix gray

src/rm_hardware_driver/ros2_hik_camera/src/hik_camera_node.cpp

@@ -240,10 +240,15 @@ private:
   {
     if (is_open_) {
       close();
+      // Wait for camera to fully release resources after close
+      std::this_thread::sleep_for(std::chrono::milliseconds(500));
     }
 
     FYT_INFO("camera_driver", "Opening Hik Camera Device!");
 
+    // Additional wait before enumerating devices
+    std::this_thread::sleep_for(std::chrono::milliseconds(200));
+
     MV_CC_DEVICE_INFO_LIST device_list;
     int ret = MV_CC_EnumDevices(MV_GIGE_DEVICE | MV_USB_DEVICE, &device_list);
     if (ret != MV_OK || device_list.nDeviceNum == 0) {
@@ -275,6 +280,24 @@ private:
       // Apply ROI/offset before querying image info
       applyRoiLocked();
 
+      // Set pixel format to RGB8 (try to force color output)
+      // PixelType_Gvsp_RGB8_Packed = 0x02100014
+      int set_ret = MV_CC_SetEnumValue(camera_handle_, "PixelFormat", PixelType_Gvsp_RGB8_Packed);
+      if (set_ret != MV_OK) {
+        FYT_WARN("camera_driver", "Failed to set PixelFormat to RGB8: [0x{:x}]", set_ret);
+      } else {
+        FYT_INFO("camera_driver", "PixelFormat set to RGB8");
+      }
+
+      // Wait for pixel format to take effect
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+      // Verify pixel format
+      MVCC_ENUMVALUE enum_value = {0};
+      if (MV_CC_GetEnumValue(camera_handle_, "PixelFormat", &enum_value) == MV_OK) {
+        FYT_INFO("camera_driver", "Current PixelFormat: 0x{:x}", enum_value.nCurValue);
+      }
+
       // Set exposure / gain
       (void)MV_CC_SetFloatValue(camera_handle_, "ExposureTime", static_cast<float>(exposure_time_));
       (void)MV_CC_SetFloatValue(camera_handle_, "Gain", static_cast<float>(gain_));
@@ -321,6 +344,9 @@ private:
       return false;
     }
 
+    // Wait for camera to be ready after starting grabbing
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
     is_open_ = true;
     FYT_INFO("camera_driver", "Hik Camera Device Open Success!");
     return true;
@@ -369,36 +395,69 @@ private:
         continue;
       }
 
-      // Ensure output buffer
-      const size_t need_size =
-        static_cast<size_t>(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<std::mutex> 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<size_t>(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}]", ret);
+        uint8_t* src = static_cast<uint8_t*>(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<size_t>(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<size_t>(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<std::mutex> 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<std::mutex> 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();