├── .github └── workflows │ └── ci.yml ├── .gitignore ├── AUTHORS.md ├── CHANGELOG.rst ├── CMakeLists.txt ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── include └── web_video_server │ ├── h264_streamer.hpp │ ├── image_streamer.hpp │ ├── jpeg_streamers.hpp │ ├── libav_streamer.hpp │ ├── multipart_stream.hpp │ ├── png_streamers.hpp │ ├── ros_compressed_streamer.hpp │ ├── utils.hpp │ ├── vp8_streamer.hpp │ ├── vp9_streamer.hpp │ └── web_video_server.hpp ├── mainpage.dox ├── package.xml └── src ├── h264_streamer.cpp ├── image_streamer.cpp ├── jpeg_streamers.cpp ├── libav_streamer.cpp ├── multipart_stream.cpp ├── png_streamers.cpp ├── ros_compressed_streamer.cpp ├── utils.cpp ├── vp8_streamer.cpp ├── vp9_streamer.cpp ├── web_video_server.cpp └── web_video_server_node.cpp /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: 4 | workflow_dispatch: 5 | push: 6 | branches-ignore: 7 | - ros1* 8 | pull_request: 9 | branches-ignore: 10 | - ros1* 11 | 12 | jobs: 13 | industrial_ci: 14 | runs-on: ubuntu-latest 15 | strategy: 16 | fail-fast: false 17 | matrix: 18 | ROS_DISTRO: [humble, jazzy, kilted, rolling] 19 | ROS_REPO: [testing, main] 20 | env: 21 | ROS_DISTRO: ${{ matrix.ROS_DISTRO }} 22 | ROS_REPO: ${{ matrix.ROS_REPO }} 23 | steps: 24 | - name: Checkout repo 25 | uses: actions/checkout@v4 26 | - name: Source tests 27 | uses: "ros-industrial/industrial_ci@master" 28 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .cproject 2 | .project 3 | -------------------------------------------------------------------------------- /AUTHORS.md: -------------------------------------------------------------------------------- 1 | Original Authors 2 | ---------------- 3 | 4 | * Mitchell Wills (mwills@wpi.edu) 5 | 6 | Contributors 7 | ------------ 8 | 9 | * [Russell Toris](http://users.wpi.edu/~rctoris/) (rctoris@wpi.edu) 10 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package web_video_server 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.1.0 (2025-05-21) 6 | ------------------ 7 | * Use target_link_libraries instead of ament_target_dependencies (#182) 8 | * Fix compile warnings (#176) 9 | * Use chrono steady clock for frame timing (#173) 10 | * Add pkg-config dependency (#175) 11 | * Separate web_video_server into a component and an executable (#168) 12 | * Contributors: Błażej Sowa, Fabian Freihube, Joe Dinius, Ph.D., Lars Lorentz Ludvigsen, Mat198 13 | 14 | 2.0.1 (2024-10-26) 15 | ------------------ 16 | * Add ros_environment to test dependencies (#166) 17 | * Contributors: Błażej Sowa 18 | 19 | 2.0.0 (2024-10-11) 20 | ------------------ 21 | * Replace boost with std (#164) 22 | * Add ament_cpplint test, resolve TODOs (#162) 23 | * Add license headers to all C++ source files, update copyrights (#161) 24 | * Add support for alpha pngs by adding per stream type decode functions (backport #106) (#163) 25 | * Add link to /stream in stream list (backport #118) (#160) 26 | * Add support for jpg compression format (backport #142) (#159) 27 | * Reformat the code with uncrustify (#158) 28 | * Use hpp extension for headers (#157) 29 | * Fix request logging, remove global parameters (#156) 30 | * Replace nh with node (#155) 31 | * Fix declaring and retrieving node parameters (#154) 32 | * Fix usage of deprecated libavcodec functions (#150) 33 | * Use cv_bridge hpp headers when available (#149) 34 | * Use target_link_libraries instead of ament_target_dependencies where applicable 35 | * Don't install headers 36 | * Add CI workflow and ament_lint tests (#148) 37 | * Update package maintainer 38 | * allow topic searches to continue past invalid multi-type topics. (#146) 39 | * Add QoS profile query parameters (#133) 40 | * Fix build for ROS2 Humble (#129) 41 | * Fix build for ROS2 Foxy (#111) 42 | * Contributors: Błażej Sowa, Domenic Rodriguez, Robert Brothers, Sebastian Castro, Tina Tian, TobinHall, Matthew Bries 43 | 44 | 1.0.0 (2019-09-20) 45 | ------------------ 46 | * Port to ROS 2 47 | 48 | 0.2.1 (2019-06-05) 49 | ------------------ 50 | * Restream buffered frames with minimum publish rate (`#88 `_) 51 | * Restream buffered frames with minimum publish rate 52 | * Implement restreaming for ros_compressed_streamer 53 | * Update travis config (`#89 `_) 54 | * Fall back to mjpeg if ros_compressed is unavailable (`#87 `_) 55 | * Contributors: Jihoon Lee, Viktor Kunovski, sfalexrog 56 | 57 | 0.2.0 (2019-01-30) 58 | ------------------ 59 | * Add "default_stream_type" parameter (`#84 `_) 60 | This allows users to specify default stream type in their .launch files. Using a "ros_compressed" stream type sometimes 61 | results in a much lower resource consumption, and having it set as a default is much nicer for end users. 62 | * Add a workaround for MultipartStream constant busy state (`#83 `_) 63 | * Add a workaround for MultipartStream constant busy state 64 | * Remove C++11 features 65 | * lax rule for topic name (`#77 `_) 66 | * Add PngStreamer (`#74 `_) 67 | * fix SteadyTimer check for backported ROS versions (`#71 `_) 68 | i.e. on current kinetic 69 | * Pkg format 2 (`#68 `_) 70 | * use package format 2 71 | * add missing dependency on sensor_msgs 72 | * fixed undeclared CODEC_FLAG_GLOBAL_HEADER (`#65 `_) 73 | * Contributors: Andreas Klintberg, Dirk Thomas, Felix Ruess, Kazuto Murase, Viktor Kunovski, sfalexrog 74 | 75 | 0.1.0 (2018-07-01) 76 | ------------------ 77 | * Avoid queuing of images on slow ethernet connection (`#64 `_) 78 | * use SteadyTimer (if available) for cleaning up inactive streams (`#63 `_) 79 | * use SteadyTimer for cleaning up inactive streams 80 | so that cleanup works correctly even if system time changes 81 | SteadyTimer is available since roscpp 1.13.1 82 | * possibility to use SteadyTimer on older ROS versions 83 | when SteadyTimer has been backported to those... 84 | * Fix segfault in libav_streamer destructor (resolves `#59 `_) (`#60 `_) 85 | * Fix vp8 in kinetic add vp9 and h264 support (`#52 `_) 86 | * fix vp8 in kinetic 87 | * add h264 and vp9 support 88 | * Add Indigo test matrix in travis configuration (`#50 `_) 89 | * Set image streamer as inactive if topic is not available (`#53 `_) 90 | * Resolves `#38 `_ 91 | * Fix Build for ubuntu 14.04 (`#48 `_) 92 | * fix issue `#47 `_ 93 | * fix double free 94 | * Revert "use SteadyTimer for cleaning up inactive streams (`#45 `_)" (`#51 `_) 95 | This reverts commit ae74f19ada22f288a7c7a99ada7a1b9b6037c7ce. 96 | * use SteadyTimer for cleaning up inactive streams (`#45 `_) 97 | so that cleanup works correctly even if system time changes 98 | * Use trusty instead of xenial. See `travis-ci/travis-ci#7260 `_ (`#49 `_) 99 | * Also see `RobotWebTools/rosbridge_suite#311 `_ 100 | * Contributors: Felix Ruess, James Bailey, Jihoon Lee, randoms, schallerr 101 | 102 | 0.0.7 (2017-11-20) 103 | ------------------ 104 | * Ffmpeg 3 (`#43 `_) 105 | * Correct use of deprecated parameters 106 | codec_context\_->rc_buffer_aggressivity marked as "currently useless", so removed 107 | codec_context\_->frame_skip_threshold access through new priv_data api 108 | * New names for pixel formats 109 | * AVPicture is deprecated, use AVFrame 110 | * Switch to non-deprecated free functions 111 | * Use new encoding api for newer versions 112 | * codec_context is deprecated, use packet flags 113 | * Update travis configuration to test against kinetic (`#44 `_) 114 | * fixed misuse of remove_if (`#35 `_) 115 | * Merge pull request `#33 `_ from achim-k/patch-1 116 | web_video_server: fix bool function not returning 117 | This fix is required when compiling the package with `clang`. Otherwise a SIGILL (Illegal instruction) is triggered. 118 | * Contributors: Hans-Joachim Krauch, Jan, Jihoon Lee, russelhowe 119 | 120 | 0.0.6 (2017-01-17) 121 | ------------------ 122 | * Fixed topic list to display all image topics, fixing Issue `#18 `_. 123 | * Contributors: Eric 124 | 125 | 0.0.5 (2016-10-13) 126 | ------------------ 127 | * Merge pull request `#23 `_ from iki-wgt/develop 128 | More information when server creation is failed 129 | * Removed empty line 130 | * More detailed exception message 131 | Programm behavior is not changed since the exception is rethrown. 132 | * Contributors: BennyRe, Russell Toris 133 | 134 | 0.0.4 (2015-08-18) 135 | ------------------ 136 | * Merge pull request #16 from mitchellwills/compressed 137 | Adds support for streaming ROS compressed image topics without the need to reencode them 138 | * Switch to checkout async_web_server_cpp from source 139 | * Upgrade for change in signature of async_web_server_cpp request handler 140 | * Added ros compressed video streamer type 141 | This directly passes the ros compressed frame data to the http socket without reencoding it 142 | * Switched from passing image transport to passing node handle to streamer constructors 143 | * Added default transport parameter for regular image streamers 144 | * Contributors: Mitchell Wills, Russell Toris 145 | 146 | 0.0.3 (2015-05-07) 147 | ------------------ 148 | * added verbose flag 149 | * Contributors: Russell Toris 150 | 151 | 0.0.2 (2015-02-20) 152 | ------------------ 153 | * Merge pull request #10 from mitchellwills/develop 154 | Added option to specify server address 155 | * Added option to specify server address 156 | * Merge pull request #3 from mitchellwills/develop 157 | Remove old http_server implementation and replace it with async_web_server_cpp package 158 | * Moved from using built in http server to new async_web_server_cpp package 159 | * Did some cleanup of streamers 160 | * Update package.xml 161 | * Contributors: Mitchell Wills, Russell Toris 162 | 163 | 0.0.1 (2014-10-30) 164 | ------------------ 165 | * missed travis file 166 | * cleanup and travis build 167 | * ROS auto-format 168 | * Merge pull request #1 from mitchellwills/develop 169 | Initial implementation of a http web server that serves ROS image topics as multiple web compatible formats 170 | * Made some changes suggested by catkin_lint and did some package cleanup 171 | * Added support for libavstreamer on Ubuntu 13.10 version of libav 172 | * Added support for specifying vp8 quality parameter 173 | * Implemented lazy initialization for libav buffers so that output size can be infered from the first image 174 | * Updated README 175 | * Added vp8 support 176 | * Broke image encodings out into different files 177 | * Made write operations async 178 | Send timestamps for mjpeg stream 179 | * Initial commit 180 | * Update README.md 181 | * Update README.md 182 | * Update README.md 183 | * Initial commit 184 | * Contributors: Mitchell Wills, Russell Toris 185 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(web_video_server) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | find_package(ament_cmake_ros REQUIRED) 9 | 10 | find_package(async_web_server_cpp REQUIRED) 11 | find_package(cv_bridge REQUIRED) 12 | find_package(image_transport REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(rclcpp_components REQUIRED) 15 | find_package(sensor_msgs REQUIRED) 16 | 17 | find_package(OpenCV REQUIRED) 18 | find_package(Boost REQUIRED COMPONENTS system) 19 | 20 | find_package(PkgConfig REQUIRED) 21 | pkg_check_modules(avcodec libavcodec REQUIRED) 22 | pkg_check_modules(avformat libavformat REQUIRED) 23 | pkg_check_modules(avutil libavutil REQUIRED) 24 | pkg_check_modules(swscale libswscale REQUIRED) 25 | 26 | if(NOT CMAKE_CXX_STANDARD) 27 | set(CMAKE_CXX_STANDARD 14) 28 | endif() 29 | 30 | ################################################### 31 | ## Declare things to be passed to other projects ## 32 | ################################################### 33 | 34 | ########### 35 | ## Build ## 36 | ########### 37 | 38 | if(${cv_bridge_VERSION} VERSION_LESS "3.3.0") 39 | add_compile_definitions(CV_BRIDGE_USES_OLD_HEADERS) 40 | endif() 41 | 42 | ## Specify additional locations of header files 43 | include_directories(include 44 | ${avcodec_INCLUDE_DIRS} 45 | ${avformat_INCLUDE_DIRS} 46 | ${avutil_INCLUDE_DIRS} 47 | ${swscale_INCLUDE_DIRS} 48 | ) 49 | 50 | ## Declare a cpp library 51 | add_library(${PROJECT_NAME} SHARED 52 | src/web_video_server.cpp 53 | src/image_streamer.cpp 54 | src/libav_streamer.cpp 55 | src/vp8_streamer.cpp 56 | src/h264_streamer.cpp 57 | src/vp9_streamer.cpp 58 | src/multipart_stream.cpp 59 | src/ros_compressed_streamer.cpp 60 | src/jpeg_streamers.cpp 61 | src/png_streamers.cpp 62 | src/utils.cpp 63 | ) 64 | 65 | ## Specify libraries to link a library or executable target against 66 | target_link_libraries(${PROJECT_NAME} 67 | async_web_server_cpp::async_web_server_cpp 68 | cv_bridge::cv_bridge 69 | image_transport::image_transport 70 | rclcpp::rclcpp 71 | rclcpp_components::component 72 | ${sensor_msgs_TARGETS} 73 | Boost::boost 74 | Boost::system 75 | ${OpenCV_LIBS} 76 | ${avcodec_LIBRARIES} 77 | ${avformat_LIBRARIES} 78 | ${avutil_LIBRARIES} 79 | ${swscale_LIBRARIES} 80 | ) 81 | 82 | ## Declare a cpp executable 83 | add_executable(${PROJECT_NAME}_node 84 | src/web_video_server_node.cpp 85 | ) 86 | 87 | target_link_libraries(${PROJECT_NAME}_node 88 | ${PROJECT_NAME} 89 | ) 90 | 91 | rclcpp_components_register_nodes(${PROJECT_NAME} "web_video_server::WebVideoServer") 92 | 93 | ############# 94 | ## Install ## 95 | ############# 96 | 97 | ## Mark executables and/or libraries for installation 98 | install( 99 | DIRECTORY include/ 100 | DESTINATION include/${PROJECT_NAME} 101 | ) 102 | 103 | install( 104 | TARGETS ${PROJECT_NAME} 105 | EXPORT export_${PROJECT_NAME} 106 | LIBRARY DESTINATION lib 107 | ARCHIVE DESTINATION lib 108 | RUNTIME DESTINATION bin 109 | ) 110 | ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) 111 | 112 | set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME ${PROJECT_NAME}) 113 | install( 114 | TARGETS ${PROJECT_NAME}_node 115 | DESTINATION lib/${PROJECT_NAME} 116 | ) 117 | 118 | ########### 119 | ## Tests ## 120 | ########### 121 | 122 | if(BUILD_TESTING) 123 | find_package(ament_lint_auto REQUIRED) 124 | 125 | # Skip ament_copyright check for humble 126 | if($ENV{ROS_DISTRO} STREQUAL "humble") 127 | list(APPEND AMENT_LINT_AUTO_EXCLUDE 128 | ament_cmake_copyright 129 | ) 130 | endif() 131 | 132 | ament_lint_auto_find_test_dependencies() 133 | endif() 134 | 135 | ament_package() 136 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Any contribution that you make to this repository will 2 | be under the 3-Clause BSD License, as dictated by that 3 | [license](https://opensource.org/licenses/BSD-3-Clause). -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2014, Worcester Polytechnic Institute 2 | Copyright (c) 2024, The Robot Web Tools Contributors 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | 15 | * Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # web_video_server - HTTP Streaming of ROS Image Topics in Multiple Formats 2 | 3 | This node provides HTTP streaming of ROS image topics in various formats, making it easy to view robot camera feeds and other image topics in a web browser without requiring special plugins or extensions. 4 | 5 | ## Features 6 | 7 | - Stream ROS image topics over HTTP in multiple formats: 8 | - MJPEG (Motion JPEG) 9 | - VP8 (WebM) 10 | - VP9 (WebM) 11 | - H264 (MP4) 12 | - PNG streams 13 | - ROS compressed image topics 14 | - Adjustable quality, size, and other streaming parameters 15 | - Web interface to browse available image topics 16 | - Single image snapshot capability 17 | - Support for different QoS profiles in ROS 2 18 | 19 | ## Installation 20 | 21 | ### Dependencies 22 | 23 | - ROS (Noetic) or ROS 2 (Humble+) 24 | - OpenCV 25 | - FFmpeg/libav 26 | - Boost 27 | - async_web_server_cpp 28 | 29 | ### Installing packages 30 | 31 | For newer ROS2 distributions (humble, jazzy, rolling) it is possible to install web_video_server as a package: 32 | 33 | ``` 34 | sudo apt install ros-${ROS_DISTRO}-web-video-server 35 | ``` 36 | 37 | ### Building from Source 38 | 39 | Create a ROS workspace if you don't have one: 40 | ```bash 41 | mkdir -p ~/ros_ws/src 42 | cd ~/ros_ws/src 43 | ``` 44 | 45 | Clone this repository: 46 | ```bash 47 | # ROS 2 48 | git clone https://github.com/RobotWebTools/web_video_server.git 49 | # ROS 1 50 | git clone https://github.com/RobotWebTools/web_video_server.git -b ros1 51 | ``` 52 | 53 | Install dependencies with rosdep: 54 | ```bash 55 | cd ~/ros_ws 56 | rosdep update 57 | rosdep install --from-paths src -i 58 | ``` 59 | 60 | Build the package and source your workspace: 61 | ```bash 62 | colcon build --packages-select web_video_server 63 | source install/setup.bash 64 | ``` 65 | 66 | ## Usage 67 | 68 | ### Starting the Server 69 | 70 | ```bash 71 | # ROS 1 72 | rosrun web_video_server web_video_server 73 | 74 | # ROS 2 75 | ros2 run web_video_server web_video_server 76 | ``` 77 | 78 | 79 | ### Configuration 80 | 81 | #### Server Configuration Parameters 82 | 83 | | Parameter | Type | Default | Possible Values | Description | 84 | |-----------|------|---------|----------------|-------------| 85 | | `port` | int | 8080 | Any valid port number | HTTP server port | 86 | | `address` | string | "0.0.0.0" | Any valid IP address | HTTP server address (0.0.0.0 allows external connections) | 87 | | `server_threads` | int | 1 | 1+ | Number of server threads for handling HTTP requests | 88 | | `ros_threads` | int | 2 | 1+ | Number of threads for ROS message handling | 89 | | `verbose` | bool | false | true, false | Enable verbose logging | 90 | | `default_stream_type` | string | "mjpeg" | "mjpeg", "vp8", "vp9", "h264", "png", "ros_compressed" | Default format for video streams | 91 | | `publish_rate` | double | -1.0 | -1.0 or positive value | Rate for republishing images (-1.0 means no republishing) | 92 | 93 | #### Running with Custom Parameters 94 | 95 | You can configure the server by passing parameters via the command line: 96 | 97 | ```bash 98 | # ROS 1 99 | rosrun web_video_server web_video_server _port:=8081 _address:=localhost _server_threads:=4 100 | 101 | # ROS 2 102 | ros2 run web_video_server web_video_server --ros-args -p port:=8081 -p address:=localhost -p server_threads:=4 103 | ``` 104 | 105 | ### View Available Streams 106 | ``` 107 | http://localhost:8080/ 108 | ``` 109 | The interface allows quick navigation between different topics and formats without having to manually construct URLs. 110 | 111 | This page displays: 112 | - All available ROS image topics currently being published 113 | - Direct links to view each topic in different formats: 114 | - Web page with streaming image 115 | - Direct stream 116 | - Single image snapshot 117 | 118 | ### Stream an Image Topic 119 | 120 | There are two ways to stream the Image, as a HTML page via 121 | ``` 122 | http://localhost:8080/stream_viewer?topic=/camera/image_raw 123 | ``` 124 | or as a HTTP multipart stream on 125 | 126 | ``` 127 | http://localhost:8080/stream?topic=/camera/image_raw 128 | ``` 129 | #### URL Parameters for Streaming 130 | 131 | The following parameters can be added to the stream URL: 132 | 133 | | Parameter | Type | Default | Possible Values | Description | 134 | |-----------|------|---------|----------------|-------------| 135 | | `topic` | string | (required) | Any valid ROS image topic | The ROS image topic to stream | 136 | | `type` | string | "mjpeg" | "mjpeg", "vp8", "vp9", "h264", "png", "ros_compressed" | Stream format | 137 | | `width` | int | 0 | 0+ | Width of output stream (0 = original width) | 138 | | `height` | int | 0 | 0+ | Height of output stream (0 = original height) | 139 | | `quality` | int | 95 | 1-100 | Quality for MJPEG and PNG streams | 140 | | `bitrate` | int | 100000 | Positive integer | Bitrate for H264/VP8/VP9 streams in bits/second | 141 | | `invert` | flag | not present | present/not present | Invert image when parameter is present | 142 | | `default_transport` | string | "raw" | "raw", "compressed", "theora" | Image transport to use | 143 | | `qos_profile` | string | "default" | "default", "system_default", "sensor_data", "services_default" | QoS profile for ROS 2 subscribers | 144 | 145 | Examples: 146 | 147 | ``` 148 | # Stream an MJPEG at 640x480 with 90% quality 149 | http://localhost:8080/stream?topic=/camera/image_raw&type=mjpeg&width=640&height=480&quality=90 150 | 151 | # Stream H264 with higher bitrate 152 | http://localhost:8080/stream?topic=/camera/image_raw&type=h264&bitrate=500000 153 | 154 | # Stream with inverted image (rotated 180°) 155 | http://localhost:8080/stream?topic=/camera/image_raw&invert 156 | 157 | ``` 158 | 159 | ### Get a Snapshot 160 | It is also possible to get a single image snapshot 161 | ``` 162 | http://localhost:8080/snapshot?topic=/camera/image_raw 163 | ``` 164 | #### URL Parameters for Snapshot 165 | 166 | | Parameter | Type | Default | Possible Values | Description | 167 | |-----------|------|---------|----------------|-------------| 168 | | `topic` | string | (required) | Any valid ROS image topic | The ROS image topic to stream | 169 | | `width` | int | 0 | 0+ | Width of output picture (0 = original width) | 170 | | `height` | int | 0 | 0+ | Height of output picture (0 = original height) | 171 | | `quality` | int | 95 | 1-100 | Quality for JPEG snapshots | 172 | | `invert` | flag | not present | present/not present | Invert image when parameter is present | 173 | | `default_transport` | string | "raw" | "raw", "compressed", "theora" | Image transport to use | 174 | | `qos_profile` | string | "default" | "default", "system_default", "sensor_data", "services_default" | QoS profile for ROS 2 subscribers | 175 | 176 | ## About 177 | 178 | This project is released as part of the [Robot Web Tools](https://robotwebtools.github.io/) effort. 179 | 180 | ## License 181 | web_video_server is released with a BSD license. For full terms and conditions, see the [LICENSE](LICENSE) file. 182 | 183 | ## Authors 184 | See the [AUTHORS](AUTHORS.md) file for a full list of contributors. 185 | -------------------------------------------------------------------------------- /include/web_video_server/h264_streamer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024, The Robot Web Tools Contributors 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from 16 | // this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | 30 | #pragma once 31 | 32 | #include 33 | #include 34 | 35 | #include "image_transport/image_transport.hpp" 36 | #include "web_video_server/libav_streamer.hpp" 37 | #include "async_web_server_cpp/http_request.hpp" 38 | #include "async_web_server_cpp/http_connection.hpp" 39 | 40 | namespace web_video_server 41 | { 42 | 43 | class H264Streamer : public LibavStreamer 44 | { 45 | public: 46 | H264Streamer( 47 | const async_web_server_cpp::HttpRequest & request, 48 | async_web_server_cpp::HttpConnectionPtr connection, 49 | rclcpp::Node::SharedPtr node); 50 | ~H264Streamer(); 51 | 52 | protected: 53 | virtual void initializeEncoder(); 54 | std::string preset_; 55 | }; 56 | 57 | class H264StreamerType : public LibavStreamerType 58 | { 59 | public: 60 | H264StreamerType(); 61 | std::shared_ptr create_streamer( 62 | const async_web_server_cpp::HttpRequest & request, 63 | async_web_server_cpp::HttpConnectionPtr connection, 64 | rclcpp::Node::SharedPtr node); 65 | }; 66 | 67 | } // namespace web_video_server 68 | -------------------------------------------------------------------------------- /include/web_video_server/image_streamer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, Worcester Polytechnic Institute 2 | // Copyright (c) 2024, The Robot Web Tools Contributors 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // * Neither the name of the copyright holder nor the names of its 16 | // contributors may be used to endorse or promote products derived from 17 | // this software without specific prior written permission. 18 | // 19 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | // POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #pragma once 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | #include "rclcpp/rclcpp.hpp" 40 | #include "image_transport/image_transport.hpp" 41 | #include "image_transport/transport_hints.hpp" 42 | #include "web_video_server/utils.hpp" 43 | #include "async_web_server_cpp/http_server.hpp" 44 | #include "async_web_server_cpp/http_request.hpp" 45 | 46 | namespace web_video_server 47 | { 48 | 49 | class ImageStreamer 50 | { 51 | public: 52 | ImageStreamer( 53 | const async_web_server_cpp::HttpRequest & request, 54 | async_web_server_cpp::HttpConnectionPtr connection, 55 | rclcpp::Node::SharedPtr node); 56 | 57 | virtual void start() = 0; 58 | virtual ~ImageStreamer(); 59 | 60 | bool isInactive() 61 | { 62 | return inactive_; 63 | } 64 | 65 | /** 66 | * Restreams the last received image frame if older than max_age. 67 | */ 68 | virtual void restreamFrame(std::chrono::duration max_age) = 0; 69 | 70 | std::string getTopic() 71 | { 72 | return topic_; 73 | } 74 | 75 | protected: 76 | async_web_server_cpp::HttpConnectionPtr connection_; 77 | async_web_server_cpp::HttpRequest request_; 78 | rclcpp::Node::SharedPtr node_; 79 | bool inactive_; 80 | image_transport::Subscriber image_sub_; 81 | std::string topic_; 82 | }; 83 | 84 | 85 | class ImageTransportImageStreamer : public ImageStreamer 86 | { 87 | public: 88 | ImageTransportImageStreamer( 89 | const async_web_server_cpp::HttpRequest & request, 90 | async_web_server_cpp::HttpConnectionPtr connection, 91 | rclcpp::Node::SharedPtr node); 92 | virtual ~ImageTransportImageStreamer(); 93 | 94 | virtual void start(); 95 | 96 | protected: 97 | virtual cv::Mat decodeImage(const sensor_msgs::msg::Image::ConstSharedPtr & msg); 98 | virtual void sendImage(const cv::Mat &, const std::chrono::steady_clock::time_point & time) = 0; 99 | virtual void restreamFrame(std::chrono::duration max_age); 100 | virtual void initialize(const cv::Mat &); 101 | 102 | image_transport::Subscriber image_sub_; 103 | int output_width_; 104 | int output_height_; 105 | bool invert_; 106 | std::string default_transport_; 107 | std::string qos_profile_name_; 108 | 109 | std::chrono::steady_clock::time_point last_frame_; 110 | cv::Mat output_size_image; 111 | std::mutex send_mutex_; 112 | 113 | private: 114 | image_transport::ImageTransport it_; 115 | bool initialized_; 116 | 117 | void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg); 118 | }; 119 | 120 | class ImageStreamerType 121 | { 122 | public: 123 | virtual std::shared_ptr create_streamer( 124 | const async_web_server_cpp::HttpRequest & request, 125 | async_web_server_cpp::HttpConnectionPtr connection, 126 | rclcpp::Node::SharedPtr node) = 0; 127 | 128 | virtual std::string create_viewer(const async_web_server_cpp::HttpRequest & request) = 0; 129 | }; 130 | 131 | } // namespace web_video_server 132 | -------------------------------------------------------------------------------- /include/web_video_server/jpeg_streamers.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, Worcester Polytechnic Institute 2 | // Copyright (c) 2024, The Robot Web Tools Contributors 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // * Neither the name of the copyright holder nor the names of its 16 | // contributors may be used to endorse or promote products derived from 17 | // this software without specific prior written permission. 18 | // 19 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | // POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #pragma once 32 | 33 | #include 34 | #include 35 | 36 | #include "image_transport/image_transport.hpp" 37 | #include "web_video_server/image_streamer.hpp" 38 | #include "async_web_server_cpp/http_request.hpp" 39 | #include "async_web_server_cpp/http_connection.hpp" 40 | #include "web_video_server/multipart_stream.hpp" 41 | 42 | namespace web_video_server 43 | { 44 | 45 | class MjpegStreamer : public ImageTransportImageStreamer 46 | { 47 | public: 48 | MjpegStreamer( 49 | const async_web_server_cpp::HttpRequest & request, 50 | async_web_server_cpp::HttpConnectionPtr connection, 51 | rclcpp::Node::SharedPtr node); 52 | ~MjpegStreamer(); 53 | 54 | protected: 55 | virtual void sendImage(const cv::Mat &, const std::chrono::steady_clock::time_point & time); 56 | 57 | private: 58 | MultipartStream stream_; 59 | int quality_; 60 | }; 61 | 62 | class MjpegStreamerType : public ImageStreamerType 63 | { 64 | public: 65 | std::shared_ptr create_streamer( 66 | const async_web_server_cpp::HttpRequest & request, 67 | async_web_server_cpp::HttpConnectionPtr connection, 68 | rclcpp::Node::SharedPtr node); 69 | std::string create_viewer(const async_web_server_cpp::HttpRequest & request); 70 | }; 71 | 72 | class JpegSnapshotStreamer : public ImageTransportImageStreamer 73 | { 74 | public: 75 | JpegSnapshotStreamer( 76 | const async_web_server_cpp::HttpRequest & request, 77 | async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node); 78 | ~JpegSnapshotStreamer(); 79 | 80 | protected: 81 | virtual void sendImage(const cv::Mat &, const std::chrono::steady_clock::time_point & time); 82 | 83 | private: 84 | int quality_; 85 | }; 86 | 87 | } // namespace web_video_server 88 | -------------------------------------------------------------------------------- /include/web_video_server/libav_streamer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, Worcester Polytechnic Institute 2 | // Copyright (c) 2024, The Robot Web Tools Contributors 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // * Neither the name of the copyright holder nor the names of its 16 | // contributors may be used to endorse or promote products derived from 17 | // this software without specific prior written permission. 18 | // 19 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | // POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #pragma once 32 | 33 | extern "C" 34 | { 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | } 44 | 45 | #include 46 | #include 47 | #include 48 | 49 | #include "image_transport/image_transport.hpp" 50 | #include "web_video_server/image_streamer.hpp" 51 | #include "async_web_server_cpp/http_request.hpp" 52 | #include "async_web_server_cpp/http_connection.hpp" 53 | 54 | namespace web_video_server 55 | { 56 | 57 | class LibavStreamer : public ImageTransportImageStreamer 58 | { 59 | public: 60 | LibavStreamer( 61 | const async_web_server_cpp::HttpRequest & request, 62 | async_web_server_cpp::HttpConnectionPtr connection, 63 | rclcpp::Node::SharedPtr node, const std::string & format_name, const std::string & codec_name, 64 | const std::string & content_type); 65 | 66 | ~LibavStreamer(); 67 | 68 | protected: 69 | virtual void initializeEncoder(); 70 | virtual void sendImage(const cv::Mat &, const std::chrono::steady_clock::time_point & time); 71 | virtual void initialize(const cv::Mat &); 72 | AVFormatContext * format_context_; 73 | const AVCodec * codec_; 74 | AVCodecContext * codec_context_; 75 | AVStream * video_stream_; 76 | 77 | AVDictionary * opt_; // container format options 78 | 79 | private: 80 | AVFrame * frame_; 81 | struct SwsContext * sws_context_; 82 | std::mutex encode_mutex_; 83 | bool first_image_received_; 84 | std::chrono::steady_clock::time_point first_image_time_; 85 | 86 | std::string format_name_; 87 | std::string codec_name_; 88 | std::string content_type_; 89 | int bitrate_; 90 | int qmin_; 91 | int qmax_; 92 | int gop_; 93 | 94 | uint8_t * io_buffer_; // custom IO buffer 95 | }; 96 | 97 | class LibavStreamerType : public ImageStreamerType 98 | { 99 | public: 100 | LibavStreamerType( 101 | const std::string & format_name, const std::string & codec_name, 102 | const std::string & content_type); 103 | 104 | std::shared_ptr create_streamer( 105 | const async_web_server_cpp::HttpRequest & request, 106 | async_web_server_cpp::HttpConnectionPtr connection, 107 | rclcpp::Node::SharedPtr node); 108 | 109 | std::string create_viewer(const async_web_server_cpp::HttpRequest & request); 110 | 111 | private: 112 | const std::string format_name_; 113 | const std::string codec_name_; 114 | const std::string content_type_; 115 | }; 116 | 117 | } // namespace web_video_server 118 | -------------------------------------------------------------------------------- /include/web_video_server/multipart_stream.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, Worcester Polytechnic Institute 2 | // Copyright (c) 2024, The Robot Web Tools Contributors 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // * Neither the name of the copyright holder nor the names of its 16 | // contributors may be used to endorse or promote products derived from 17 | // this software without specific prior written permission. 18 | // 19 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | // POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #pragma once 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "rclcpp/rclcpp.hpp" 39 | #include "async_web_server_cpp/http_connection.hpp" 40 | 41 | namespace web_video_server 42 | { 43 | 44 | struct PendingFooter 45 | { 46 | std::chrono::steady_clock::time_point timestamp; 47 | std::weak_ptr contents; 48 | }; 49 | 50 | class MultipartStream 51 | { 52 | public: 53 | MultipartStream( 54 | async_web_server_cpp::HttpConnectionPtr & connection, 55 | const std::string & boundry = "boundarydonotcross", 56 | std::size_t max_queue_size = 1); 57 | 58 | void sendInitialHeader(); 59 | void sendPartHeader( 60 | const std::chrono::steady_clock::time_point & time, const std::string & type, 61 | size_t payload_size); 62 | void sendPartFooter(const std::chrono::steady_clock::time_point & time); 63 | void sendPartAndClear( 64 | const std::chrono::steady_clock::time_point & time, const std::string & type, 65 | std::vector & data); 66 | void sendPart( 67 | const std::chrono::steady_clock::time_point & time, const std::string & type, 68 | const boost::asio::const_buffer & buffer, 69 | async_web_server_cpp::HttpConnection::ResourcePtr resource); 70 | 71 | private: 72 | bool isBusy(); 73 | 74 | private: 75 | const std::size_t max_queue_size_; 76 | async_web_server_cpp::HttpConnectionPtr connection_; 77 | std::string boundry_; 78 | std::queue pending_footers_; 79 | }; 80 | 81 | } // namespace web_video_server 82 | -------------------------------------------------------------------------------- /include/web_video_server/png_streamers.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024, The Robot Web Tools Contributors 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from 16 | // this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | 30 | #pragma once 31 | 32 | #include 33 | #include 34 | 35 | #include "image_transport/image_transport.hpp" 36 | #include "web_video_server/image_streamer.hpp" 37 | #include "async_web_server_cpp/http_request.hpp" 38 | #include "async_web_server_cpp/http_connection.hpp" 39 | #include "web_video_server/multipart_stream.hpp" 40 | 41 | namespace web_video_server 42 | { 43 | 44 | class PngStreamer : public ImageTransportImageStreamer 45 | { 46 | public: 47 | PngStreamer( 48 | const async_web_server_cpp::HttpRequest & request, 49 | async_web_server_cpp::HttpConnectionPtr connection, 50 | rclcpp::Node::SharedPtr node); 51 | ~PngStreamer(); 52 | 53 | protected: 54 | virtual void sendImage(const cv::Mat &, const std::chrono::steady_clock::time_point & time); 55 | virtual cv::Mat decodeImage(const sensor_msgs::msg::Image::ConstSharedPtr & msg); 56 | 57 | private: 58 | MultipartStream stream_; 59 | int quality_; 60 | }; 61 | 62 | class PngStreamerType : public ImageStreamerType 63 | { 64 | public: 65 | std::shared_ptr create_streamer( 66 | const async_web_server_cpp::HttpRequest & request, 67 | async_web_server_cpp::HttpConnectionPtr connection, 68 | rclcpp::Node::SharedPtr node); 69 | std::string create_viewer(const async_web_server_cpp::HttpRequest & request); 70 | }; 71 | 72 | class PngSnapshotStreamer : public ImageTransportImageStreamer 73 | { 74 | public: 75 | PngSnapshotStreamer( 76 | const async_web_server_cpp::HttpRequest & request, 77 | async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node); 78 | ~PngSnapshotStreamer(); 79 | 80 | protected: 81 | virtual void sendImage(const cv::Mat &, const std::chrono::steady_clock::time_point & time); 82 | virtual cv::Mat decodeImage(const sensor_msgs::msg::Image::ConstSharedPtr & msg); 83 | 84 | private: 85 | int quality_; 86 | }; 87 | 88 | } // namespace web_video_server 89 | -------------------------------------------------------------------------------- /include/web_video_server/ros_compressed_streamer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, Worcester Polytechnic Institute 2 | // Copyright (c) 2024, The Robot Web Tools Contributors 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // * Neither the name of the copyright holder nor the names of its 16 | // contributors may be used to endorse or promote products derived from 17 | // this software without specific prior written permission. 18 | // 19 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | // POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #pragma once 32 | 33 | #include 34 | #include 35 | 36 | #include "sensor_msgs/msg/compressed_image.hpp" 37 | #include "web_video_server/image_streamer.hpp" 38 | #include "async_web_server_cpp/http_request.hpp" 39 | #include "async_web_server_cpp/http_connection.hpp" 40 | #include "web_video_server/multipart_stream.hpp" 41 | 42 | namespace web_video_server 43 | { 44 | 45 | class RosCompressedStreamer : public ImageStreamer 46 | { 47 | public: 48 | RosCompressedStreamer( 49 | const async_web_server_cpp::HttpRequest & request, 50 | async_web_server_cpp::HttpConnectionPtr connection, 51 | rclcpp::Node::SharedPtr node); 52 | ~RosCompressedStreamer(); 53 | virtual void start(); 54 | virtual void restreamFrame(std::chrono::duration max_age); 55 | 56 | protected: 57 | virtual void sendImage( 58 | const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg, 59 | const std::chrono::steady_clock::time_point & time); 60 | 61 | private: 62 | void imageCallback(const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg); 63 | MultipartStream stream_; 64 | rclcpp::Subscription::SharedPtr image_sub_; 65 | std::chrono::steady_clock::time_point last_frame_; 66 | sensor_msgs::msg::CompressedImage::ConstSharedPtr last_msg; 67 | std::mutex send_mutex_; 68 | std::string qos_profile_name_; 69 | }; 70 | 71 | class RosCompressedStreamerType : public ImageStreamerType 72 | { 73 | public: 74 | std::shared_ptr create_streamer( 75 | const async_web_server_cpp::HttpRequest & request, 76 | async_web_server_cpp::HttpConnectionPtr connection, 77 | rclcpp::Node::SharedPtr node); 78 | std::string create_viewer(const async_web_server_cpp::HttpRequest & request); 79 | }; 80 | 81 | } // namespace web_video_server 82 | -------------------------------------------------------------------------------- /include/web_video_server/utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024, The Robot Web Tools Contributors 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from 16 | // this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | 30 | #pragma once 31 | 32 | #include 33 | #include 34 | #include "rmw/qos_profiles.h" 35 | 36 | namespace web_video_server 37 | { 38 | 39 | /** 40 | * @brief Gets a QoS profile given an input name, if valid. 41 | * @param name The name of the QoS profile name. 42 | * @return An optional containing the matching QoS profile. 43 | */ 44 | std::optional get_qos_profile_from_name(const std::string name); 45 | 46 | } // namespace web_video_server 47 | -------------------------------------------------------------------------------- /include/web_video_server/vp8_streamer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, Worcester Polytechnic Institute 2 | // Copyright (c) 2024, The Robot Web Tools Contributors 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // * Neither the name of the copyright holder nor the names of its 16 | // contributors may be used to endorse or promote products derived from 17 | // this software without specific prior written permission. 18 | // 19 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | // POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #pragma once 32 | 33 | #include 34 | #include 35 | 36 | #include "image_transport/image_transport.hpp" 37 | #include "web_video_server/libav_streamer.hpp" 38 | #include "async_web_server_cpp/http_request.hpp" 39 | #include "async_web_server_cpp/http_connection.hpp" 40 | 41 | namespace web_video_server 42 | { 43 | 44 | class Vp8Streamer : public LibavStreamer 45 | { 46 | public: 47 | Vp8Streamer( 48 | const async_web_server_cpp::HttpRequest & request, 49 | async_web_server_cpp::HttpConnectionPtr connection, 50 | rclcpp::Node::SharedPtr node); 51 | ~Vp8Streamer(); 52 | 53 | protected: 54 | virtual void initializeEncoder(); 55 | 56 | private: 57 | std::string quality_; 58 | }; 59 | 60 | class Vp8StreamerType : public LibavStreamerType 61 | { 62 | public: 63 | Vp8StreamerType(); 64 | std::shared_ptr create_streamer( 65 | const async_web_server_cpp::HttpRequest & request, 66 | async_web_server_cpp::HttpConnectionPtr connection, 67 | rclcpp::Node::SharedPtr node); 68 | }; 69 | 70 | } // namespace web_video_server 71 | -------------------------------------------------------------------------------- /include/web_video_server/vp9_streamer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024, The Robot Web Tools Contributors 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from 16 | // this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | 30 | #pragma once 31 | 32 | #include 33 | 34 | #include "image_transport/image_transport.hpp" 35 | #include "web_video_server/libav_streamer.hpp" 36 | #include "async_web_server_cpp/http_request.hpp" 37 | #include "async_web_server_cpp/http_connection.hpp" 38 | 39 | namespace web_video_server 40 | { 41 | 42 | class Vp9Streamer : public LibavStreamer 43 | { 44 | public: 45 | Vp9Streamer( 46 | const async_web_server_cpp::HttpRequest & request, 47 | async_web_server_cpp::HttpConnectionPtr connection, 48 | rclcpp::Node::SharedPtr node); 49 | ~Vp9Streamer(); 50 | 51 | protected: 52 | virtual void initializeEncoder(); 53 | }; 54 | 55 | class Vp9StreamerType : public LibavStreamerType 56 | { 57 | public: 58 | Vp9StreamerType(); 59 | std::shared_ptr create_streamer( 60 | const async_web_server_cpp::HttpRequest & request, 61 | async_web_server_cpp::HttpConnectionPtr connection, 62 | rclcpp::Node::SharedPtr node); 63 | }; 64 | 65 | } // namespace web_video_server 66 | -------------------------------------------------------------------------------- /include/web_video_server/web_video_server.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, Worcester Polytechnic Institute 2 | // Copyright (c) 2024, The Robot Web Tools Contributors 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // * Neither the name of the copyright holder nor the names of its 16 | // contributors may be used to endorse or promote products derived from 17 | // this software without specific prior written permission. 18 | // 19 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | // POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #pragma once 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #ifdef CV_BRIDGE_USES_OLD_HEADERS 39 | #include "cv_bridge/cv_bridge.h" 40 | #else 41 | #include "cv_bridge/cv_bridge.hpp" 42 | #endif 43 | 44 | #include "rclcpp/rclcpp.hpp" 45 | #include "web_video_server/image_streamer.hpp" 46 | #include "async_web_server_cpp/http_server.hpp" 47 | #include "async_web_server_cpp/http_request.hpp" 48 | #include "async_web_server_cpp/http_connection.hpp" 49 | 50 | namespace web_video_server 51 | { 52 | 53 | /** 54 | * @class WebVideoServer 55 | * @brief 56 | */ 57 | class WebVideoServer : public rclcpp::Node 58 | { 59 | public: 60 | /** 61 | * @brief Constructor 62 | * @return 63 | */ 64 | explicit WebVideoServer(const rclcpp::NodeOptions & options); 65 | 66 | /** 67 | * @brief Destructor - Cleans up 68 | */ 69 | virtual ~WebVideoServer(); 70 | 71 | bool handle_request( 72 | const async_web_server_cpp::HttpRequest & request, 73 | async_web_server_cpp::HttpConnectionPtr connection, 74 | const char * begin, const char * end); 75 | 76 | bool handle_stream( 77 | const async_web_server_cpp::HttpRequest & request, 78 | async_web_server_cpp::HttpConnectionPtr connection, 79 | const char * begin, const char * end); 80 | 81 | bool handle_stream_viewer( 82 | const async_web_server_cpp::HttpRequest & request, 83 | async_web_server_cpp::HttpConnectionPtr connection, 84 | const char * begin, const char * end); 85 | 86 | bool handle_snapshot( 87 | const async_web_server_cpp::HttpRequest & request, 88 | async_web_server_cpp::HttpConnectionPtr connection, 89 | const char * begin, const char * end); 90 | 91 | bool handle_list_streams( 92 | const async_web_server_cpp::HttpRequest & request, 93 | async_web_server_cpp::HttpConnectionPtr connection, 94 | const char * begin, const char * end); 95 | 96 | private: 97 | void restreamFrames(std::chrono::duration max_age); 98 | void cleanup_inactive_streams(); 99 | 100 | rclcpp::TimerBase::SharedPtr cleanup_timer_; 101 | 102 | // Parameters 103 | int ros_threads_; 104 | double publish_rate_; 105 | int port_; 106 | std::string address_; 107 | bool verbose_; 108 | std::string default_stream_type_; 109 | 110 | std::shared_ptr server_; 111 | async_web_server_cpp::HttpRequestHandlerGroup handler_group_; 112 | 113 | std::vector> image_subscribers_; 114 | std::map> stream_types_; 115 | std::mutex subscriber_mutex_; 116 | }; 117 | 118 | } // namespace web_video_server 119 | -------------------------------------------------------------------------------- /mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | 4 | \b A ROS Node to Stream Image Topics Via multiple formats including MJPEG and VP8 5 | web_video_server converts ROS image streams into an MJPEG or other encoding and streams them via a socket connection. 6 | */ 7 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | web_video_server 5 | 2.1.0 6 | HTTP Streaming of ROS Image Topics in Multiple Formats 7 | 8 | Błażej Sowa 9 | 10 | BSD 11 | 12 | http://ros.org/wiki/web_video_server 13 | https://github.com/RobotWebTools/web_video_server/issues 14 | https://github.com/RobotWebTools/web_video_server 15 | 16 | Mitchell Wills 17 | 18 | ament_cmake_ros 19 | pkg-config 20 | 21 | rclcpp 22 | rclcpp_components 23 | cv_bridge 24 | image_transport 25 | async_web_server_cpp 26 | ffmpeg 27 | sensor_msgs 28 | 29 | rclcpp 30 | rclcpp_components 31 | cv_bridge 32 | image_transport 33 | async_web_server_cpp 34 | ffmpeg 35 | sensor_msgs 36 | 37 | ament_lint_auto 38 | ament_cmake_copyright 39 | ament_cmake_cpplint 40 | ament_cmake_lint_cmake 41 | ament_cmake_xmllint 42 | ament_cmake_uncrustify 43 | ros_environment 44 | 45 | 46 | ament_cmake 47 | 48 | 49 | -------------------------------------------------------------------------------- /src/h264_streamer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024, The Robot Web Tools Contributors 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from 16 | // this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | 30 | #include "web_video_server/h264_streamer.hpp" 31 | 32 | namespace web_video_server 33 | { 34 | 35 | H264Streamer::H264Streamer( 36 | const async_web_server_cpp::HttpRequest & request, 37 | async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) 38 | : LibavStreamer(request, connection, node, "mp4", "libx264", "video/mp4") 39 | { 40 | /* possible quality presets: 41 | * ultrafast, superfast, veryfast, faster, fast, medium, slow, slower, veryslow, placebo 42 | * no latency improvements observed with ultrafast instead of medium 43 | */ 44 | preset_ = request.get_query_param_value_or_default("preset", "ultrafast"); 45 | } 46 | 47 | H264Streamer::~H264Streamer() 48 | { 49 | } 50 | 51 | void H264Streamer::initializeEncoder() 52 | { 53 | av_opt_set(codec_context_->priv_data, "preset", preset_.c_str(), 0); 54 | av_opt_set(codec_context_->priv_data, "tune", "zerolatency", 0); 55 | av_opt_set_int(codec_context_->priv_data, "crf", 20, 0); 56 | av_opt_set_int(codec_context_->priv_data, "bufsize", 100, 0); 57 | av_opt_set_int(codec_context_->priv_data, "keyint", 30, 0); 58 | av_opt_set_int(codec_context_->priv_data, "g", 1, 0); 59 | 60 | // container format options 61 | if (!strcmp(format_context_->oformat->name, "mp4")) { 62 | // set up mp4 for streaming (instead of seekable file output) 63 | av_dict_set(&opt_, "movflags", "+frag_keyframe+empty_moov+faststart", 0); 64 | } 65 | } 66 | 67 | H264StreamerType::H264StreamerType() 68 | : LibavStreamerType("mp4", "libx264", "video/mp4") 69 | { 70 | } 71 | 72 | std::shared_ptr H264StreamerType::create_streamer( 73 | const async_web_server_cpp::HttpRequest & request, 74 | async_web_server_cpp::HttpConnectionPtr connection, 75 | rclcpp::Node::SharedPtr node) 76 | { 77 | return std::make_shared(request, connection, node); 78 | } 79 | 80 | } // namespace web_video_server 81 | -------------------------------------------------------------------------------- /src/image_streamer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, Worcester Polytechnic Institute 2 | // Copyright (c) 2024, The Robot Web Tools Contributors 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // * Neither the name of the copyright holder nor the names of its 16 | // contributors may be used to endorse or promote products derived from 17 | // this software without specific prior written permission. 18 | // 19 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | // POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #include "web_video_server/image_streamer.hpp" 32 | 33 | #ifdef CV_BRIDGE_USES_OLD_HEADERS 34 | #include 35 | #else 36 | #include 37 | #endif 38 | 39 | #include 40 | 41 | namespace web_video_server 42 | { 43 | 44 | ImageStreamer::ImageStreamer( 45 | const async_web_server_cpp::HttpRequest & request, 46 | async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) 47 | : connection_(connection), request_(request), node_(node), inactive_(false) 48 | { 49 | topic_ = request.get_query_param_value_or_default("topic", ""); 50 | } 51 | 52 | ImageStreamer::~ImageStreamer() 53 | { 54 | } 55 | 56 | ImageTransportImageStreamer::ImageTransportImageStreamer( 57 | const async_web_server_cpp::HttpRequest & request, 58 | async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) 59 | : ImageStreamer(request, connection, node), it_(node), initialized_(false) 60 | { 61 | output_width_ = request.get_query_param_value_or_default("width", -1); 62 | output_height_ = request.get_query_param_value_or_default("height", -1); 63 | invert_ = request.has_query_param("invert"); 64 | default_transport_ = request.get_query_param_value_or_default("default_transport", "raw"); 65 | qos_profile_name_ = request.get_query_param_value_or_default("qos_profile", "default"); 66 | } 67 | 68 | ImageTransportImageStreamer::~ImageTransportImageStreamer() 69 | { 70 | } 71 | 72 | void ImageTransportImageStreamer::start() 73 | { 74 | image_transport::TransportHints hints(node_.get(), default_transport_); 75 | auto tnat = node_->get_topic_names_and_types(); 76 | inactive_ = true; 77 | for (auto topic_and_types : tnat) { 78 | if (topic_and_types.second.size() > 1) { 79 | // skip over topics with more than one type 80 | continue; 81 | } 82 | auto & topic_name = topic_and_types.first; 83 | if (topic_name == topic_ || (topic_name.find("/") == 0 && topic_name.substr(1) == topic_)) { 84 | inactive_ = false; 85 | break; 86 | } 87 | } 88 | 89 | // Get QoS profile from query parameter 90 | RCLCPP_INFO( 91 | node_->get_logger(), "Streaming topic %s with QoS profile %s", topic_.c_str(), 92 | qos_profile_name_.c_str()); 93 | auto qos_profile = get_qos_profile_from_name(qos_profile_name_); 94 | if (!qos_profile) { 95 | qos_profile = rmw_qos_profile_default; 96 | RCLCPP_ERROR( 97 | node_->get_logger(), 98 | "Invalid QoS profile %s specified. Using default profile.", 99 | qos_profile_name_.c_str()); 100 | } 101 | 102 | // Create subscriber 103 | image_sub_ = image_transport::create_subscription( 104 | node_.get(), topic_, 105 | std::bind(&ImageTransportImageStreamer::imageCallback, this, std::placeholders::_1), 106 | default_transport_, qos_profile.value()); 107 | } 108 | 109 | void ImageTransportImageStreamer::initialize(const cv::Mat &) 110 | { 111 | } 112 | 113 | void ImageTransportImageStreamer::restreamFrame(std::chrono::duration max_age) 114 | { 115 | if (inactive_ || !initialized_) { 116 | return; 117 | } 118 | try { 119 | if (last_frame_ + max_age < std::chrono::steady_clock::now()) { 120 | std::scoped_lock lock(send_mutex_); 121 | // don't update last_frame, it may remain an old value. 122 | sendImage(output_size_image, std::chrono::steady_clock::now()); 123 | } 124 | } catch (boost::system::system_error & e) { 125 | // happens when client disconnects 126 | RCLCPP_DEBUG(node_->get_logger(), "system_error exception: %s", e.what()); 127 | inactive_ = true; 128 | return; 129 | } catch (std::exception & e) { 130 | auto & clk = *node_->get_clock(); 131 | RCLCPP_ERROR_THROTTLE(node_->get_logger(), clk, 40, "exception: %s", e.what()); 132 | inactive_ = true; 133 | return; 134 | } catch (...) { 135 | auto & clk = *node_->get_clock(); 136 | RCLCPP_ERROR_THROTTLE(node_->get_logger(), clk, 40, "exception"); 137 | inactive_ = true; 138 | return; 139 | } 140 | } 141 | 142 | cv::Mat ImageTransportImageStreamer::decodeImage( 143 | const sensor_msgs::msg::Image::ConstSharedPtr & msg) 144 | { 145 | if (msg->encoding.find("F") != std::string::npos) { 146 | // scale floating point images 147 | cv::Mat float_image_bridge = cv_bridge::toCvCopy(msg, msg->encoding)->image; 148 | cv::Mat_ float_image = float_image_bridge; 149 | double max_val; 150 | cv::minMaxIdx(float_image, 0, &max_val); 151 | 152 | if (max_val > 0) { 153 | float_image *= (255 / max_val); 154 | } 155 | return float_image; 156 | } else { 157 | // Convert to OpenCV native BGR color 158 | return cv_bridge::toCvCopy(msg, "bgr8")->image; 159 | } 160 | } 161 | 162 | void ImageTransportImageStreamer::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg) 163 | { 164 | if (inactive_) { 165 | return; 166 | } 167 | 168 | cv::Mat img; 169 | try { 170 | img = decodeImage(msg); 171 | int input_width = img.cols; 172 | int input_height = img.rows; 173 | 174 | if (output_width_ == -1) { 175 | output_width_ = input_width; 176 | } 177 | if (output_height_ == -1) { 178 | output_height_ = input_height; 179 | } 180 | 181 | if (invert_) { 182 | // Rotate 180 degrees 183 | cv::flip(img, img, false); 184 | cv::flip(img, img, true); 185 | } 186 | 187 | std::scoped_lock lock(send_mutex_); // protects output_size_image 188 | if (output_width_ != input_width || output_height_ != input_height) { 189 | cv::Mat img_resized; 190 | cv::Size new_size(output_width_, output_height_); 191 | cv::resize(img, img_resized, new_size); 192 | output_size_image = img_resized; 193 | } else { 194 | output_size_image = img; 195 | } 196 | 197 | if (!initialized_) { 198 | initialize(output_size_image); 199 | initialized_ = true; 200 | } 201 | 202 | last_frame_ = std::chrono::steady_clock::now(); 203 | sendImage(output_size_image, last_frame_); 204 | } catch (cv_bridge::Exception & e) { 205 | auto & clk = *node_->get_clock(); 206 | RCLCPP_ERROR_THROTTLE(node_->get_logger(), clk, 40, "cv_bridge exception: %s", e.what()); 207 | inactive_ = true; 208 | return; 209 | } catch (cv::Exception & e) { 210 | auto & clk = *node_->get_clock(); 211 | RCLCPP_ERROR_THROTTLE(node_->get_logger(), clk, 40, "cv_bridge exception: %s", e.what()); 212 | inactive_ = true; 213 | return; 214 | } catch (boost::system::system_error & e) { 215 | // happens when client disconnects 216 | RCLCPP_DEBUG(node_->get_logger(), "system_error exception: %s", e.what()); 217 | inactive_ = true; 218 | return; 219 | } catch (std::exception & e) { 220 | auto & clk = *node_->get_clock(); 221 | RCLCPP_ERROR_THROTTLE(node_->get_logger(), clk, 40, "exception: %s", e.what()); 222 | inactive_ = true; 223 | return; 224 | } catch (...) { 225 | auto & clk = *node_->get_clock(); 226 | RCLCPP_ERROR_THROTTLE(node_->get_logger(), clk, 40, "exception"); 227 | inactive_ = true; 228 | return; 229 | } 230 | } 231 | 232 | } // namespace web_video_server 233 | -------------------------------------------------------------------------------- /src/jpeg_streamers.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, Worcester Polytechnic Institute 2 | // Copyright (c) 2024, The Robot Web Tools Contributors 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // * Neither the name of the copyright holder nor the names of its 16 | // contributors may be used to endorse or promote products derived from 17 | // this software without specific prior written permission. 18 | // 19 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | // POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #include "web_video_server/jpeg_streamers.hpp" 32 | #include "async_web_server_cpp/http_reply.hpp" 33 | 34 | namespace web_video_server 35 | { 36 | 37 | MjpegStreamer::MjpegStreamer( 38 | const async_web_server_cpp::HttpRequest & request, 39 | async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) 40 | : ImageTransportImageStreamer(request, connection, node), 41 | stream_(connection) 42 | { 43 | quality_ = request.get_query_param_value_or_default("quality", 95); 44 | stream_.sendInitialHeader(); 45 | } 46 | 47 | MjpegStreamer::~MjpegStreamer() 48 | { 49 | this->inactive_ = true; 50 | std::scoped_lock lock(send_mutex_); // protects sendImage. 51 | } 52 | 53 | void MjpegStreamer::sendImage( 54 | const cv::Mat & img, 55 | const std::chrono::steady_clock::time_point & time) 56 | { 57 | std::vector encode_params; 58 | encode_params.push_back(cv::IMWRITE_JPEG_QUALITY); 59 | encode_params.push_back(quality_); 60 | 61 | std::vector encoded_buffer; 62 | cv::imencode(".jpeg", img, encoded_buffer, encode_params); 63 | 64 | stream_.sendPartAndClear(time, "image/jpeg", encoded_buffer); 65 | } 66 | 67 | std::shared_ptr MjpegStreamerType::create_streamer( 68 | const async_web_server_cpp::HttpRequest & request, 69 | async_web_server_cpp::HttpConnectionPtr connection, 70 | rclcpp::Node::SharedPtr node) 71 | { 72 | return std::make_shared(request, connection, node); 73 | } 74 | 75 | std::string MjpegStreamerType::create_viewer(const async_web_server_cpp::HttpRequest & request) 76 | { 77 | std::stringstream ss; 78 | ss << ""; 81 | return ss.str(); 82 | } 83 | 84 | JpegSnapshotStreamer::JpegSnapshotStreamer( 85 | const async_web_server_cpp::HttpRequest & request, 86 | async_web_server_cpp::HttpConnectionPtr connection, 87 | rclcpp::Node::SharedPtr node) 88 | : ImageTransportImageStreamer(request, connection, node) 89 | { 90 | quality_ = request.get_query_param_value_or_default("quality", 95); 91 | } 92 | 93 | JpegSnapshotStreamer::~JpegSnapshotStreamer() 94 | { 95 | this->inactive_ = true; 96 | std::scoped_lock lock(send_mutex_); // protects sendImage. 97 | } 98 | 99 | void JpegSnapshotStreamer::sendImage( 100 | const cv::Mat & img, 101 | const std::chrono::steady_clock::time_point & time) 102 | { 103 | std::vector encode_params; 104 | encode_params.push_back(cv::IMWRITE_JPEG_QUALITY); 105 | encode_params.push_back(quality_); 106 | 107 | std::vector encoded_buffer; 108 | cv::imencode(".jpeg", img, encoded_buffer, encode_params); 109 | 110 | char stamp[20]; 111 | snprintf( 112 | stamp, sizeof(stamp), "%.06lf", 113 | std::chrono::duration_cast>(time.time_since_epoch()).count()); 114 | async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok) 115 | .header("Connection", "close") 116 | .header("Server", "web_video_server") 117 | .header( 118 | "Cache-Control", 119 | "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0") 120 | .header("X-Timestamp", stamp) 121 | .header("Pragma", "no-cache") 122 | .header("Content-type", "image/jpeg") 123 | .header("Access-Control-Allow-Origin", "*") 124 | .header("Content-Length", std::to_string(encoded_buffer.size())) 125 | .write(connection_); 126 | connection_->write_and_clear(encoded_buffer); 127 | inactive_ = true; 128 | } 129 | 130 | } // namespace web_video_server 131 | -------------------------------------------------------------------------------- /src/libav_streamer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, Worcester Polytechnic Institute 2 | // Copyright (c) 2024, The Robot Web Tools Contributors 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // * Neither the name of the copyright holder nor the names of its 16 | // contributors may be used to endorse or promote products derived from 17 | // this software without specific prior written permission. 18 | // 19 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | // POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #include "web_video_server/libav_streamer.hpp" 32 | #include "async_web_server_cpp/http_reply.hpp" 33 | 34 | // https://stackoverflow.com/questions/46884682/error-in-building-opencv-with-ffmpeg 35 | #define AV_CODEC_FLAG_GLOBAL_HEADER (1 << 22) 36 | #define CODEC_FLAG_GLOBAL_HEADER AV_CODEC_FLAG_GLOBAL_HEADER 37 | 38 | namespace web_video_server 39 | { 40 | 41 | LibavStreamer::LibavStreamer( 42 | const async_web_server_cpp::HttpRequest & request, 43 | async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node, 44 | const std::string & format_name, const std::string & codec_name, 45 | const std::string & content_type) 46 | : ImageTransportImageStreamer(request, connection, node), format_context_(0), codec_(0), 47 | codec_context_(0), video_stream_(0), opt_(0), frame_(0), sws_context_(0), 48 | first_image_received_(false), first_image_time_(), format_name_(format_name), 49 | codec_name_(codec_name), content_type_(content_type), io_buffer_(0) 50 | { 51 | bitrate_ = request.get_query_param_value_or_default("bitrate", 100000); 52 | qmin_ = request.get_query_param_value_or_default("qmin", 10); 53 | qmax_ = request.get_query_param_value_or_default("qmax", 42); 54 | gop_ = request.get_query_param_value_or_default("gop", 25); 55 | } 56 | 57 | LibavStreamer::~LibavStreamer() 58 | { 59 | if (codec_context_) { 60 | avcodec_free_context(&codec_context_); 61 | } 62 | if (frame_) { 63 | av_frame_free(&frame_); 64 | } 65 | if (io_buffer_) { 66 | delete io_buffer_; 67 | } 68 | if (format_context_) { 69 | if (format_context_->pb) { 70 | av_free(format_context_->pb); 71 | } 72 | avformat_free_context(format_context_); 73 | } 74 | if (sws_context_) { 75 | sws_freeContext(sws_context_); 76 | } 77 | } 78 | 79 | // output callback for ffmpeg IO context 80 | static int dispatch_output_packet(void * opaque, uint8_t * buffer, int buffer_size) 81 | { 82 | async_web_server_cpp::HttpConnectionPtr connection = 83 | *((async_web_server_cpp::HttpConnectionPtr *) opaque); 84 | std::vector encoded_frame; 85 | encoded_frame.assign(buffer, buffer + buffer_size); 86 | connection->write_and_clear(encoded_frame); 87 | return 0; 88 | } 89 | 90 | void LibavStreamer::initialize(const cv::Mat & /* img */) 91 | { 92 | // Load format 93 | format_context_ = avformat_alloc_context(); 94 | if (!format_context_) { 95 | async_web_server_cpp::HttpReply::stock_reply( 96 | async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL); 97 | throw std::runtime_error("Error allocating ffmpeg format context"); 98 | } 99 | 100 | format_context_->oformat = av_guess_format(format_name_.c_str(), NULL, NULL); 101 | if (!format_context_->oformat) { 102 | async_web_server_cpp::HttpReply::stock_reply( 103 | async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL); 104 | throw std::runtime_error("Error looking up output format"); 105 | } 106 | 107 | // Set up custom IO callback. 108 | size_t io_buffer_size = 3 * 1024; // 3M seen elsewhere and adjudged good 109 | io_buffer_ = new unsigned char[io_buffer_size]; 110 | AVIOContext * io_ctx = avio_alloc_context( 111 | io_buffer_, io_buffer_size, AVIO_FLAG_WRITE, 112 | &connection_, NULL, dispatch_output_packet, NULL); 113 | if (!io_ctx) { 114 | async_web_server_cpp::HttpReply::stock_reply( 115 | async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL); 116 | throw std::runtime_error("Error setting up IO context"); 117 | } 118 | io_ctx->seekable = 0; // no seeking, it's a stream 119 | format_context_->pb = io_ctx; 120 | format_context_->max_interleave_delta = 0; 121 | 122 | // Load codec 123 | if (codec_name_.empty()) { // use default codec if none specified 124 | codec_ = avcodec_find_encoder(format_context_->oformat->video_codec); 125 | } else { 126 | codec_ = avcodec_find_encoder_by_name(codec_name_.c_str()); 127 | } 128 | if (!codec_) { 129 | async_web_server_cpp::HttpReply::stock_reply( 130 | async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL); 131 | throw std::runtime_error("Error looking up codec"); 132 | } 133 | video_stream_ = avformat_new_stream(format_context_, codec_); 134 | if (!video_stream_) { 135 | async_web_server_cpp::HttpReply::stock_reply( 136 | async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL); 137 | throw std::runtime_error("Error creating video stream"); 138 | } 139 | 140 | codec_context_ = avcodec_alloc_context3(codec_); 141 | 142 | // Set options 143 | codec_context_->codec_id = codec_->id; 144 | codec_context_->bit_rate = bitrate_; 145 | 146 | codec_context_->width = output_width_; 147 | codec_context_->height = output_height_; 148 | codec_context_->delay = 0; 149 | 150 | video_stream_->time_base.num = 1; 151 | video_stream_->time_base.den = 1000; 152 | 153 | codec_context_->time_base.num = 1; 154 | codec_context_->time_base.den = 1; 155 | codec_context_->gop_size = gop_; 156 | codec_context_->pix_fmt = AV_PIX_FMT_YUV420P; 157 | codec_context_->max_b_frames = 0; 158 | 159 | // Quality settings 160 | codec_context_->qmin = qmin_; 161 | codec_context_->qmax = qmax_; 162 | 163 | codec_context_->flags |= AV_CODEC_FLAG_LOW_DELAY; 164 | 165 | initializeEncoder(); 166 | 167 | avcodec_parameters_from_context(video_stream_->codecpar, codec_context_); 168 | 169 | // Open Codec 170 | if (avcodec_open2(codec_context_, codec_, NULL) < 0) { 171 | async_web_server_cpp::HttpReply::stock_reply( 172 | async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL); 173 | throw std::runtime_error("Could not open video codec"); 174 | } 175 | 176 | // Allocate frame buffers 177 | frame_ = av_frame_alloc(); 178 | 179 | av_image_alloc( 180 | frame_->data, frame_->linesize, output_width_, output_height_, 181 | codec_context_->pix_fmt, 1); 182 | 183 | frame_->width = output_width_; 184 | frame_->height = output_height_; 185 | frame_->format = codec_context_->pix_fmt; 186 | 187 | // define meta data 188 | av_dict_set(&format_context_->metadata, "author", "ROS web_video_server", 0); 189 | av_dict_set(&format_context_->metadata, "title", topic_.c_str(), 0); 190 | 191 | // Send response headers 192 | async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok) 193 | .header("Connection", "close") 194 | .header("Server", "web_video_server") 195 | .header( 196 | "Cache-Control", 197 | "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0") 198 | .header("Pragma", "no-cache") 199 | .header("Expires", "0") 200 | .header("Max-Age", "0") 201 | .header("Trailer", "Expires") 202 | .header("Content-type", content_type_) 203 | .header("Access-Control-Allow-Origin", "*") 204 | .write(connection_); 205 | 206 | // Send video stream header 207 | if (avformat_write_header(format_context_, &opt_) < 0) { 208 | async_web_server_cpp::HttpReply::stock_reply( 209 | async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL); 210 | throw std::runtime_error("Error openning dynamic buffer"); 211 | } 212 | } 213 | 214 | void LibavStreamer::initializeEncoder() 215 | { 216 | } 217 | 218 | void LibavStreamer::sendImage( 219 | const cv::Mat & img, 220 | const std::chrono::steady_clock::time_point & time) 221 | { 222 | std::scoped_lock lock(encode_mutex_); 223 | if (!first_image_received_) { 224 | first_image_received_ = true; 225 | first_image_time_ = time; 226 | } 227 | 228 | AVPixelFormat input_coding_format = AV_PIX_FMT_BGR24; 229 | 230 | AVFrame * raw_frame = av_frame_alloc(); 231 | av_image_fill_arrays( 232 | raw_frame->data, raw_frame->linesize, 233 | img.data, input_coding_format, output_width_, output_height_, 1); 234 | 235 | // Convert from opencv to libav 236 | if (!sws_context_) { 237 | static int sws_flags = SWS_BICUBIC; 238 | sws_context_ = sws_getContext( 239 | output_width_, output_height_, input_coding_format, output_width_, 240 | output_height_, codec_context_->pix_fmt, sws_flags, NULL, NULL, NULL); 241 | if (!sws_context_) { 242 | throw std::runtime_error("Could not initialize the conversion context"); 243 | } 244 | } 245 | 246 | 247 | int ret = sws_scale( 248 | sws_context_, 249 | (const uint8_t * const *)raw_frame->data, raw_frame->linesize, 0, 250 | output_height_, frame_->data, frame_->linesize); 251 | 252 | av_frame_free(&raw_frame); 253 | 254 | // Encode the frame 255 | AVPacket * pkt = av_packet_alloc(); 256 | 257 | ret = avcodec_send_frame(codec_context_, frame_); 258 | if (ret == AVERROR_EOF) { 259 | RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_send_frame() encoder flushed\n"); 260 | } else if (ret == AVERROR(EAGAIN)) { 261 | RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_send_frame() need output read out\n"); 262 | } 263 | if (ret < 0) { 264 | throw std::runtime_error("Error encoding video frame"); 265 | } 266 | 267 | ret = avcodec_receive_packet(codec_context_, pkt); 268 | bool got_packet = pkt->size > 0; 269 | if (ret == AVERROR_EOF) { 270 | RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_receive_packet() encoder flushed\n"); 271 | } else if (ret == AVERROR(EAGAIN)) { 272 | RCLCPP_DEBUG_STREAM(node_->get_logger(), "avcodec_receive_packet() needs more input\n"); 273 | got_packet = false; 274 | } 275 | 276 | if (got_packet) { 277 | double seconds = std::chrono::duration_cast>( 278 | time - first_image_time_).count(); 279 | // Encode video at 1/0.95 to minimize delay 280 | pkt->pts = (int64_t)(seconds / av_q2d(video_stream_->time_base) * 0.95); 281 | if (pkt->pts <= 0) { 282 | pkt->pts = 1; 283 | } 284 | pkt->dts = pkt->pts; 285 | 286 | if (pkt->flags & AV_PKT_FLAG_KEY) { 287 | pkt->flags |= AV_PKT_FLAG_KEY; 288 | } 289 | 290 | pkt->stream_index = video_stream_->index; 291 | 292 | if (av_write_frame(format_context_, pkt)) { 293 | throw std::runtime_error("Error when writing frame"); 294 | } 295 | } 296 | 297 | av_packet_unref(pkt); 298 | } 299 | 300 | LibavStreamerType::LibavStreamerType( 301 | const std::string & format_name, const std::string & codec_name, 302 | const std::string & content_type) 303 | : format_name_(format_name), codec_name_(codec_name), content_type_(content_type) 304 | { 305 | } 306 | 307 | std::shared_ptr LibavStreamerType::create_streamer( 308 | const async_web_server_cpp::HttpRequest & request, 309 | async_web_server_cpp::HttpConnectionPtr connection, 310 | rclcpp::Node::SharedPtr node) 311 | { 312 | return std::make_shared( 313 | request, connection, node, format_name_, codec_name_, 314 | content_type_); 315 | } 316 | 317 | std::string LibavStreamerType::create_viewer(const async_web_server_cpp::HttpRequest & request) 318 | { 319 | std::stringstream ss; 320 | ss << ""; 323 | return ss.str(); 324 | } 325 | 326 | } // namespace web_video_server 327 | -------------------------------------------------------------------------------- /src/multipart_stream.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, Worcester Polytechnic Institute 2 | // Copyright (c) 2024, The Robot Web Tools Contributors 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // * Neither the name of the copyright holder nor the names of its 16 | // contributors may be used to endorse or promote products derived from 17 | // this software without specific prior written permission. 18 | // 19 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | // POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #include "web_video_server/multipart_stream.hpp" 32 | #include "async_web_server_cpp/http_reply.hpp" 33 | 34 | namespace web_video_server 35 | { 36 | 37 | MultipartStream::MultipartStream( 38 | async_web_server_cpp::HttpConnectionPtr & connection, 39 | const std::string & boundry, 40 | std::size_t max_queue_size) 41 | : max_queue_size_(max_queue_size), connection_(connection), boundry_(boundry) 42 | {} 43 | 44 | void MultipartStream::sendInitialHeader() 45 | { 46 | async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok) 47 | .header("Connection", "close") 48 | .header("Server", "web_video_server") 49 | .header( 50 | "Cache-Control", 51 | "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0") 52 | .header("Pragma", "no-cache") 53 | .header("Content-type", "multipart/x-mixed-replace;boundary=" + boundry_) 54 | .header("Access-Control-Allow-Origin", "*") 55 | .write(connection_); 56 | connection_->write("--" + boundry_ + "\r\n"); 57 | } 58 | 59 | void MultipartStream::sendPartHeader( 60 | const std::chrono::steady_clock::time_point & time, const std::string & type, 61 | size_t payload_size) 62 | { 63 | char stamp[20]; 64 | snprintf( 65 | stamp, sizeof(stamp), "%.06lf", 66 | std::chrono::duration_cast>(time.time_since_epoch()).count()); 67 | std::shared_ptr> headers( 68 | new std::vector()); 69 | headers->push_back(async_web_server_cpp::HttpHeader("Content-type", type)); 70 | headers->push_back(async_web_server_cpp::HttpHeader("X-Timestamp", stamp)); 71 | headers->push_back( 72 | async_web_server_cpp::HttpHeader( 73 | "Content-Length", std::to_string(payload_size))); 74 | connection_->write(async_web_server_cpp::HttpReply::to_buffers(*headers), headers); 75 | } 76 | 77 | void MultipartStream::sendPartFooter(const std::chrono::steady_clock::time_point & time) 78 | { 79 | std::shared_ptr str(new std::string("\r\n--" + boundry_ + "\r\n")); 80 | PendingFooter pf; 81 | pf.timestamp = time; 82 | pf.contents = str; 83 | connection_->write(boost::asio::buffer(*str), str); 84 | if (max_queue_size_ > 0) {pending_footers_.push(pf);} 85 | } 86 | 87 | void MultipartStream::sendPartAndClear( 88 | const std::chrono::steady_clock::time_point & time, const std::string & type, 89 | std::vector & data) 90 | { 91 | if (!isBusy()) { 92 | sendPartHeader(time, type, data.size()); 93 | connection_->write_and_clear(data); 94 | sendPartFooter(time); 95 | } 96 | } 97 | 98 | void MultipartStream::sendPart( 99 | const std::chrono::steady_clock::time_point & time, const std::string & type, 100 | const boost::asio::const_buffer & buffer, 101 | async_web_server_cpp::HttpConnection::ResourcePtr resource) 102 | { 103 | if (!isBusy()) { 104 | sendPartHeader(time, type, boost::asio::buffer_size(buffer)); 105 | connection_->write(buffer, resource); 106 | sendPartFooter(time); 107 | } 108 | } 109 | 110 | bool MultipartStream::isBusy() 111 | { 112 | auto current_time = std::chrono::steady_clock::now(); 113 | while (!pending_footers_.empty()) { 114 | if (pending_footers_.front().contents.expired()) { 115 | pending_footers_.pop(); 116 | } else { 117 | auto footer_time = pending_footers_.front().timestamp; 118 | if (std::chrono::duration_cast>( 119 | (current_time - footer_time)).count() > 0.5) 120 | { 121 | pending_footers_.pop(); 122 | } else { 123 | break; 124 | } 125 | } 126 | } 127 | return !(max_queue_size_ == 0 || pending_footers_.size() < max_queue_size_); 128 | } 129 | 130 | } // namespace web_video_server 131 | -------------------------------------------------------------------------------- /src/png_streamers.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024, The Robot Web Tools Contributors 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from 16 | // this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | 30 | #include "web_video_server/png_streamers.hpp" 31 | #include "async_web_server_cpp/http_reply.hpp" 32 | 33 | #ifdef CV_BRIDGE_USES_OLD_HEADERS 34 | #include "cv_bridge/cv_bridge.h" 35 | #else 36 | #include "cv_bridge/cv_bridge.hpp" 37 | #endif 38 | 39 | namespace web_video_server 40 | { 41 | 42 | PngStreamer::PngStreamer( 43 | const async_web_server_cpp::HttpRequest & request, 44 | async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) 45 | : ImageTransportImageStreamer(request, connection, node), stream_(connection) 46 | { 47 | quality_ = request.get_query_param_value_or_default("quality", 3); 48 | stream_.sendInitialHeader(); 49 | } 50 | 51 | PngStreamer::~PngStreamer() 52 | { 53 | this->inactive_ = true; 54 | std::scoped_lock lock(send_mutex_); // protects sendImage. 55 | } 56 | 57 | cv::Mat PngStreamer::decodeImage(const sensor_msgs::msg::Image::ConstSharedPtr & msg) 58 | { 59 | // Handle alpha values since PNG supports it 60 | if (sensor_msgs::image_encodings::hasAlpha(msg->encoding)) { 61 | return cv_bridge::toCvCopy(msg, "bgra8")->image; 62 | } else { 63 | // Use the normal decode otherwise 64 | return ImageTransportImageStreamer::decodeImage(msg); 65 | } 66 | } 67 | 68 | void PngStreamer::sendImage(const cv::Mat & img, const std::chrono::steady_clock::time_point & time) 69 | { 70 | std::vector encode_params; 71 | encode_params.push_back(cv::IMWRITE_PNG_COMPRESSION); 72 | encode_params.push_back(quality_); 73 | 74 | std::vector encoded_buffer; 75 | cv::imencode(".png", img, encoded_buffer, encode_params); 76 | 77 | stream_.sendPartAndClear(time, "image/png", encoded_buffer); 78 | } 79 | 80 | std::shared_ptr PngStreamerType::create_streamer( 81 | const async_web_server_cpp::HttpRequest & request, 82 | async_web_server_cpp::HttpConnectionPtr connection, 83 | rclcpp::Node::SharedPtr node) 84 | { 85 | return std::make_shared(request, connection, node); 86 | } 87 | 88 | std::string PngStreamerType::create_viewer(const async_web_server_cpp::HttpRequest & request) 89 | { 90 | std::stringstream ss; 91 | ss << ""; 94 | return ss.str(); 95 | } 96 | 97 | PngSnapshotStreamer::PngSnapshotStreamer( 98 | const async_web_server_cpp::HttpRequest & request, 99 | async_web_server_cpp::HttpConnectionPtr connection, 100 | rclcpp::Node::SharedPtr node) 101 | : ImageTransportImageStreamer(request, connection, node) 102 | { 103 | quality_ = request.get_query_param_value_or_default("quality", 3); 104 | } 105 | 106 | PngSnapshotStreamer::~PngSnapshotStreamer() 107 | { 108 | this->inactive_ = true; 109 | std::scoped_lock lock(send_mutex_); // protects sendImage. 110 | } 111 | 112 | cv::Mat PngSnapshotStreamer::decodeImage(const sensor_msgs::msg::Image::ConstSharedPtr & msg) 113 | { 114 | // Handle alpha values since PNG supports it 115 | if (sensor_msgs::image_encodings::hasAlpha(msg->encoding)) { 116 | return cv_bridge::toCvCopy(msg, "bgra8")->image; 117 | } else { 118 | // Use the normal decode otherwise 119 | return ImageTransportImageStreamer::decodeImage(msg); 120 | } 121 | } 122 | 123 | void PngSnapshotStreamer::sendImage( 124 | const cv::Mat & img, 125 | const std::chrono::steady_clock::time_point & time) 126 | { 127 | std::vector encode_params; 128 | encode_params.push_back(cv::IMWRITE_PNG_COMPRESSION); 129 | encode_params.push_back(quality_); 130 | 131 | std::vector encoded_buffer; 132 | cv::imencode(".png", img, encoded_buffer, encode_params); 133 | 134 | char stamp[20]; 135 | snprintf( 136 | stamp, sizeof(stamp), "%.06lf", 137 | std::chrono::duration_cast>(time.time_since_epoch()).count()); 138 | async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok) 139 | .header("Connection", "close") 140 | .header("Server", "web_video_server") 141 | .header( 142 | "Cache-Control", 143 | "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0") 144 | .header("X-Timestamp", stamp) 145 | .header("Pragma", "no-cache") 146 | .header("Content-type", "image/png") 147 | .header("Access-Control-Allow-Origin", "*") 148 | .header("Content-Length", std::to_string(encoded_buffer.size())) 149 | .write(connection_); 150 | connection_->write_and_clear(encoded_buffer); 151 | inactive_ = true; 152 | } 153 | 154 | } // namespace web_video_server 155 | -------------------------------------------------------------------------------- /src/ros_compressed_streamer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, Worcester Polytechnic Institute 2 | // Copyright (c) 2024, The Robot Web Tools Contributors 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // * Neither the name of the copyright holder nor the names of its 16 | // contributors may be used to endorse or promote products derived from 17 | // this software without specific prior written permission. 18 | // 19 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | // POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #include "web_video_server/ros_compressed_streamer.hpp" 32 | 33 | namespace web_video_server 34 | { 35 | 36 | RosCompressedStreamer::RosCompressedStreamer( 37 | const async_web_server_cpp::HttpRequest & request, 38 | async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) 39 | : ImageStreamer(request, connection, node), stream_(connection) 40 | { 41 | stream_.sendInitialHeader(); 42 | qos_profile_name_ = request.get_query_param_value_or_default("qos_profile", "default"); 43 | } 44 | 45 | RosCompressedStreamer::~RosCompressedStreamer() 46 | { 47 | this->inactive_ = true; 48 | std::scoped_lock lock(send_mutex_); // protects sendImage. 49 | } 50 | 51 | void RosCompressedStreamer::start() 52 | { 53 | const std::string compressed_topic = topic_ + "/compressed"; 54 | 55 | // Get QoS profile from query parameter 56 | RCLCPP_INFO( 57 | node_->get_logger(), "Streaming topic %s with QoS profile %s", 58 | compressed_topic.c_str(), qos_profile_name_.c_str()); 59 | auto qos_profile = get_qos_profile_from_name(qos_profile_name_); 60 | if (!qos_profile) { 61 | qos_profile = rmw_qos_profile_default; 62 | RCLCPP_ERROR( 63 | node_->get_logger(), 64 | "Invalid QoS profile %s specified. Using default profile.", 65 | qos_profile_name_.c_str()); 66 | } 67 | 68 | // Create subscriber 69 | const auto qos = rclcpp::QoS( 70 | rclcpp::QoSInitialization(qos_profile.value().history, 1), 71 | qos_profile.value()); 72 | image_sub_ = node_->create_subscription( 73 | compressed_topic, qos, 74 | std::bind(&RosCompressedStreamer::imageCallback, this, std::placeholders::_1)); 75 | } 76 | 77 | void RosCompressedStreamer::restreamFrame(std::chrono::duration max_age) 78 | { 79 | if (inactive_ || (last_msg == 0)) { 80 | return; 81 | } 82 | 83 | if (last_frame_ + max_age < std::chrono::steady_clock::now()) { 84 | std::scoped_lock lock(send_mutex_); 85 | // don't update last_frame, it may remain an old value. 86 | sendImage(last_msg, std::chrono::steady_clock::now()); 87 | } 88 | } 89 | 90 | void RosCompressedStreamer::sendImage( 91 | const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg, 92 | const std::chrono::steady_clock::time_point & time) 93 | { 94 | try { 95 | std::string content_type; 96 | if (msg->format.find("jpeg") != std::string::npos || 97 | msg->format.find("jpg") != std::string::npos) 98 | { 99 | content_type = "image/jpeg"; 100 | } else if (msg->format.find("png") != std::string::npos) { 101 | content_type = "image/png"; 102 | } else { 103 | RCLCPP_WARN( 104 | node_->get_logger(), "Unknown ROS compressed image format: %s", 105 | msg->format.c_str()); 106 | return; 107 | } 108 | 109 | stream_.sendPart(time, content_type, boost::asio::buffer(msg->data), msg); 110 | } catch (boost::system::system_error & e) { 111 | // happens when client disconnects 112 | RCLCPP_DEBUG(node_->get_logger(), "system_error exception: %s", e.what()); 113 | inactive_ = true; 114 | return; 115 | } catch (std::exception & e) { 116 | auto & clk = *node_->get_clock(); 117 | RCLCPP_ERROR_THROTTLE(node_->get_logger(), clk, 40, "exception: %s", e.what()); 118 | inactive_ = true; 119 | return; 120 | } catch (...) { 121 | auto & clk = *node_->get_clock(); 122 | RCLCPP_ERROR_THROTTLE(node_->get_logger(), clk, 40, "exception"); 123 | inactive_ = true; 124 | return; 125 | } 126 | } 127 | 128 | 129 | void RosCompressedStreamer::imageCallback( 130 | const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg) 131 | { 132 | std::scoped_lock lock(send_mutex_); // protects last_msg and last_frame 133 | last_msg = msg; 134 | last_frame_ = std::chrono::steady_clock::now(); 135 | sendImage(last_msg, last_frame_); 136 | } 137 | 138 | 139 | std::shared_ptr RosCompressedStreamerType::create_streamer( 140 | const async_web_server_cpp::HttpRequest & request, 141 | async_web_server_cpp::HttpConnectionPtr connection, 142 | rclcpp::Node::SharedPtr node) 143 | { 144 | return std::make_shared(request, connection, node); 145 | } 146 | 147 | std::string RosCompressedStreamerType::create_viewer( 148 | const async_web_server_cpp::HttpRequest & request) 149 | { 150 | std::stringstream ss; 151 | ss << ""; 154 | return ss.str(); 155 | } 156 | 157 | } // namespace web_video_server 158 | -------------------------------------------------------------------------------- /src/utils.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024, The Robot Web Tools Contributors 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from 16 | // this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | 30 | #include "web_video_server/utils.hpp" 31 | 32 | namespace web_video_server 33 | { 34 | 35 | std::optional get_qos_profile_from_name(const std::string name) 36 | { 37 | if (name == "default") { 38 | return rmw_qos_profile_default; 39 | } else if (name == "system_default") { 40 | return rmw_qos_profile_system_default; 41 | } else if (name == "sensor_data") { 42 | return rmw_qos_profile_sensor_data; 43 | } else { 44 | return std::nullopt; 45 | } 46 | } 47 | 48 | } // namespace web_video_server 49 | -------------------------------------------------------------------------------- /src/vp8_streamer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, Worcester Polytechnic Institute 2 | // Copyright (c) 2024, The Robot Web Tools Contributors 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // * Neither the name of the copyright holder nor the names of its 16 | // contributors may be used to endorse or promote products derived from 17 | // this software without specific prior written permission. 18 | // 19 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | // POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #include "web_video_server/vp8_streamer.hpp" 32 | 33 | namespace web_video_server 34 | { 35 | 36 | Vp8Streamer::Vp8Streamer( 37 | const async_web_server_cpp::HttpRequest & request, 38 | async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) 39 | : LibavStreamer(request, connection, node, "webm", "libvpx", "video/webm") 40 | { 41 | quality_ = request.get_query_param_value_or_default("quality", "realtime"); 42 | } 43 | Vp8Streamer::~Vp8Streamer() 44 | { 45 | } 46 | 47 | void Vp8Streamer::initializeEncoder() 48 | { 49 | typedef std::map AvOptMap; 50 | AvOptMap av_opt_map; 51 | av_opt_map["quality"] = quality_; 52 | av_opt_map["deadline"] = "1"; 53 | av_opt_map["auto-alt-ref"] = "0"; 54 | av_opt_map["lag-in-frames"] = "1"; 55 | av_opt_map["rc_lookahead"] = "1"; 56 | av_opt_map["drop_frame"] = "1"; 57 | av_opt_map["error-resilient"] = "1"; 58 | 59 | for (auto & opt : av_opt_map) { 60 | av_opt_set(codec_context_->priv_data, opt.first.c_str(), opt.second.c_str(), 0); 61 | } 62 | 63 | // Buffering settings 64 | int bufsize = 10; 65 | codec_context_->rc_buffer_size = bufsize; 66 | codec_context_->rc_initial_buffer_occupancy = bufsize; // bitrate/3; 67 | av_opt_set_int(codec_context_->priv_data, "bufsize", bufsize, 0); 68 | av_opt_set_int(codec_context_->priv_data, "buf-initial", bufsize, 0); 69 | av_opt_set_int(codec_context_->priv_data, "buf-optimal", bufsize, 0); 70 | av_opt_set_int(codec_context_->priv_data, "skip_threshold", 10, 0); 71 | } 72 | 73 | Vp8StreamerType::Vp8StreamerType() 74 | : LibavStreamerType("webm", "libvpx", "video/webm") 75 | { 76 | } 77 | 78 | std::shared_ptr Vp8StreamerType::create_streamer( 79 | const async_web_server_cpp::HttpRequest & request, 80 | async_web_server_cpp::HttpConnectionPtr connection, 81 | rclcpp::Node::SharedPtr node) 82 | { 83 | return std::make_shared(request, connection, node); 84 | } 85 | 86 | } // namespace web_video_server 87 | -------------------------------------------------------------------------------- /src/vp9_streamer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024, The Robot Web Tools Contributors 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // 14 | // * Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from 16 | // this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | 30 | #include "web_video_server/vp9_streamer.hpp" 31 | 32 | namespace web_video_server 33 | { 34 | 35 | Vp9Streamer::Vp9Streamer( 36 | const async_web_server_cpp::HttpRequest & request, 37 | async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node) 38 | : LibavStreamer(request, connection, node, "webm", "libvpx-vp9", "video/webm") 39 | { 40 | } 41 | Vp9Streamer::~Vp9Streamer() 42 | { 43 | } 44 | 45 | void Vp9Streamer::initializeEncoder() 46 | { 47 | // codec options set up to provide somehow reasonable performance in cost of poor quality 48 | // should be updated as soon as VP9 encoding matures 49 | av_opt_set_int(codec_context_->priv_data, "pass", 1, 0); 50 | av_opt_set_int(codec_context_->priv_data, "speed", 8, 0); 51 | av_opt_set_int(codec_context_->priv_data, "cpu-used", 4, 0); // 8 is max 52 | av_opt_set_int(codec_context_->priv_data, "crf", 20, 0); // 0..63 (higher is lower quality) 53 | } 54 | 55 | Vp9StreamerType::Vp9StreamerType() 56 | : LibavStreamerType("webm", "libvpx-vp9", "video/webm") 57 | { 58 | } 59 | 60 | std::shared_ptr Vp9StreamerType::create_streamer( 61 | const async_web_server_cpp::HttpRequest & request, 62 | async_web_server_cpp::HttpConnectionPtr connection, 63 | rclcpp::Node::SharedPtr node) 64 | { 65 | return std::make_shared(request, connection, node); 66 | } 67 | 68 | } // namespace web_video_server 69 | -------------------------------------------------------------------------------- /src/web_video_server.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, Worcester Polytechnic Institute 2 | // Copyright (c) 2024, The Robot Web Tools Contributors 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // * Neither the name of the copyright holder nor the names of its 16 | // contributors may be used to endorse or promote products derived from 17 | // this software without specific prior written permission. 18 | // 19 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | // POSSIBILITY OF SUCH DAMAGE. 30 | 31 | #include "web_video_server/web_video_server.hpp" 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include "rclcpp/rclcpp.hpp" 40 | 41 | #include "sensor_msgs/image_encodings.hpp" 42 | #include "web_video_server/ros_compressed_streamer.hpp" 43 | #include "web_video_server/jpeg_streamers.hpp" 44 | #include "web_video_server/png_streamers.hpp" 45 | #include "web_video_server/vp8_streamer.hpp" 46 | #include "web_video_server/h264_streamer.hpp" 47 | #include "web_video_server/vp9_streamer.hpp" 48 | #include "async_web_server_cpp/http_reply.hpp" 49 | 50 | using namespace std::chrono_literals; 51 | using namespace boost::placeholders; // NOLINT 52 | 53 | namespace web_video_server 54 | { 55 | 56 | WebVideoServer::WebVideoServer(const rclcpp::NodeOptions & options) 57 | : rclcpp::Node("web_video_server", options), handler_group_( 58 | async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::not_found)) 59 | { 60 | declare_parameter("port", 8080); 61 | declare_parameter("verbose", true); 62 | declare_parameter("address", "0.0.0.0"); 63 | declare_parameter("server_threads", 1); 64 | declare_parameter("publish_rate", -1.0); 65 | declare_parameter("default_stream_type", "mjpeg"); 66 | 67 | get_parameter("port", port_); 68 | get_parameter("verbose", verbose_); 69 | get_parameter("address", address_); 70 | int server_threads; 71 | get_parameter("server_threads", server_threads); 72 | get_parameter("publish_rate", publish_rate_); 73 | get_parameter("default_stream_type", default_stream_type_); 74 | 75 | stream_types_["mjpeg"] = std::make_shared(); 76 | stream_types_["png"] = std::make_shared(); 77 | stream_types_["ros_compressed"] = std::make_shared(); 78 | stream_types_["vp8"] = std::make_shared(); 79 | stream_types_["h264"] = std::make_shared(); 80 | stream_types_["vp9"] = std::make_shared(); 81 | 82 | handler_group_.addHandlerForPath( 83 | "/", 84 | boost::bind(&WebVideoServer::handle_list_streams, this, _1, _2, _3, _4)); 85 | handler_group_.addHandlerForPath( 86 | "/stream", 87 | boost::bind(&WebVideoServer::handle_stream, this, _1, _2, _3, _4)); 88 | handler_group_.addHandlerForPath( 89 | "/stream_viewer", 90 | boost::bind(&WebVideoServer::handle_stream_viewer, this, _1, _2, _3, _4)); 91 | handler_group_.addHandlerForPath( 92 | "/snapshot", 93 | boost::bind(&WebVideoServer::handle_snapshot, this, _1, _2, _3, _4)); 94 | 95 | try { 96 | server_.reset( 97 | new async_web_server_cpp::HttpServer( 98 | address_, std::to_string(port_), 99 | boost::bind(&WebVideoServer::handle_request, this, _1, _2, _3, _4), 100 | server_threads 101 | ) 102 | ); 103 | } catch (boost::exception & e) { 104 | RCLCPP_ERROR( 105 | get_logger(), "Exception when creating the web server! %s:%d", 106 | address_.c_str(), port_); 107 | throw; 108 | } 109 | 110 | RCLCPP_INFO(get_logger(), "Waiting For connections on %s:%d", address_.c_str(), port_); 111 | 112 | if (publish_rate_ > 0) { 113 | create_wall_timer(1s / publish_rate_, [this]() {restreamFrames(1s / publish_rate_);}); 114 | } 115 | 116 | cleanup_timer_ = create_wall_timer(500ms, [this]() {cleanup_inactive_streams();}); 117 | 118 | server_->run(); 119 | } 120 | 121 | WebVideoServer::~WebVideoServer() 122 | { 123 | server_->stop(); 124 | } 125 | 126 | void WebVideoServer::restreamFrames(std::chrono::duration max_age) 127 | { 128 | std::scoped_lock lock(subscriber_mutex_); 129 | 130 | for (auto & subscriber : image_subscribers_) { 131 | subscriber->restreamFrame(max_age); 132 | } 133 | } 134 | 135 | void WebVideoServer::cleanup_inactive_streams() 136 | { 137 | std::unique_lock lock(subscriber_mutex_, std::try_to_lock); 138 | if (lock) { 139 | auto new_end = std::partition( 140 | image_subscribers_.begin(), image_subscribers_.end(), 141 | [](const std::shared_ptr & streamer) {return !streamer->isInactive();}); 142 | if (verbose_) { 143 | for (auto itr = new_end; itr < image_subscribers_.end(); ++itr) { 144 | RCLCPP_INFO(get_logger(), "Removed Stream: %s", (*itr)->getTopic().c_str()); 145 | } 146 | } 147 | image_subscribers_.erase(new_end, image_subscribers_.end()); 148 | } 149 | } 150 | 151 | bool WebVideoServer::handle_request( 152 | const async_web_server_cpp::HttpRequest & request, 153 | async_web_server_cpp::HttpConnectionPtr connection, const char * begin, 154 | const char * end) 155 | { 156 | if (verbose_) { 157 | RCLCPP_INFO(get_logger(), "Handling Request: %s", request.uri.c_str()); 158 | } 159 | 160 | try { 161 | return handler_group_(request, connection, begin, end); 162 | } catch (std::exception & e) { 163 | RCLCPP_WARN(get_logger(), "Error Handling Request: %s", e.what()); 164 | return false; 165 | } 166 | } 167 | 168 | bool WebVideoServer::handle_stream( 169 | const async_web_server_cpp::HttpRequest & request, 170 | async_web_server_cpp::HttpConnectionPtr connection, const char * begin, 171 | const char * end) 172 | { 173 | std::string type = request.get_query_param_value_or_default("type", default_stream_type_); 174 | if (stream_types_.find(type) != stream_types_.end()) { 175 | std::string topic = request.get_query_param_value_or_default("topic", ""); 176 | // Fallback for topics without corresponding compressed topics 177 | if (type == std::string("ros_compressed")) { 178 | std::string compressed_topic_name = topic + "/compressed"; 179 | auto tnat = get_topic_names_and_types(); 180 | bool did_find_compressed_topic = false; 181 | for (auto topic_and_types : tnat) { 182 | if (topic_and_types.second.size() > 1) { 183 | // skip over topics with more than one type 184 | continue; 185 | } 186 | auto & topic_name = topic_and_types.first; 187 | if (topic_name == compressed_topic_name || 188 | (topic_name.find("/") == 0 && topic_name.substr(1) == compressed_topic_name)) 189 | { 190 | did_find_compressed_topic = true; 191 | break; 192 | } 193 | } 194 | if (!did_find_compressed_topic) { 195 | RCLCPP_WARN( 196 | get_logger(), 197 | "Could not find compressed image topic for %s, falling back to mjpeg", topic.c_str()); 198 | type = "mjpeg"; 199 | } 200 | } 201 | std::shared_ptr streamer = stream_types_[type]->create_streamer( 202 | request, connection, shared_from_this()); 203 | streamer->start(); 204 | std::scoped_lock lock(subscriber_mutex_); 205 | image_subscribers_.push_back(streamer); 206 | } else { 207 | async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::not_found)( 208 | request, connection, begin, end); 209 | } 210 | return true; 211 | } 212 | 213 | bool WebVideoServer::handle_snapshot( 214 | const async_web_server_cpp::HttpRequest & request, 215 | async_web_server_cpp::HttpConnectionPtr connection, const char * /* begin */, 216 | const char * /* end */) 217 | { 218 | std::shared_ptr streamer = std::make_shared( 219 | request, connection, shared_from_this()); 220 | streamer->start(); 221 | 222 | std::scoped_lock lock(subscriber_mutex_); 223 | image_subscribers_.push_back(streamer); 224 | return true; 225 | } 226 | 227 | bool WebVideoServer::handle_stream_viewer( 228 | const async_web_server_cpp::HttpRequest & request, 229 | async_web_server_cpp::HttpConnectionPtr connection, const char * begin, 230 | const char * end) 231 | { 232 | std::string type = request.get_query_param_value_or_default("type", default_stream_type_); 233 | if (stream_types_.find(type) != stream_types_.end()) { 234 | std::string topic = request.get_query_param_value_or_default("topic", ""); 235 | // Fallback for topics without corresponding compressed topics 236 | if (type == std::string("ros_compressed")) { 237 | std::string compressed_topic_name = topic + "/compressed"; 238 | auto tnat = get_topic_names_and_types(); 239 | bool did_find_compressed_topic = false; 240 | for (auto topic_and_types : tnat) { 241 | if (topic_and_types.second.size() > 1) { 242 | // skip over topics with more than one type 243 | continue; 244 | } 245 | auto & topic_name = topic_and_types.first; 246 | if (topic_name == compressed_topic_name || 247 | (topic_name.find("/") == 0 && topic_name.substr(1) == compressed_topic_name)) 248 | { 249 | did_find_compressed_topic = true; 250 | break; 251 | } 252 | } 253 | if (!did_find_compressed_topic) { 254 | RCLCPP_WARN( 255 | get_logger(), 256 | "Could not find compressed image topic for %s, falling back to mjpeg", topic.c_str()); 257 | type = "mjpeg"; 258 | } 259 | } 260 | 261 | async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok) 262 | .header("Connection", "close") 263 | .header("Server", "web_video_server") 264 | .header("Content-type", "text/html;") 265 | .write(connection); 266 | 267 | std::stringstream ss; 268 | ss << "" << topic << ""; 269 | ss << "

" << topic << "

"; 270 | ss << stream_types_[type]->create_viewer(request); 271 | ss << ""; 272 | connection->write(ss.str()); 273 | } else { 274 | async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::not_found)( 275 | request, connection, begin, end); 276 | } 277 | return true; 278 | } 279 | 280 | bool WebVideoServer::handle_list_streams( 281 | const async_web_server_cpp::HttpRequest & /* request */, 282 | async_web_server_cpp::HttpConnectionPtr connection, const char * /* begin */, 283 | const char * /* end */) 284 | { 285 | std::vector image_topics; 286 | std::vector camera_info_topics; 287 | auto tnat = get_topic_names_and_types(); 288 | for (auto topic_and_types : tnat) { 289 | if (topic_and_types.second.size() > 1) { 290 | // skip over topics with more than one type 291 | continue; 292 | } 293 | auto & topic_name = topic_and_types.first; 294 | auto & topic_type = topic_and_types.second[0]; // explicitly take the first 295 | 296 | if (topic_type == "sensor_msgs/msg/Image") { 297 | image_topics.push_back(topic_name); 298 | } else if (topic_type == "sensor_msgs/msg/CameraInfo") { 299 | camera_info_topics.push_back(topic_name); 300 | } 301 | } 302 | 303 | async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok) 304 | .header("Connection", "close") 305 | .header("Server", "web_video_server") 306 | .header( 307 | "Cache-Control", 308 | "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0") 309 | .header("Pragma", "no-cache").header("Content-type", "text/html;").write(connection); 310 | 311 | connection->write( 312 | "" 313 | "ROS Image Topic List" 314 | "

Available ROS Image Topics:

"); 315 | connection->write(""); 350 | // Add the rest of the image topics that don't have camera_info. 351 | connection->write(""); 370 | return true; 371 | } 372 | 373 | } // namespace web_video_server 374 | 375 | #include "rclcpp_components/register_node_macro.hpp" 376 | 377 | // Register the component with class_loader. 378 | // This acts as a sort of entry point, allowing the component to be discoverable when its library 379 | // is being loaded into a running process. 380 | RCLCPP_COMPONENTS_REGISTER_NODE(web_video_server::WebVideoServer) 381 | -------------------------------------------------------------------------------- /src/web_video_server_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014, Worcester Polytechnic Institute 2 | // Copyright (c) 2024, The Robot Web Tools Contributors 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // 11 | // * Redistributions in binary form must reproduce the above copyright 12 | // notice, this list of conditions and the following disclaimer in the 13 | // documentation and/or other materials provided with the distribution. 14 | // 15 | // * Neither the name of the copyright holder nor the names of its 16 | // contributors may be used to endorse or promote products derived from 17 | // this software without specific prior written permission. 18 | // 19 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | // POSSIBILITY OF SUCH DAMAGE. 30 | #include "rclcpp/rclcpp.hpp" 31 | 32 | #include "web_video_server/web_video_server.hpp" 33 | 34 | int main(int argc, char ** argv) 35 | { 36 | rclcpp::init(argc, argv); 37 | auto node = std::make_shared(rclcpp::NodeOptions()); 38 | 39 | node->declare_parameter("ros_threads", 2); 40 | int ros_threads{2}; 41 | node->get_parameter("ros_threads", ros_threads); 42 | rclcpp::executors::MultiThreadedExecutor spinner(rclcpp::ExecutorOptions(), ros_threads); 43 | spinner.add_node(node); 44 | spinner.spin(); 45 | 46 | return 0; 47 | } 48 | --------------------------------------------------------------------------------