First Version

src/image2rtsp/CMakeLists.txt

@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 3.5)
+project(image2rtsp)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+pkg_check_modules(GST REQUIRED 
+  gstreamer-1.0
+  gstreamer-app-1.0
+  gstreamer-rtsp-server-1.0
+)
+file(GLOB SOURCES src/video.cpp)
+
+add_executable(image2rtsp src/image2rtsp.cpp ${SOURCES})
+ament_target_dependencies(image2rtsp rclcpp sensor_msgs)
+
+include_directories(
+  ${GST_INCLUDE_DIRS}
+  )
+target_link_libraries(image2rtsp 
+  ${GST_LIBRARIES} 
+  )
+
+install(TARGETS
+  image2rtsp
+  DESTINATION lib/${PROJECT_NAME}
+  )
+
+install(
+  DIRECTORY 
+  config 
+  launch
+  DESTINATION share/${PROJECT_NAME}
+  )
+
+# Install Python modules
+ament_python_install_package(python)
+
+# Install Python executables
+install(PROGRAMS
+  python/rtsp.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+
+ament_package()

src/image2rtsp/config/parameters.yaml

@@ -0,0 +1,12 @@
+/image2rtsp:
+   ros__parameters:
+      topic: "/color/image_raw"
+      mountpoint: "/back"
+      bitrate: "500"
+      caps: "video/x-raw,framerate=10/1,width=640,height=480"
+      port: "8554"
+      local_only: True     # True = rtsp://127.0.0.1:port (The stream is accessible only from the local machine)
+                           # False = rtsp://0.0.0.0:portAndMountpoint (The stream is accessible from the outside) 
+                           # For example, to access the stream running on the machine with IP = 192.168.20.20,
+                           # use rtsp://192.186.20.20:portAndMountpoint
+                           # True = rtsp://127.0.0.1:portAndMountpoint (The stream is accessible only from the local machine)

src/image2rtsp/include/image2rtsp.hpp

@@ -0,0 +1,41 @@
+#ifndef IMAGE2RTSP_IMAGE2RTSP_HPP
+#define IMAGE2RTSP_IMAGE2RTSP_HPP
+
+#include <gst/gst.h>
+#include <gst/rtsp-server/rtsp-server.h>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include "sensor_msgs/msg/image.hpp"
+
+using namespace std;
+
+class Image2rtsp : public rclcpp::Node{
+public:
+    Image2rtsp();
+    GstRTSPServer *rtsp_server;
+
+private:
+    string topic;
+    string mountpoint;
+    string bitrate;
+    string caps;
+    string port;
+    string pipeline;
+    string pipeline_head;
+    string pipeline_tail;
+    bool local_only;
+    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);
+    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_;
+};
+
+static void media_configure(GstRTSPMediaFactory *factory, GstRTSPMedia *media, GstElement **appsrc);
+static void *mainloop(void *arg);
+static gboolean session_cleanup(Image2rtsp *node, rclcpp::Logger logger, gboolean ignored);
+
+#endif // IMAGE2RTSP_IMAGE2RTSP_HPP

src/image2rtsp/include/image_encodings.h

