diff --git a/include/mrs_lib/impl/param_provider.hpp b/include/mrs_lib/impl/param_provider.hpp index 07b8b346..482128d7 100644 --- a/include/mrs_lib/impl/param_provider.hpp +++ b/include/mrs_lib/impl/param_provider.hpp @@ -19,24 +19,6 @@ namespace mrs_lib } } - bool ParamProvider::getParam(const std::string& param_name, XmlRpc::XmlRpcValue& value_out) const - { - if (m_use_rosparam && m_nh.getParam(param_name, value_out)) - return true; - - try - { - const auto found_node = findYamlNode(param_name); - if (found_node.has_value()) - ROS_WARN_STREAM("[" << m_node_name << "]: Parameter \"" << param_name << "\" of desired type XmlRpc::XmlRpcValue is only available as a static parameter, which doesn't support loading of this type."); - } - catch (const YAML::Exception& e) - { - ROS_ERROR_STREAM("[" << m_node_name << "]: YAML-CPP threw an unknown exception: " << e.what()); - } - return false; - } - template bool ParamProvider::getParamImpl(const std::string& param_name, T& value_out) const { @@ -61,59 +43,6 @@ namespace mrs_lib return false; } - - std::optional ParamProvider::findYamlNode(const std::string& param_name) const - { - for (const auto& yaml : m_yamls) - { - // Try to load the parameter sequentially as a map. - auto cur_node_it = std::cbegin(yaml); - // The root should always be a pam - if (!cur_node_it->second.IsMap()) - continue; - - bool loaded = true; - { - constexpr char delimiter = '/'; - auto substr_start = std::cbegin(param_name); - auto substr_end = substr_start; - do - { - substr_end = std::find(substr_start, std::cend(param_name), delimiter); - // why can't substr or string_view take iterators? :'( - const auto start_pos = std::distance(std::cbegin(param_name), substr_start); - const auto count = std::distance(substr_start, substr_end); - const std::string param_substr = param_name.substr(start_pos, count); - substr_start = substr_end+1; - - bool found = false; - for (auto node_it = std::cbegin(cur_node_it->second); node_it != std::cend(cur_node_it->second); ++node_it) - { - if (node_it->first.as() == param_substr) - { - cur_node_it = node_it; - found = true; - break; - } - } - - if (!found) - { - loaded = false; - break; - } - } - while (substr_end != std::end(param_name) && cur_node_it->second.IsMap()); - } - - if (loaded) - { - return cur_node_it->second; - } - } - - return std::nullopt; - } } #endif // PARAM_PROVIDER_HPP diff --git a/src/param_loader/param_provider.cpp b/src/param_loader/param_provider.cpp index 3e5ca787..1d4f367d 100644 --- a/src/param_loader/param_provider.cpp +++ b/src/param_loader/param_provider.cpp @@ -43,4 +43,74 @@ namespace mrs_lib return false; } + bool ParamProvider::getParam(const std::string& param_name, XmlRpc::XmlRpcValue& value_out) const + { + if (m_use_rosparam && m_nh.getParam(param_name, value_out)) + return true; + + try + { + const auto found_node = findYamlNode(param_name); + if (found_node.has_value()) + ROS_WARN_STREAM("[" << m_node_name << "]: Parameter \"" << param_name << "\" of desired type XmlRpc::XmlRpcValue is only available as a static parameter, which doesn't support loading of this type."); + } + catch (const YAML::Exception& e) + { + ROS_ERROR_STREAM("[" << m_node_name << "]: YAML-CPP threw an unknown exception: " << e.what()); + } + return false; + } + + std::optional ParamProvider::findYamlNode(const std::string& param_name) const + { + for (const auto& yaml : m_yamls) + { + // Try to load the parameter sequentially as a map. + auto cur_node_it = std::cbegin(yaml); + // The root should always be a pam + if (!cur_node_it->second.IsMap()) + continue; + + bool loaded = true; + { + constexpr char delimiter = '/'; + auto substr_start = std::cbegin(param_name); + auto substr_end = substr_start; + do + { + substr_end = std::find(substr_start, std::cend(param_name), delimiter); + // why can't substr or string_view take iterators? :'( + const auto start_pos = std::distance(std::cbegin(param_name), substr_start); + const auto count = std::distance(substr_start, substr_end); + const std::string param_substr = param_name.substr(start_pos, count); + substr_start = substr_end+1; + + bool found = false; + for (auto node_it = std::cbegin(cur_node_it->second); node_it != std::cend(cur_node_it->second); ++node_it) + { + if (node_it->first.as() == param_substr) + { + cur_node_it = node_it; + found = true; + break; + } + } + + if (!found) + { + loaded = false; + break; + } + } + while (substr_end != std::end(param_name) && cur_node_it->second.IsMap()); + } + + if (loaded) + { + return cur_node_it->second; + } + } + + return std::nullopt; + } } diff --git a/test/param_loader/test.cpp b/test/param_loader/test.cpp index 74df9099..da8400c2 100644 --- a/test/param_loader/test.cpp +++ b/test/param_loader/test.cpp @@ -37,7 +37,7 @@ TEST(TESTSuite, main_test) { mrs_lib::ParamLoader pl(nh); XmlRpc::XmlRpcValue list; - pl.loadParam("services", list, XmlRpc::XmlRpcValue()); + EXPECT_TRUE(pl.loadParam("services", list, XmlRpc::XmlRpcValue())); ROS_INFO("[%s]: testing ParamLoader", ros::this_node::getName().c_str()); std::vector loaded_nd_matrix = pl.loadMatrixArray2("test_param_nd_matrix"); diff --git a/test/param_loader/test.yaml b/test/param_loader/test.yaml index ef6aa560..f98dcd60 100644 --- a/test/param_loader/test.yaml +++ b/test/param_loader/test.yaml @@ -9,3 +9,6 @@ test_param_nd_matrix: 2.0, 4.0] test_param_matrix: [] + +services: + complex_structure: ["asd"]