diff --git a/README.md b/README.md index 7abe2dc..73622b7 100644 --- a/README.md +++ b/README.md @@ -63,6 +63,9 @@ example: param6: [2, 2, 2, 1, 0, 0, 0] # Eigen::Isometry3d - x, y, z, qw, qx, qy, qz param7: [1, 1, 1, 3.14, 0, 0] # geometry_msgs::Pose - x, y, z, roll, pitch, yaw param8: [2, 2, 2, 1, 0, 0, 0] # geometry_msgs::Pose - x, y, z, qw, qx, qy, qz + param9: [1, 2, 3] # geometry_msgs::Vector3 - x, y, z + param10: [25, 20, 200, 0.8] # std_msgs::ColorRGBA - r, g, b, a + ``` ## Testing and Linting diff --git a/config/example.yaml b/config/example.yaml index 7791941..a4e9bd2 100644 --- a/config/example.yaml +++ b/config/example.yaml @@ -8,3 +8,5 @@ example: param6: [2, 2, 2, 1, 0, 0, 0] # Eigen::Isometry3d - x, y, z, qw, qx, qy, qz param7: [1, 1, 1, 3.14, 0, 0] # geometry_msgs::Pose - x, y, z, roll, pitch, yaw param8: [2, 2, 2, 1, 0, 0, 0] # geometry_msgs::Pose - x, y, z, qw, qx, qy, qz + param9: [1, 2, 3] # geometry_msgs::Vector3 - x, y, z + param10: [25, 20, 200, 0.8] # std_msgs::ColorRGBA - r, g, b, a diff --git a/include/rosparam_shortcuts/rosparam_shortcuts.h b/include/rosparam_shortcuts/rosparam_shortcuts.h index 1fd30a9..6858b25 100644 --- a/include/rosparam_shortcuts/rosparam_shortcuts.h +++ b/include/rosparam_shortcuts/rosparam_shortcuts.h @@ -56,6 +56,12 @@ // geometry_msgs/Pose #include +// geometry_msgs/Vector3 +#include + +// std_msgs/ColorRGBA +#include + namespace rosparam_shortcuts { // ------------------------------------------------------------------------------------------------- @@ -99,6 +105,13 @@ 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, geometry_msgs::Pose &value); +bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, + geometry_msgs::Vector3 &value); + +bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, + std_msgs::ColorRGBA &value); + + /** * \brief Output a string of values from an array for debugging * \param array of values diff --git a/src/example.cpp b/src/example.cpp index 9468e7e..6449427 100644 --- a/src/example.cpp +++ b/src/example.cpp @@ -66,6 +66,8 @@ int main(int argc, char** argv) Eigen::Isometry3d param6; geometry_msgs::Pose param7; geometry_msgs::Pose param8; + geometry_msgs::Vector3 param9; + std_msgs::ColorRGBA param10; // Load rosparams ros::NodeHandle rpnh(nh, name); @@ -79,6 +81,8 @@ int main(int argc, char** argv) error += !rosparam_shortcuts::get(name, rpnh, "param6", param6); // Isometry3d param error += !rosparam_shortcuts::get(name, rpnh, "param7", param7); // geometry_msgs::Pose param error += !rosparam_shortcuts::get(name, rpnh, "param8", param8); // geometry_msgs::Pose param + error += !rosparam_shortcuts::get(name, rpnh, "param9", param9); // geometry_msgs::Vector3 param + error += !rosparam_shortcuts::get(name, rpnh, "param10", param10); // std_msgs::ColorRGBA param // add more parameters here to load if desired rosparam_shortcuts::shutdownIfError(name, error); @@ -93,6 +97,8 @@ int main(int argc, char** argv) ROS_INFO_STREAM_NAMED(name, "param6: Translation:\n" << param6.translation()); ROS_INFO_STREAM_NAMED(name, "param7: Pose:\n" << param7); ROS_INFO_STREAM_NAMED(name, "param8: Pose:\n" << param8); + ROS_INFO_STREAM_NAMED(name, "param9: Vector:\n" << param9); + ROS_INFO_STREAM_NAMED(name, "param10: ColorRGBA:\n" << param10); ROS_INFO_STREAM_NAMED(name, "Shutting down."); ros::shutdown(); diff --git a/src/rosparam_shortcuts.cpp b/src/rosparam_shortcuts.cpp index c610704..e60b8a3 100644 --- a/src/rosparam_shortcuts.cpp +++ b/src/rosparam_shortcuts.cpp @@ -49,261 +49,304 @@ namespace rosparam_shortcuts { -bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, bool &value) -{ - // Load a param - if (!nh.hasParam(param_name)) + bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, bool &value) { - ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'."); - return false; - } - nh.getParam(param_name, value); - ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value " - << value); + // 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, value); + ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value " + << value); - return true; -} - -bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶ms_namespace, - std::map ¶meters) -{ - // Load a param - if (!nh.hasParam(params_namespace)) - { - ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameters in namespace '" << nh.getNamespace() << "/" - << params_namespace << "'."); - return false; + return true; } - nh.getParam(params_namespace, parameters); - // Debug - for (std::map::const_iterator param_it = parameters.begin(); param_it != parameters.end(); - param_it++) + bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶ms_namespace, + std::map ¶meters) { - ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << params_namespace << "/" - << param_it->first << "' with value " << param_it->second); - } - - return true; -} + // Load a param + if (!nh.hasParam(params_namespace)) + { + ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameters in namespace '" << nh.getNamespace() << "/" + << params_namespace << "'."); + return false; + } + nh.getParam(params_namespace, parameters); + + // Debug + for (std::map::const_iterator param_it = parameters.begin(); param_it != parameters.end(); + param_it++) + { + ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << params_namespace << "/" + << param_it->first << "' with value " << param_it->second); + } -bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, double &value) -{ - // Load a param - if (!nh.hasParam(param_name)) - { - ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'."); - return false; + return true; } - nh.getParam(param_name, value); - ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value " - << value); - - 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)) + bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, double &value) { - ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'."); - return false; + // 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, value); + ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value " + << value); + + return true; } - nh.getParam(param_name, values); - if (values.empty()) - ROS_WARN_STREAM_NAMED(parent_name, "Empty vector for parameter '" << nh.getNamespace() << "/" << param_name << "'" - "."); + 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); - ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name - << "' with values [" << getDebugArrayString(values) << "]"); + if (values.empty()) + ROS_WARN_STREAM_NAMED(parent_name, "Empty vector for parameter '" << nh.getNamespace() << "/" << param_name << "'" + "."); - return true; -} + ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name + << "' with values [" << getDebugArrayString(values) << "]"); -bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, int &value) -{ - // Load a param - if (!nh.hasParam(param_name)) - { - ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'."); - return false; + return true; } - nh.getParam(param_name, value); - ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value " - << value); - - return true; -} -bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, std::size_t &value) -{ - // Load a param - if (!nh.hasParam(param_name)) + bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, int &value) { - ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'."); - return false; - } - int nonsigned_value; - nh.getParam(param_name, nonsigned_value); - value = nonsigned_value; - ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value " - << value); + // 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, value); + ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value " + << value); - return true; -} - -bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, std::string &value) -{ - // Load a param - if (!nh.hasParam(param_name)) - { - ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'."); - return false; + return true; } - nh.getParam(param_name, value); - ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value " - << value); - 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)) + bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, std::size_t &value) { - ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'."); - return false; - } - nh.getParam(param_name, values); + // Load a param + if (!nh.hasParam(param_name)) + { + ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'."); + return false; + } + int nonsigned_value; + nh.getParam(param_name, nonsigned_value); + value = nonsigned_value; + ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value " + << value); - if (values.empty()) - ROS_WARN_STREAM_NAMED(parent_name, "Empty vector for parameter '" << nh.getNamespace() << "/" << param_name << "'" - "."); + return true; + } - ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value " - << getDebugArrayString(values)); + bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, std::string &value) + { + // 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, value); + ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value " + << value); - return true; -} + return true; + } -bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, ros::Duration &value) -{ - double temp_value; - // Load a param - if (!nh.hasParam(param_name)) + bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, + std::vector &values) { - ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'."); - return false; - } - nh.getParam(param_name, temp_value); - ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value " - << value); + // 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); - // Convert to ros::Duration - value = ros::Duration(temp_value); + if (values.empty()) + ROS_WARN_STREAM_NAMED(parent_name, "Empty vector for parameter '" << nh.getNamespace() << "/" << param_name << "'" + "."); - return true; -} + ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value " + << getDebugArrayString(values)); -bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, - Eigen::Isometry3d &value) -{ - std::vector values; + return true; + } - // Load a param - if (!nh.hasParam(param_name)) + bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, ros::Duration &value) { - ROS_ERROR_STREAM_NAMED(parent_name, "Missing parameter '" << nh.getNamespace() << "/" << param_name << "'."); - return false; + double temp_value; + // 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, temp_value); + ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name << "' with value " + << value); + + // Convert to ros::Duration + value = ros::Duration(temp_value); + + return true; } - nh.getParam(param_name, values); - if (values.empty()) - ROS_WARN_STREAM_NAMED(parent_name, "Empty vector for parameter '" << nh.getNamespace() << "/" << param_name << "'" - "."); + bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, + Eigen::Isometry3d &value) + { + std::vector values; - ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name - << "' with values [" << getDebugArrayString(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); - // Convert to Eigen::Isometry3d - convertDoublesToEigen(parent_name, values, value); + if (values.empty()) + ROS_WARN_STREAM_NAMED(parent_name, "Empty vector for parameter '" << nh.getNamespace() << "/" << param_name << "'" + "."); - return true; -} + ROS_DEBUG_STREAM_NAMED(parent_name, "Loaded parameter '" << nh.getNamespace() << "/" << param_name + << "' with values [" << getDebugArrayString(values) << "]"); -bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, - geometry_msgs::Pose &value) -{ - Eigen::Isometry3d eigen_pose; - if (!get(parent_name, nh, param_name, eigen_pose)) - return false; + // Convert to Eigen::Isometry3d + convertDoublesToEigen(parent_name, values, value); - tf::poseEigenToMsg(eigen_pose, value); - return true; -} + return true; + } -std::string getDebugArrayString(std::vector values) -{ - std::stringstream debug_values; - for (std::size_t i = 0; i < values.size(); ++i) + bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, + geometry_msgs::Pose &value) { - debug_values << values[i] << ","; + Eigen::Isometry3d eigen_pose; + if (!get(parent_name, nh, param_name, eigen_pose)) + return false; + + tf::poseEigenToMsg(eigen_pose, value); + return true; } - return debug_values.str(); -} -std::string getDebugArrayString(std::vector values) -{ - std::stringstream debug_values; - for (std::size_t i = 0; i < values.size(); ++i) + bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, + geometry_msgs::Vector3 &value) { - debug_values << values[i] << ","; + std::vector values; + if (!get(parent_name, nh, param_name, values)) + return false; + + if (values.size() == 3) + { + value.x = values[0]; + value.y = values[1]; + value.z = values[2]; + } + else + { + ROS_ERROR_STREAM_NAMED(parent_name, "Invalid number of elements for parameter '" << nh.getNamespace() << "/" << param_name << "'."); + return false; + } + return true; } - return debug_values.str(); -} -bool convertDoublesToEigen(const std::string &parent_name, std::vector values, Eigen::Isometry3d &transform) -{ - if (values.size() == 6) + bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string ¶m_name, + std_msgs::ColorRGBA &value) { - // This version is correct RPY - Eigen::AngleAxisd roll_angle(values[3], Eigen::Vector3d::UnitX()); - Eigen::AngleAxisd pitch_angle(values[4], Eigen::Vector3d::UnitY()); - Eigen::AngleAxisd yaw_angle(values[5], Eigen::Vector3d::UnitZ()); - Eigen::Quaternion quaternion = roll_angle * pitch_angle * yaw_angle; - - transform = Eigen::Translation3d(values[0], values[1], values[2]) * quaternion; - + std::vector values; + if (!get(parent_name, nh, param_name, values)) + return false; + + if (values.size() == 4) + { + value.r = values[0]; + value.g = values[1]; + value.b = values[2]; + value.a = values[3]; + } + else + { + ROS_ERROR_STREAM_NAMED(parent_name, "Invalid number of elements for parameter '" << nh.getNamespace() << "/" << param_name << "'."); + return false; + } return true; } - else if (values.size() == 7) + + std::string getDebugArrayString(std::vector values) { - // Quaternion - transform = Eigen::Translation3d(values[0], values[1], values[2]) * - Eigen::Quaterniond(values[3], values[4], values[5], values[6]); - return true; + std::stringstream debug_values; + for (std::size_t i = 0; i < values.size(); ++i) + { + debug_values << values[i] << ","; + } + return debug_values.str(); } - else + + std::string getDebugArrayString(std::vector values) { - ROS_ERROR_STREAM_NAMED(parent_name, "Invalid number of doubles provided for transform, size=" << values.size()); - return false; + std::stringstream debug_values; + for (std::size_t i = 0; i < values.size(); ++i) + { + debug_values << values[i] << ","; + } + return debug_values.str(); } -} -void shutdownIfError(const std::string &parent_name, std::size_t error_count) -{ - if (!error_count) - return; + bool convertDoublesToEigen(const std::string &parent_name, std::vector values, Eigen::Isometry3d &transform) + { + if (values.size() == 6) + { + // This version is correct RPY + Eigen::AngleAxisd roll_angle(values[3], Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd pitch_angle(values[4], Eigen::Vector3d::UnitY()); + Eigen::AngleAxisd yaw_angle(values[5], Eigen::Vector3d::UnitZ()); + Eigen::Quaternion quaternion = roll_angle * pitch_angle * yaw_angle; + + transform = Eigen::Translation3d(values[0], values[1], values[2]) * quaternion; + + return true; + } + else if (values.size() == 7) + { + // Quaternion + transform = Eigen::Translation3d(values[0], values[1], values[2]) * + Eigen::Quaterniond(values[3], values[4], values[5], values[6]); + return true; + } + else + { + ROS_ERROR_STREAM_NAMED(parent_name, "Invalid number of doubles provided for transform, size=" << values.size()); + return false; + } + } - ROS_ERROR_STREAM_NAMED(parent_name, "Missing " << error_count << " ros parameters that are required. Shutting down " - "to prevent undefined behaviors"); - ros::shutdown(); - exit(0); -} + void shutdownIfError(const std::string &parent_name, std::size_t error_count) + { + if (!error_count) + return; + + ROS_ERROR_STREAM_NAMED(parent_name, "Missing " << error_count << " ros parameters that are required. Shutting down " + "to prevent undefined behaviors"); + ros::shutdown(); + exit(0); + } -} // namespace rosparam_shortcuts +} // namespace rosparam_shortcuts