diff --git a/include/rosparam_shortcuts/rosparam_shortcuts.h b/include/rosparam_shortcuts/rosparam_shortcuts.h index 5e28f13..e5d2f44 100644 --- a/include/rosparam_shortcuts/rosparam_shortcuts.h +++ b/include/rosparam_shortcuts/rosparam_shortcuts.h @@ -78,6 +78,9 @@ bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::s bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, std::vector &values); +bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, + std::vector &values); + bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, int &value); bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, std::size_t &value); @@ -96,11 +99,24 @@ bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::s /** * \brief Output a string of values from an array for debugging * \param array of values + * \tparam ValueType Type of the elements in @values * \return string of numbers separated by commas */ -std::string getDebugArrayString(std::vector values); +template +std::string getDebugArrayString(std::vector values) +{ + std::string debug_values{}; + for (const auto &value : values) debug_values += std::to_string(value) + ","; + return debug_values; +} -std::string getDebugArrayString(std::vector values); +template <> +std::string getDebugArrayString(std::vector values) +{ + std::string debug_values{}; + for (const auto &value : values) debug_values += value + ","; + return debug_values; +} /** * \brief Convert from 6 doubles of [x,y,z] [r,p,y] or 7 doubles of [x, y, z, qw, qx, qy, qz] to a transform diff --git a/src/rosparam_shortcuts.cpp b/src/rosparam_shortcuts.cpp index 81e3e1b..72213a1 100644 --- a/src/rosparam_shortcuts.cpp +++ b/src/rosparam_shortcuts.cpp @@ -120,6 +120,27 @@ bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::s return true; } +bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, + std::vector &values) +{ + // Load a param + if (!nh.hasParam(param_name)) + { + ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'."); + return false; + } + nh.getParam(param_name, values); + + if (values.empty()) + ROS_WARN_STREAM_NAMED(parent_name, "Empty vector for parameter '" << nh.getNamespace() << "/" << param_name << "'" + "."); + + ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name + << "' with values [" << getDebugArrayString(values) << "]"); + + return true; +} + bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, int &value) { // Load a param @@ -233,26 +254,6 @@ bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::s return true; } -std::string getDebugArrayString(std::vector values) -{ - std::stringstream debug_values; - for (std::size_t i = 0; i < values.size(); ++i) - { - debug_values << values[i] << ","; - } - return debug_values.str(); -} - -std::string getDebugArrayString(std::vector values) -{ - std::stringstream debug_values; - for (std::size_t i = 0; i < values.size(); ++i) - { - debug_values << values[i] << ","; - } - return debug_values.str(); -} - bool convertDoublesToEigen(const std::string &parent_name, std::vector values, Eigen::Affine3d &transform) { if (values.size() == 6)