@@ -0,0 +1,233 @@
+
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef SENSOR_MSGS_IMAGE_ENCODINGS_H
+#define SENSOR_MSGS_IMAGE_ENCODINGS_H
+
+#include <cstdlib>
+#include <stdexcept>
+#include <string>
+
+namespace sensor_msgs
+{
+  namespace image_encodings
+  {
+    const std::string RGB8 = "rgb8";
+    const std::string RGBA8 = "rgba8";
+    const std::string RGB16 = "rgb16";
+    const std::string RGBA16 = "rgba16";
+    const std::string BGR8 = "bgr8";
+    const std::string BGRA8 = "bgra8";
+    const std::string BGR16 = "bgr16";
+    const std::string BGRA16 = "bgra16";
+    const std::string MONO8="mono8";
+    const std::string MONO16="mono16";
+
+    // OpenCV CvMat types
+    const std::string TYPE_8UC1="8UC1";
+    const std::string TYPE_8UC2="8UC2";
+    const std::string TYPE_8UC3="8UC3";
+    const std::string TYPE_8UC4="8UC4";
+    const std::string TYPE_8SC1="8SC1";
+    const std::string TYPE_8SC2="8SC2";
+    const std::string TYPE_8SC3="8SC3";
+    const std::string TYPE_8SC4="8SC4";
+    const std::string TYPE_16UC1="16UC1";
+    const std::string TYPE_16UC2="16UC2";
+    const std::string TYPE_16UC3="16UC3";
+    const std::string TYPE_16UC4="16UC4";
+    const std::string TYPE_16SC1="16SC1";
+    const std::string TYPE_16SC2="16SC2";
+    const std::string TYPE_16SC3="16SC3";
+    const std::string TYPE_16SC4="16SC4";
+    const std::string TYPE_32SC1="32SC1";
+    const std::string TYPE_32SC2="32SC2";
+    const std::string TYPE_32SC3="32SC3";
+    const std::string TYPE_32SC4="32SC4";
+    const std::string TYPE_32FC1="32FC1";
+    const std::string TYPE_32FC2="32FC2";
+    const std::string TYPE_32FC3="32FC3";
+    const std::string TYPE_32FC4="32FC4";
+    const std::string TYPE_64FC1="64FC1";
+    const std::string TYPE_64FC2="64FC2";
+    const std::string TYPE_64FC3="64FC3";
+    const std::string TYPE_64FC4="64FC4";
+
+    // Bayer encodings
+    const std::string BAYER_RGGB8="bayer_rggb8";
+    const std::string BAYER_BGGR8="bayer_bggr8";
+    const std::string BAYER_GBRG8="bayer_gbrg8";
+    const std::string BAYER_GRBG8="bayer_grbg8";
+    const std::string BAYER_RGGB16="bayer_rggb16";
+    const std::string BAYER_BGGR16="bayer_bggr16";
+    const std::string BAYER_GBRG16="bayer_gbrg16";
+    const std::string BAYER_GRBG16="bayer_grbg16";
+
+    // Miscellaneous
+    // This is the UYVY version of YUV422 codec http://www.fourcc.org/yuv.php#UYVY
+    // with an 8-bit depth
+    const std::string YUV422="yuv422";
+
+    // Prefixes for abstract image encodings
+    const std::string ABSTRACT_ENCODING_PREFIXES[] = {
+        "8UC", "8SC", "16UC", "16SC", "32SC", "32FC", "64FC"};
+
+    // Utility functions for inspecting an encoding string
+    static inline bool isColor(const std::string& encoding)
+    {
+      return encoding == RGB8  || encoding == BGR8 ||
+             encoding == RGBA8 || encoding == BGRA8 ||
+             encoding == RGB16 || encoding == BGR16 ||
+             encoding == RGBA16 || encoding == BGRA16;
+    }
+
+    static inline bool isMono(const std::string& encoding)
+    {
+      return encoding == MONO8 || encoding == MONO16;
+    }
+
+    static inline bool isBayer(const std::string& encoding)
+    {
+      return encoding == BAYER_RGGB8 || encoding == BAYER_BGGR8 ||
+             encoding == BAYER_GBRG8 || encoding == BAYER_GRBG8 ||
+             encoding == BAYER_RGGB16 || encoding == BAYER_BGGR16 ||
+             encoding == BAYER_GBRG16 || encoding == BAYER_GRBG16;
+    }
+
+    static inline bool hasAlpha(const std::string& encoding)
+    {
+      return encoding == RGBA8 || encoding == BGRA8 ||
+             encoding == RGBA16 || encoding == BGRA16;
+    }
+
+    static inline int numChannels(const std::string& encoding)
+    {
+      // First do the common-case encodings
+      if (encoding == MONO8 ||
+          encoding == MONO16)
+        return 1;
+      if (encoding == BGR8 ||
+          encoding == RGB8 ||
+          encoding == BGR16 ||
+          encoding == RGB16)
+        return 3;
+      if (encoding == BGRA8 ||
+          encoding == RGBA8 ||
+          encoding == BGRA16 ||
+          encoding == RGBA16)
+        return 4;
+      if (encoding == BAYER_RGGB8 ||
+          encoding == BAYER_BGGR8 ||
+          encoding == BAYER_GBRG8 ||
+          encoding == BAYER_GRBG8 ||
+          encoding == BAYER_RGGB16 ||
+          encoding == BAYER_BGGR16 ||
+          encoding == BAYER_GBRG16 ||
+          encoding == BAYER_GRBG16)
+        return 1;
+
+      // Now all the generic content encodings
+      // TODO: Rewrite with regex when ROS supports C++11
+      for (size_t i=0; i < sizeof(ABSTRACT_ENCODING_PREFIXES) / sizeof(*ABSTRACT_ENCODING_PREFIXES); i++)
+      {
+        std::string prefix = ABSTRACT_ENCODING_PREFIXES[i];
+        if (encoding.substr(0, prefix.size()) != prefix)
+          continue;
+        if (encoding.size() == prefix.size())
+          return 1;  // ex. 8UC -> 1
+        int n_channel = atoi(encoding.substr(prefix.size(),
+                                             encoding.size() - prefix.size()).c_str());  // ex. 8UC5 -> 5
+        if (n_channel != 0)
+          return n_channel;  // valid encoding string
+      }
+
+      if (encoding == YUV422)
+        return 2;
+
+      throw std::runtime_error("Unknown encoding " + encoding);
+      return -1;
+    }
+
+    static inline int bitDepth(const std::string& encoding)
+    {
+      if (encoding == MONO16)
+        return 16;
+      if (encoding == MONO8       ||
+          encoding == BGR8        ||
+          encoding == RGB8        ||
+          encoding == BGRA8       ||
+          encoding == RGBA8       ||
+          encoding == BAYER_RGGB8 ||
+          encoding == BAYER_BGGR8 ||
+          encoding == BAYER_GBRG8 ||
+          encoding == BAYER_GRBG8)
+        return 8;
+
+      if (encoding == MONO16       ||
+          encoding == BGR16        ||
+          encoding == RGB16        ||
+          encoding == BGRA16       ||
+          encoding == RGBA16       ||
+          encoding == BAYER_RGGB16 ||
+          encoding == BAYER_BGGR16 ||
+          encoding == BAYER_GBRG16 ||
+          encoding == BAYER_GRBG16)
+        return 16;
+
+      // Now all the generic content encodings
+      // TODO: Rewrite with regex when ROS supports C++11
+      for (size_t i=0; i < sizeof(ABSTRACT_ENCODING_PREFIXES) / sizeof(*ABSTRACT_ENCODING_PREFIXES); i++)
+      {
+        std::string prefix = ABSTRACT_ENCODING_PREFIXES[i];
+        if (encoding.substr(0, prefix.size()) != prefix)
+          continue;
+        if (encoding.size() == prefix.size())
+          return atoi(prefix.c_str());  // ex. 8UC -> 8
+        int n_channel = atoi(encoding.substr(prefix.size(),
+                                             encoding.size() - prefix.size()).c_str());  // ex. 8UC10 -> 10
+        if (n_channel != 0)
+          return atoi(prefix.c_str());  // valid encoding string
+      }
+
+      if (encoding == YUV422)
+        return 8;
+
+      throw std::runtime_error("Unknown encoding " + encoding);
+      return -1;
+    }
+  }
+}
+
+#endif

