First Version

This commit is contained in:
dmalad 2023-11-28 18:13:18 +01:00
parent 9e426c282a
commit ca9a6185be
12 changed files with 635 additions and 0 deletions

View File

@ -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()

View File

@ -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)

View File

@ -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

View File

@ -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

View File

@ -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]
)
])

View File

@ -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>

View File

View File

@ -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()

View File

@ -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;
}

View File

@ -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);
}
}