diff --git a/sensor_msgs/include/sensor_msgs/image_encodings.hpp b/sensor_msgs/include/sensor_msgs/image_encodings.hpp index ba15351d..387628d7 100644 --- a/sensor_msgs/include/sensor_msgs/image_encodings.hpp +++ b/sensor_msgs/include/sensor_msgs/image_encodings.hpp @@ -33,6 +33,7 @@ #define SENSOR_MSGS__IMAGE_ENCODINGS_HPP_ #include +#include #include #include @@ -109,6 +110,8 @@ const char ABSTRACT_ENCODING_PREFIXES[][5] = { "8UC", "8SC", "16UC", "16SC", "32SC", "32FC", "64FC" }; +const std::regex cv_type_regex("(8|16|32|64)(U|S|F)C([0-9]*)"); + // Utility functions for inspecting an encoding string static inline bool isColor(const std::string & encoding) { @@ -172,24 +175,10 @@ static inline int numChannels(const std::string & encoding) } // Now all the generic content encodings - // TODO(clalancette): Rewrite with regex when ROS supports C++11 - for (size_t i = 0; i < sizeof(ABSTRACT_ENCODING_PREFIXES) / sizeof(*ABSTRACT_ENCODING_PREFIXES); - i++) - { - std::string prefix = ABSTRACT_ENCODING_PREFIXES[i]; - if (encoding.substr(0, prefix.size()) != prefix) { - continue; - } - if (encoding.size() == prefix.size()) { - return 1; // ex. 8UC -> 1 - } - int n_channel = atoi( - encoding.substr( - prefix.size(), encoding.size() - prefix.size() - ).c_str()); // ex. 8UC5 -> 5 - if (n_channel != 0) { - return n_channel; // valid encoding string - } + std::cmatch m; + // ex. 8UC -> 1, 8UC5 -> 5 + if (std::regex_match(encoding.c_str(), m, cv_type_regex)) { + return (m[3] == "") ? 1 : std::atoi(m[3].str().c_str()); } if (encoding == YUV422 || @@ -236,24 +225,10 @@ static inline int bitDepth(const std::string & encoding) } // Now all the generic content encodings - // TODO(clalancette): Rewrite with regex when ROS supports C++11 - for (size_t i = 0; i < sizeof(ABSTRACT_ENCODING_PREFIXES) / sizeof(*ABSTRACT_ENCODING_PREFIXES); - i++) - { - std::string prefix = ABSTRACT_ENCODING_PREFIXES[i]; - if (encoding.substr(0, prefix.size()) != prefix) { - continue; - } - if (encoding.size() == prefix.size()) { - return atoi(prefix.c_str()); // ex. 8UC -> 8 - } - int n_channel = atoi( - encoding.substr( - prefix.size(), encoding.size() - prefix.size() - ).c_str()); // ex. 8UC10 -> 10 - if (n_channel != 0) { - return atoi(prefix.c_str()); // valid encoding string - } + std::cmatch m; + // ex. 8UC -> 8, 8UC10 -> 10 + if (std::regex_match(encoding.c_str(), m, cv_type_regex)) { + return std::atoi(m[0].str().c_str()); } if (encoding == YUV422 ||