From eb9af5f0c5a78d3ebd08afe7cf1a484b272012fd Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Fri, 28 Jun 2024 10:44:04 -0400 Subject: [PATCH 01/42] Add QoS profile query parameters (#133) --- CMakeLists.txt | 1 + include/web_video_server/image_streamer.h | 2 ++ .../ros_compressed_streamer.h | 1 + include/web_video_server/utils.h | 17 ++++++++++++ src/image_streamer.cpp | 18 ++++++++++++- src/ros_compressed_streamer.cpp | 19 ++++++++++++-- src/utils.cpp | 26 +++++++++++++++++++ 7 files changed, 81 insertions(+), 3 deletions(-) create mode 100644 include/web_video_server/utils.h create mode 100644 src/utils.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ce84c17..ac9c890 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,6 +51,7 @@ add_executable(${PROJECT_NAME} src/ros_compressed_streamer.cpp src/jpeg_streamers.cpp src/png_streamers.cpp + src/utils.cpp ) ament_target_dependencies(${PROJECT_NAME} diff --git a/include/web_video_server/image_streamer.h b/include/web_video_server/image_streamer.h index 59a1a92..ca474e8 100644 --- a/include/web_video_server/image_streamer.h +++ b/include/web_video_server/image_streamer.h @@ -5,6 +5,7 @@ #include #include #include +#include "web_video_server/utils.h" #include "async_web_server_cpp/http_server.hpp" #include "async_web_server_cpp/http_request.hpp" @@ -66,6 +67,7 @@ class ImageTransportImageStreamer : public ImageStreamer int output_height_; bool invert_; std::string default_transport_; + std::string qos_profile_name_; rclcpp::Time last_frame; cv::Mat output_size_image; diff --git a/include/web_video_server/ros_compressed_streamer.h b/include/web_video_server/ros_compressed_streamer.h index 7219dfd..f188d7e 100644 --- a/include/web_video_server/ros_compressed_streamer.h +++ b/include/web_video_server/ros_compressed_streamer.h @@ -29,6 +29,7 @@ class RosCompressedStreamer : public ImageStreamer rclcpp::Time last_frame; sensor_msgs::msg::CompressedImage::ConstSharedPtr last_msg; boost::mutex send_mutex_; + std::string qos_profile_name_; }; class RosCompressedStreamerType : public ImageStreamerType diff --git a/include/web_video_server/utils.h b/include/web_video_server/utils.h new file mode 100644 index 0000000..f2b291b --- /dev/null +++ b/include/web_video_server/utils.h @@ -0,0 +1,17 @@ +#pragma once + +#include +#include +#include "rmw/qos_profiles.h" + +namespace web_video_server +{ + +/** + * @brief Gets a QoS profile given an input name, if valid. + * @param name The name of the QoS profile name. + * @return An optional containing the matching QoS profile. + */ +std::optional get_qos_profile_from_name(const std::string name); + +} // namespace web_video_server diff --git a/src/image_streamer.cpp b/src/image_streamer.cpp index 05012c2..95bfc05 100644 --- a/src/image_streamer.cpp +++ b/src/image_streamer.cpp @@ -24,6 +24,7 @@ ImageTransportImageStreamer::ImageTransportImageStreamer(const async_web_server_ output_height_ = request.get_query_param_value_or_default("height", -1); invert_ = request.has_query_param("invert"); default_transport_ = request.get_query_param_value_or_default("default_transport", "raw"); + qos_profile_name_ = request.get_query_param_value_or_default("qos_profile", "default"); } ImageTransportImageStreamer::~ImageTransportImageStreamer() @@ -46,7 +47,22 @@ void ImageTransportImageStreamer::start() break; } } - image_sub_ = it_.subscribe(topic_, 1, &ImageTransportImageStreamer::imageCallback, this, &hints); + + // Get QoS profile from query parameter + RCLCPP_INFO(nh_->get_logger(), "Streaming topic %s with QoS profile %s", topic_.c_str(), qos_profile_name_.c_str()); + auto qos_profile = get_qos_profile_from_name(qos_profile_name_); + if (!qos_profile) { + qos_profile = rmw_qos_profile_default; + RCLCPP_ERROR( + nh_->get_logger(), + "Invalid QoS profile %s specified. Using default profile.", + qos_profile_name_.c_str()); + } + + // Create subscriber + image_sub_ = image_transport::create_subscription( + nh_.get(), topic_, std::bind(&ImageTransportImageStreamer::imageCallback, this, std::placeholders::_1), + default_transport_, qos_profile.value()); } void ImageTransportImageStreamer::initialize(const cv::Mat &) diff --git a/src/ros_compressed_streamer.cpp b/src/ros_compressed_streamer.cpp index 0fedaae..02f13ef 100644 --- a/src/ros_compressed_streamer.cpp +++ b/src/ros_compressed_streamer.cpp @@ -8,6 +8,7 @@ RosCompressedStreamer::RosCompressedStreamer(const async_web_server_cpp::HttpReq ImageStreamer(request, connection, nh), stream_(std::bind(&rclcpp::Node::now, nh), connection) { stream_.sendInitialHeader(); + qos_profile_name_ = request.get_query_param_value_or_default("qos_profile", "default"); } RosCompressedStreamer::~RosCompressedStreamer() @@ -17,9 +18,23 @@ RosCompressedStreamer::~RosCompressedStreamer() } void RosCompressedStreamer::start() { - std::string compressed_topic = topic_ + "/compressed"; + const std::string compressed_topic = topic_ + "/compressed"; + + // Get QoS profile from query parameter + RCLCPP_INFO(nh_->get_logger(), "Streaming topic %s with QoS profile %s", compressed_topic.c_str(), qos_profile_name_.c_str()); + auto qos_profile = get_qos_profile_from_name(qos_profile_name_); + if (!qos_profile) { + qos_profile = rmw_qos_profile_default; + RCLCPP_ERROR( + nh_->get_logger(), + "Invalid QoS profile %s specified. Using default profile.", + qos_profile_name_.c_str()); + } + + // Create subscriber + const auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.value().history, 1), qos_profile.value()); image_sub_ = nh_->create_subscription( - compressed_topic, 1, std::bind(&RosCompressedStreamer::imageCallback, this, std::placeholders::_1)); + compressed_topic, qos, std::bind(&RosCompressedStreamer::imageCallback, this, std::placeholders::_1)); } void RosCompressedStreamer::restreamFrame(double max_age) diff --git a/src/utils.cpp b/src/utils.cpp new file mode 100644 index 0000000..314adf5 --- /dev/null +++ b/src/utils.cpp @@ -0,0 +1,26 @@ +#include "web_video_server/utils.h" + +namespace web_video_server +{ + +std::optional get_qos_profile_from_name(const std::string name) +{ + if (name == "default") + { + return rmw_qos_profile_default; + } + else if (name == "system_default") + { + return rmw_qos_profile_system_default; + } + else if (name == "sensor_data") + { + return rmw_qos_profile_sensor_data; + } + else + { + return std::nullopt; + } +} + +} // namespace web_video_server From ea298d68ae4def8049c3a40b9aa8f42b3907c07e Mon Sep 17 00:00:00 2001 From: Robert Brothers <33141599+rjb0026@users.noreply.github.com> Date: Thu, 19 Sep 2024 19:50:59 -0500 Subject: [PATCH 02/42] allow topic searches to continue past invalid multi-type topics. (#146) --- src/image_streamer.cpp | 4 ++-- src/web_video_server.cpp | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/image_streamer.cpp b/src/image_streamer.cpp index 95bfc05..1bd6708 100644 --- a/src/image_streamer.cpp +++ b/src/image_streamer.cpp @@ -38,8 +38,8 @@ void ImageTransportImageStreamer::start() inactive_ = true; for (auto topic_and_types : tnat) { if (topic_and_types.second.size() > 1) { - // explicitly avoid topics with more than one type - break; + // skip over topics with more than one type + continue; } auto & topic_name = topic_and_types.first; if(topic_name == topic_ || (topic_name.find("/") == 0 && topic_name.substr(1) == topic_)){ diff --git a/src/web_video_server.cpp b/src/web_video_server.cpp index c0eebf2..45a5226 100644 --- a/src/web_video_server.cpp +++ b/src/web_video_server.cpp @@ -192,8 +192,8 @@ bool WebVideoServer::handle_stream(const async_web_server_cpp::HttpRequest &requ bool did_find_compressed_topic = false; for (auto topic_and_types : tnat) { if (topic_and_types.second.size() > 1) { - // explicitly avoid topics with more than one type - break; + // skip over topics with more than one type + continue; } auto & topic_name = topic_and_types.first; if(topic_name == compressed_topic_name || (topic_name.find("/") == 0 && topic_name.substr(1) == compressed_topic_name)){ @@ -249,8 +249,8 @@ bool WebVideoServer::handle_stream_viewer(const async_web_server_cpp::HttpReques bool did_find_compressed_topic = false; for (auto topic_and_types : tnat) { if (topic_and_types.second.size() > 1) { - // explicitly avoid topics with more than one type - break; + // skip over topics with more than one type + continue; } auto & topic_name = topic_and_types.first; if(topic_name == compressed_topic_name || (topic_name.find("/") == 0 && topic_name.substr(1) == compressed_topic_name)){ @@ -292,8 +292,8 @@ bool WebVideoServer::handle_list_streams(const async_web_server_cpp::HttpRequest auto tnat = nh_->get_topic_names_and_types(); for (auto topic_and_types : tnat) { if (topic_and_types.second.size() > 1) { - // explicitly avoid topics with more than one type - break; + // skip over topics with more than one type + continue; } auto & topic_name = topic_and_types.first; auto & topic_type = topic_and_types.second[0]; // explicitly take the first From 3a5432b58ad768edad88c2dc12ba7feead1ac13f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sun, 29 Sep 2024 14:06:11 +0200 Subject: [PATCH 03/42] Update package maintainer --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 6e3a11c..6097cac 100644 --- a/package.xml +++ b/package.xml @@ -4,7 +4,7 @@ 1.0.0 HTTP Streaming of ROS Image Topics in Multiple Formats - Russell Toris + Błażej Sowa Mitchell Wills BSD From 576dd6d70fbeaade8a64c4cb4efbd6f89caad668 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 30 Sep 2024 15:13:10 +0200 Subject: [PATCH 04/42] Add CI workflow and ament_lint tests (#148) * Add CI workflow * Add ament_lint tests * Disable uncrustify for now * Use package format 3 and add xml schema * Fix cmake lint error --- .github/workflows/ci.yml | 27 +++++++++++++++++++++++++++ CMakeLists.txt | 13 +++++++++++-- package.xml | 11 +++++++++-- 3 files changed, 47 insertions(+), 4 deletions(-) create mode 100644 .github/workflows/ci.yml diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..749a442 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,27 @@ +name: CI + +on: + workflow_dispatch: + push: + branches: + - ros2 + pull_request: + branches: + - ros2 + +jobs: + industrial_ci: + runs-on: ubuntu-22.04 + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [humble, iron, jazzy, rolling] + ROS_REPO: [testing, main] + env: + ROS_DISTRO: ${{ matrix.ROS_DISTRO }} + ROS_REPO: ${{ matrix.ROS_REPO }} + steps: + - name: Checkout repo + uses: actions/checkout@v4 + - name: Source tests + uses: "ros-industrial/industrial_ci@master" diff --git a/CMakeLists.txt b/CMakeLists.txt index ac9c890..eeade93 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -77,8 +77,17 @@ install(TARGETS ${PROJECT_NAME} ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} - FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" ) +########### +## Tests ## +########### + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + ament_package() diff --git a/package.xml b/package.xml index 6097cac..59ad8c5 100644 --- a/package.xml +++ b/package.xml @@ -1,11 +1,11 @@ - + + web_video_server 1.0.0 HTTP Streaming of ROS Image Topics in Multiple Formats Błażej Sowa - Mitchell Wills BSD @@ -13,6 +13,8 @@ https://github.com/RobotWebTools/web_video_server/issues https://github.com/RobotWebTools/web_video_server + Mitchell Wills + ament_cmake_ros rclcpp @@ -29,6 +31,11 @@ ffmpeg sensor_msgs + ament_lint_auto + ament_cmake_lint_cmake + ament_cmake_xmllint + + ament_cmake From e9ec49863eb0ae9bc45a6579c11eda7f3302bb15 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 30 Sep 2024 13:33:28 +0000 Subject: [PATCH 05/42] Don't install headers The package does not export any libraries so there's no reason to install header files --- CMakeLists.txt | 5 ----- 1 file changed, 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index eeade93..b3d9225 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,11 +76,6 @@ install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} - FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" -) - ########### ## Tests ## ########### From 36cf45351ad8f851350e886061b479cd1e0a2999 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 30 Sep 2024 13:38:38 +0000 Subject: [PATCH 06/42] Use target_link_libraries instead of ament_target_dependencies where applicable This is a more modern way to link ROS libraries in CMake --- CMakeLists.txt | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b3d9225..3f77066 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,10 +55,15 @@ add_executable(${PROJECT_NAME} ) ament_target_dependencies(${PROJECT_NAME} - async_web_server_cpp cv_bridge image_transport rclcpp sensor_msgs) + sensor_msgs +) ## Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME} + async_web_server_cpp::async_web_server_cpp + cv_bridge::cv_bridge + image_transport::image_transport + rclcpp::rclcpp ${Boost_LIBRARIES} ${OpenCV_LIBS} ${avcodec_LIBRARIES} From 07c936ccba0db9b4851311c684b9c21fff22cd3c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Mon, 30 Sep 2024 16:32:48 +0200 Subject: [PATCH 07/42] Use cv_bridge hpp headers when available (#149) --- CMakeLists.txt | 4 ++++ include/web_video_server/web_video_server.h | 6 ++++++ src/image_streamer.cpp | 6 ++++++ 3 files changed, 16 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3f77066..90a2e8e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,6 +30,10 @@ endif() ## Build ## ########### +if(${cv_bridge_VERSION} VERSION_LESS "3.3.0") + add_compile_definitions(CV_BRIDGE_USES_OLD_HEADERS) +endif() + ## Specify additional locations of header files include_directories(include ${Boost_INCLUDE_DIRS} diff --git a/include/web_video_server/web_video_server.h b/include/web_video_server/web_video_server.h index 1da6015..254618f 100644 --- a/include/web_video_server/web_video_server.h +++ b/include/web_video_server/web_video_server.h @@ -2,7 +2,13 @@ #define WEB_VIDEO_SERVER_H_ #include + +#ifdef CV_BRIDGE_USES_OLD_HEADERS #include +#else +#include +#endif + #include #include "web_video_server/image_streamer.h" #include "async_web_server_cpp/http_server.hpp" diff --git a/src/image_streamer.cpp b/src/image_streamer.cpp index 1bd6708..9309be8 100644 --- a/src/image_streamer.cpp +++ b/src/image_streamer.cpp @@ -1,5 +1,11 @@ #include "web_video_server/image_streamer.h" + +#ifdef CV_BRIDGE_USES_OLD_HEADERS #include +#else +#include +#endif + #include namespace web_video_server From 6d76f92efbb5d796f5d7bd715061f3aff9d9b24c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 2 Oct 2024 09:30:05 +0200 Subject: [PATCH 08/42] (actions) also run on feature branches (#152) --- .github/workflows/ci.yml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 749a442..dafd6b2 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -3,15 +3,15 @@ name: CI on: workflow_dispatch: push: - branches: - - ros2 + branches-ignore: + - ros1* pull_request: - branches: - - ros2 + branches-ignore: + - ros1* jobs: industrial_ci: - runs-on: ubuntu-22.04 + runs-on: ubuntu-latest strategy: fail-fast: false matrix: From 1f6a5b75262c5b7ddbcc99bc0c0eadded96bf346 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Wed, 2 Oct 2024 12:08:21 +0200 Subject: [PATCH 09/42] Fix usage of deprecated libavcodec functions (#150) * Fix usage of deprecated libavcodec functions * Drop compatibility with old ffmpeg libraries * backport #103 * Remove remaining avcodec version check * Use rclcpp debug logs instead of cerr * Remove redundant vector * Fix segfault on write_header * Allow overriding width and height * Fix warning about missing timestamps --- include/web_video_server/libav_streamer.h | 3 +- src/image_streamer.cpp | 3 +- src/libav_streamer.cpp | 177 ++++++---------------- 3 files changed, 48 insertions(+), 135 deletions(-) diff --git a/include/web_video_server/libav_streamer.h b/include/web_video_server/libav_streamer.h index 643e3b3..635be1c 100644 --- a/include/web_video_server/libav_streamer.h +++ b/include/web_video_server/libav_streamer.h @@ -34,9 +34,8 @@ class LibavStreamer : public ImageTransportImageStreamer virtual void initializeEncoder(); virtual void sendImage(const cv::Mat&, const rclcpp::Time& time); virtual void initialize(const cv::Mat&); - AVOutputFormat* output_format_; AVFormatContext* format_context_; - AVCodec* codec_; + const AVCodec* codec_; AVCodecContext* codec_context_; AVStream* video_stream_; diff --git a/src/image_streamer.cpp b/src/image_streamer.cpp index 9309be8..a91c31a 100644 --- a/src/image_streamer.cpp +++ b/src/image_streamer.cpp @@ -171,8 +171,7 @@ void ImageTransportImageStreamer::imageCallback(const sensor_msgs::msg::Image::C } last_frame = nh_->now(); - sendImage(output_size_image, last_frame ); - + sendImage(output_size_image, msg->header.stamp); } catch (cv_bridge::Exception &e) { diff --git a/src/libav_streamer.cpp b/src/libav_streamer.cpp index 28d0425..0e490ac 100644 --- a/src/libav_streamer.cpp +++ b/src/libav_streamer.cpp @@ -8,51 +8,11 @@ namespace web_video_server { -static int ffmpeg_boost_mutex_lock_manager(void **mutex, enum AVLockOp op) -{ - if (NULL == mutex) - return -1; - - switch (op) - { - case AV_LOCK_CREATE: - { - *mutex = NULL; - boost::mutex *m = new boost::mutex(); - *mutex = static_cast(m); - break; - } - case AV_LOCK_OBTAIN: - { - boost::mutex *m = static_cast(*mutex); - m->lock(); - break; - } - case AV_LOCK_RELEASE: - { - boost::mutex *m = static_cast(*mutex); - m->unlock(); - break; - } - case AV_LOCK_DESTROY: - { - boost::mutex *m = static_cast(*mutex); - m->lock(); - m->unlock(); - delete m; - break; - } - default: - break; - } - return 0; -} - LibavStreamer::LibavStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh, const std::string &format_name, const std::string &codec_name, const std::string &content_type) : - ImageTransportImageStreamer(request, connection, nh), output_format_(0), format_context_(0), codec_(0), codec_context_(0), video_stream_( + ImageTransportImageStreamer(request, connection, nh), format_context_(0), codec_(0), codec_context_(0), video_stream_( 0), frame_(0), sws_context_(0), first_image_timestamp_(0), format_name_( format_name), codec_name_(codec_name), content_type_(content_type), opt_(0), io_buffer_(0) { @@ -60,24 +20,18 @@ LibavStreamer::LibavStreamer(const async_web_server_cpp::HttpRequest &request, bitrate_ = request.get_query_param_value_or_default("bitrate", 100000); qmin_ = request.get_query_param_value_or_default("qmin", 10); qmax_ = request.get_query_param_value_or_default("qmax", 42); - gop_ = request.get_query_param_value_or_default("gop", 250); - - av_lockmgr_register(&ffmpeg_boost_mutex_lock_manager); - av_register_all(); + gop_ = request.get_query_param_value_or_default("gop", 25); } LibavStreamer::~LibavStreamer() { if (codec_context_) - avcodec_close(codec_context_); + { + avcodec_free_context(&codec_context_); + } if (frame_) { -#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1) - av_free(frame_); - frame_ = NULL; -#else av_frame_free(&frame_); -#endif } if (io_buffer_) delete io_buffer_; @@ -111,15 +65,15 @@ void LibavStreamer::initialize(const cv::Mat &img) NULL, NULL); throw std::runtime_error("Error allocating ffmpeg format context"); } - output_format_ = av_guess_format(format_name_.c_str(), NULL, NULL); - if (!output_format_) + + format_context_->oformat = av_guess_format(format_name_.c_str(), NULL, NULL); + if (!format_context_->oformat) { async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL); throw std::runtime_error("Error looking up output format"); } - format_context_->oformat = output_format_; // Set up custom IO callback. size_t io_buffer_size = 3 * 1024; // 3M seen elsewhere and adjudged good @@ -134,12 +88,11 @@ void LibavStreamer::initialize(const cv::Mat &img) } io_ctx->seekable = 0; // no seeking, it's a stream format_context_->pb = io_ctx; - output_format_->flags |= AVFMT_FLAG_CUSTOM_IO; - output_format_->flags |= AVFMT_NOFILE; + format_context_->max_interleave_delta = 0; // Load codec if (codec_name_.empty()) // use default codec if none specified - codec_ = avcodec_find_encoder(output_format_->video_codec); + codec_ = avcodec_find_encoder(format_context_->oformat->video_codec); else codec_ = avcodec_find_encoder_by_name(codec_name_.c_str()); if (!codec_) @@ -157,11 +110,10 @@ void LibavStreamer::initialize(const cv::Mat &img) NULL, NULL); throw std::runtime_error("Error creating video stream"); } - codec_context_ = video_stream_->codec; - // Set options - avcodec_get_context_defaults3(codec_context_, codec_); + codec_context_ = avcodec_alloc_context3(codec_); + // Set options codec_context_->codec_id = codec_->id; codec_context_->bit_rate = bitrate_; @@ -182,11 +134,11 @@ void LibavStreamer::initialize(const cv::Mat &img) codec_context_->qmin = qmin_; codec_context_->qmax = qmax_; + codec_context_->flags |= AV_CODEC_FLAG_LOW_DELAY; + initializeEncoder(); - // Some formats want stream headers to be separate - if (format_context_->oformat->flags & AVFMT_GLOBALHEADER) - codec_context_->flags |= CODEC_FLAG_GLOBAL_HEADER; + avcodec_parameters_from_context(video_stream_->codecpar, codec_context_); // Open Codec if (avcodec_open2(codec_context_, codec_, NULL) < 0) @@ -198,23 +150,15 @@ void LibavStreamer::initialize(const cv::Mat &img) } // Allocate frame buffers -#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1) - frame_ = avcodec_alloc_frame(); -#else frame_ = av_frame_alloc(); -#endif + av_image_alloc(frame_->data, frame_->linesize, output_width_, output_height_, codec_context_->pix_fmt, 1); frame_->width = output_width_; frame_->height = output_height_; frame_->format = codec_context_->pix_fmt; - output_format_->flags |= AVFMT_NOFILE; - // Generate header - std::vector header_buffer; - std::size_t header_size; - uint8_t *header_raw_buffer; // define meta data av_dict_set(&format_context_->metadata, "author", "ROS web_video_server", 0); av_dict_set(&format_context_->metadata, "title", topic_.c_str(), 0); @@ -247,23 +191,12 @@ void LibavStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) { first_image_timestamp_ = time; } - std::vector encoded_frame; -#if (LIBAVUTIL_VERSION_MAJOR < 53) - PixelFormat input_coding_format = PIX_FMT_BGR24; -#else + AVPixelFormat input_coding_format = AV_PIX_FMT_BGR24; -#endif -#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1) - AVPicture *raw_frame = new AVPicture; - avpicture_fill(raw_frame, img.data, input_coding_format, output_width_, output_height_); -#else AVFrame *raw_frame = av_frame_alloc(); av_image_fill_arrays(raw_frame->data, raw_frame->linesize, img.data, input_coding_format, output_width_, output_height_, 1); -#endif - - // Convert from opencv to libav if (!sws_context_) @@ -282,41 +215,36 @@ void LibavStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) (const uint8_t * const *)raw_frame->data, raw_frame->linesize, 0, output_height_, frame_->data, frame_->linesize); -#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1) - delete raw_frame; -#else av_frame_free(&raw_frame); -#endif // Encode the frame - AVPacket pkt; - int got_packet; - av_init_packet(&pkt); - -#if (LIBAVCODEC_VERSION_MAJOR < 54) - int buf_size = 6 * output_width_ * output_height_; - pkt.data = (uint8_t*)av_malloc(buf_size); - pkt.size = avcodec_encode_video(codec_context_, pkt.data, buf_size, frame_); - got_packet = pkt.size > 0; -#elif (LIBAVCODEC_VERSION_MAJOR < 57) - pkt.data = NULL; // packet data will be allocated by the encoder - pkt.size = 0; - if (avcodec_encode_video2(codec_context_, &pkt, frame_, &got_packet) < 0) + AVPacket* pkt = av_packet_alloc(); + + ret = avcodec_send_frame(codec_context_, frame_); + if (ret == AVERROR_EOF) { - throw std::runtime_error("Error encoding video frame"); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "avcodec_send_frame() encoder flushed\n"); } -#else - pkt.data = NULL; // packet data will be allocated by the encoder - pkt.size = 0; - if (avcodec_send_frame(codec_context_, frame_) < 0) + else if (ret == AVERROR(EAGAIN)) + { + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "avcodec_send_frame() need output read out\n"); + } + if (ret < 0) { throw std::runtime_error("Error encoding video frame"); } - if (avcodec_receive_packet(codec_context_, &pkt) < 0) + + ret = avcodec_receive_packet(codec_context_, pkt); + bool got_packet = pkt->size > 0; + if (ret == AVERROR_EOF) + { + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "avcodec_receive_packet() encoder flushed\n"); + } + else if (ret == AVERROR(EAGAIN)) { - throw std::runtime_error("Error retrieving encoded packet"); + RCLCPP_DEBUG_STREAM(nh_->get_logger(), "avcodec_receive_packet() needs more input\n"); + got_packet = false; } -#endif if (got_packet) { @@ -325,36 +253,23 @@ void LibavStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) double seconds = (time - first_image_timestamp_).seconds(); // Encode video at 1/0.95 to minimize delay - pkt.pts = (int64_t)(seconds / av_q2d(video_stream_->time_base) * 0.95); - if (pkt.pts <= 0) - pkt.pts = 1; - pkt.dts = AV_NOPTS_VALUE; + pkt->pts = (int64_t)(seconds / av_q2d(video_stream_->time_base) * 0.95); + if (pkt->pts <= 0) + pkt->pts = 1; + pkt->dts = pkt->pts; - if (pkt.flags&AV_PKT_FLAG_KEY) - pkt.flags |= AV_PKT_FLAG_KEY; + if (pkt->flags&AV_PKT_FLAG_KEY) + pkt->flags |= AV_PKT_FLAG_KEY; - pkt.stream_index = video_stream_->index; + pkt->stream_index = video_stream_->index; - if (av_write_frame(format_context_, &pkt)) + if (av_write_frame(format_context_, pkt)) { throw std::runtime_error("Error when writing frame"); } } - else - { - encoded_frame.clear(); - } -#if LIBAVCODEC_VERSION_INT < 54 - av_free(pkt.data); -#endif - -#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1) - av_free_packet(&pkt); -#else - av_packet_unref(&pkt); -#endif - connection_->write_and_clear(encoded_frame); + av_packet_unref(pkt); } LibavStreamerType::LibavStreamerType(const std::string &format_name, const std::string &codec_name, From da1f1bbd0e613044a0eaff90d20bd05f32d1406d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Wed, 2 Oct 2024 18:23:32 +0200 Subject: [PATCH 10/42] Fix declaring and retrieving node parameters (#154) --- include/web_video_server/web_video_server.h | 2 +- src/web_video_server.cpp | 62 ++++++--------------- 2 files changed, 19 insertions(+), 45 deletions(-) diff --git a/include/web_video_server/web_video_server.h b/include/web_video_server/web_video_server.h index 254618f..dce4aa6 100644 --- a/include/web_video_server/web_video_server.h +++ b/include/web_video_server/web_video_server.h @@ -29,7 +29,7 @@ class WebVideoServer * @brief Constructor * @return */ - WebVideoServer(rclcpp::Node::SharedPtr &nh, rclcpp::Node::SharedPtr &private_nh); + WebVideoServer(rclcpp::Node::SharedPtr &nh); /** * @brief Destructor - Cleans up diff --git a/src/web_video_server.cpp b/src/web_video_server.cpp index 45a5226..dcfd012 100644 --- a/src/web_video_server.cpp +++ b/src/web_video_server.cpp @@ -49,51 +49,26 @@ static bool ros_connection_logger(async_web_server_cpp::HttpServerRequestHandler return false; } -WebVideoServer::WebVideoServer(rclcpp::Node::SharedPtr &nh, rclcpp::Node::SharedPtr &private_nh) : +WebVideoServer::WebVideoServer(rclcpp::Node::SharedPtr & nh) : nh_(nh), handler_group_( async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::not_found)) { - rclcpp::Parameter parameter; - if (private_nh->get_parameter("port", parameter)) { - port_ = parameter.as_int(); - } else { - port_ = 8080; - } - if (private_nh->get_parameter("verbose", parameter)) { - __verbose = parameter.as_bool(); - } else { - __verbose = true; - } - - if (private_nh->get_parameter("address", parameter)) { - address_ = parameter.as_string(); - } else { - address_ = "0.0.0.0"; - } - + nh_->declare_parameter("port", 8080); + nh_->declare_parameter("verbose", true); + nh_->declare_parameter("address", "0.0.0.0"); + nh_->declare_parameter("server_threads", 1); + nh_->declare_parameter("ros_threads", 2); + nh_->declare_parameter("publish_rate", -1.0); + nh_->declare_parameter("default_stream_type", "mjpeg"); + + nh_->get_parameter("port", port_); + nh_->get_parameter("verbose", __verbose); + nh_->get_parameter("address", address_); int server_threads; - if (private_nh->get_parameter("server_threads", parameter)) { - server_threads = parameter.as_int(); - } else { - server_threads = 1; - } - - if (private_nh->get_parameter("ros_threads", parameter)) { - ros_threads_ = parameter.as_int(); - } else { - ros_threads_ = 2; - } - if (private_nh->get_parameter("publish_rate", parameter)) { - publish_rate_ = parameter.as_double(); - } else { - publish_rate_ = -1.0; - } - - if (private_nh->get_parameter("default_stream_type", parameter)) { - __default_stream_type = parameter.as_string(); - } else { - __default_stream_type = "mjpeg"; - } + nh_->get_parameter("server_threads", server_threads); + nh_->get_parameter("ros_threads", ros_threads_); + nh_->get_parameter("publish_rate", publish_rate_); + nh_->get_parameter("default_stream_type", __default_stream_type); stream_types_["mjpeg"] = boost::shared_ptr(new MjpegStreamerType()); stream_types_["png"] = boost::shared_ptr(new PngStreamerType()); @@ -378,10 +353,9 @@ bool WebVideoServer::handle_list_streams(const async_web_server_cpp::HttpRequest int main(int argc, char **argv) { rclcpp::init(argc, argv); - auto nh = std::make_shared("web_video_server"); - auto private_nh = std::make_shared("_web_video_server"); + auto node = std::make_shared("web_video_server"); - web_video_server::WebVideoServer server(nh, private_nh); + web_video_server::WebVideoServer server(node); server.setup_cleanup_inactive_streams(); server.spin(); From 9fc0d63e30d278ba616e8d8e06ba06997bd38e48 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Wed, 2 Oct 2024 18:39:27 +0200 Subject: [PATCH 11/42] Replace nh with node (#155) --- include/web_video_server/h264_streamer.h | 4 +- include/web_video_server/image_streamer.h | 8 +-- include/web_video_server/jpeg_streamers.h | 6 +- include/web_video_server/libav_streamer.h | 4 +- include/web_video_server/png_streamers.h | 6 +- .../ros_compressed_streamer.h | 4 +- include/web_video_server/vp8_streamer.h | 4 +- include/web_video_server/vp9_streamer.h | 4 +- include/web_video_server/web_video_server.h | 4 +- src/h264_streamer.cpp | 8 +-- src/image_streamer.cpp | 40 ++++++------ src/jpeg_streamers.cpp | 12 ++-- src/libav_streamer.cpp | 16 ++--- src/png_streamers.cpp | 12 ++-- src/ros_compressed_streamer.cpp | 26 ++++---- src/vp8_streamer.cpp | 8 +-- src/vp9_streamer.cpp | 8 +-- src/web_video_server.cpp | 64 +++++++++---------- 18 files changed, 119 insertions(+), 119 deletions(-) diff --git a/include/web_video_server/h264_streamer.h b/include/web_video_server/h264_streamer.h index 9961389..223d079 100644 --- a/include/web_video_server/h264_streamer.h +++ b/include/web_video_server/h264_streamer.h @@ -13,7 +13,7 @@ class H264Streamer : public LibavStreamer { public: H264Streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); ~H264Streamer(); protected: virtual void initializeEncoder(); @@ -26,7 +26,7 @@ class H264StreamerType : public LibavStreamerType H264StreamerType(); virtual boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); }; } diff --git a/include/web_video_server/image_streamer.h b/include/web_video_server/image_streamer.h index ca474e8..f0dc2ad 100644 --- a/include/web_video_server/image_streamer.h +++ b/include/web_video_server/image_streamer.h @@ -17,7 +17,7 @@ class ImageStreamer public: ImageStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); virtual void start() = 0; virtual ~ImageStreamer(); @@ -41,7 +41,7 @@ class ImageStreamer protected: async_web_server_cpp::HttpConnectionPtr connection_; async_web_server_cpp::HttpRequest request_; - rclcpp::Node::SharedPtr nh_; + rclcpp::Node::SharedPtr node_; bool inactive_; image_transport::Subscriber image_sub_; std::string topic_; @@ -52,7 +52,7 @@ class ImageTransportImageStreamer : public ImageStreamer { public: ImageTransportImageStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); virtual ~ImageTransportImageStreamer(); virtual void start(); @@ -85,7 +85,7 @@ class ImageStreamerType public: virtual boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) = 0; + rclcpp::Node::SharedPtr node) = 0; virtual std::string create_viewer(const async_web_server_cpp::HttpRequest &request) = 0; }; diff --git a/include/web_video_server/jpeg_streamers.h b/include/web_video_server/jpeg_streamers.h index ad788fa..a79b6e2 100644 --- a/include/web_video_server/jpeg_streamers.h +++ b/include/web_video_server/jpeg_streamers.h @@ -14,7 +14,7 @@ class MjpegStreamer : public ImageTransportImageStreamer { public: MjpegStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); ~MjpegStreamer(); protected: virtual void sendImage(const cv::Mat &, const rclcpp::Time &time); @@ -29,7 +29,7 @@ class MjpegStreamerType : public ImageStreamerType public: boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); std::string create_viewer(const async_web_server_cpp::HttpRequest &request); }; @@ -37,7 +37,7 @@ class JpegSnapshotStreamer : public ImageTransportImageStreamer { public: JpegSnapshotStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh); + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node); ~JpegSnapshotStreamer(); protected: virtual void sendImage(const cv::Mat &, const rclcpp::Time &time); diff --git a/include/web_video_server/libav_streamer.h b/include/web_video_server/libav_streamer.h index 635be1c..9bb0012 100644 --- a/include/web_video_server/libav_streamer.h +++ b/include/web_video_server/libav_streamer.h @@ -25,7 +25,7 @@ class LibavStreamer : public ImageTransportImageStreamer { public: LibavStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh, const std::string &format_name, const std::string &codec_name, + rclcpp::Node::SharedPtr node, const std::string &format_name, const std::string &codec_name, const std::string &content_type); ~LibavStreamer(); @@ -65,7 +65,7 @@ class LibavStreamerType : public ImageStreamerType boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); std::string create_viewer(const async_web_server_cpp::HttpRequest &request); diff --git a/include/web_video_server/png_streamers.h b/include/web_video_server/png_streamers.h index a6edabc..56fdcec 100644 --- a/include/web_video_server/png_streamers.h +++ b/include/web_video_server/png_streamers.h @@ -14,7 +14,7 @@ class PngStreamer : public ImageTransportImageStreamer { public: PngStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); ~PngStreamer(); protected: virtual void sendImage(const cv::Mat &, const rclcpp::Time &time); @@ -29,7 +29,7 @@ class PngStreamerType : public ImageStreamerType public: boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); std::string create_viewer(const async_web_server_cpp::HttpRequest &request); }; @@ -37,7 +37,7 @@ class PngSnapshotStreamer : public ImageTransportImageStreamer { public: PngSnapshotStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh); + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node); ~PngSnapshotStreamer(); protected: virtual void sendImage(const cv::Mat &, const rclcpp::Time &time); diff --git a/include/web_video_server/ros_compressed_streamer.h b/include/web_video_server/ros_compressed_streamer.h index f188d7e..c1f676b 100644 --- a/include/web_video_server/ros_compressed_streamer.h +++ b/include/web_video_server/ros_compressed_streamer.h @@ -14,7 +14,7 @@ class RosCompressedStreamer : public ImageStreamer { public: RosCompressedStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); ~RosCompressedStreamer(); virtual void start(); virtual void restreamFrame(double max_age); @@ -37,7 +37,7 @@ class RosCompressedStreamerType : public ImageStreamerType public: boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); std::string create_viewer(const async_web_server_cpp::HttpRequest &request); }; diff --git a/include/web_video_server/vp8_streamer.h b/include/web_video_server/vp8_streamer.h index 46e8bed..01cd7e7 100644 --- a/include/web_video_server/vp8_streamer.h +++ b/include/web_video_server/vp8_streamer.h @@ -49,7 +49,7 @@ class Vp8Streamer : public LibavStreamer { public: Vp8Streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); ~Vp8Streamer(); protected: virtual void initializeEncoder(); @@ -63,7 +63,7 @@ class Vp8StreamerType : public LibavStreamerType Vp8StreamerType(); virtual boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); }; } diff --git a/include/web_video_server/vp9_streamer.h b/include/web_video_server/vp9_streamer.h index 06c48f8..e6d0356 100644 --- a/include/web_video_server/vp9_streamer.h +++ b/include/web_video_server/vp9_streamer.h @@ -13,7 +13,7 @@ class Vp9Streamer : public LibavStreamer { public: Vp9Streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); ~Vp9Streamer(); protected: virtual void initializeEncoder(); @@ -25,7 +25,7 @@ class Vp9StreamerType : public LibavStreamerType Vp9StreamerType(); virtual boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); + rclcpp::Node::SharedPtr node); }; } diff --git a/include/web_video_server/web_video_server.h b/include/web_video_server/web_video_server.h index dce4aa6..33c5d7e 100644 --- a/include/web_video_server/web_video_server.h +++ b/include/web_video_server/web_video_server.h @@ -29,7 +29,7 @@ class WebVideoServer * @brief Constructor * @return */ - WebVideoServer(rclcpp::Node::SharedPtr &nh); + WebVideoServer(rclcpp::Node::SharedPtr &node); /** * @brief Destructor - Cleans up @@ -59,7 +59,7 @@ class WebVideoServer void restreamFrames(double max_age); void cleanup_inactive_streams(); - rclcpp::Node::SharedPtr nh_; + rclcpp::Node::SharedPtr node_; rclcpp::WallTimer::SharedPtr cleanup_timer_; int ros_threads_; double publish_rate_; diff --git a/src/h264_streamer.cpp b/src/h264_streamer.cpp index a866aa1..05a6c72 100644 --- a/src/h264_streamer.cpp +++ b/src/h264_streamer.cpp @@ -4,8 +4,8 @@ namespace web_video_server { H264Streamer::H264Streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - LibavStreamer(request, connection, nh, "mp4", "libx264", "video/mp4") + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : + LibavStreamer(request, connection, node, "mp4", "libx264", "video/mp4") { /* possible quality presets: * ultrafast, superfast, veryfast, faster, fast, medium, slow, slower, veryslow, placebo @@ -41,9 +41,9 @@ H264StreamerType::H264StreamerType() : boost::shared_ptr H264StreamerType::create_streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) + rclcpp::Node::SharedPtr node) { - return boost::shared_ptr(new H264Streamer(request, connection, nh)); + return boost::shared_ptr(new H264Streamer(request, connection, node)); } } diff --git a/src/image_streamer.cpp b/src/image_streamer.cpp index a91c31a..7dcc4cb 100644 --- a/src/image_streamer.cpp +++ b/src/image_streamer.cpp @@ -12,8 +12,8 @@ namespace web_video_server { ImageStreamer::ImageStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - request_(request), connection_(connection), nh_(nh), inactive_(false) + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : + request_(request), connection_(connection), node_(node), inactive_(false) { topic_ = request.get_query_param_value_or_default("topic", ""); } @@ -23,8 +23,8 @@ ImageStreamer::~ImageStreamer() } ImageTransportImageStreamer::ImageTransportImageStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - ImageStreamer(request, connection, nh), it_(nh), initialized_(false) + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : + ImageStreamer(request, connection, node), it_(node), initialized_(false) { output_width_ = request.get_query_param_value_or_default("width", -1); output_height_ = request.get_query_param_value_or_default("height", -1); @@ -39,8 +39,8 @@ ImageTransportImageStreamer::~ImageTransportImageStreamer() void ImageTransportImageStreamer::start() { - image_transport::TransportHints hints(nh_.get(), default_transport_); - auto tnat = nh_->get_topic_names_and_types(); + image_transport::TransportHints hints(node_.get(), default_transport_); + auto tnat = node_->get_topic_names_and_types(); inactive_ = true; for (auto topic_and_types : tnat) { if (topic_and_types.second.size() > 1) { @@ -55,19 +55,19 @@ void ImageTransportImageStreamer::start() } // Get QoS profile from query parameter - RCLCPP_INFO(nh_->get_logger(), "Streaming topic %s with QoS profile %s", topic_.c_str(), qos_profile_name_.c_str()); + RCLCPP_INFO(node_->get_logger(), "Streaming topic %s with QoS profile %s", topic_.c_str(), qos_profile_name_.c_str()); auto qos_profile = get_qos_profile_from_name(qos_profile_name_); if (!qos_profile) { qos_profile = rmw_qos_profile_default; RCLCPP_ERROR( - nh_->get_logger(), + node_->get_logger(), "Invalid QoS profile %s specified. Using default profile.", qos_profile_name_.c_str()); } // Create subscriber image_sub_ = image_transport::create_subscription( - nh_.get(), topic_, std::bind(&ImageTransportImageStreamer::imageCallback, this, std::placeholders::_1), + node_.get(), topic_, std::bind(&ImageTransportImageStreamer::imageCallback, this, std::placeholders::_1), default_transport_, qos_profile.value()); } @@ -80,29 +80,29 @@ void ImageTransportImageStreamer::restreamFrame(double max_age) if (inactive_ || !initialized_ ) return; try { - if ( last_frame + rclcpp::Duration::from_seconds(max_age) < nh_->now() ) { + if ( last_frame + rclcpp::Duration::from_seconds(max_age) < node_->now() ) { boost::mutex::scoped_lock lock(send_mutex_); - sendImage(output_size_image, nh_->now() ); // don't update last_frame, it may remain an old value. + sendImage(output_size_image, node_->now() ); // don't update last_frame, it may remain an old value. } } catch (boost::system::system_error &e) { // happens when client disconnects - RCLCPP_DEBUG(nh_->get_logger(), "system_error exception: %s", e.what()); + RCLCPP_DEBUG(node_->get_logger(), "system_error exception: %s", e.what()); inactive_ = true; return; } catch (std::exception &e) { // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception: %s", e.what()); + RCLCPP_ERROR(node_->get_logger(), "exception: %s", e.what()); inactive_ = true; return; } catch (...) { // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception"); + RCLCPP_ERROR(node_->get_logger(), "exception"); inactive_ = true; return; } @@ -170,41 +170,41 @@ void ImageTransportImageStreamer::imageCallback(const sensor_msgs::msg::Image::C initialized_ = true; } - last_frame = nh_->now(); + last_frame = node_->now(); sendImage(output_size_image, msg->header.stamp); } catch (cv_bridge::Exception &e) { // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "cv_bridge exception: %s", e.what()); + RCLCPP_ERROR(node_->get_logger(), "cv_bridge exception: %s", e.what()); inactive_ = true; return; } catch (cv::Exception &e) { // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "cv_bridge exception: %s", e.what()); + RCLCPP_ERROR(node_->get_logger(), "cv_bridge exception: %s", e.what()); inactive_ = true; return; } catch (boost::system::system_error &e) { // happens when client disconnects - RCLCPP_DEBUG(nh_->get_logger(), "system_error exception: %s", e.what()); + RCLCPP_DEBUG(node_->get_logger(), "system_error exception: %s", e.what()); inactive_ = true; return; } catch (std::exception &e) { // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception: %s", e.what()); + RCLCPP_ERROR(node_->get_logger(), "exception: %s", e.what()); inactive_ = true; return; } catch (...) { // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception"); + RCLCPP_ERROR(node_->get_logger(), "exception"); inactive_ = true; return; } diff --git a/src/jpeg_streamers.cpp b/src/jpeg_streamers.cpp index 0c5cf88..c48bc4a 100644 --- a/src/jpeg_streamers.cpp +++ b/src/jpeg_streamers.cpp @@ -5,8 +5,8 @@ namespace web_video_server { MjpegStreamer::MjpegStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - ImageTransportImageStreamer(request, connection, nh), stream_(std::bind(&rclcpp::Node::now, nh), connection) + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : + ImageTransportImageStreamer(request, connection, node), stream_(std::bind(&rclcpp::Node::now, node), connection) { quality_ = request.get_query_param_value_or_default("quality", 95); stream_.sendInitialHeader(); @@ -32,9 +32,9 @@ void MjpegStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) boost::shared_ptr MjpegStreamerType::create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) + rclcpp::Node::SharedPtr node) { - return boost::shared_ptr(new MjpegStreamer(request, connection, nh)); + return boost::shared_ptr(new MjpegStreamer(request, connection, node)); } std::string MjpegStreamerType::create_viewer(const async_web_server_cpp::HttpRequest &request) @@ -48,8 +48,8 @@ std::string MjpegStreamerType::create_viewer(const async_web_server_cpp::HttpReq JpegSnapshotStreamer::JpegSnapshotStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) : - ImageTransportImageStreamer(request, connection, nh) + rclcpp::Node::SharedPtr node) : + ImageTransportImageStreamer(request, connection, node) { quality_ = request.get_query_param_value_or_default("quality", 95); } diff --git a/src/libav_streamer.cpp b/src/libav_streamer.cpp index 0e490ac..9707b31 100644 --- a/src/libav_streamer.cpp +++ b/src/libav_streamer.cpp @@ -9,10 +9,10 @@ namespace web_video_server { LibavStreamer::LibavStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh, + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node, const std::string &format_name, const std::string &codec_name, const std::string &content_type) : - ImageTransportImageStreamer(request, connection, nh), format_context_(0), codec_(0), codec_context_(0), video_stream_( + ImageTransportImageStreamer(request, connection, node), format_context_(0), codec_(0), codec_context_(0), video_stream_( 0), frame_(0), sws_context_(0), first_image_timestamp_(0), format_name_( format_name), codec_name_(codec_name), content_type_(content_type), opt_(0), io_buffer_(0) { @@ -223,11 +223,11 @@ void LibavStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) ret = avcodec_send_frame(codec_context_, frame_); if (ret == AVERROR_EOF) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "avcodec_send_frame() encoder flushed\n"); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_send_frame() encoder flushed\n"); } else if (ret == AVERROR(EAGAIN)) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "avcodec_send_frame() need output read out\n"); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_send_frame() need output read out\n"); } if (ret < 0) { @@ -238,11 +238,11 @@ void LibavStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) bool got_packet = pkt->size > 0; if (ret == AVERROR_EOF) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "avcodec_receive_packet() encoder flushed\n"); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_receive_packet() encoder flushed\n"); } else if (ret == AVERROR(EAGAIN)) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "avcodec_receive_packet() needs more input\n"); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_receive_packet() needs more input\n"); got_packet = false; } @@ -280,10 +280,10 @@ LibavStreamerType::LibavStreamerType(const std::string &format_name, const std:: boost::shared_ptr LibavStreamerType::create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) + rclcpp::Node::SharedPtr node) { return boost::shared_ptr( - new LibavStreamer(request, connection, nh, format_name_, codec_name_, content_type_)); + new LibavStreamer(request, connection, node, format_name_, codec_name_, content_type_)); } std::string LibavStreamerType::create_viewer(const async_web_server_cpp::HttpRequest &request) diff --git a/src/png_streamers.cpp b/src/png_streamers.cpp index 1a9f874..8d008f1 100644 --- a/src/png_streamers.cpp +++ b/src/png_streamers.cpp @@ -5,8 +5,8 @@ namespace web_video_server { PngStreamer::PngStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - ImageTransportImageStreamer(request, connection, nh), stream_(std::bind(&rclcpp::Node::now, nh), connection) + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : + ImageTransportImageStreamer(request, connection, node), stream_(std::bind(&rclcpp::Node::now, node), connection) { quality_ = request.get_query_param_value_or_default("quality", 3); stream_.sendInitialHeader(); @@ -32,9 +32,9 @@ void PngStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) boost::shared_ptr PngStreamerType::create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) + rclcpp::Node::SharedPtr node) { - return boost::shared_ptr(new PngStreamer(request, connection, nh)); + return boost::shared_ptr(new PngStreamer(request, connection, node)); } std::string PngStreamerType::create_viewer(const async_web_server_cpp::HttpRequest &request) @@ -48,8 +48,8 @@ std::string PngStreamerType::create_viewer(const async_web_server_cpp::HttpReque PngSnapshotStreamer::PngSnapshotStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) : - ImageTransportImageStreamer(request, connection, nh) + rclcpp::Node::SharedPtr node) : + ImageTransportImageStreamer(request, connection, node) { quality_ = request.get_query_param_value_or_default("quality", 3); } diff --git a/src/ros_compressed_streamer.cpp b/src/ros_compressed_streamer.cpp index 02f13ef..e1c89f6 100644 --- a/src/ros_compressed_streamer.cpp +++ b/src/ros_compressed_streamer.cpp @@ -4,8 +4,8 @@ namespace web_video_server { RosCompressedStreamer::RosCompressedStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - ImageStreamer(request, connection, nh), stream_(std::bind(&rclcpp::Node::now, nh), connection) + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : + ImageStreamer(request, connection, node), stream_(std::bind(&rclcpp::Node::now, node), connection) { stream_.sendInitialHeader(); qos_profile_name_ = request.get_query_param_value_or_default("qos_profile", "default"); @@ -21,19 +21,19 @@ void RosCompressedStreamer::start() { const std::string compressed_topic = topic_ + "/compressed"; // Get QoS profile from query parameter - RCLCPP_INFO(nh_->get_logger(), "Streaming topic %s with QoS profile %s", compressed_topic.c_str(), qos_profile_name_.c_str()); + RCLCPP_INFO(node_->get_logger(), "Streaming topic %s with QoS profile %s", compressed_topic.c_str(), qos_profile_name_.c_str()); auto qos_profile = get_qos_profile_from_name(qos_profile_name_); if (!qos_profile) { qos_profile = rmw_qos_profile_default; RCLCPP_ERROR( - nh_->get_logger(), + node_->get_logger(), "Invalid QoS profile %s specified. Using default profile.", qos_profile_name_.c_str()); } // Create subscriber const auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.value().history, 1), qos_profile.value()); - image_sub_ = nh_->create_subscription( + image_sub_ = node_->create_subscription( compressed_topic, qos, std::bind(&RosCompressedStreamer::imageCallback, this, std::placeholders::_1)); } @@ -42,9 +42,9 @@ void RosCompressedStreamer::restreamFrame(double max_age) if (inactive_ || (last_msg == 0)) return; - if ( last_frame + rclcpp::Duration::from_seconds(max_age) < nh_->now() ) { + if ( last_frame + rclcpp::Duration::from_seconds(max_age) < node_->now() ) { boost::mutex::scoped_lock lock(send_mutex_); - sendImage(last_msg, nh_->now() ); // don't update last_frame, it may remain an old value. + sendImage(last_msg, node_->now() ); // don't update last_frame, it may remain an old value. } } @@ -59,7 +59,7 @@ void RosCompressedStreamer::sendImage(const sensor_msgs::msg::CompressedImage::C content_type = "image/png"; } else { - RCLCPP_WARN(nh_->get_logger(), "Unknown ROS compressed image format: %s", msg->format.c_str()); + RCLCPP_WARN(node_->get_logger(), "Unknown ROS compressed image format: %s", msg->format.c_str()); return; } @@ -68,21 +68,21 @@ void RosCompressedStreamer::sendImage(const sensor_msgs::msg::CompressedImage::C catch (boost::system::system_error &e) { // happens when client disconnects - RCLCPP_DEBUG(nh_->get_logger(), "system_error exception: %s", e.what()); + RCLCPP_DEBUG(node_->get_logger(), "system_error exception: %s", e.what()); inactive_ = true; return; } catch (std::exception &e) { // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception: %s", e.what()); + RCLCPP_ERROR(node_->get_logger(), "exception: %s", e.what()); inactive_ = true; return; } catch (...) { // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception"); + RCLCPP_ERROR(node_->get_logger(), "exception"); inactive_ = true; return; } @@ -99,9 +99,9 @@ void RosCompressedStreamer::imageCallback(const sensor_msgs::msg::CompressedImag boost::shared_ptr RosCompressedStreamerType::create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) + rclcpp::Node::SharedPtr node) { - return boost::shared_ptr(new RosCompressedStreamer(request, connection, nh)); + return boost::shared_ptr(new RosCompressedStreamer(request, connection, node)); } std::string RosCompressedStreamerType::create_viewer(const async_web_server_cpp::HttpRequest &request) diff --git a/src/vp8_streamer.cpp b/src/vp8_streamer.cpp index f45b7ca..33fb8ee 100644 --- a/src/vp8_streamer.cpp +++ b/src/vp8_streamer.cpp @@ -40,8 +40,8 @@ namespace web_video_server { Vp8Streamer::Vp8Streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - LibavStreamer(request, connection, nh, "webm", "libvpx", "video/webm") + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : + LibavStreamer(request, connection, node, "webm", "libvpx", "video/webm") { quality_ = request.get_query_param_value_or_default("quality", "realtime"); } @@ -83,9 +83,9 @@ Vp8StreamerType::Vp8StreamerType() : boost::shared_ptr Vp8StreamerType::create_streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) + rclcpp::Node::SharedPtr node) { - return boost::shared_ptr(new Vp8Streamer(request, connection, nh)); + return boost::shared_ptr(new Vp8Streamer(request, connection, node)); } } diff --git a/src/vp9_streamer.cpp b/src/vp9_streamer.cpp index 8fcefd8..da8fa65 100644 --- a/src/vp9_streamer.cpp +++ b/src/vp9_streamer.cpp @@ -4,8 +4,8 @@ namespace web_video_server { Vp9Streamer::Vp9Streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - LibavStreamer(request, connection, nh, "webm", "libvpx-vp9", "video/webm") + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : + LibavStreamer(request, connection, node, "webm", "libvpx-vp9", "video/webm") { } Vp9Streamer::~Vp9Streamer() @@ -30,9 +30,9 @@ Vp9StreamerType::Vp9StreamerType() : boost::shared_ptr Vp9StreamerType::create_streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) + rclcpp::Node::SharedPtr node) { - return boost::shared_ptr(new Vp9Streamer(request, connection, nh)); + return boost::shared_ptr(new Vp9Streamer(request, connection, node)); } } diff --git a/src/web_video_server.cpp b/src/web_video_server.cpp index dcfd012..190df47 100644 --- a/src/web_video_server.cpp +++ b/src/web_video_server.cpp @@ -33,7 +33,7 @@ static bool ros_connection_logger(async_web_server_cpp::HttpServerRequestHandler if (__verbose) { // TODO reenable - // RCLCPP_INFO(nh->get_logger(), "Handling Request: %s", request.uri.c_str()); + // RCLCPP_INFO(node->get_logger(), "Handling Request: %s", request.uri.c_str()); } try { @@ -43,32 +43,32 @@ static bool ros_connection_logger(async_web_server_cpp::HttpServerRequestHandler catch (std::exception &e) { // TODO reenable - // RCLCPP_WARN(nh->get_logger(), "Error Handling Request: %s", e.what()); + // RCLCPP_WARN(node->get_logger(), "Error Handling Request: %s", e.what()); return false; } return false; } -WebVideoServer::WebVideoServer(rclcpp::Node::SharedPtr & nh) : - nh_(nh), handler_group_( +WebVideoServer::WebVideoServer(rclcpp::Node::SharedPtr & node) : + node_(node), handler_group_( async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::not_found)) { - nh_->declare_parameter("port", 8080); - nh_->declare_parameter("verbose", true); - nh_->declare_parameter("address", "0.0.0.0"); - nh_->declare_parameter("server_threads", 1); - nh_->declare_parameter("ros_threads", 2); - nh_->declare_parameter("publish_rate", -1.0); - nh_->declare_parameter("default_stream_type", "mjpeg"); - - nh_->get_parameter("port", port_); - nh_->get_parameter("verbose", __verbose); - nh_->get_parameter("address", address_); + node_->declare_parameter("port", 8080); + node_->declare_parameter("verbose", true); + node_->declare_parameter("address", "0.0.0.0"); + node_->declare_parameter("server_threads", 1); + node_->declare_parameter("ros_threads", 2); + node_->declare_parameter("publish_rate", -1.0); + node_->declare_parameter("default_stream_type", "mjpeg"); + + node_->get_parameter("port", port_); + node_->get_parameter("verbose", __verbose); + node_->get_parameter("address", address_); int server_threads; - nh_->get_parameter("server_threads", server_threads); - nh_->get_parameter("ros_threads", ros_threads_); - nh_->get_parameter("publish_rate", publish_rate_); - nh_->get_parameter("default_stream_type", __default_stream_type); + node_->get_parameter("server_threads", server_threads); + node_->get_parameter("ros_threads", ros_threads_); + node_->get_parameter("publish_rate", publish_rate_); + node_->get_parameter("default_stream_type", __default_stream_type); stream_types_["mjpeg"] = boost::shared_ptr(new MjpegStreamerType()); stream_types_["png"] = boost::shared_ptr(new PngStreamerType()); @@ -92,7 +92,7 @@ WebVideoServer::WebVideoServer(rclcpp::Node::SharedPtr & nh) : } catch(boost::exception& e) { - RCLCPP_ERROR(nh_->get_logger(), "Exception when creating the web server! %s:%d", address_.c_str(), port_); + RCLCPP_ERROR(node_->get_logger(), "Exception when creating the web server! %s:%d", address_.c_str(), port_); throw; } } @@ -104,17 +104,17 @@ WebVideoServer::~WebVideoServer() void WebVideoServer::setup_cleanup_inactive_streams() { std::function callback = std::bind(&WebVideoServer::cleanup_inactive_streams, this); - cleanup_timer_ = nh_->create_wall_timer(500ms, callback); + cleanup_timer_ = node_->create_wall_timer(500ms, callback); } void WebVideoServer::spin() { server_->run(); - RCLCPP_INFO(nh_->get_logger(), "Waiting For connections on %s:%d", address_.c_str(), port_); + RCLCPP_INFO(node_->get_logger(), "Waiting For connections on %s:%d", address_.c_str(), port_); rclcpp::executors::MultiThreadedExecutor spinner(rclcpp::ExecutorOptions(), ros_threads_); - spinner.add_node(nh_); + spinner.add_node(node_); if ( publish_rate_ > 0 ) { - nh_->create_wall_timer(1s / publish_rate_, [this](){restreamFrames(1.0 / publish_rate_);}); + node_->create_wall_timer(1s / publish_rate_, [this](){restreamFrames(1.0 / publish_rate_);}); } spinner.spin(); server_->stop(); @@ -144,7 +144,7 @@ void WebVideoServer::cleanup_inactive_streams() { for (itr_type itr = new_end; itr < image_subscribers_.end(); ++itr) { - RCLCPP_INFO(nh_->get_logger(), "Removed Stream: %s", (*itr)->getTopic().c_str()); + RCLCPP_INFO(node_->get_logger(), "Removed Stream: %s", (*itr)->getTopic().c_str()); } } image_subscribers_.erase(new_end, image_subscribers_.end()); @@ -163,7 +163,7 @@ bool WebVideoServer::handle_stream(const async_web_server_cpp::HttpRequest &requ if (type == std::string("ros_compressed")) { std::string compressed_topic_name = topic + "/compressed"; - auto tnat = nh_->get_topic_names_and_types(); + auto tnat = node_->get_topic_names_and_types(); bool did_find_compressed_topic = false; for (auto topic_and_types : tnat) { if (topic_and_types.second.size() > 1) { @@ -178,11 +178,11 @@ bool WebVideoServer::handle_stream(const async_web_server_cpp::HttpRequest &requ } if (!did_find_compressed_topic) { - RCLCPP_WARN(nh_->get_logger(), "Could not find compressed image topic for %s, falling back to mjpeg", topic.c_str()); + RCLCPP_WARN(node_->get_logger(), "Could not find compressed image topic for %s, falling back to mjpeg", topic.c_str()); type = "mjpeg"; } } - boost::shared_ptr streamer = stream_types_[type]->create_streamer(request, connection, nh_); + boost::shared_ptr streamer = stream_types_[type]->create_streamer(request, connection, node_); streamer->start(); boost::mutex::scoped_lock lock(subscriber_mutex_); image_subscribers_.push_back(streamer); @@ -199,7 +199,7 @@ bool WebVideoServer::handle_snapshot(const async_web_server_cpp::HttpRequest &re async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end) { - boost::shared_ptr streamer(new JpegSnapshotStreamer(request, connection, nh_)); + boost::shared_ptr streamer(new JpegSnapshotStreamer(request, connection, node_)); streamer->start(); boost::mutex::scoped_lock lock(subscriber_mutex_); @@ -220,7 +220,7 @@ bool WebVideoServer::handle_stream_viewer(const async_web_server_cpp::HttpReques { std::string compressed_topic_name = topic + "/compressed"; - auto tnat = nh_->get_topic_names_and_types(); + auto tnat = node_->get_topic_names_and_types(); bool did_find_compressed_topic = false; for (auto topic_and_types : tnat) { if (topic_and_types.second.size() > 1) { @@ -235,7 +235,7 @@ bool WebVideoServer::handle_stream_viewer(const async_web_server_cpp::HttpReques } if (!did_find_compressed_topic) { - RCLCPP_WARN(nh_->get_logger(), "Could not find compressed image topic for %s, falling back to mjpeg", topic.c_str()); + RCLCPP_WARN(node_->get_logger(), "Could not find compressed image topic for %s, falling back to mjpeg", topic.c_str()); type = "mjpeg"; } } @@ -264,7 +264,7 @@ bool WebVideoServer::handle_list_streams(const async_web_server_cpp::HttpRequest { std::vector image_topics; std::vector camera_info_topics; - auto tnat = nh_->get_topic_names_and_types(); + auto tnat = node_->get_topic_names_and_types(); for (auto topic_and_types : tnat) { if (topic_and_types.second.size() > 1) { // skip over topics with more than one type From cadc7c241b1d91f56d407e8a12f79ddc752a00e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Wed, 2 Oct 2024 19:15:58 +0200 Subject: [PATCH 12/42] Fix request logging, remove global parameters (#156) --- include/web_video_server/web_video_server.h | 8 +++ src/web_video_server.cpp | 60 +++++++++------------ 2 files changed, 34 insertions(+), 34 deletions(-) diff --git a/include/web_video_server/web_video_server.h b/include/web_video_server/web_video_server.h index 33c5d7e..93b4589 100644 --- a/include/web_video_server/web_video_server.h +++ b/include/web_video_server/web_video_server.h @@ -43,6 +43,9 @@ class WebVideoServer void setup_cleanup_inactive_streams(); + bool handle_request(const async_web_server_cpp::HttpRequest &request, + async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end); + bool handle_stream(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end); @@ -61,10 +64,15 @@ class WebVideoServer rclcpp::Node::SharedPtr node_; rclcpp::WallTimer::SharedPtr cleanup_timer_; + + // Parameters int ros_threads_; double publish_rate_; int port_; std::string address_; + bool verbose_; + std::string default_stream_type_; + boost::shared_ptr server_; async_web_server_cpp::HttpRequestHandlerGroup handler_group_; diff --git a/src/web_video_server.cpp b/src/web_video_server.cpp index 190df47..11acfc3 100644 --- a/src/web_video_server.cpp +++ b/src/web_video_server.cpp @@ -21,34 +21,6 @@ using namespace boost::placeholders; namespace web_video_server { -static bool __verbose; - -static std::string __default_stream_type; - -static bool ros_connection_logger(async_web_server_cpp::HttpServerRequestHandler forward, - const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, const char* begin, - const char* end) -{ - if (__verbose) - { - // TODO reenable - // RCLCPP_INFO(node->get_logger(), "Handling Request: %s", request.uri.c_str()); - } - try - { - forward(request, connection, begin, end); - return true; - } - catch (std::exception &e) - { - // TODO reenable - // RCLCPP_WARN(node->get_logger(), "Error Handling Request: %s", e.what()); - return false; - } - return false; -} - WebVideoServer::WebVideoServer(rclcpp::Node::SharedPtr & node) : node_(node), handler_group_( async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::not_found)) @@ -62,13 +34,13 @@ WebVideoServer::WebVideoServer(rclcpp::Node::SharedPtr & node) : node_->declare_parameter("default_stream_type", "mjpeg"); node_->get_parameter("port", port_); - node_->get_parameter("verbose", __verbose); + node_->get_parameter("verbose", verbose_); node_->get_parameter("address", address_); int server_threads; node_->get_parameter("server_threads", server_threads); node_->get_parameter("ros_threads", ros_threads_); node_->get_parameter("publish_rate", publish_rate_); - node_->get_parameter("default_stream_type", __default_stream_type); + node_->get_parameter("default_stream_type", default_stream_type_); stream_types_["mjpeg"] = boost::shared_ptr(new MjpegStreamerType()); stream_types_["png"] = boost::shared_ptr(new PngStreamerType()); @@ -87,7 +59,7 @@ WebVideoServer::WebVideoServer(rclcpp::Node::SharedPtr & node) : { server_.reset( new async_web_server_cpp::HttpServer(address_, boost::lexical_cast(port_), - boost::bind(ros_connection_logger, handler_group_, _1, _2, _3, _4), + boost::bind(&WebVideoServer::handle_request, this, _1, _2, _3, _4), server_threads)); } catch(boost::exception& e) @@ -140,7 +112,7 @@ void WebVideoServer::cleanup_inactive_streams() typedef std::vector >::iterator itr_type; itr_type new_end = std::partition(image_subscribers_.begin(), image_subscribers_.end(), !boost::bind(&ImageStreamer::isInactive, _1)); - if (__verbose) + if (verbose_) { for (itr_type itr = new_end; itr < image_subscribers_.end(); ++itr) { @@ -151,11 +123,31 @@ void WebVideoServer::cleanup_inactive_streams() } } +bool WebVideoServer::handle_request(const async_web_server_cpp::HttpRequest &request, + async_web_server_cpp::HttpConnectionPtr connection, const char* begin, + const char* end) +{ + if (verbose_) + { + RCLCPP_INFO(node_->get_logger(), "Handling Request: %s", request.uri.c_str()); + } + + try + { + return handler_group_(request, connection, begin, end); + } + catch (std::exception &e) + { + RCLCPP_WARN(node_->get_logger(), "Error Handling Request: %s", e.what()); + return false; + } +} + bool WebVideoServer::handle_stream(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end) { - std::string type = request.get_query_param_value_or_default("type", __default_stream_type); + std::string type = request.get_query_param_value_or_default("type", default_stream_type_); if (stream_types_.find(type) != stream_types_.end()) { std::string topic = request.get_query_param_value_or_default("topic", ""); @@ -211,7 +203,7 @@ bool WebVideoServer::handle_stream_viewer(const async_web_server_cpp::HttpReques async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end) { - std::string type = request.get_query_param_value_or_default("type", __default_stream_type); + std::string type = request.get_query_param_value_or_default("type", default_stream_type_); if (stream_types_.find(type) != stream_types_.end()) { std::string topic = request.get_query_param_value_or_default("topic", ""); From ff6a8e631baf2855469bb58af964daa7c0801af9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Wed, 2 Oct 2024 20:16:26 +0200 Subject: [PATCH 13/42] Use hpp extension for headers (#157) --- .../{h264_streamer.h => h264_streamer.hpp} | 2 +- .../{image_streamer.h => image_streamer.hpp} | 2 +- .../{jpeg_streamers.h => jpeg_streamers.hpp} | 4 ++-- .../{libav_streamer.h => libav_streamer.hpp} | 2 +- .../{multipart_stream.h => multipart_stream.hpp} | 0 .../{png_streamers.h => png_streamers.hpp} | 4 ++-- ...ssed_streamer.h => ros_compressed_streamer.hpp} | 4 ++-- include/web_video_server/{utils.h => utils.hpp} | 0 .../{vp8_streamer.h => vp8_streamer.hpp} | 2 +- .../{vp9_streamer.h => vp9_streamer.hpp} | 2 +- .../{web_video_server.h => web_video_server.hpp} | 2 +- src/h264_streamer.cpp | 2 +- src/image_streamer.cpp | 2 +- src/jpeg_streamers.cpp | 2 +- src/libav_streamer.cpp | 2 +- src/multipart_stream.cpp | 2 +- src/png_streamers.cpp | 2 +- src/ros_compressed_streamer.cpp | 2 +- src/utils.cpp | 2 +- src/vp8_streamer.cpp | 2 +- src/vp9_streamer.cpp | 2 +- src/web_video_server.cpp | 14 +++++++------- 22 files changed, 29 insertions(+), 29 deletions(-) rename include/web_video_server/{h264_streamer.h => h264_streamer.hpp} (95%) rename include/web_video_server/{image_streamer.h => image_streamer.hpp} (98%) rename include/web_video_server/{jpeg_streamers.h => jpeg_streamers.hpp} (93%) rename include/web_video_server/{libav_streamer.h => libav_streamer.hpp} (97%) rename include/web_video_server/{multipart_stream.h => multipart_stream.hpp} (100%) rename include/web_video_server/{png_streamers.h => png_streamers.hpp} (93%) rename include/web_video_server/{ros_compressed_streamer.h => ros_compressed_streamer.hpp} (93%) rename include/web_video_server/{utils.h => utils.hpp} (100%) rename include/web_video_server/{vp8_streamer.h => vp8_streamer.hpp} (98%) rename include/web_video_server/{vp9_streamer.h => vp9_streamer.hpp} (95%) rename include/web_video_server/{web_video_server.h => web_video_server.hpp} (98%) diff --git a/include/web_video_server/h264_streamer.h b/include/web_video_server/h264_streamer.hpp similarity index 95% rename from include/web_video_server/h264_streamer.h rename to include/web_video_server/h264_streamer.hpp index 223d079..50ac4e8 100644 --- a/include/web_video_server/h264_streamer.h +++ b/include/web_video_server/h264_streamer.hpp @@ -2,7 +2,7 @@ #define H264_STREAMERS_H_ #include -#include "web_video_server/libav_streamer.h" +#include "web_video_server/libav_streamer.hpp" #include "async_web_server_cpp/http_request.hpp" #include "async_web_server_cpp/http_connection.hpp" diff --git a/include/web_video_server/image_streamer.h b/include/web_video_server/image_streamer.hpp similarity index 98% rename from include/web_video_server/image_streamer.h rename to include/web_video_server/image_streamer.hpp index f0dc2ad..21a76e8 100644 --- a/include/web_video_server/image_streamer.h +++ b/include/web_video_server/image_streamer.hpp @@ -5,7 +5,7 @@ #include #include #include -#include "web_video_server/utils.h" +#include "web_video_server/utils.hpp" #include "async_web_server_cpp/http_server.hpp" #include "async_web_server_cpp/http_request.hpp" diff --git a/include/web_video_server/jpeg_streamers.h b/include/web_video_server/jpeg_streamers.hpp similarity index 93% rename from include/web_video_server/jpeg_streamers.h rename to include/web_video_server/jpeg_streamers.hpp index a79b6e2..416aa2d 100644 --- a/include/web_video_server/jpeg_streamers.h +++ b/include/web_video_server/jpeg_streamers.hpp @@ -2,10 +2,10 @@ #define JPEG_STREAMERS_H_ #include -#include "web_video_server/image_streamer.h" +#include "web_video_server/image_streamer.hpp" #include "async_web_server_cpp/http_request.hpp" #include "async_web_server_cpp/http_connection.hpp" -#include "web_video_server/multipart_stream.h" +#include "web_video_server/multipart_stream.hpp" namespace web_video_server { diff --git a/include/web_video_server/libav_streamer.h b/include/web_video_server/libav_streamer.hpp similarity index 97% rename from include/web_video_server/libav_streamer.h rename to include/web_video_server/libav_streamer.hpp index 9bb0012..c856a7a 100644 --- a/include/web_video_server/libav_streamer.h +++ b/include/web_video_server/libav_streamer.hpp @@ -2,7 +2,7 @@ #define LIBAV_STREAMERS_H_ #include -#include "web_video_server/image_streamer.h" +#include "web_video_server/image_streamer.hpp" #include "async_web_server_cpp/http_request.hpp" #include "async_web_server_cpp/http_connection.hpp" diff --git a/include/web_video_server/multipart_stream.h b/include/web_video_server/multipart_stream.hpp similarity index 100% rename from include/web_video_server/multipart_stream.h rename to include/web_video_server/multipart_stream.hpp diff --git a/include/web_video_server/png_streamers.h b/include/web_video_server/png_streamers.hpp similarity index 93% rename from include/web_video_server/png_streamers.h rename to include/web_video_server/png_streamers.hpp index 56fdcec..d3fdb8f 100644 --- a/include/web_video_server/png_streamers.h +++ b/include/web_video_server/png_streamers.hpp @@ -2,10 +2,10 @@ #define PNG_STREAMERS_H_ #include -#include "web_video_server/image_streamer.h" +#include "web_video_server/image_streamer.hpp" #include "async_web_server_cpp/http_request.hpp" #include "async_web_server_cpp/http_connection.hpp" -#include "web_video_server/multipart_stream.h" +#include "web_video_server/multipart_stream.hpp" namespace web_video_server { diff --git a/include/web_video_server/ros_compressed_streamer.h b/include/web_video_server/ros_compressed_streamer.hpp similarity index 93% rename from include/web_video_server/ros_compressed_streamer.h rename to include/web_video_server/ros_compressed_streamer.hpp index c1f676b..9cbf6fe 100644 --- a/include/web_video_server/ros_compressed_streamer.h +++ b/include/web_video_server/ros_compressed_streamer.hpp @@ -2,10 +2,10 @@ #define ROS_COMPRESSED_STREAMERS_H_ #include -#include "web_video_server/image_streamer.h" +#include "web_video_server/image_streamer.hpp" #include "async_web_server_cpp/http_request.hpp" #include "async_web_server_cpp/http_connection.hpp" -#include "web_video_server/multipart_stream.h" +#include "web_video_server/multipart_stream.hpp" namespace web_video_server { diff --git a/include/web_video_server/utils.h b/include/web_video_server/utils.hpp similarity index 100% rename from include/web_video_server/utils.h rename to include/web_video_server/utils.hpp diff --git a/include/web_video_server/vp8_streamer.h b/include/web_video_server/vp8_streamer.hpp similarity index 98% rename from include/web_video_server/vp8_streamer.h rename to include/web_video_server/vp8_streamer.hpp index 01cd7e7..3fd0554 100644 --- a/include/web_video_server/vp8_streamer.h +++ b/include/web_video_server/vp8_streamer.hpp @@ -38,7 +38,7 @@ #define VP8_STREAMERS_H_ #include -#include "web_video_server/libav_streamer.h" +#include "web_video_server/libav_streamer.hpp" #include "async_web_server_cpp/http_request.hpp" #include "async_web_server_cpp/http_connection.hpp" diff --git a/include/web_video_server/vp9_streamer.h b/include/web_video_server/vp9_streamer.hpp similarity index 95% rename from include/web_video_server/vp9_streamer.h rename to include/web_video_server/vp9_streamer.hpp index e6d0356..91c6ac2 100644 --- a/include/web_video_server/vp9_streamer.h +++ b/include/web_video_server/vp9_streamer.hpp @@ -2,7 +2,7 @@ #define VP9_STREAMERS_H_ #include -#include "web_video_server/libav_streamer.h" +#include "web_video_server/libav_streamer.hpp" #include "async_web_server_cpp/http_request.hpp" #include "async_web_server_cpp/http_connection.hpp" diff --git a/include/web_video_server/web_video_server.h b/include/web_video_server/web_video_server.hpp similarity index 98% rename from include/web_video_server/web_video_server.h rename to include/web_video_server/web_video_server.hpp index 93b4589..19c59ca 100644 --- a/include/web_video_server/web_video_server.h +++ b/include/web_video_server/web_video_server.hpp @@ -10,7 +10,7 @@ #endif #include -#include "web_video_server/image_streamer.h" +#include "web_video_server/image_streamer.hpp" #include "async_web_server_cpp/http_server.hpp" #include "async_web_server_cpp/http_request.hpp" #include "async_web_server_cpp/http_connection.hpp" diff --git a/src/h264_streamer.cpp b/src/h264_streamer.cpp index 05a6c72..89affcc 100644 --- a/src/h264_streamer.cpp +++ b/src/h264_streamer.cpp @@ -1,4 +1,4 @@ -#include "web_video_server/h264_streamer.h" +#include "web_video_server/h264_streamer.hpp" namespace web_video_server { diff --git a/src/image_streamer.cpp b/src/image_streamer.cpp index 7dcc4cb..c06f0a6 100644 --- a/src/image_streamer.cpp +++ b/src/image_streamer.cpp @@ -1,4 +1,4 @@ -#include "web_video_server/image_streamer.h" +#include "web_video_server/image_streamer.hpp" #ifdef CV_BRIDGE_USES_OLD_HEADERS #include diff --git a/src/jpeg_streamers.cpp b/src/jpeg_streamers.cpp index c48bc4a..d20adc5 100644 --- a/src/jpeg_streamers.cpp +++ b/src/jpeg_streamers.cpp @@ -1,4 +1,4 @@ -#include "web_video_server/jpeg_streamers.h" +#include "web_video_server/jpeg_streamers.hpp" #include "async_web_server_cpp/http_reply.hpp" namespace web_video_server diff --git a/src/libav_streamer.cpp b/src/libav_streamer.cpp index 9707b31..52d348f 100644 --- a/src/libav_streamer.cpp +++ b/src/libav_streamer.cpp @@ -1,4 +1,4 @@ -#include "web_video_server/libav_streamer.h" +#include "web_video_server/libav_streamer.hpp" #include "async_web_server_cpp/http_reply.hpp" /*https://stackoverflow.com/questions/46884682/error-in-building-opencv-with-ffmpeg*/ diff --git a/src/multipart_stream.cpp b/src/multipart_stream.cpp index caf60b7..096a96b 100644 --- a/src/multipart_stream.cpp +++ b/src/multipart_stream.cpp @@ -1,4 +1,4 @@ -#include "web_video_server/multipart_stream.h" +#include "web_video_server/multipart_stream.hpp" #include "async_web_server_cpp/http_reply.hpp" namespace web_video_server diff --git a/src/png_streamers.cpp b/src/png_streamers.cpp index 8d008f1..7fbd2a5 100644 --- a/src/png_streamers.cpp +++ b/src/png_streamers.cpp @@ -1,4 +1,4 @@ -#include "web_video_server/png_streamers.h" +#include "web_video_server/png_streamers.hpp" #include "async_web_server_cpp/http_reply.hpp" namespace web_video_server diff --git a/src/ros_compressed_streamer.cpp b/src/ros_compressed_streamer.cpp index e1c89f6..1366e5a 100644 --- a/src/ros_compressed_streamer.cpp +++ b/src/ros_compressed_streamer.cpp @@ -1,4 +1,4 @@ -#include "web_video_server/ros_compressed_streamer.h" +#include "web_video_server/ros_compressed_streamer.hpp" namespace web_video_server { diff --git a/src/utils.cpp b/src/utils.cpp index 314adf5..a7623c7 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -1,4 +1,4 @@ -#include "web_video_server/utils.h" +#include "web_video_server/utils.hpp" namespace web_video_server { diff --git a/src/vp8_streamer.cpp b/src/vp8_streamer.cpp index 33fb8ee..d9cdb90 100644 --- a/src/vp8_streamer.cpp +++ b/src/vp8_streamer.cpp @@ -34,7 +34,7 @@ * *********************************************************************/ -#include "web_video_server/vp8_streamer.h" +#include "web_video_server/vp8_streamer.hpp" namespace web_video_server { diff --git a/src/vp9_streamer.cpp b/src/vp9_streamer.cpp index da8fa65..90b80a9 100644 --- a/src/vp9_streamer.cpp +++ b/src/vp9_streamer.cpp @@ -1,4 +1,4 @@ -#include "web_video_server/vp9_streamer.h" +#include "web_video_server/vp9_streamer.hpp" namespace web_video_server { diff --git a/src/web_video_server.cpp b/src/web_video_server.cpp index 11acfc3..125e92b 100644 --- a/src/web_video_server.cpp +++ b/src/web_video_server.cpp @@ -6,13 +6,13 @@ #include #include -#include "web_video_server/web_video_server.h" -#include "web_video_server/ros_compressed_streamer.h" -#include "web_video_server/jpeg_streamers.h" -#include "web_video_server/png_streamers.h" -#include "web_video_server/vp8_streamer.h" -#include "web_video_server/h264_streamer.h" -#include "web_video_server/vp9_streamer.h" +#include "web_video_server/web_video_server.hpp" +#include "web_video_server/ros_compressed_streamer.hpp" +#include "web_video_server/jpeg_streamers.hpp" +#include "web_video_server/png_streamers.hpp" +#include "web_video_server/vp8_streamer.hpp" +#include "web_video_server/h264_streamer.hpp" +#include "web_video_server/vp9_streamer.hpp" #include "async_web_server_cpp/http_reply.hpp" using namespace std::chrono_literals; From 140ff5392a23078490145869357f1b603b736b31 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Thu, 3 Oct 2024 19:17:26 +0200 Subject: [PATCH 14/42] Reformat the code with uncrustify (#158) * Reformat using ament_uncrustify * Add ament_cmake_uncrustify test * Fix ament_uncrustify errors on humble --- include/web_video_server/h264_streamer.hpp | 15 +- include/web_video_server/image_streamer.hpp | 29 ++- include/web_video_server/jpeg_streamers.hpp | 26 +- include/web_video_server/libav_streamer.hpp | 41 +-- include/web_video_server/multipart_stream.hpp | 28 ++- include/web_video_server/png_streamers.hpp | 26 +- .../ros_compressed_streamer.hpp | 19 +- include/web_video_server/vp8_streamer.hpp | 16 +- include/web_video_server/vp9_streamer.hpp | 14 +- include/web_video_server/web_video_server.hpp | 36 ++- package.xml | 2 +- src/h264_streamer.cpp | 18 +- src/image_streamer.cpp | 99 +++----- src/jpeg_streamers.cpp | 53 ++-- src/libav_streamer.cpp | 218 ++++++++-------- src/multipart_stream.cpp | 76 +++--- src/png_streamers.cpp | 53 ++-- src/ros_compressed_streamer.cpp | 70 +++--- src/utils.cpp | 17 +- src/vp8_streamer.cpp | 21 +- src/vp9_streamer.cpp | 18 +- src/web_video_server.cpp | 235 +++++++++--------- 22 files changed, 598 insertions(+), 532 deletions(-) diff --git a/include/web_video_server/h264_streamer.hpp b/include/web_video_server/h264_streamer.hpp index 50ac4e8..b5661be 100644 --- a/include/web_video_server/h264_streamer.hpp +++ b/include/web_video_server/h264_streamer.hpp @@ -12,9 +12,12 @@ namespace web_video_server class H264Streamer : public LibavStreamer { public: - H264Streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node); + H264Streamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node); ~H264Streamer(); + protected: virtual void initializeEncoder(); std::string preset_; @@ -24,12 +27,12 @@ class H264StreamerType : public LibavStreamerType { public: H264StreamerType(); - virtual boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node); + virtual boost::shared_ptr create_streamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node); }; } #endif - diff --git a/include/web_video_server/image_streamer.hpp b/include/web_video_server/image_streamer.hpp index 21a76e8..81c6029 100644 --- a/include/web_video_server/image_streamer.hpp +++ b/include/web_video_server/image_streamer.hpp @@ -15,9 +15,10 @@ namespace web_video_server class ImageStreamer { public: - ImageStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node); + ImageStreamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node); virtual void start() = 0; virtual ~ImageStreamer(); @@ -26,7 +27,6 @@ class ImageStreamer { return inactive_; } - ; /** * Restreams the last received image frame if older than max_age. @@ -37,7 +37,7 @@ class ImageStreamer { return topic_; } - ; + protected: async_web_server_cpp::HttpConnectionPtr connection_; async_web_server_cpp::HttpRequest request_; @@ -51,14 +51,16 @@ class ImageStreamer class ImageTransportImageStreamer : public ImageStreamer { public: - ImageTransportImageStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node); + ImageTransportImageStreamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node); virtual ~ImageTransportImageStreamer(); virtual void start(); protected: - virtual void sendImage(const cv::Mat &, const rclcpp::Time &time) = 0; + virtual void sendImage(const cv::Mat &, const rclcpp::Time & time) = 0; virtual void restreamFrame(double max_age); virtual void initialize(const cv::Mat &); @@ -77,17 +79,18 @@ class ImageTransportImageStreamer : public ImageStreamer image_transport::ImageTransport it_; bool initialized_; - void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr &msg); + void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg); }; class ImageStreamerType { public: - virtual boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node) = 0; + virtual boost::shared_ptr create_streamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node) = 0; - virtual std::string create_viewer(const async_web_server_cpp::HttpRequest &request) = 0; + virtual std::string create_viewer(const async_web_server_cpp::HttpRequest & request) = 0; }; } diff --git a/include/web_video_server/jpeg_streamers.hpp b/include/web_video_server/jpeg_streamers.hpp index 416aa2d..ef1b025 100644 --- a/include/web_video_server/jpeg_streamers.hpp +++ b/include/web_video_server/jpeg_streamers.hpp @@ -13,11 +13,14 @@ namespace web_video_server class MjpegStreamer : public ImageTransportImageStreamer { public: - MjpegStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node); + MjpegStreamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node); ~MjpegStreamer(); + protected: - virtual void sendImage(const cv::Mat &, const rclcpp::Time &time); + virtual void sendImage(const cv::Mat &, const rclcpp::Time & time); private: MultipartStream stream_; @@ -27,20 +30,23 @@ class MjpegStreamer : public ImageTransportImageStreamer class MjpegStreamerType : public ImageStreamerType { public: - boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node); - std::string create_viewer(const async_web_server_cpp::HttpRequest &request); + boost::shared_ptr create_streamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node); + std::string create_viewer(const async_web_server_cpp::HttpRequest & request); }; class JpegSnapshotStreamer : public ImageTransportImageStreamer { public: - JpegSnapshotStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node); + JpegSnapshotStreamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node); ~JpegSnapshotStreamer(); + protected: - virtual void sendImage(const cv::Mat &, const rclcpp::Time &time); + virtual void sendImage(const cv::Mat &, const rclcpp::Time & time); private: int quality_; diff --git a/include/web_video_server/libav_streamer.hpp b/include/web_video_server/libav_streamer.hpp index c856a7a..ca62ea3 100644 --- a/include/web_video_server/libav_streamer.hpp +++ b/include/web_video_server/libav_streamer.hpp @@ -24,26 +24,28 @@ namespace web_video_server class LibavStreamer : public ImageTransportImageStreamer { public: - LibavStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node, const std::string &format_name, const std::string &codec_name, - const std::string &content_type); + LibavStreamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node, const std::string & format_name, const std::string & codec_name, + const std::string & content_type); ~LibavStreamer(); protected: virtual void initializeEncoder(); - virtual void sendImage(const cv::Mat&, const rclcpp::Time& time); - virtual void initialize(const cv::Mat&); - AVFormatContext* format_context_; - const AVCodec* codec_; - AVCodecContext* codec_context_; - AVStream* video_stream_; + virtual void sendImage(const cv::Mat &, const rclcpp::Time & time); + virtual void initialize(const cv::Mat &); + AVFormatContext * format_context_; + const AVCodec * codec_; + AVCodecContext * codec_context_; + AVStream * video_stream_; - AVDictionary* opt_; // container format options + AVDictionary * opt_; // container format options private: - AVFrame* frame_; - struct SwsContext* sws_context_; + AVFrame * frame_; + struct SwsContext * sws_context_; rclcpp::Time first_image_timestamp_; boost::mutex encode_mutex_; @@ -55,19 +57,22 @@ class LibavStreamer : public ImageTransportImageStreamer int qmax_; int gop_; - uint8_t* io_buffer_; // custom IO buffer + uint8_t * io_buffer_; // custom IO buffer }; class LibavStreamerType : public ImageStreamerType { public: - LibavStreamerType(const std::string &format_name, const std::string &codec_name, const std::string &content_type); + LibavStreamerType( + const std::string & format_name, const std::string & codec_name, + const std::string & content_type); - boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node); + boost::shared_ptr create_streamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node); - std::string create_viewer(const async_web_server_cpp::HttpRequest &request); + std::string create_viewer(const async_web_server_cpp::HttpRequest & request); private: const std::string format_name_; diff --git a/include/web_video_server/multipart_stream.hpp b/include/web_video_server/multipart_stream.hpp index d6ea359..80489b2 100644 --- a/include/web_video_server/multipart_stream.hpp +++ b/include/web_video_server/multipart_stream.hpp @@ -9,24 +9,30 @@ namespace web_video_server { -struct PendingFooter { +struct PendingFooter +{ rclcpp::Time timestamp; std::weak_ptr contents; }; -class MultipartStream { +class MultipartStream +{ public: - MultipartStream(std::function get_now, - async_web_server_cpp::HttpConnectionPtr& connection, - const std::string& boundry="boundarydonotcross", - std::size_t max_queue_size=1); + MultipartStream( + std::function get_now, + async_web_server_cpp::HttpConnectionPtr & connection, + const std::string & boundry = "boundarydonotcross", + std::size_t max_queue_size = 1); void sendInitialHeader(); - void sendPartHeader(const rclcpp::Time &time, const std::string& type, size_t payload_size); - void sendPartFooter(const rclcpp::Time &time); - void sendPartAndClear(const rclcpp::Time &time, const std::string& type, std::vector &data); - void sendPart(const rclcpp::Time &time, const std::string& type, const boost::asio::const_buffer &buffer, - async_web_server_cpp::HttpConnection::ResourcePtr resource); + void sendPartHeader(const rclcpp::Time & time, const std::string & type, size_t payload_size); + void sendPartFooter(const rclcpp::Time & time); + void sendPartAndClear( + const rclcpp::Time & time, const std::string & type, + std::vector & data); + void sendPart( + const rclcpp::Time & time, const std::string & type, const boost::asio::const_buffer & buffer, + async_web_server_cpp::HttpConnection::ResourcePtr resource); private: bool isBusy(); diff --git a/include/web_video_server/png_streamers.hpp b/include/web_video_server/png_streamers.hpp index d3fdb8f..20cd569 100644 --- a/include/web_video_server/png_streamers.hpp +++ b/include/web_video_server/png_streamers.hpp @@ -13,11 +13,14 @@ namespace web_video_server class PngStreamer : public ImageTransportImageStreamer { public: - PngStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node); + PngStreamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node); ~PngStreamer(); + protected: - virtual void sendImage(const cv::Mat &, const rclcpp::Time &time); + virtual void sendImage(const cv::Mat &, const rclcpp::Time & time); private: MultipartStream stream_; @@ -27,20 +30,23 @@ class PngStreamer : public ImageTransportImageStreamer class PngStreamerType : public ImageStreamerType { public: - boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node); - std::string create_viewer(const async_web_server_cpp::HttpRequest &request); + boost::shared_ptr create_streamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node); + std::string create_viewer(const async_web_server_cpp::HttpRequest & request); }; class PngSnapshotStreamer : public ImageTransportImageStreamer { public: - PngSnapshotStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node); + PngSnapshotStreamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node); ~PngSnapshotStreamer(); + protected: - virtual void sendImage(const cv::Mat &, const rclcpp::Time &time); + virtual void sendImage(const cv::Mat &, const rclcpp::Time & time); private: int quality_; diff --git a/include/web_video_server/ros_compressed_streamer.hpp b/include/web_video_server/ros_compressed_streamer.hpp index 9cbf6fe..db3886a 100644 --- a/include/web_video_server/ros_compressed_streamer.hpp +++ b/include/web_video_server/ros_compressed_streamer.hpp @@ -13,14 +13,18 @@ namespace web_video_server class RosCompressedStreamer : public ImageStreamer { public: - RosCompressedStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node); + RosCompressedStreamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node); ~RosCompressedStreamer(); virtual void start(); virtual void restreamFrame(double max_age); protected: - virtual void sendImage(const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg, const rclcpp::Time &time); + virtual void sendImage( + const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg, + const rclcpp::Time & time); private: void imageCallback(const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg); @@ -35,10 +39,11 @@ class RosCompressedStreamer : public ImageStreamer class RosCompressedStreamerType : public ImageStreamerType { public: - boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node); - std::string create_viewer(const async_web_server_cpp::HttpRequest &request); + boost::shared_ptr create_streamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node); + std::string create_viewer(const async_web_server_cpp::HttpRequest & request); }; } diff --git a/include/web_video_server/vp8_streamer.hpp b/include/web_video_server/vp8_streamer.hpp index 3fd0554..215fa75 100644 --- a/include/web_video_server/vp8_streamer.hpp +++ b/include/web_video_server/vp8_streamer.hpp @@ -48,11 +48,15 @@ namespace web_video_server class Vp8Streamer : public LibavStreamer { public: - Vp8Streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node); + Vp8Streamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node); ~Vp8Streamer(); + protected: virtual void initializeEncoder(); + private: std::string quality_; }; @@ -61,12 +65,12 @@ class Vp8StreamerType : public LibavStreamerType { public: Vp8StreamerType(); - virtual boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node); + virtual boost::shared_ptr create_streamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node); }; } #endif - diff --git a/include/web_video_server/vp9_streamer.hpp b/include/web_video_server/vp9_streamer.hpp index 91c6ac2..19208cc 100644 --- a/include/web_video_server/vp9_streamer.hpp +++ b/include/web_video_server/vp9_streamer.hpp @@ -12,9 +12,12 @@ namespace web_video_server class Vp9Streamer : public LibavStreamer { public: - Vp9Streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node); + Vp9Streamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node); ~Vp9Streamer(); + protected: virtual void initializeEncoder(); }; @@ -23,9 +26,10 @@ class Vp9StreamerType : public LibavStreamerType { public: Vp9StreamerType(); - virtual boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node); + virtual boost::shared_ptr create_streamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node); }; } diff --git a/include/web_video_server/web_video_server.hpp b/include/web_video_server/web_video_server.hpp index 19c59ca..a0a5be9 100644 --- a/include/web_video_server/web_video_server.hpp +++ b/include/web_video_server/web_video_server.hpp @@ -29,7 +29,7 @@ class WebVideoServer * @brief Constructor * @return */ - WebVideoServer(rclcpp::Node::SharedPtr &node); + WebVideoServer(rclcpp::Node::SharedPtr & node); /** * @brief Destructor - Cleans up @@ -43,20 +43,30 @@ class WebVideoServer void setup_cleanup_inactive_streams(); - bool handle_request(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end); + bool handle_request( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + const char * begin, const char * end); - bool handle_stream(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end); + bool handle_stream( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + const char * begin, const char * end); - bool handle_stream_viewer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end); + bool handle_stream_viewer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + const char * begin, const char * end); - bool handle_snapshot(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end); + bool handle_snapshot( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + const char * begin, const char * end); - bool handle_list_streams(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end); + bool handle_list_streams( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + const char * begin, const char * end); private: void restreamFrames(double max_age); @@ -76,8 +86,8 @@ class WebVideoServer boost::shared_ptr server_; async_web_server_cpp::HttpRequestHandlerGroup handler_group_; - std::vector > image_subscribers_; - std::map > stream_types_; + std::vector> image_subscribers_; + std::map> stream_types_; boost::mutex subscriber_mutex_; }; diff --git a/package.xml b/package.xml index 59ad8c5..f4f6c1c 100644 --- a/package.xml +++ b/package.xml @@ -34,7 +34,7 @@ ament_lint_auto ament_cmake_lint_cmake ament_cmake_xmllint - + ament_cmake_uncrustify ament_cmake diff --git a/src/h264_streamer.cpp b/src/h264_streamer.cpp index 89affcc..7a889ec 100644 --- a/src/h264_streamer.cpp +++ b/src/h264_streamer.cpp @@ -3,9 +3,10 @@ namespace web_video_server { -H264Streamer::H264Streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : - LibavStreamer(request, connection, node, "mp4", "libx264", "video/mp4") +H264Streamer::H264Streamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) +: LibavStreamer(request, connection, node, "mp4", "libx264", "video/mp4") { /* possible quality presets: * ultrafast, superfast, veryfast, faster, fast, medium, slow, slower, veryslow, placebo @@ -34,14 +35,15 @@ void H264Streamer::initializeEncoder() } } -H264StreamerType::H264StreamerType() : - LibavStreamerType("mp4", "libx264", "video/mp4") +H264StreamerType::H264StreamerType() +: LibavStreamerType("mp4", "libx264", "video/mp4") { } -boost::shared_ptr H264StreamerType::create_streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node) +boost::shared_ptr H264StreamerType::create_streamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node) { return boost::shared_ptr(new H264Streamer(request, connection, node)); } diff --git a/src/image_streamer.cpp b/src/image_streamer.cpp index c06f0a6..1347759 100644 --- a/src/image_streamer.cpp +++ b/src/image_streamer.cpp @@ -11,9 +11,10 @@ namespace web_video_server { -ImageStreamer::ImageStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : - request_(request), connection_(connection), node_(node), inactive_(false) +ImageStreamer::ImageStreamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) +: request_(request), connection_(connection), node_(node), inactive_(false) { topic_ = request.get_query_param_value_or_default("topic", ""); } @@ -22,9 +23,10 @@ ImageStreamer::~ImageStreamer() { } -ImageTransportImageStreamer::ImageTransportImageStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : - ImageStreamer(request, connection, node), it_(node), initialized_(false) +ImageTransportImageStreamer::ImageTransportImageStreamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) +: ImageStreamer(request, connection, node), it_(node), initialized_(false) { output_width_ = request.get_query_param_value_or_default("width", -1); output_height_ = request.get_query_param_value_or_default("height", -1); @@ -48,14 +50,16 @@ void ImageTransportImageStreamer::start() continue; } auto & topic_name = topic_and_types.first; - if(topic_name == topic_ || (topic_name.find("/") == 0 && topic_name.substr(1) == topic_)){ + if (topic_name == topic_ || (topic_name.find("/") == 0 && topic_name.substr(1) == topic_)) { inactive_ = false; break; } } // Get QoS profile from query parameter - RCLCPP_INFO(node_->get_logger(), "Streaming topic %s with QoS profile %s", topic_.c_str(), qos_profile_name_.c_str()); + RCLCPP_INFO( + node_->get_logger(), "Streaming topic %s with QoS profile %s", topic_.c_str(), + qos_profile_name_.c_str()); auto qos_profile = get_qos_profile_from_name(qos_profile_name_); if (!qos_profile) { qos_profile = rmw_qos_profile_default; @@ -67,8 +71,9 @@ void ImageTransportImageStreamer::start() // Create subscriber image_sub_ = image_transport::create_subscription( - node_.get(), topic_, std::bind(&ImageTransportImageStreamer::imageCallback, this, std::placeholders::_1), - default_transport_, qos_profile.value()); + node_.get(), topic_, + std::bind(&ImageTransportImageStreamer::imageCallback, this, std::placeholders::_1), + default_transport_, qos_profile.value()); } void ImageTransportImageStreamer::initialize(const cv::Mat &) @@ -77,30 +82,25 @@ void ImageTransportImageStreamer::initialize(const cv::Mat &) void ImageTransportImageStreamer::restreamFrame(double max_age) { - if (inactive_ || !initialized_ ) + if (inactive_ || !initialized_) { return; + } try { - if ( last_frame + rclcpp::Duration::from_seconds(max_age) < node_->now() ) { + if (last_frame + rclcpp::Duration::from_seconds(max_age) < node_->now() ) { boost::mutex::scoped_lock lock(send_mutex_); sendImage(output_size_image, node_->now() ); // don't update last_frame, it may remain an old value. } - } - catch (boost::system::system_error &e) - { + } catch (boost::system::system_error & e) { // happens when client disconnects RCLCPP_DEBUG(node_->get_logger(), "system_error exception: %s", e.what()); inactive_ = true; return; - } - catch (std::exception &e) - { + } catch (std::exception & e) { // TODO THROTTLE with 30 RCLCPP_ERROR(node_->get_logger(), "exception: %s", e.what()); inactive_ = true; return; - } - catch (...) - { + } catch (...) { // TODO THROTTLE with 30 RCLCPP_ERROR(node_->get_logger(), "exception"); inactive_ = true; @@ -108,30 +108,26 @@ void ImageTransportImageStreamer::restreamFrame(double max_age) } } -void ImageTransportImageStreamer::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr &msg) +void ImageTransportImageStreamer::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg) { - if (inactive_) + if (inactive_) { return; + } cv::Mat img; - try - { - if (msg->encoding.find("F") != std::string::npos) - { + try { + if (msg->encoding.find("F") != std::string::npos) { // scale floating point images cv::Mat float_image_bridge = cv_bridge::toCvCopy(msg, msg->encoding)->image; cv::Mat_ float_image = float_image_bridge; double max_val; cv::minMaxIdx(float_image, 0, &max_val); - if (max_val > 0) - { + if (max_val > 0) { float_image *= (255 / max_val); } img = float_image; - } - else - { + } else { // Convert to OpenCV native BGR color img = cv_bridge::toCvCopy(msg, "bgr8")->image; } @@ -139,70 +135,57 @@ void ImageTransportImageStreamer::imageCallback(const sensor_msgs::msg::Image::C int input_width = img.cols; int input_height = img.rows; - if (output_width_ == -1) + if (output_width_ == -1) { output_width_ = input_width; - if (output_height_ == -1) + } + if (output_height_ == -1) { output_height_ = input_height; + } - if (invert_) - { + if (invert_) { // Rotate 180 degrees cv::flip(img, img, false); cv::flip(img, img, true); } boost::mutex::scoped_lock lock(send_mutex_); // protects output_size_image - if (output_width_ != input_width || output_height_ != input_height) - { + if (output_width_ != input_width || output_height_ != input_height) { cv::Mat img_resized; cv::Size new_size(output_width_, output_height_); cv::resize(img, img_resized, new_size); output_size_image = img_resized; - } - else - { + } else { output_size_image = img; } - if (!initialized_) - { + if (!initialized_) { initialize(output_size_image); initialized_ = true; } last_frame = node_->now(); sendImage(output_size_image, msg->header.stamp); - } - catch (cv_bridge::Exception &e) - { + } catch (cv_bridge::Exception & e) { // TODO THROTTLE with 30 RCLCPP_ERROR(node_->get_logger(), "cv_bridge exception: %s", e.what()); inactive_ = true; return; - } - catch (cv::Exception &e) - { + } catch (cv::Exception & e) { // TODO THROTTLE with 30 RCLCPP_ERROR(node_->get_logger(), "cv_bridge exception: %s", e.what()); inactive_ = true; return; - } - catch (boost::system::system_error &e) - { + } catch (boost::system::system_error & e) { // happens when client disconnects RCLCPP_DEBUG(node_->get_logger(), "system_error exception: %s", e.what()); inactive_ = true; return; - } - catch (std::exception &e) - { + } catch (std::exception & e) { // TODO THROTTLE with 30 RCLCPP_ERROR(node_->get_logger(), "exception: %s", e.what()); inactive_ = true; return; - } - catch (...) - { + } catch (...) { // TODO THROTTLE with 30 RCLCPP_ERROR(node_->get_logger(), "exception"); inactive_ = true; diff --git a/src/jpeg_streamers.cpp b/src/jpeg_streamers.cpp index d20adc5..0187991 100644 --- a/src/jpeg_streamers.cpp +++ b/src/jpeg_streamers.cpp @@ -4,9 +4,11 @@ namespace web_video_server { -MjpegStreamer::MjpegStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) : - ImageTransportImageStreamer(request, connection, node), stream_(std::bind(&rclcpp::Node::now, node), connection) +MjpegStreamer::MjpegStreamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) +: ImageTransportImageStreamer(request, connection, node), + stream_(std::bind(&rclcpp::Node::now, node), connection) { quality_ = request.get_query_param_value_or_default("quality", 95); stream_.sendInitialHeader(); @@ -18,7 +20,7 @@ MjpegStreamer::~MjpegStreamer() boost::mutex::scoped_lock lock(send_mutex_); // protects sendImage. } -void MjpegStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) +void MjpegStreamer::sendImage(const cv::Mat & img, const rclcpp::Time & time) { std::vector encode_params; encode_params.push_back(cv::IMWRITE_JPEG_QUALITY); @@ -30,14 +32,15 @@ void MjpegStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) stream_.sendPartAndClear(time, "image/jpeg", encoded_buffer); } -boost::shared_ptr MjpegStreamerType::create_streamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node) +boost::shared_ptr MjpegStreamerType::create_streamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node) { return boost::shared_ptr(new MjpegStreamer(request, connection, node)); } -std::string MjpegStreamerType::create_viewer(const async_web_server_cpp::HttpRequest &request) +std::string MjpegStreamerType::create_viewer(const async_web_server_cpp::HttpRequest & request) { std::stringstream ss; ss << "("quality", 95); } @@ -60,7 +64,7 @@ JpegSnapshotStreamer::~JpegSnapshotStreamer() boost::mutex::scoped_lock lock(send_mutex_); // protects sendImage. } -void JpegSnapshotStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) +void JpegSnapshotStreamer::sendImage(const cv::Mat & img, const rclcpp::Time & time) { std::vector encode_params; encode_params.push_back(cv::IMWRITE_JPEG_QUALITY); @@ -72,18 +76,17 @@ void JpegSnapshotStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &tim char stamp[20]; sprintf(stamp, "%.06lf", time.seconds()); async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok) - .header("Connection", "close") - .header("Server", "web_video_server") - .header("Cache-Control", - "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, " - "max-age=0") - .header("X-Timestamp", stamp) - .header("Pragma", "no-cache") - .header("Content-type", "image/jpeg") - .header("Access-Control-Allow-Origin", "*") - .header("Content-Length", - boost::lexical_cast(encoded_buffer.size())) - .write(connection_); + .header("Connection", "close") + .header("Server", "web_video_server") + .header( + "Cache-Control", + "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0") + .header("X-Timestamp", stamp) + .header("Pragma", "no-cache") + .header("Content-type", "image/jpeg") + .header("Access-Control-Allow-Origin", "*") + .header("Content-Length", boost::lexical_cast(encoded_buffer.size())) + .write(connection_); connection_->write_and_clear(encoded_buffer); inactive_ = true; } diff --git a/src/libav_streamer.cpp b/src/libav_streamer.cpp index 52d348f..22fc2af 100644 --- a/src/libav_streamer.cpp +++ b/src/libav_streamer.cpp @@ -8,15 +8,16 @@ namespace web_video_server { -LibavStreamer::LibavStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node, - const std::string &format_name, const std::string &codec_name, - const std::string &content_type) : - ImageTransportImageStreamer(request, connection, node), format_context_(0), codec_(0), codec_context_(0), video_stream_( - 0), frame_(0), sws_context_(0), first_image_timestamp_(0), format_name_( - format_name), codec_name_(codec_name), content_type_(content_type), opt_(0), io_buffer_(0) +LibavStreamer::LibavStreamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node, + const std::string & format_name, const std::string & codec_name, + const std::string & content_type) +: ImageTransportImageStreamer(request, connection, node), format_context_(0), codec_(0), + codec_context_(0), video_stream_(0), frame_(0), sws_context_(0), first_image_timestamp_(0), + format_name_(format_name), codec_name_(codec_name), content_type_(content_type), opt_(0), + io_buffer_(0) { - bitrate_ = request.get_query_param_value_or_default("bitrate", 100000); qmin_ = request.get_query_param_value_or_default("qmin", 10); qmax_ = request.get_query_param_value_or_default("qmax", 42); @@ -25,65 +26,63 @@ LibavStreamer::LibavStreamer(const async_web_server_cpp::HttpRequest &request, LibavStreamer::~LibavStreamer() { - if (codec_context_) - { + if (codec_context_) { avcodec_free_context(&codec_context_); } - if (frame_) - { + if (frame_) { av_frame_free(&frame_); } - if (io_buffer_) + if (io_buffer_) { delete io_buffer_; + } if (format_context_) { - if (format_context_->pb) + if (format_context_->pb) { av_free(format_context_->pb); + } avformat_free_context(format_context_); } - if (sws_context_) + if (sws_context_) { sws_freeContext(sws_context_); + } } // output callback for ffmpeg IO context -static int dispatch_output_packet(void* opaque, uint8_t* buffer, int buffer_size) +static int dispatch_output_packet(void * opaque, uint8_t * buffer, int buffer_size) { - async_web_server_cpp::HttpConnectionPtr connection = *((async_web_server_cpp::HttpConnectionPtr*) opaque); + async_web_server_cpp::HttpConnectionPtr connection = + *((async_web_server_cpp::HttpConnectionPtr *) opaque); std::vector encoded_frame; encoded_frame.assign(buffer, buffer + buffer_size); connection->write_and_clear(encoded_frame); return 0; // TODO: can this fail? } -void LibavStreamer::initialize(const cv::Mat &img) +void LibavStreamer::initialize(const cv::Mat & img) { // Load format format_context_ = avformat_alloc_context(); - if (!format_context_) - { - async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_, - connection_, - NULL, NULL); + if (!format_context_) { + async_web_server_cpp::HttpReply::stock_reply( + async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL); throw std::runtime_error("Error allocating ffmpeg format context"); } format_context_->oformat = av_guess_format(format_name_.c_str(), NULL, NULL); - if (!format_context_->oformat) - { - async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_, - connection_, - NULL, NULL); + if (!format_context_->oformat) { + async_web_server_cpp::HttpReply::stock_reply( + async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL); throw std::runtime_error("Error looking up output format"); } // Set up custom IO callback. size_t io_buffer_size = 3 * 1024; // 3M seen elsewhere and adjudged good io_buffer_ = new unsigned char[io_buffer_size]; - AVIOContext* io_ctx = avio_alloc_context(io_buffer_, io_buffer_size, AVIO_FLAG_WRITE, &connection_, NULL, dispatch_output_packet, NULL); - if (!io_ctx) - { - async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_, - connection_, - NULL, NULL); + AVIOContext * io_ctx = avio_alloc_context( + io_buffer_, io_buffer_size, AVIO_FLAG_WRITE, + &connection_, NULL, dispatch_output_packet, NULL); + if (!io_ctx) { + async_web_server_cpp::HttpReply::stock_reply( + async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL); throw std::runtime_error("Error setting up IO context"); } io_ctx->seekable = 0; // no seeking, it's a stream @@ -91,27 +90,24 @@ void LibavStreamer::initialize(const cv::Mat &img) format_context_->max_interleave_delta = 0; // Load codec - if (codec_name_.empty()) // use default codec if none specified + if (codec_name_.empty()) { // use default codec if none specified codec_ = avcodec_find_encoder(format_context_->oformat->video_codec); - else + } else { codec_ = avcodec_find_encoder_by_name(codec_name_.c_str()); - if (!codec_) - { - async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_, - connection_, - NULL, NULL); + } + if (!codec_) { + async_web_server_cpp::HttpReply::stock_reply( + async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL); throw std::runtime_error("Error looking up codec"); } video_stream_ = avformat_new_stream(format_context_, codec_); - if (!video_stream_) - { - async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_, - connection_, - NULL, NULL); + if (!video_stream_) { + async_web_server_cpp::HttpReply::stock_reply( + async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL); throw std::runtime_error("Error creating video stream"); } - codec_context_ = avcodec_alloc_context3(codec_); + codec_context_ = avcodec_alloc_context3(codec_); // Set options codec_context_->codec_id = codec_->id; @@ -141,19 +137,18 @@ void LibavStreamer::initialize(const cv::Mat &img) avcodec_parameters_from_context(video_stream_->codecpar, codec_context_); // Open Codec - if (avcodec_open2(codec_context_, codec_, NULL) < 0) - { - async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_, - connection_, - NULL, NULL); + if (avcodec_open2(codec_context_, codec_, NULL) < 0) { + async_web_server_cpp::HttpReply::stock_reply( + async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL); throw std::runtime_error("Could not open video codec"); } // Allocate frame buffers frame_ = av_frame_alloc(); - av_image_alloc(frame_->data, frame_->linesize, output_width_, output_height_, - codec_context_->pix_fmt, 1); + av_image_alloc( + frame_->data, frame_->linesize, output_width_, output_height_, + codec_context_->pix_fmt, 1); frame_->width = output_width_; frame_->height = output_height_; @@ -164,18 +159,24 @@ void LibavStreamer::initialize(const cv::Mat &img) av_dict_set(&format_context_->metadata, "title", topic_.c_str(), 0); // Send response headers - async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok).header("Connection", "close").header( - "Server", "web_video_server").header("Cache-Control", - "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0").header( - "Pragma", "no-cache").header("Expires", "0").header("Max-Age", "0").header("Trailer", "Expires").header( - "Content-type", content_type_).header("Access-Control-Allow-Origin", "*").write(connection_); + async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok) + .header("Connection", "close") + .header("Server", "web_video_server") + .header( + "Cache-Control", + "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0") + .header("Pragma", "no-cache") + .header("Expires", "0") + .header("Max-Age", "0") + .header("Trailer", "Expires") + .header("Content-type", content_type_) + .header("Access-Control-Allow-Origin", "*") + .write(connection_); // Send video stream header - if (avformat_write_header(format_context_, &opt_) < 0) - { - async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_, - connection_, - NULL, NULL); + if (avformat_write_header(format_context_, &opt_) < 0) { + async_web_server_cpp::HttpReply::stock_reply( + async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL); throw std::runtime_error("Error openning dynamic buffer"); } } @@ -184,87 +185,80 @@ void LibavStreamer::initializeEncoder() { } -void LibavStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) +void LibavStreamer::sendImage(const cv::Mat & img, const rclcpp::Time & time) { boost::mutex::scoped_lock lock(encode_mutex_); - if (0 == first_image_timestamp_.nanoseconds()) - { + if (0 == first_image_timestamp_.nanoseconds()) { first_image_timestamp_ = time; } AVPixelFormat input_coding_format = AV_PIX_FMT_BGR24; - AVFrame *raw_frame = av_frame_alloc(); - av_image_fill_arrays(raw_frame->data, raw_frame->linesize, - img.data, input_coding_format, output_width_, output_height_, 1); + AVFrame * raw_frame = av_frame_alloc(); + av_image_fill_arrays( + raw_frame->data, raw_frame->linesize, + img.data, input_coding_format, output_width_, output_height_, 1); // Convert from opencv to libav - if (!sws_context_) - { + if (!sws_context_) { static int sws_flags = SWS_BICUBIC; - sws_context_ = sws_getContext(output_width_, output_height_, input_coding_format, output_width_, output_height_, - codec_context_->pix_fmt, sws_flags, NULL, NULL, NULL); - if (!sws_context_) - { + sws_context_ = sws_getContext( + output_width_, output_height_, input_coding_format, output_width_, + output_height_, codec_context_->pix_fmt, sws_flags, NULL, NULL, NULL); + if (!sws_context_) { throw std::runtime_error("Could not initialize the conversion context"); } } - int ret = sws_scale(sws_context_, - (const uint8_t * const *)raw_frame->data, raw_frame->linesize, 0, - output_height_, frame_->data, frame_->linesize); + int ret = sws_scale( + sws_context_, + (const uint8_t * const *)raw_frame->data, raw_frame->linesize, 0, + output_height_, frame_->data, frame_->linesize); av_frame_free(&raw_frame); // Encode the frame - AVPacket* pkt = av_packet_alloc(); + AVPacket * pkt = av_packet_alloc(); ret = avcodec_send_frame(codec_context_, frame_); - if (ret == AVERROR_EOF) - { - RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_send_frame() encoder flushed\n"); + if (ret == AVERROR_EOF) { + RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_send_frame() encoder flushed\n"); + } else if (ret == AVERROR(EAGAIN)) { + RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_send_frame() need output read out\n"); } - else if (ret == AVERROR(EAGAIN)) - { - RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_send_frame() need output read out\n"); - } - if (ret < 0) - { + if (ret < 0) { throw std::runtime_error("Error encoding video frame"); } ret = avcodec_receive_packet(codec_context_, pkt); bool got_packet = pkt->size > 0; - if (ret == AVERROR_EOF) - { - RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_receive_packet() encoder flushed\n"); - } - else if (ret == AVERROR(EAGAIN)) - { - RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_receive_packet() needs more input\n"); + if (ret == AVERROR_EOF) { + RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_receive_packet() encoder flushed\n"); + } else if (ret == AVERROR(EAGAIN)) { + RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_receive_packet() needs more input\n"); got_packet = false; } - if (got_packet) - { + if (got_packet) { std::size_t size; - uint8_t *output_buf; + uint8_t * output_buf; double seconds = (time - first_image_timestamp_).seconds(); // Encode video at 1/0.95 to minimize delay pkt->pts = (int64_t)(seconds / av_q2d(video_stream_->time_base) * 0.95); - if (pkt->pts <= 0) + if (pkt->pts <= 0) { pkt->pts = 1; + } pkt->dts = pkt->pts; - if (pkt->flags&AV_PKT_FLAG_KEY) + if (pkt->flags & AV_PKT_FLAG_KEY) { pkt->flags |= AV_PKT_FLAG_KEY; + } pkt->stream_index = video_stream_->index; - if (av_write_frame(format_context_, pkt)) - { + if (av_write_frame(format_context_, pkt)) { throw std::runtime_error("Error when writing frame"); } } @@ -272,21 +266,23 @@ void LibavStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) av_packet_unref(pkt); } -LibavStreamerType::LibavStreamerType(const std::string &format_name, const std::string &codec_name, - const std::string &content_type) : - format_name_(format_name), codec_name_(codec_name), content_type_(content_type) +LibavStreamerType::LibavStreamerType( + const std::string & format_name, const std::string & codec_name, + const std::string & content_type) +: format_name_(format_name), codec_name_(codec_name), content_type_(content_type) { } -boost::shared_ptr LibavStreamerType::create_streamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr node) +boost::shared_ptr LibavStreamerType::create_streamer( + const async_web_server_cpp::HttpRequest & request, + async_web_server_cpp::HttpConnectionPtr connection, + rclcpp::Node::SharedPtr node) { return boost::shared_ptr( - new LibavStreamer(request, connection, node, format_name_, codec_name_, content_type_)); + new LibavStreamer(request, connection, node, format_name_, codec_name_, content_type_)); } -std::string LibavStreamerType::create_viewer(const async_web_server_cpp::HttpRequest &request) +std::string LibavStreamerType::create_viewer(const async_web_server_cpp::HttpRequest & request) { std::stringstream ss; ss << "