Gstreaming: Provide encoder statistics for rate control

hofbi · May 18, 2021 · 322a5e9 · 322a5e9
1 parent 8eeb6c2
commit 322a5e9
Show file tree

Hide file tree

Showing 22 changed files with 104 additions and 104 deletions.
diff --git a/gstreaming/CMakeLists.txt b/gstreaming/CMakeLists.txt
@@ -16,6 +16,7 @@ find_package(
   COMPONENTS roscpp
              std_msgs
              sensor_msgs
+             telecarla_msgs
              cv_bridge
              image_transport
              roslaunch

diff --git a/gstreaming/launch/rtsp/rtsp_client.launch b/gstreaming/launch/rtsp/rtsp_client.launch
@@ -3,7 +3,7 @@
     <arg name='port' default='8551'/>
     <arg name='mount' default='mainstream'/>
     <arg name='client_id' default='$(arg mount)'/>
-    <arg name='out_topic' default='/camera/rgb/image_color'/>
+    <arg name='out_topic' default='/camera/rgb/image'/>
     <arg name='graph_viz' default="false"/>
 
     <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}] [${node}]: ${message}"/>

diff --git a/gstreaming/launch/rtsp/rtsp_server.launch b/gstreaming/launch/rtsp/rtsp_server.launch
@@ -3,7 +3,8 @@
     <arg name='mount' default='mainstream'/>
     <arg name='stream_source' default='appsrc'/>
     <arg name='server_id' default='$(arg mount)'/>
-    <arg name='in_topic' default='/carla/ego_vehicle/camera/rgb/front/image_color'/>
+    <arg name='stat_topic' default='/camera/stats'/>
+    <arg name='in_topic' default='/carla/ego_vehicle/camera/front/image'/>
     <arg name='graph_viz' default="false"/>
 
     <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}] [${node}]: ${message}"/>
@@ -16,6 +17,7 @@
             <param name="mount" value="$(arg mount)"/>
             <param name="stream_source" value="$(arg stream_source)"/>
             <param name="in_topic" value="$(arg in_topic)"/>
+            <param name="stat_topic" value="$(arg stat_topic)"/>
         </node>
     </group>
 </launch>
diff --git a/gstreaming/package.xml b/gstreaming/package.xml
@@ -10,6 +10,7 @@
     <build_depend>roscpp</build_depend>
     <build_depend>std_msgs</build_depend>
     <build_depend>sensor_msgs</build_depend>
+    <build_depend>telecarla_msgs</build_depend>
     <build_depend>cv_bridge</build_depend>
     <build_depend>image_transport</build_depend>
     <build_depend>roslaunch</build_depend>

diff --git a/gstreaming/source/include/gstreaming_server.h b/gstreaming/source/include/gstreaming_server.h
@@ -15,6 +15,7 @@ class GStreamingServer
 {
   public:
     GStreamingServer(ros::NodeHandle& nh, ros::NodeHandle& pnh, int argc, char* argv[]);
+    GstPadProbeReturn frameStatCallback(GstPad* pad, GstPadProbeInfo* info);
 
   private:
     void startStreaming();
@@ -29,6 +30,7 @@ class GStreamingServer
     std::thread threadGstreamer_;
     std::unique_ptr<rtsp::server::RTSPServer> rtspServer_{nullptr};
     ros::Subscriber subCamera_;
+    ros::Publisher pubStat_;
     dynamic_reconfigure::Server<gstreaming::RateControlConfig> rateControl_;
 };
 

diff --git a/gstreaming/source/rtsp/client/src/rtsp_client.cpp b/gstreaming/source/rtsp/client/src/rtsp_client.cpp
@@ -20,8 +20,8 @@ GstFlowReturn RTSPClient::onNewSampleFromSink(GstAppSink* appSink, RTSPClient* d
     }
 
     GstStructure* s = gst_caps_get_structure(caps, 0);
