| 272 | } |
| 273 | |
| 274 | cv::Mat UtilsOpenCV::readRosImage(const sensor_msgs::msg::Image::ConstSharedPtr img_msg, bool grayscale) { |
| 275 | cv_bridge::CvImageConstPtr cv_ptr; |
| 276 | try { |
| 277 | // TODO(Toni): here we should consider using toCvShare... |
| 278 | cv_ptr = cv_bridge::toCvCopy(img_msg); |
| 279 | } catch (cv_bridge::Exception& exception) { |
| 280 | LOG(FATAL) << "cv_bridge exception: " << exception.what(); |
| 281 | } |
| 282 | |
| 283 | const cv::Mat img_const = cv_ptr->image; // Don't modify shared image in ROS. |
| 284 | cv::Mat converted_img(img_const.size(), CV_8U); |
| 285 | if (img_msg->encoding == sensor_msgs::image_encodings::BGR8) { |
| 286 | if (grayscale) { |
| 287 | cv::cvtColor(img_const, converted_img, cv::COLOR_BGR2GRAY); |
| 288 | } else { |
| 289 | converted_img = img_const; |
| 290 | } |
| 291 | return converted_img; |
| 292 | } else if (img_msg->encoding == sensor_msgs::image_encodings::RGB8) { |
| 293 | if (grayscale) { |
| 294 | cv::cvtColor(img_const, converted_img, cv::COLOR_RGB2GRAY); |
| 295 | } else { |
| 296 | cv::cvtColor(img_const, converted_img, cv::COLOR_RGB2BGR); |
| 297 | } |
| 298 | return converted_img; |
| 299 | } else { |
| 300 | LOG_IF(ERROR, cv_ptr->encoding != sensor_msgs::image_encodings::MONO8) |
| 301 | << "Expected image with MONO8, BGR8, or RGB8 encoding." |
| 302 | "Add in here more conversions if you wish."; |
| 303 | return img_const; |
| 304 | } |
| 305 | } |