src/image2rtsp/launch/image2rtsp.launch.py

@@ -0,0 +1,20 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+   config = os.path.join(
+      get_package_share_directory('image2rtsp'),
+      'config',
+      'parameters.yaml'
+      )
+
+   return LaunchDescription([
+      Node(
+         package='image2rtsp',
+         executable='image2rtsp',
+         name='image2rtsp',
+         parameters=[config]
+      )
+   ])

src/image2rtsp/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>image2rtsp</name>
+  <version>0.0.0</version>
+  <description>topic to RTSP stream package</description>
+  <maintainer email="dmalad@rptu.de">dmitry</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <exec_depend>ros2launch</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/image2rtsp/python/rtsp.py

@@ -0,0 +1,15 @@
+import cv2
+
+cap = cv2.VideoCapture('rtsp://127.0.0.1:8554/back')
+
+while True:
+    ret, frame = cap.read()
+
+    if ret:
+        cv2.imshow('RTSP Stream', frame)
+
+    if cv2.waitKey(1) & 0xFF == ord('q'):
+        break
+
+cap.release()
+cv2.destroyAllWindows()

src/image2rtsp/src/image2rtsp.cpp

@@ -0,0 +1,61 @@
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include <gst/gst.h>
+#include <gst/rtsp-server/rtsp-server.h>
+#include <gst/app/gstappsrc.h>
+#include "../include/image2rtsp.hpp"
+
+using std::placeholders::_1;
+
+Image2rtsp::Image2rtsp() : Node("image2rtsp"){
+    // Declare and get the parameters
+    this->declare_parameter("topic");
+    this->declare_parameter("mountpoint");
+    this->declare_parameter("bitrate");
+    this->declare_parameter("caps");
+    this->declare_parameter("port");
+    this->declare_parameter("local_only");
+
+    topic = this->get_parameter("topic").as_string();
+    mountpoint = this->get_parameter("mountpoint").as_string();
+    bitrate = this->get_parameter("bitrate").as_string();
+    caps = this->get_parameter("caps").as_string();
+    port = this->get_parameter("port").as_string();
+    local_only = this->get_parameter("local_only").as_bool();
+
+    // Check if the parameter is set, since no default value is provided
+    if (!this->has_parameter("topic") || !this->has_parameter("mountpoint") || !this->has_parameter("bitrate") || !this->has_parameter("caps") || !this->has_parameter("port")){
+        rclcpp::shutdown(); // Shutdown the node if there are some issues with launch file
+        return;
+    }
+
+    // Start the subscription
+    subscription_ = this->create_subscription<sensor_msgs::msg::Image>(topic, 10, std::bind(&Image2rtsp::topic_callback, this, _1));
+
+    // Start the RTSP server
+    video_mainloop_start();
+    rtsp_server = rtsp_server_create(port, local_only);
+    appsrc = NULL;
+    // Setup the pipeline
+    pipeline_head = "( appsrc name=imagesrc do-timestamp=true min-latency=0 max-latency=0 max-bytes=1000 is-live=true ! videoconvert ! videoscale ! ";
+    pipeline_tail = "key-int-max=30 ! video/x-h264, profile=baseline ! rtph264pay name=pay0 pt=96 )";
+    pipeline = pipeline_head + caps + " ! x264enc tune=zerolatency bitrate=" + bitrate + pipeline_tail;
+    rtsp_server_add_url(mountpoint.c_str(), pipeline.c_str(), (GstElement **)&(appsrc));
+    RCLCPP_INFO(this->get_logger(), "Stream available at rtsp://%s:%s%s", gst_rtsp_server_get_address(rtsp_server), port.c_str(), mountpoint.c_str());
+}
+
+int main(int argc, char *argv[]){
+    rclcpp::init(argc, argv);
+    try{
+        auto node = std::make_shared<Image2rtsp>();
+        if (rclcpp::ok()){
+            rclcpp::spin(node);
+        }
+    }catch (const std::exception &e){
+        RCLCPP_INFO(rclcpp::get_logger("image2rtsp"), "One or more required parameters are not set. Shutting down the node.");
+        RCLCPP_INFO(rclcpp::get_logger("image2rtsp"), "Tip: Start the node only using launch file. Also rebuild after making changes in the YAML file");
+        RCLCPP_ERROR(rclcpp::get_logger("image2rtsp"), "Parameter is not set: %s", e.what());
+    }
+    rclcpp::shutdown();
+    return 0;
+}

