增加手动曝光模式程序控制曝光

gloryhry · Dec 22, 2023 · c0cce7b · c0cce7b
1 parent 5ff7399
commit c0cce7b
Show file tree

Hide file tree

Showing 5 changed files with 55 additions and 12 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -3,8 +3,8 @@ project(hik_camera_driver)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
-# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -march=native -O3 -pthread")
-# SET(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -march=native -O3 -pthread")
+SET(CMAKE_BUILD_TYPE "Release")
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)

diff --git a/config/camera.yaml b/config/camera.yaml
@@ -31,13 +31,15 @@ Camera:
   # 0:Off
   # 1:Once
   # 2:Continuous
-  Exposure: 2
+  Exposure: 0
   # 曝光时间 ≥0.0 ，单位 us
   Exposure_time: 5000
   # 自动曝光时间上限
   ExposureTimeUp: 10000
   # 自动曝光时间下限
   ExposureTimeLow: 100
+  # 程序控制曝光，仅当自动曝光设为手动时启用
+  Exposure_control: true
 
   # 自动增益
   # 0:Off

diff --git a/include/hik_camera_driver/camera.h b/include/hik_camera_driver/camera.h
@@ -17,7 +17,9 @@ namespace HIKCAMERA
     // cv::Mat frame;            // 临时存放当前帧
     extern sensor_msgs::ImagePtr frame; // 临时存放当前帧
     extern pthread_mutex_t mutex;       // 存放帧的锁
-    extern bool frame_empty;    // 用于标志是否有新帧未发布
+    extern bool frame_empty;            // 用于标志是否有新帧未发布
+    extern float exposure_time_set;     // 用于存放下次设置的曝光时间
+    extern int exposure_auto;          // 是否自动曝光
 
     class Hik_camera_base
     {
@@ -56,6 +58,9 @@ namespace HIKCAMERA
         boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_;
         image_transport::CameraPublisher camera_pub;
         ros::Subscriber exposure_sub;
+        bool exposure_control = false;             // 程序控制曝光（当外部触发,无法使用自动曝光时启用）
+        float exposure_time_up, exposure_time_low; // 曝光时间上下限
+        float scale = 1.03;                        // 曝光时间变化率
     };
 
 } // namespace HIKCAMERA

diff --git a/src/camera.cpp b/src/camera.cpp
@@ -5,6 +5,8 @@ namespace HIKCAMERA
     sensor_msgs::ImagePtr frame; // 临时存放当前帧
     pthread_mutex_t mutex;       // 存放帧的锁
     bool frame_empty = true;     // 用于标志是否有新帧未发布
+    float exposure_time_set;     // 用于存放下次设置的曝光时间
+    int exposure_auto;           // 是否自动曝光
 
     Hik_camera_base::Hik_camera_base(ros::NodeHandle &nh, ros::NodeHandle &private_nh)
     {
@@ -53,9 +55,12 @@ namespace HIKCAMERA
         private_nh.param<bool>("Camera/GammaEnable", Gamma, false);
         private_nh.param<float>("Camera/Gamma_value", Gamma_value, 1.0);
         private_nh.param<int>("Camera/Gamma_selector", Gamma_selector, 1);
+        private_nh.param<bool>("Camera/Exposure_control", exposure_control, false);
 
-        // Exposure_low_time = ExposureTimeLow;
-        // Exposure_up_time = ExposureTimeUp;
+        exposure_time_set = Exposure_time;
+        exposure_auto = Exposure;
+        exposure_time_low = ExposureTimeLow;
+        exposure_time_up = ExposureTimeUp;
 
         setEnumValue("AcquisitionMode", MV_ACQ_MODE_CONTINUOUS);
         ROS_INFO_STREAM("AcquisitionMode set to Continuous.");
@@ -121,7 +126,7 @@ namespace HIKCAMERA
         else
         {
             ROS_ERROR_STREAM("Not Exist Exposure Mode: " << Exposure);
-            return EXIT_FAILURE;
+            return false;
         }
         // 设置Gain
         if (Gain_mode == 0)
@@ -139,7 +144,7 @@ namespace HIKCAMERA
         else
         {
             ROS_ERROR_STREAM("Not Exist Gain Mode: " << Gain_mode);
-            return EXIT_FAILURE;
+            return false;
         }
         // 设置白平衡
         setEnumValue("BalanceWhiteAuto", MV_BALANCEWHITE_AUTO_CONTINUOUS);
@@ -197,6 +202,7 @@ namespace HIKCAMERA
         //         ROS_ERROR_STREAM("Not Exist Gamma Selector Mode: " << Gamma_selector);
         //     }
         // }
+        return true;
     }
 
     bool Hik_camera_base::set_camera(MV_CC_DEVICE_INFO &camera)
