diff --git a/CMakeLists.txt b/CMakeLists.txt index 16ac1cb..0956b02 100644 --- a/CMakeLists.txt +++ b/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}) @@ -37,7 +40,8 @@ include_directories( ${GST_INCLUDE_DIRS} ) target_link_libraries(image2rtsp - ${GST_LIBRARIES} + ${GST_LIBRARIES} + ${OpenCV_LIBS} ) install(TARGETS diff --git a/config/parameters.yaml b/config/parameters.yaml index 72cc3ea..8020cb1 100644 --- a/config/parameters.yaml +++ b/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" diff --git a/include/image2rtsp.hpp b/include/image2rtsp.hpp index 28dda0c..00dfb58 100644 --- a/include/image2rtsp.hpp +++ b/include/image2rtsp.hpp @@ -6,6 +6,8 @@ #include #include #include "sensor_msgs/msg/image.hpp" +#include "sensor_msgs/msg/compressed_image.hpp" +#include 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::SharedPtr subscription_; + rclcpp::Subscription::SharedPtr subscription_compressed_; }; static void media_configure(GstRTSPMediaFactory *factory, GstRTSPMedia *media, GstElement **appsrc); diff --git a/package.xml b/package.xml index dd26606..c32b726 100644 --- a/package.xml +++ b/package.xml @@ -11,6 +11,7 @@ rclcpp sensor_msgs + open_CV ament_lint_auto ament_lint_common diff --git a/src/image2rtsp.cpp b/src/image2rtsp.cpp index d78c3a7..150e640 100644 --- a/src/image2rtsp.cpp +++ b/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(topic, 10, std::bind(&Image2rtsp::topic_callback, this, _1)); + if (compressed == false){ + subscription_ = this->create_subscription(topic, 10, std::bind(&Image2rtsp::topic_callback, this, _1)); + } + else{ + subscription_compressed_ = this->create_subscription(topic, 10, std::bind(&Image2rtsp::compressed_topic_callback, this, _1)); + } // Start the RTSP server video_mainloop_start(); diff --git a/src/video.cpp b/src/video.cpp index b7f6f9c..9e50d21 100644 --- a/src/video.cpp +++ b/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); +} \ No newline at end of file