From 9d25181ee6841acb51031ccb77db1217b2f5d6b0 Mon Sep 17 00:00:00 2001 From: Jakub Hazik Date: Mon, 14 Oct 2019 15:11:59 +0200 Subject: [PATCH] fix node fail when single_patter_exposure is not supported by firmware --- src/RosInterface.cpp | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/src/RosInterface.cpp b/src/RosInterface.cpp index 93fa7fa..e8e8c35 100644 --- a/src/RosInterface.cpp +++ b/src/RosInterface.cpp @@ -496,8 +496,12 @@ namespace phoxi_camera { if (level & (1 << 15)) { this->isOk(); std::vector supportedSPE = scanner->SupportedSinglePatternExposures; - scanner->CapturingSettings->SinglePatternExposure = supportedSPE.at(config.single_pattern_exposure); - this->dynamicReconfigureConfig.single_pattern_exposure = config.single_pattern_exposure; + if (!supportedSPE.empty()) { // ignore setting if setting is not supported + scanner->CapturingSettings->SinglePatternExposure = supportedSPE.at(config.single_pattern_exposure); + this->dynamicReconfigureConfig.single_pattern_exposure = config.single_pattern_exposure; + } else { + ROS_WARN("Scanner setting 'Single pattern exposure' is not supported by the scanner firmware."); + } } if (level & (1 << 16)) { @@ -611,14 +615,18 @@ namespace phoxi_camera { this->dynamicReconfigureConfig.ambient_light_suppression = capturingSettings.AmbientLightSuppression; std::vector supportedSPE = scanner->SupportedSinglePatternExposures; - auto actualParam_it = std::find(supportedSPE.begin(), supportedSPE.end(), capturingSettings.SinglePatternExposure); - if (actualParam_it == supportedSPE.end()) { - int singlePatternExposure_index; - nh.getParam("single_pattern_exposure", singlePatternExposure_index); - this->dynamicReconfigureConfig.single_pattern_exposure = singlePatternExposure_index; - ROS_WARN("Can not update Single Pattern Exposure parameter in dynamic reconfigure, set default value from config."); + if (!supportedSPE.empty()) { + auto actualParam_it = std::find(supportedSPE.begin(), supportedSPE.end(), capturingSettings.SinglePatternExposure); + if (actualParam_it != supportedSPE.end()) { + this->dynamicReconfigureConfig.single_pattern_exposure = actualParam_it - supportedSPE.begin(); + } else { + int singlePatternExposure_index; + nh.getParam("single_pattern_exposure", singlePatternExposure_index); + this->dynamicReconfigureConfig.single_pattern_exposure = singlePatternExposure_index; + ROS_WARN("Can not update Single Pattern Exposure parameter in dynamic reconfigure, set default value from config."); + } } else { - this->dynamicReconfigureConfig.single_pattern_exposure = actualParam_it - supportedSPE.begin(); + ROS_WARN("Scanner setting 'Single pattern exposure' is not supported by the scanner firmware."); } this->dynamicReconfigureConfig.camera_only_mode = capturingSettings.CameraOnlyMode;