From d2cf23f4160c09fa0993704392e779c64ed936d7 Mon Sep 17 00:00:00 2001 From: julled Date: Tue, 22 Dec 2020 20:28:10 -0500 Subject: [PATCH 1/2] added MONO16 support --- src/gscam.cpp | 36 ++++++++++++++++++++++++++++-------- 1 file changed, 28 insertions(+), 8 deletions(-) diff --git a/src/gscam.cpp b/src/gscam.cpp index d0c6cc9..4d8ff9b 100644 --- a/src/gscam.cpp +++ b/src/gscam.cpp @@ -84,6 +84,7 @@ namespace gscam { nh_private_.param("image_encoding", image_encoding_, sensor_msgs::image_encodings::RGB8); if (image_encoding_ != sensor_msgs::image_encodings::RGB8 && image_encoding_ != sensor_msgs::image_encodings::MONO8 && + image_encoding_ != sensor_msgs::image_encodings::MONO16 && image_encoding_ != "jpeg") { ROS_FATAL_STREAM("Unsupported image encoding: " + image_encoding_); } @@ -139,6 +140,10 @@ namespace gscam { caps = gst_caps_new_simple( "video/x-raw", "format", G_TYPE_STRING, "GRAY8", NULL); + } else if (image_encoding_ == sensor_msgs::image_encodings::MONO16) { + caps = gst_caps_new_simple( "video/x-raw", + "format", G_TYPE_STRING, "GRAY16_LE", + NULL); } else if (image_encoding_ == "jpeg") { caps = gst_caps_new_simple("image/jpeg", NULL, NULL); } @@ -147,6 +152,11 @@ namespace gscam { caps = gst_caps_new_simple( "video/x-raw-rgb", NULL,NULL); } else if (image_encoding_ == sensor_msgs::image_encodings::MONO8) { caps = gst_caps_new_simple("video/x-raw-gray", NULL, NULL); + } else if (image_encoding_ == sensor_msgs::image_encodings::MONO16) { + caps = gst_caps_new_simple("video/x-raw-gray", + "bpp", G_TYPE_INT,16, + "depth", G_TYPE_INT,16, + "endianness", G_TYPE_INT,1234, NULL); } else if (image_encoding_ == "jpeg") { caps = gst_caps_new_simple("image/jpeg", NULL, NULL); } @@ -346,10 +356,16 @@ namespace gscam { cinfo_pub_.publish(cinfo); } else { // Complain if the returned buffer is smaller than we expect - const unsigned int expected_frame_size = - image_encoding_ == sensor_msgs::image_encodings::RGB8 - ? width_ * height_ * 3 - : width_ * height_; + unsigned int expected_frame_size; + if(image_encoding_ == sensor_msgs::image_encodings::RGB8){ + expected_frame_size = width_ * height_ * 3; + } + else if ( image_encoding_ == sensor_msgs::image_encodings::MONO8) { + expected_frame_size = width_ * height_; + } + else if ( image_encoding_ == sensor_msgs::image_encodings::MONO16) { + expected_frame_size = width_ * height_ * 2; + } if (buf_size < expected_frame_size) { ROS_WARN_STREAM( "GStreamer image buffer underflow: Expected frame to be " @@ -372,10 +388,14 @@ namespace gscam { // Copy only the data we received // Since we're publishing shared pointers, we need to copy the image so // we can free the buffer allocated by gstreamer - if (image_encoding_ == sensor_msgs::image_encodings::RGB8) { - img->step = width_ * 3; - } else { - img->step = width_; + if(image_encoding_ == sensor_msgs::image_encodings::RGB8){ + img->step = width_ * 3; + } + else if ( image_encoding_ == sensor_msgs::image_encodings::MONO8) { + img->step = width_; + } + else if ( image_encoding_ == sensor_msgs::image_encodings::MONO16) { + img->step = width_ * 2; } std::copy( buf_data, From 9f12898a20cbf02a7891c29342ffb86903d63265 Mon Sep 17 00:00:00 2001 From: julled Date: Tue, 22 Dec 2020 20:47:52 -0500 Subject: [PATCH 2/2] added image encoding comment to README --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 6c6a1e7..4d345d8 100644 --- a/README.md +++ b/README.md @@ -76,6 +76,7 @@ This can be run as both a node and a nodelet. * `~frame_id`: The [TF](http://www.ros.org/wiki/tf) frame ID. * `~reopen_on_eof`: Re-open the stream if it ends (EOF). * `~sync_sink`: Synchronize the app sink (sometimes setting this to `false` can resolve problems with sub-par framerates). +* `~image_encoding`: Encoding of the stream. Can be {`rgb8`,`mono8`,`mono16`}. Defaults to `rgb8`. C++ API (unstable) ------------------