Compressed images support

CMakeLists.txt

@@ -23,11 +23,14 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(PkgConfig REQUIRED)
+find_package(OpenCV REQUIRED)
 pkg_check_modules(GST REQUIRED 
   gstreamer-1.0
   gstreamer-app-1.0
   gstreamer-rtsp-server-1.0
 )
+include_directories(${OpenCV_INCLUDE_DIRS})
+
 file(GLOB SOURCES src/video.cpp)
 
 add_executable(image2rtsp src/image2rtsp.cpp ${SOURCES})
@@ -38,6 +41,7 @@ include_directories(
   )
 target_link_libraries(image2rtsp 
   ${GST_LIBRARIES}
+  ${OpenCV_LIBS}
   )
 
 install(TARGETS

config/parameters.yaml

@@ -6,7 +6,8 @@
       source: "v4l2src device=/dev/video0"
 
       # If the source is a ros2 topic (default case)
-      topic: "/color/image_raw"
+      compressed: True
+      topic: "/image_raw/compressed"
 
       # General setup
       mountpoint: "/back"

include/image2rtsp.hpp

@@ -6,6 +6,8 @@
 #include <rclcpp/rclcpp.hpp>
 #include <string>
 #include "sensor_msgs/msg/image.hpp"
+#include "sensor_msgs/msg/compressed_image.hpp"
+#include <opencv2/opencv.hpp>
 
 using namespace std;
 
@@ -28,14 +30,17 @@ private:
     string pipeline_tail;
     bool local_only;
     bool camera;
+    bool compressed;
     GstAppSrc *appsrc;
 
     void video_mainloop_start();
     void rtsp_server_add_url(const char *url, const char *sPipeline, GstElement **appsrc);
     void topic_callback(const sensor_msgs::msg::Image::SharedPtr msg);
+    void compressed_topic_callback(const sensor_msgs::msg::CompressedImage::SharedPtr msg);
     GstRTSPServer *rtsp_server_create(const string &port, const bool local_only);
     GstCaps *gst_caps_new_from_image(const sensor_msgs::msg::Image::SharedPtr &msg);
     rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
+    rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr subscription_compressed_;
 };
 
 static void media_configure(GstRTSPMediaFactory *factory, GstRTSPMedia *media, GstElement **appsrc);

package.xml

@@ -11,6 +11,7 @@
 
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>
+  <depend>open_CV</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/image2rtsp.cpp

@@ -19,6 +19,7 @@ Image2rtsp::Image2rtsp() : Node("image2rtsp"){
     this->declare_parameter("port", "8554");
     this->declare_parameter("local_only", true);
     this->declare_parameter("camera", false);
+    this->declare_parameter("compressed", false);
 
     source = this->get_parameter("source").as_string();
     topic = this->get_parameter("topic").as_string();
@@ -30,9 +31,15 @@ Image2rtsp::Image2rtsp() : Node("image2rtsp"){
     port = this->get_parameter("port").as_string();
     local_only = this->get_parameter("local_only").as_bool();
     camera = this->get_parameter("camera").as_bool();
+    compressed = this->get_parameter("compressed").as_bool();
 
     // Start the subscription
-    subscription_ = this->create_subscription<sensor_msgs::msg::Image>(topic, 10, std::bind(&Image2rtsp::topic_callback, this, _1));
+    if (compressed == false){
+        subscription_ = this->create_subscription<sensor_msgs::msg::Image>(topic, 10, std::bind(&Image2rtsp::topic_callback, this, _1));
+    }
+    else{
+        subscription_compressed_ = this->create_subscription<sensor_msgs::msg::CompressedImage>(topic, 10, std::bind(&Image2rtsp::compressed_topic_callback, this, _1));
+    }
 
     // Start the RTSP server
     video_mainloop_start();

src/video.cpp

@@ -168,3 +168,43 @@ void Image2rtsp::topic_callback(const sensor_msgs::msg::Image::SharedPtr msg){
         gst_app_src_push_buffer(appsrc, buf);
     }
 }
+
+void Image2rtsp::compressed_topic_callback(const sensor_msgs::msg::CompressedImage::SharedPtr msg){
+    if (appsrc == NULL) return;
+    // Decompress the image
+    cv::Mat img = cv::imdecode(cv::Mat(msg->data), cv::IMREAD_UNCHANGED);
+    if (img.empty()) {
+        RCLCPP_ERROR(this->get_logger(), "Failed to decompress image");
+        return;
+    }
+
+    // Determine the GStreamer caps
+    std::string gst_format;
+    switch (img.type()) {
+        case CV_8UC3: gst_format = "RGB"; break; // RGB images
+        case CV_8UC4: gst_format = "RGBA"; break; // RGBA images
+        case CV_8UC1: gst_format = "GRAY8"; break; // Grayscale images
+        default:
+            RCLCPP_ERROR(this->get_logger(), "Unsupported image type");
+            return;
+    }
+
+    GstCaps *caps = gst_caps_new_simple("video/x-raw",
+                                        "format", G_TYPE_STRING, gst_format.c_str(),
+                                        "width", G_TYPE_INT, img.cols,
+                                        "height", G_TYPE_INT, img.rows,
+                                        "framerate", GST_TYPE_FRACTION, stoi(framerate), 1,
+                                        nullptr);
+
+    // Set caps on appsrc
+    gst_app_src_set_caps(appsrc, caps);
+    gst_caps_unref(caps);
+
+    // Create a GstBuffer and fill it with the image data
+    GstBuffer *buf = gst_buffer_new_allocate(nullptr, img.total() * img.elemSize(), nullptr);
+    gst_buffer_fill(buf, 0, img.data, img.total() * img.elemSize());
+    GST_BUFFER_FLAG_SET(buf, GST_BUFFER_FLAG_LIVE);
+
+    // Push the buffer to GStreamer
+    gst_app_src_push_buffer(appsrc, buf);
+}