src/image2rtsp/src/video.cpp

@@ -0,0 +1,169 @@
+#include <gst/gst.h>
+#include <gst/rtsp-server/rtsp-server.h>
+#include <gst/app/gstappsrc.h>
+#include <rclcpp/rclcpp.hpp>
+
+#include "../include/image2rtsp.hpp"
+#include "../include/image_encodings.h"
+
+using namespace std;
+
+static void *mainloop(void *arg){
+    GMainLoop *loop = g_main_loop_new(NULL, FALSE);
+    g_main_loop_run(loop);
+    g_main_destroy(loop);
+    return NULL;
+}
+
+void Image2rtsp::video_mainloop_start(){
+    pthread_t tloop;
+    gst_init(NULL, NULL);
+    pthread_create(&tloop, NULL, &mainloop, NULL);
+}
+
+GstRTSPServer *Image2rtsp::rtsp_server_create(const std::string &port, const bool local_only){
+    GstRTSPServer *server;
+
+    /* create a server instance */
+    server = gst_rtsp_server_new();
+    // char *port = (char *) port;
+    g_object_set(server, "service", port.c_str(), NULL);
+    /* attach the server to the default maincontext */
+    if (local_only){
+    g_object_set(server, "address", "127.0.0.1", NULL);
+    }
+    gst_rtsp_server_attach(server, NULL);
+    /* add a timeout for the session cleanup */
+    g_timeout_add_seconds(2, (GSourceFunc)session_cleanup, this);
+    return server;
+}
+
+void Image2rtsp::rtsp_server_add_url(const char *url, const char *sPipeline, GstElement **appsrc){
+    GstRTSPMountPoints *mounts;
+    GstRTSPMediaFactory *factory;
+
+    /* get the mount points for this server, every server has a default object
+     * that be used to map uri mount points to media factories */
+    mounts = gst_rtsp_server_get_mount_points(rtsp_server);
+
+    /* make a media factory for a test stream. The default media factory can use
+     * gst-launch syntax to create pipelines.
+     * any launch line works as long as it contains elements named pay%d. Each
+     * element with pay%d names will be a stream */
+    factory = gst_rtsp_media_factory_new();
+    gst_rtsp_media_factory_set_launch(factory, sPipeline);
+
+    /* notify when our media is ready, This is called whenever someone asks for
+     * the media and a new pipeline is created */
+    g_signal_connect(factory, "media-configure", (GCallback)media_configure, appsrc);
+
+    gst_rtsp_media_factory_set_shared(factory, TRUE);
+
+    /* attach the factory to the url */
+    gst_rtsp_mount_points_add_factory(mounts, url, factory);
+
+    /* don't need the ref to the mounts anymore */
+    g_object_unref(mounts);
+}
+
+static void media_configure(GstRTSPMediaFactory *factory, GstRTSPMedia *media, GstElement **appsrc){
+    if(appsrc){
+        GstElement *pipeline = gst_rtsp_media_get_element(media);
+
+        *appsrc = gst_bin_get_by_name(GST_BIN(pipeline), "imagesrc");
+
+        /* this instructs appsrc that we will be dealing with timed buffer */
+        gst_util_set_object_arg(G_OBJECT(*appsrc), "format", "time");
+
+        gst_object_unref(pipeline);
+    }else{
+        guint i, n_streams;
+        n_streams = gst_rtsp_media_n_streams(media);
+
+        for (i = 0; i < n_streams; i++){
+            GstRTSPAddressPool *pool;
+            GstRTSPStream *stream;
+            gchar *min, *max;
+
+            stream = gst_rtsp_media_get_stream(media, i);
+
+            /* make a new address pool */
+            pool = gst_rtsp_address_pool_new();
+
+            min = g_strdup_printf("224.3.0.%d", (2 * i) + 1);
+            max = g_strdup_printf("224.3.0.%d", (2 * i) + 2);
+            gst_rtsp_address_pool_add_range(pool, min, max, 5000 + (10 * i), 5010 + (10 * i), 1);
+            g_free(min);
+            g_free(max);
+            gst_rtsp_stream_set_address_pool(stream, pool);
+            g_object_unref(pool);
+        }
+    }
+}
+
+GstCaps *Image2rtsp::gst_caps_new_from_image(const sensor_msgs::msg::Image::SharedPtr &msg){
+    // http://gstreamer.freedesktop.org/data/doc/gstreamer/head/pwg/html/section-types-definitions.html
+    static const std::map<std::string, std::string> known_formats = {
+        {sensor_msgs::image_encodings::RGB8, "RGB"},
+        {sensor_msgs::image_encodings::RGB16, "RGB16"},
+        {sensor_msgs::image_encodings::RGBA8, "RGBA"},
+        {sensor_msgs::image_encodings::RGBA16, "RGBA16"},
+        {sensor_msgs::image_encodings::BGR8, "BGR"},
+        {sensor_msgs::image_encodings::BGR16, "BGR16"},
+        {sensor_msgs::image_encodings::BGRA8, "BGRA"},
+        {sensor_msgs::image_encodings::BGRA16, "BGRA16"},
+        {sensor_msgs::image_encodings::MONO8, "GRAY8"},
+        {sensor_msgs::image_encodings::MONO16, "GRAY16_LE"},
+    };
+
+    if (msg->is_bigendian){
+        RCLCPP_ERROR(this->get_logger(), "GST: big endian image format is not supported");
+        return nullptr;
+    }
+
+    auto format = known_formats.find(msg->encoding);
+    if (format == known_formats.end()){
+        RCLCPP_ERROR(this->get_logger(), "GST: image format '%s' unknown", msg->encoding.c_str());
+        return nullptr;
+    }
+
+    return gst_caps_new_simple("video/x-raw",
+                               "format", G_TYPE_STRING, format->second.c_str(),
+                               "width", G_TYPE_INT, msg->width,
+                               "height", G_TYPE_INT, msg->height,
+                               "framerate", GST_TYPE_FRACTION, 10, 1,
+                               nullptr);
+}
+
+static gboolean session_cleanup(Image2rtsp *node, rclcpp::Logger logger, gboolean ignored){
+    GstRTSPServer *server = node->rtsp_server;
+    GstRTSPSessionPool *pool;
+    int num;
+
+    pool = gst_rtsp_server_get_session_pool(server);
+    num = gst_rtsp_session_pool_cleanup(pool);
+    g_object_unref(pool);
+
+    if (num > 0)
+    {
+        char s[32];
+        snprintf(s, 32, (char *)"Sessions cleaned: %d", num);
+        RCLCPP_INFO(node->get_logger(), s);
+    }
+    return TRUE;
+}
+
+void Image2rtsp::topic_callback(const sensor_msgs::msg::Image::SharedPtr msg){
+    GstBuffer *buf;
+    GstCaps *caps; // image properties. see return of Image2rtsp::gst_caps_new_from_image
+    char *gst_type, *gst_format = (char *)"";
+    if (appsrc != NULL){
+        // Set caps from message
+        caps = gst_caps_new_from_image(msg);
+        gst_app_src_set_caps(appsrc, caps);
+        buf = gst_buffer_new_allocate(nullptr, msg->data.size(), nullptr);
+        gst_buffer_fill(buf, 0, msg->data.data(), msg->data.size());
+        GST_BUFFER_FLAG_SET(buf, GST_BUFFER_FLAG_LIVE);
+        gst_app_src_push_buffer(appsrc, buf);
+    }
+}