diff --git a/src/devices/realsense2/realsense2Driver.cpp b/src/devices/realsense2/realsense2Driver.cpp index a3f0518..84723c5 100644 --- a/src/devices/realsense2/realsense2Driver.cpp +++ b/src/devices/realsense2/realsense2Driver.cpp @@ -8,6 +8,7 @@ #include #include +#include #include #include @@ -588,6 +589,13 @@ bool realsense2Driver::initializeRealsenseDevice() } if (!pipelineStartup()) return false; + + if (m_usePreset) + { + if(!setPreset(m_presetName)) + yCError(REALSENSE2) << "Unable to set preset: "<< m_presetName; + } + m_initialized = true; //TODO: if more are connected?! @@ -835,6 +843,27 @@ bool realsense2Driver::open(Searchable& config) m_stereoMode = config.find("stereoMode").asBool(); } + if (config.check("usePreset")) { + m_usePreset = config.find("usePreset").asBool(); + yCInfo(REALSENSE2) << "Enabled Using Presets"; + } + else + yCInfo(REALSENSE2) << "Presets disabled"; + + if (m_usePreset) + { + std::string presetName = config.find("presetName").asString(); + std::transform(presetName.begin(), presetName.end(), presetName.begin(), ::toupper); + if (presetsMap.find(presetName) == presetsMap.end()) { + yCError(REALSENSE2) << "Value " << presetName << " not allowed as camera preset, see documentation for supported values."; + } + else + { + m_presetName = presetsMap.at(presetName); + yCInfo(REALSENSE2) << "Found requested preset: " << presetName; + } + } + if (!m_paramParser.parseParam(config, params)) { yCError(REALSENSE2) << "Failed to parse the parameters"; @@ -1679,3 +1708,19 @@ int realsense2Driver::width() const { return m_infrared_intrin.width*2; } + +bool realsense2Driver::setPreset(rs2_rs400_visual_preset preset) +{ + try + { + auto sensor = m_profile.get_device().first(); + sensor.set_option(rs2_option::RS2_OPTION_VISUAL_PRESET, + preset); + } + catch(const std::exception& e) + { + yCError(REALSENSE2) << "Error while setting preset: " << e.what(); + return false; + } + return true; +} diff --git a/src/devices/realsense2/realsense2Driver.h b/src/devices/realsense2/realsense2Driver.h index 32d0d15..bfa2f1d 100644 --- a/src/devices/realsense2/realsense2Driver.h +++ b/src/devices/realsense2/realsense2Driver.h @@ -24,6 +24,7 @@ #include #include #include +#include class realsense2Driver : @@ -37,7 +38,15 @@ class realsense2Driver : typedef yarp::os::Stamp Stamp; typedef yarp::os::Property Property; typedef yarp::sig::FlexImage FlexImage; - + const std::map presetsMap{ + {"CUSTOM", rs2_rs400_visual_preset::RS2_RS400_VISUAL_PRESET_CUSTOM}, + {"DEFAULT", rs2_rs400_visual_preset::RS2_RS400_VISUAL_PRESET_DEFAULT}, + {"HAND", rs2_rs400_visual_preset::RS2_RS400_VISUAL_PRESET_HAND}, + {"HIGH_ACCURACY", rs2_rs400_visual_preset::RS2_RS400_VISUAL_PRESET_HIGH_ACCURACY}, + {"PRESET_HIGH_DENSITY", rs2_rs400_visual_preset::RS2_RS400_VISUAL_PRESET_HIGH_DENSITY}, + {"MEDIUM_DENSITY", rs2_rs400_visual_preset::RS2_RS400_VISUAL_PRESET_MEDIUM_DENSITY}, + {"REMOVE_IR_PATTERN", rs2_rs400_visual_preset::RS2_RS400_VISUAL_PRESET_REMOVE_IR_PATTERN} + }; public: realsense2Driver(); @@ -116,6 +125,7 @@ class realsense2Driver : bool pipelineRestart(); bool setFramerate(const int _fps); void fallback(); + bool setPreset(rs2_rs400_visual_preset preset); // realsense classes @@ -149,5 +159,7 @@ class realsense2Driver : float m_scale; bool m_rotateImage180{false}; std::vector m_supportedFeatures; + bool m_usePreset{false}; + rs2_rs400_visual_preset m_presetName; }; #endif