-    int width = 0;
-    int height = 0;
+    int width{0};
+    int height{0};
     if (!(gst_structure_get_int(s, "width", &width) && gst_structure_get_int(s, "height", &height)))
     {
         GST_ERROR("Could not get image width and height from filter caps");
@@ -50,7 +50,7 @@ RTSPState RTSPClient::start(const std::string& serverHost, int serverPort, const
     {
         case RTSPState::stopped: {
             // rtspsrc default: protocols=tcp+udp-mcast+udp
-            std::string cmd =
+            const auto cmd =
                 "rtspsrc location=rtsp://" + serverHost + ":" + std::to_string(serverPort) + "/" + serverMount +
                 " ! rtph264depay ! avdec_h264 ! videoconvert ! video/x-raw,format=RGB ! appsink name=" + serverMount +
                 " sync=false";

diff --git a/gstreaming/source/rtsp/server/include/gst_rtsp_server_util.h b/gstreaming/source/rtsp/server/include/gst_rtsp_server_util.h
@@ -11,7 +11,7 @@ namespace rtsp
 {
 namespace server
 {
-sensor_msgs::ImageConstPtr getDefaultImage(int width, int height, int frameCount);
+sensor_msgs::ImageConstPtr getDefaultImage(int width, int height) noexcept;
 
 GstCaps* gstCapsFromImage(const sensor_msgs::Image::ConstPtr& msg, int framerate);
 

diff --git a/gstreaming/source/rtsp/server/include/rtsp_server.h b/gstreaming/source/rtsp/server/include/rtsp_server.h
@@ -16,7 +16,7 @@ namespace server
 class RTSPServer
 {
   public:
-    explicit RTSPServer(const std::string& mountName);
+    RTSPServer(const std::string& mountName, const RTSPServerEncoder::PadProbeCallback& encoderProbeCallback);
     ~RTSPServer();
 
     RTSPServer(const RTSPServer& rhs) = delete;
@@ -41,4 +41,4 @@ class RTSPServer
 }  // namespace rtsp
 }  // namespace lmt
 
-#endif /* _LMT_VIDEO_STREAM_ROS_HH_ */
+#endif /* _LMT_RTSP_SERVER_HH_ */
diff --git a/gstreaming/source/rtsp/server/include/rtsp_server_app_source.h b/gstreaming/source/rtsp/server/include/rtsp_server_app_source.h
@@ -13,16 +13,16 @@ namespace rtsp
 {
 namespace server
 {
-class RTPSServerAppSource
+class RTSPServerAppSource
 {
   public:
-    explicit RTPSServerAppSource(std::string mountName);
+    explicit RTSPServerAppSource(std::string mountName) noexcept;
 
     void setVideoData(const sensor_msgs::ImageConstPtr& msg);
-    void updateSpatioTemporalResolution(uint8_t fps, uint8_t spatialScale);
+    void updateSpatioTemporalResolution(uint8_t fps, uint8_t spatialScale) noexcept;
     void bufferNewData(GstElement* appSrc);
 
-    const std::string& getName() const;
+    const std::string& getName() const noexcept;
 
   private:
     sensor_msgs::ImageConstPtr getScaledImagePtr(const sensor_msgs::ImageConstPtr& msg) const;
@@ -32,9 +32,6 @@ class RTPSServerAppSource
     GstCaps* caps_{nullptr};
     sensor_msgs::ImageConstPtr imageMsg_;
 
-    bool isImageUpdated_{false};
-    int timeOut_{0};
-    gint64 nFrames_{0};
     std::string name_;
 
     uint8_t fps_{0};

diff --git a/gstreaming/source/rtsp/server/include/rtsp_server_context.h b/gstreaming/source/rtsp/server/include/rtsp_server_context.h
@@ -12,9 +12,11 @@ namespace server
 {
 struct RTSPServerContext
 {
-    RTSPServerContext(const std::string& mountName, const std::string& encoderName) noexcept;
+    RTSPServerContext(const std::string& mountName,
+                      const std::string& encoderName,
+                      const RTSPServerEncoder::PadProbeCallback& encoderProbeCallback) noexcept;
 
-    std::unique_ptr<RTPSServerAppSource> app{nullptr};
+    std::unique_ptr<RTSPServerAppSource> app{nullptr};
     std::unique_ptr<RTSPServerEncoder> encoder{nullptr};
 };
 }  // namespace server

diff --git a/gstreaming/source/rtsp/server/include/rtsp_server_encoder.h b/gstreaming/source/rtsp/server/include/rtsp_server_encoder.h
@@ -1,6 +1,7 @@
 #ifndef GSTREAMING_RTSP_SERVER_ENCODER_H
 #define GSTREAMING_RTSP_SERVER_ENCODER_H
 
+#include <functional>
 #include <memory>
 #include <string>
 
@@ -15,15 +16,18 @@ namespace server
 class RTSPServerEncoder
 {
   public:
-    explicit RTSPServerEncoder(std::string name);
+    using PadProbeCallback = std::function<GstPadProbeReturn(GstPad*, GstPadProbeInfo*)>;
 
-    void setEncoderElement(std::unique_ptr<GstElement> encoderElement);
+    RTSPServerEncoder(std::string name, PadProbeCallback encoderProbeCallback) noexcept;
+
+    void configureEncoderElement(std::unique_ptr<GstElement> encoderElement) noexcept;
     void updateRateControlParameter(int bitrate);
 
-    const std::string& getName() const;
+    const std::string& getName() const noexcept;
 
   private:
     std::string name_;
+    std::unique_ptr<PadProbeCallback> encoderProbeCallback_{nullptr};
     std::unique_ptr<GstElement> encoderElement_{nullptr};
 };
 }  // namespace server

diff --git a/gstreaming/source/rtsp/server/src/gst_rtsp_server_util.cpp b/gstreaming/source/rtsp/server/src/gst_rtsp_server_util.cpp
@@ -13,34 +13,10 @@ namespace rtsp
 {
 namespace server
 {
-sensor_msgs::ImageConstPtr getDefaultImage(int width, int height, int frameCount)
+sensor_msgs::ImageConstPtr getDefaultImage(int width, int height) noexcept
 {
-    cv_bridge::CvImage imgBridge;
-    std_msgs::Header header;
-    header.stamp = ros::Time::now();
-
-    cv::Mat overlay(height, width, CV_8UC3, cv::Scalar(0, 0, 0));
-
-    const CvPoint topLeft{10, 10};
-    const CvPoint bottomRight{200, 50};
-    const int lineType{8};
-    cv::rectangle(overlay, topLeft, bottomRight, cvScalar(1, 1, 1), cv::FILLED, lineType, 0);
-
-    const CvPoint bottomLeftOfText{30, 30};
-    const auto fontScale{0.8};
-    const CvScalar white{255, 255, 255};
-    cv::putText(overlay,
-                "Frame " + std::to_string(frameCount),
-                bottomLeftOfText,
-                cv::FONT_HERSHEY_COMPLEX_SMALL,
-                fontScale,
-                white,
-                1,
-                CV_AVX);
-
-    imgBridge = cv_bridge::CvImage(header, sensor_msgs::image_encodings::RGB8, overlay);
-
-    return imgBridge.toImageMsg();
+    return cv_bridge::CvImage(std_msgs::Header(), sensor_msgs::image_encodings::RGB8, cv::Mat(height, width, CV_8UC3))
+        .toImageMsg();
 }
 
 GstCaps* gstCapsFromImage(const sensor_msgs::Image::ConstPtr& msg, int framerate)
@@ -65,7 +41,7 @@ GstCaps* gstCapsFromImage(const sensor_msgs::Image::ConstPtr& msg, int framerate
         return nullptr;
     }
 
-    auto format = knownFormats.find(msg->encoding);
+    const auto format = knownFormats.find(msg->encoding);
     if (format == knownFormats.end())
     {
         ROS_ERROR("GST: image format '%s' unknown", msg->encoding.c_str());
@@ -102,7 +78,7 @@ void mediaConfigure(GstRTSPMediaFactory* /*factory*/, GstRTSPMedia* media, RTSPS
     auto appSrc = std::unique_ptr<GstElement, decltype(&gst_object_unref)>(
         gst_bin_get_by_name_recurse_up(GST_BIN(element.get()), context->app->getName().c_str()), gst_object_unref);
 
-    context->encoder->setEncoderElement(std::unique_ptr<GstElement>(
+    context->encoder->configureEncoderElement(std::unique_ptr<GstElement>(
         gst_bin_get_by_name_recurse_up(GST_BIN(element.get()), context->encoder->getName().c_str())));
 
     gst_util_set_object_arg(G_OBJECT(appSrc.get()), "format", "time");

diff --git a/gstreaming/source/rtsp/server/src/rtsp_server.cpp b/gstreaming/source/rtsp/server/src/rtsp_server.cpp
@@ -19,8 +19,11 @@ RTSPState RTSPServer::start(int serverPort, const std::string& src)
 
         std::stringstream command;
         command << src << " name=" << context_->app->getName()
-                << " is-live=true ! videorate !  videoconvert ! x264enc tune=zerolatency speed-preset=ultrafast "
-                   "sliced-threads=true byte-stream=true threads=1 key-int-max=15 intra-refresh=true name="
+                << " is-live=true ! videorate ! videoconvert ! timeoverlay halignment=right valignment=top text=\""
+                << context_->app->getName()
+                << "\" font-desc=\"12\" !"
+                   " x264enc tune=zerolatency speed-preset=ultrafast"
+                   " sliced-threads=true byte-stream=true threads=1 key-int-max=15 intra-refresh=true name="
                 << context_->encoder->getName() << " ! h264parse ! rtph264pay pt=96 name=pay0";
 
         GstRTSPMediaFactory* factory = gst_rtsp_media_factory_new();
@@ -29,7 +32,7 @@ RTSPState RTSPServer::start(int serverPort, const std::string& src)
         //        gst_rtsp_media_factory_set_protocols(factory, GST_RTSP_LOWER_TRANS_TCP);
         //        gst_rtsp_media_factory_set_protocols(factory, GST_RTSP_LOWER_TRANS_UDP);
 
-        std::string path = "/" + context_->app->getName();
+        const auto path = "/" + context_->app->getName();
         gst_rtsp_mount_points_add_factory(mounts.get(), path.c_str(), factory);
 
         g_signal_connect(factory, "media-configure", (GCallback)mediaConfigure, context_.get());
@@ -57,12 +60,12 @@ void RTSPServer::stop()
     }
 }
 
-RTSPServer::RTSPServer(const std::string& mountName)
-    : context_(std::make_unique<RTSPServerContext>(mountName, "x264encoder"))
+RTSPServer::RTSPServer(const std::string& mountName, const RTSPServerEncoder::PadProbeCallback& encoderProbeCallback)
+    : context_(std::make_unique<RTSPServerContext>(mountName, "x264encoder", encoderProbeCallback))
 {
-    const auto videoWidthInPixels{640};
-    const auto videoHeightInPixels{480};
-    context_->app->setVideoData(getDefaultImage(videoWidthInPixels, videoHeightInPixels, 0));
+    constexpr auto videoWidthInPixels{640};
+    constexpr auto videoHeightInPixels{480};
+    context_->app->setVideoData(getDefaultImage(videoWidthInPixels, videoHeightInPixels));
 }
 
 RTSPServer::~RTSPServer()

diff --git a/gstreaming/source/rtsp/server/src/rtsp_server_app_source.cpp b/gstreaming/source/rtsp/server/src/rtsp_server_app_source.cpp
@@ -8,20 +8,19 @@
 
 using namespace lmt::rtsp::server;
 
-RTPSServerAppSource::RTPSServerAppSource(std::string mountName) : name_(std::move(mountName)) {}
+RTSPServerAppSource::RTSPServerAppSource(std::string mountName) noexcept : name_(std::move(mountName)) {}
 
-void RTPSServerAppSource::setVideoData(const sensor_msgs::ImageConstPtr& msg)
+void RTSPServerAppSource::setVideoData(const sensor_msgs::ImageConstPtr& msg)
 {
     mutexLock_.lock();
 
     imageMsg_ = getScaledImagePtr(msg);
     caps_ = gstCapsFromImage(imageMsg_, fps_);
-    isImageUpdated_ = true;
 
     mutexLock_.unlock();
 }
 
-sensor_msgs::ImageConstPtr RTPSServerAppSource::getScaledImagePtr(const sensor_msgs::ImageConstPtr& msg) const
+sensor_msgs::ImageConstPtr RTSPServerAppSource::getScaledImagePtr(const sensor_msgs::ImageConstPtr& msg) const
 {
     const auto defaultSpatialScale{100};
     if (spatialScale_ != defaultSpatialScale)
@@ -38,43 +37,24 @@ sensor_msgs::ImageConstPtr RTPSServerAppSource::getScaledImagePtr(const sensor_m
     return msg;
 }
 
-const std::string& RTPSServerAppSource::getName() const
+const std::string& RTSPServerAppSource::getName() const noexcept
 {
     return name_;
 }
 
-void RTPSServerAppSource::bufferNewData(GstElement* appSrc)
+void RTSPServerAppSource::bufferNewData(GstElement* appSrc)
 {
     mutexLock_.lock();
-
-    if (!isImageUpdated_)
-    {
-        ++timeOut_;
-
-        const auto timeout{40};
-        if (timeOut_ >= timeout)
-        {
-            const auto maximumTimeout{50};
-            timeOut_ = maximumTimeout;
-            imageMsg_ = getDefaultImage(imageMsg_->width, imageMsg_->height, nFrames_);
-        }
-    }
-    else
-    {
-        timeOut_ = 0;
-        isImageUpdated_ = false;
-    }
-
     g_object_set(G_OBJECT(appSrc), "caps", caps_, nullptr);
 
-    GstBuffer* buffer = gst_buffer_new_allocate(nullptr, imageMsg_->data.size(), nullptr);
+    auto* buffer = gst_buffer_new_allocate(nullptr, imageMsg_->data.size(), nullptr);
     gst_buffer_fill(buffer, 0, imageMsg_->data.data(), imageMsg_->data.size());
     mutexLock_.unlock();
 
     if (forceKeyFrame_)
     {
-        GstEvent* event = gst_video_event_new_downstream_force_key_unit(timestamp_, 0, 0, TRUE, 1);
-        GstPad* pad = gst_element_get_static_pad(appSrc, "src");
+        auto* event = gst_video_event_new_downstream_force_key_unit(timestamp_, 0, 0, TRUE, 1);
+        auto* pad = gst_element_get_static_pad(appSrc, "src");
         gst_pad_push_event(pad, event);
 
         forceKeyFrame_ = false;
@@ -88,13 +68,11 @@ void RTPSServerAppSource::bufferNewData(GstElement* appSrc)
 
     GST_BUFFER_DTS(buffer) = GST_BUFFER_PTS(buffer);
 
-    ++nFrames_;
-
     GstFlowReturn ret;
     g_signal_emit_by_name(appSrc, "push-buffer", buffer, &ret);
 }
 
-void RTPSServerAppSource::updateSpatioTemporalResolution(uint8_t fps, uint8_t spatialScale)
+void RTSPServerAppSource::updateSpatioTemporalResolution(uint8_t fps, uint8_t spatialScale) noexcept
 {
     fps_ = fps;
     if (spatialScale_ != spatialScale)

diff --git a/gstreaming/source/rtsp/server/src/rtsp_server_context.cpp b/gstreaming/source/rtsp/server/src/rtsp_server_context.cpp
@@ -2,7 +2,10 @@
 
 using namespace lmt::rtsp::server;
 
-RTSPServerContext::RTSPServerContext(const std::string& mountName, const std::string& /*encoderName*/) noexcept
-    : app(std::make_unique<RTPSServerAppSource>(mountName)), encoder(std::make_unique<RTSPServerEncoder>("x264encoder"))
+RTSPServerContext::RTSPServerContext(const std::string& mountName,
+                                     const std::string& encoderName,
+                                     const RTSPServerEncoder::PadProbeCallback& encoderProbeCallback) noexcept
+    : app(std::make_unique<RTSPServerAppSource>(mountName)),
+      encoder(std::make_unique<RTSPServerEncoder>(encoderName, encoderProbeCallback))
 {
 }