diff --git a/ariac_sensors/src/ariac_camera_plugin.cpp b/ariac_sensors/src/ariac_camera_plugin.cpp index 3440a24e4..9fcc58712 100644 --- a/ariac_sensors/src/ariac_camera_plugin.cpp +++ b/ariac_sensors/src/ariac_camera_plugin.cpp @@ -14,6 +14,8 @@ #include #include +#include +#include #include #include #include @@ -215,6 +217,17 @@ void AriacCameraPlugin::OnNewImageFrame( if (impl_->publish_sensor_data_) { impl_->image_pub_.publish(impl_->image_msg_); } + else { + cv_bridge::CvImage blank; + blank.image = cv::Mat(_height, _width, CV_8UC3, cv::Scalar(0, 0, 0)); + + auto blank_msg = blank.toImageMsg(); + + blank_msg->header = impl_->image_msg_.header; + blank_msg->encoding = sensor_msgs::image_encodings::RGB8; + + impl_->image_pub_.publish(blank_msg); + } } void AriacCameraPlugin::OnNewDepthFrame( @@ -261,6 +274,18 @@ void AriacCameraPlugin::OnNewDepthFrame( if (impl_->publish_sensor_data_) { impl_->depth_image_pub_.publish(image_msg); } + else { + cv_bridge::CvImage blank; + blank.image = cv::Mat(_height, _width, CV_32FC1, cv::Scalar(0)); + + auto blank_msg = blank.toImageMsg(); + + blank_msg->header = image_msg.header; + + blank_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1; + + impl_->depth_image_pub_.publish(blank_msg); + } }