| 287 | // } |
| 288 | |
| 289 | const cv::Mat Subscriber::readRosImage(const sensor_msgs::msg::Image::ConstSharedPtr img_msg) const { |
| 290 | CHECK(img_msg); |
| 291 | cv_bridge::CvImageConstPtr cv_ptr; |
| 292 | try { |
| 293 | // TODO(Toni): here we should consider using toCvShare... |
| 294 | cv_ptr = cv_bridge::toCvCopy(img_msg); |
| 295 | RCLCPP_INFO(node_->get_logger(), "Received image - encoding: %s, width: %d, height: %d, size: %zu", |
| 296 | img_msg->encoding.c_str(), |
| 297 | img_msg->width, |
| 298 | img_msg->height, |
| 299 | img_msg->data.size()); |
| 300 | } catch (cv_bridge::Exception& exception) { |
| 301 | RCLCPP_FATAL(node_->get_logger(), "cv_bridge exception: %s", exception.what()); |
| 302 | rclcpp::shutdown(); |
| 303 | } |
| 304 | |
| 305 | CHECK(cv_ptr); |
| 306 | const cv::Mat img_const = cv_ptr->image; // Don't modify shared image in ROS. |
| 307 | cv::Mat converted_img(img_const.size(), CV_8U); |
| 308 | if (img_msg->encoding == sensor_msgs::image_encodings::BGR8) { |
| 309 | // LOG_EVERY_N(WARNING, 10) << "Converting image..."; |
| 310 | cv::cvtColor(img_const, converted_img, cv::COLOR_BGR2GRAY); |
| 311 | return converted_img; |
| 312 | } else if (img_msg->encoding == sensor_msgs::image_encodings::RGB8) { |
| 313 | // LOG_EVERY_N(WARNING, 10) << "Converting image..."; |
| 314 | cv::cvtColor(img_const, converted_img, cv::COLOR_RGB2GRAY); |
| 315 | return converted_img; |
| 316 | } else { |
| 317 | CHECK_EQ(cv_ptr->encoding, sensor_msgs::image_encodings::MONO8) |
| 318 | << "Expected image with MONO8, BGR8, or RGB8 encoding." |
| 319 | "Add in here more conversions if you wish."; |
| 320 | return img_const; |
| 321 | } |
| 322 | } |
| 323 | |
| 324 | } // namespace okvis |