Rename auto_exposure_damping to auto_exposure_gain

apl-ocean-engineering · Feb 7, 2024 · b72c9d2 · b72c9d2
1 parent 4b2496a
commit b72c9d2
Show file tree

Hide file tree

Showing 3 changed files with 94 additions and 74 deletions.
diff --git a/arena_camera/cfg/ArenaCamera.cfg b/arena_camera/cfg/ArenaCamera.cfg
@@ -12,7 +12,7 @@ gen.add("target_brightness", int_t,    SensorLevels.RECONFIGURE_RUNNING, "Target
 
 gen.add("auto_exposure",     bool_t,   SensorLevels.RECONFIGURE_RUNNING, "Use camera auto-exposure.", True);
 gen.add("auto_exposure_max_ms", double_t, SensorLevels.RECONFIGURE_RUNNING, "Exposure time (ms).  Set to 0.0 to use autolimit.", 25.0, 0, 500.0)
-gen.add("auto_exposure_damping", double_t, SensorLevels.RECONFIGURE_RUNNING, "Auto Exposure damping.  Might actually be a proportional term; small values converge more slowly ", 90.0, 0, 100.0)
+gen.add("auto_exposure_gain", double_t, SensorLevels.RECONFIGURE_RUNNING, "Auto Exposure proportional gain; converge more slowly for small values, may oscillate for large values", 90.0, 0, 100.0)
 
 gen.add("exposure_ms",       double_t, SensorLevels.RECONFIGURE_RUNNING, "Exposure time (ms) if auto-exposure is disabled.", 20.0, 0.1, 500.0)
 

diff --git a/arena_camera/include/arena_camera/arena_camera_nodelet.h b/arena_camera/include/arena_camera/arena_camera_nodelet.h
@@ -147,7 +147,7 @@ class ArenaCameraNodeletBase : public nodelet::Nodelet {
   //                   for the auto-exposure algorithm
   void setExposure(AutoExposureMode exp_mode, float exposure_ms);
 
-  void setExposureDamping(float exposure_damping);
+  void setAutoExposureGain(float exposure_damping);
 
   /// Return current camera ExposureTime
   /// @param immediate If true, will directly query the camera. Otherwise uses a

diff --git a/arena_camera/src/nodelet_base.cpp b/arena_camera/src/nodelet_base.cpp
@@ -709,79 +709,87 @@ void ArenaCameraNodeletBase::onExposureChangeCallback(GenApi::INode *pNode) {
 
 void ArenaCameraNodeletBase::setExposure(
     ArenaCameraNodeletBase::AutoExposureMode exp_mode, float exposure_ms) {
-  auto pNodeMap = pDevice_->GetNodeMap();
-  // exposure_auto_ will be already set to false if exposure_given_ is true
-  // read params () solved the priority between them
-  if (exp_mode == ArenaCameraNodeletBase::AutoExposureMode::Off) {
-    Arena::SetNodeValue<GenICam::gcstring>(pNodeMap, "ExposureAuto", "Off");
-
-    GenApi::CFloatPtr pExposureTime =
-        pDevice_->GetNodeMap()->GetNode("ExposureTime");
-
-    float exposure_to_set = exposure_ms * 1000;
-    if (exposure_to_set < pExposureTime->GetMin()) {
-      NODELET_WARN_STREAM("Desired exposure ("
-                          << exposure_to_set << ") "
-                          << "time unreachable! Setting to lower limit: "
-                          << pExposureTime->GetMin());
-      exposure_to_set = pExposureTime->GetMin();
-    } else if (exposure_to_set > pExposureTime->GetMax()) {
-      NODELET_WARN_STREAM("Desired exposure ("
-                          << exposure_to_set << ") "
-                          << "time unreachable! Setting to upper limit: "
-                          << pExposureTime->GetMax());
-      exposure_to_set = pExposureTime->GetMax();
-    }
+  try {
+    auto pNodeMap = pDevice_->GetNodeMap();
+    // exposure_auto_ will be already set to false if exposure_given_ is true
+    // read params () solved the priority between them
+    if (exp_mode == ArenaCameraNodeletBase::AutoExposureMode::Off) {
+      Arena::SetNodeValue<GenICam::gcstring>(pNodeMap, "ExposureAuto", "Off");
+
+      GenApi::CFloatPtr pExposureTime =
+          pDevice_->GetNodeMap()->GetNode("ExposureTime");
+
+      float exposure_to_set = exposure_ms * 1000;
+      if (exposure_to_set < pExposureTime->GetMin()) {
+        NODELET_WARN_STREAM("Desired exposure ("
+                            << exposure_to_set << ") "
+                            << "time unreachable! Setting to lower limit: "
+                            << pExposureTime->GetMin());
+        exposure_to_set = pExposureTime->GetMin();
+      } else if (exposure_to_set > pExposureTime->GetMax()) {
+        NODELET_WARN_STREAM("Desired exposure ("
+                            << exposure_to_set << ") "
+                            << "time unreachable! Setting to upper limit: "
+                            << pExposureTime->GetMax());
+        exposure_to_set = pExposureTime->GetMax();
+      }
 
-    pExposureTime->SetValue(exposure_to_set);
+      pExposureTime->SetValue(exposure_to_set);
 
-    NODELET_INFO_STREAM("Setting auto-exposure _off_ with exposure of "
-                        << pExposureTime->GetValue() << " ms");
-  } else {
-    if (exp_mode == ArenaCameraNodeletBase::AutoExposureMode::Once) {
-      NODELET_INFO_STREAM("Setting auto-exposure to _on_ / Once");
-      Arena::SetNodeValue<GenICam::gcstring>(pNodeMap, "ExposureAuto", "Once");
+      NODELET_INFO_STREAM("Setting auto-exposure _off_ with exposure of "
+                          << pExposureTime->GetValue() << " ms");
     } else {
-      NODELET_INFO_STREAM("Setting auto-exposure to _on_ / Continuous");
-      Arena::SetNodeValue<GenICam::gcstring>(pNodeMap, "ExposureAuto",
-                                             "Continuous");
-    }
+      if (exp_mode == ArenaCameraNodeletBase::AutoExposureMode::Once) {
+        NODELET_INFO_STREAM("Setting auto-exposure to _on_ / Once");
+        Arena::SetNodeValue<GenICam::gcstring>(pNodeMap, "ExposureAuto",
+                                               "Once");
+      } else {
+        NODELET_INFO_STREAM("Setting auto-exposure to _on_ / Continuous");
+        Arena::SetNodeValue<GenICam::gcstring>(pNodeMap, "ExposureAuto",
+                                               "Continuous");
+      }
+
+      if (exposure_ms > 0) {
+        Arena::SetNodeValue<GenICam::gcstring>(pNodeMap,
+                                               "ExposureAutoLimitAuto", "Off");
+        GenApi::CFloatPtr pExposureUpperLimit =
+            pDevice_->GetNodeMap()->GetNode("ExposureAutoUpperLimit");
+        if (GenApi::IsWritable(pExposureUpperLimit)) {
+          // The parameter in the camera is in us
+          pExposureUpperLimit->SetValue(static_cast<int64_t>(exposure_ms) *
+                                        1000);
+        } else {
+          NODELET_INFO("ExposureAutoUpperLimit is not writeable");
+        }
 
-    if (exposure_ms > 0) {
-      Arena::SetNodeValue<GenICam::gcstring>(pNodeMap, "ExposureAutoLimitAuto",
-                                             "Off");
-      GenApi::CFloatPtr pExposureUpperLimit =
-          pDevice_->GetNodeMap()->GetNode("ExposureAutoUpperLimit");
-      if (GenApi::IsWritable(pExposureUpperLimit)) {
-        // The parameter in the camera is in us
-        pExposureUpperLimit->SetValue(static_cast<int64_t>(exposure_ms) * 1000);
       } else {
-        NODELET_INFO("ExposureAutoUpperLimit is not writeable");
+        // Use automatic auto-exposure limits
+        Arena::SetNodeValue<GenICam::gcstring>(
+            pNodeMap, "ExposureAutoLimitAuto", "Continuous");
       }
 
-    } else {
-      // Use automatic auto-exposure limits
-      Arena::SetNodeValue<GenICam::gcstring>(pNodeMap, "ExposureAutoLimitAuto",
-                                             "Continuous");
+      NODELET_INFO_STREAM(
+          "Enabling autoexposure with limits "
+          << Arena::GetNodeValue<double>(pNodeMap, "ExposureAutoLowerLimit")
+          << " to "
+          << Arena::GetNodeValue<double>(pNodeMap, "ExposureAutoUpperLimit"));
     }
 
-    NODELET_INFO_STREAM(
-        "Enabling autoexposure with limits "
-        << Arena::GetNodeValue<double>(pNodeMap, "ExposureAutoLowerLimit")
-        << " to "
-        << Arena::GetNodeValue<double>(pNodeMap, "ExposureAutoUpperLimit"));
+  } catch (const GenICam::GenericException &e) {
+    NODELET_ERROR_STREAM("Caught exception while setting exposure damping: "
+                         << e.GetDescription());
   }
 }
 
-void ArenaCameraNodeletBase::setExposureDamping(float exposure_damping) {
-  try {
-    auto pNodeMap = pDevice_->GetNodeMap();
+void ArenaCameraNodeletBase::setAutoExposureGain(float exposure_gain) {
+  auto pNodeMap = pDevice_->GetNodeMap();
 
+  try {
     GenApi::CFloatPtr pExposureDamping =
-        pDevice_->GetNodeMap()->GetNode("ExposureAutoDamping");
+        pNodeMap->GetNode("ExposureAutoDamping");
+
     if (GenApi::IsWritable(pExposureDamping)) {
-      // The parameter in the camera is in us
-      pExposureDamping->SetValue(exposure_damping);
+      pExposureDamping->SetValue(exposure_gain);
     } else {
       NODELET_INFO("ExposureAutoDamping is not writeable");
     }
@@ -804,7 +812,8 @@ float ArenaCameraNodeletBase::currentGain(bool immediate) {
     try {
       return Arena::GetNodeValue<double>(pDevice_->GetNodeMap(), "Gain");
     } catch (const GenICam::GenericException &e) {
-      NODELET_WARN_STREAM("Unable to read exposure time");
+      NODELET_WARN_STREAM(
+          "Exception whie querying gain: " << e.GetDescription());
       return -1.0;
     }
   } else {
@@ -816,7 +825,8 @@ void ArenaCameraNodeletBase::onGainChangeCallback(GenApi::INode *pNode) {
   try {
     cached_gain_ = Arena::GetNodeValue<double>(pDevice_->GetNodeMap(), "Gain");
   } catch (const GenICam::GenericException &e) {
-    ;
+    NODELET_WARN_STREAM(
+        "Exception whie updating cached gain: " << e.GetDescription());
   }
 }
 
@@ -860,7 +870,6 @@ bool ArenaCameraNodeletBase::setGain(
       Arena::SetNodeValue<GenICam::gcstring>(pNodeMap, "GainAuto",
                                              "Continuous");
       NODELET_INFO_STREAM("Setting auto-gain to _on_ / Continuous");
-    } else {
     }
 
   } catch (const GenICam::GenericException &e) {
@@ -878,22 +887,33 @@ bool ArenaCameraNodeletBase::setGain(
 float ArenaCameraNodeletBase::currentGamma() {
   GenApi::CFloatPtr pGamma = pDevice_->GetNodeMap()->GetNode("Gamma");
 
-  if (!pGamma || !GenApi::IsReadable(pGamma)) {
-    NODELET_WARN_STREAM("No gamma value, returning -1");
-    return -1.;
-  } else {
-    float gammaValue = pGamma->GetValue();
-    return gammaValue;
+  try {
+    if (!pGamma || !GenApi::IsReadable(pGamma)) {
+      NODELET_WARN_STREAM("No gamma value, returning -1");
+      return -1.;
+    } else {
+      float gammaValue = pGamma->GetValue();
+      return gammaValue;
+    }
+  } catch (const GenICam::GenericException &e) {
+    NODELET_ERROR_STREAM(
+        "Caught exception while querying gamma: " << e.GetDescription());
+    return false;
   }
 }
 
 bool ArenaCameraNodeletBase::setGamma(const float &target_gamma) {
   // for GigE cameras you have to enable gamma first
+  try {
+    GenApi::CBooleanPtr pGammaEnable =
+        pDevice_->GetNodeMap()->GetNode("GammaEnable");
+    if (GenApi::IsWritable(pGammaEnable)) {
+      pGammaEnable->SetValue(true);
+    }
 
-  GenApi::CBooleanPtr pGammaEnable =
-      pDevice_->GetNodeMap()->GetNode("GammaEnable");
-  if (GenApi::IsWritable(pGammaEnable)) {
-    pGammaEnable->SetValue(true);
+  } catch (const GenICam::GenericException &e) {
+    NODELET_ERROR_STREAM(
+        "Caught exception while querying gamma: " << e.GetDescription());
   }
 
   GenApi::CFloatPtr pGamma = pDevice_->GetNodeMap()->GetNode("Gamma");
@@ -1255,7 +1275,7 @@ void ArenaCameraNodeletBase::reconfigureCallback(ArenaCameraConfig &config,
   if (config.auto_exposure) {
     setExposure(ArenaCameraNodeletBase::AutoExposureMode::Continuous,
                 config.auto_exposure_max_ms);
-    setExposureDamping(config.auto_exposure_damping);
+    setAutoExposureGain(config.auto_exposure_gain);
   } else {
     setExposure(ArenaCameraNodeletBase::AutoExposureMode::Off,
                 config.exposure_ms);