@@ -458,7 +464,8 @@ namespace HIKCAMERA
         getFloatValue("ExposureTime", get_exposure);
         if (get_exposure != exposure)
         {
-            changeExposureTime(exposure);
+            // changeExposureTime(exposure);
+            exposure_time_set = exposure;
         }
     }
 
@@ -485,11 +492,20 @@ namespace HIKCAMERA
         unsigned int nDataSize = stParam.nCurValue;
         while (ros::ok())
         {
+            if (exposure_auto == 0)
+            {
+                // 设置曝光
+                nRet = MV_CC_SetExposureTime(p_handle, exposure_time_set);
+                if (MV_OK != nRet)
+                {
+                    ROS_WARN("Exposure time set failed! nRet [%x]\n", nRet);
+                }
+            }
             nRet = MV_CC_GetOneFrameTimeout(p_handle, pData, nDataSize, &stImageInfo, 50);
             if (nRet == MV_OK)
             {
                 ros::Time rcv_time = ros::Time::now();
-                std::string debug_msg;
+                // std::string debug_msg;
                 // ROS_INFO_STREAM("GetOneFrame,nFrameNum[" << stImageInfo.nFrameNum << "],FrameTime:" + std::to_string(rcv_time.toSec()));
                 stConvertParam.nWidth = stImageInfo.nWidth;
                 stConvertParam.nHeight = stImageInfo.nHeight;
@@ -521,6 +537,7 @@ namespace HIKCAMERA
             free(m_pBufForSaveImage);
             m_pBufForSaveImage = NULL;
         }
+        return NULL;
     }
 
     void Hik_camera_base::ImagePub()
@@ -534,6 +551,25 @@ namespace HIKCAMERA
             ci_->header.stamp = frame->header.stamp;
             camera_pub.publish(*frame, *ci_);
             frame_empty = true;
+
+            if (exposure_control)
+            {
+                cv_bridge::CvImagePtr cv_ptr;
+                cv_ptr = cv_bridge::toCvCopy(frame, sensor_msgs::image_encodings::BGR8);
+                cv::Mat temp_img = cv_ptr->image;
+                cv::Mat imgGray;
+                cv::cvtColor(temp_img, imgGray, CV_BGR2GRAY);
+                cv::Scalar grayScalar = cv::mean(imgGray);
+                float imgGrayLight = grayScalar.val[0];
+                if (imgGrayLight < 90 && exposure_time_set * scale < exposure_time_up)
+                {
+                    exposure_time_set *= scale;
+                }
+                else if (imgGrayLight > 110 && exposure_time_set / scale > exposure_time_low)
+                {
+                    exposure_time_set /= scale;
+                }
+            }
         }
         pthread_mutex_unlock(&mutex);
     }

diff --git a/src/hik_driver.cpp b/src/hik_driver.cpp
@@ -87,7 +87,7 @@ int main(int argc, char **argv)
     cameras = get_all_camera();
     if (cameras.size() == 0)
     {
-        return EXIT_FAILURE;
+        return EXIT_SUCCESS;
     }
 
     std::string serial_number;
@@ -123,7 +123,7 @@ int main(int argc, char **argv)
         if (!flag)
         {
             ROS_ERROR_STREAM("Can Not Find Camera: " << serial_number);
-            return EXIT_FAILURE;
+            return EXIT_SUCCESS;
         }
     }
     camera.set_params();