├── .gitignore ├── .travis.yml ├── AUTHORS.md ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include └── mjpeg_server │ └── mjpeg_server.h ├── mainpage.dox ├── package.xml └── src └── mjpeg_server.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .cproject 2 | .project 3 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: 2 | - cpp 3 | - python 4 | python: 5 | - "2.7" 6 | compiler: 7 | - gcc 8 | 9 | branches: 10 | only: 11 | - master 12 | - develop 13 | 14 | install: 15 | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list' 16 | - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 17 | - sudo apt-get update -qq 18 | - sudo apt-get install python-catkin-pkg python-rosdep ros-groovy-catkin ros-hydro-catkin -qq 19 | - sudo rosdep init 20 | - rosdep update 21 | - mkdir -p /tmp/ws/src 22 | - ln -s `pwd` /tmp/ws/src/package 23 | - cd /tmp/ws 24 | - rosdep install --from-paths src --ignore-src --rosdistro groovy -y 25 | - rosdep install --from-paths src --ignore-src --rosdistro hydro -y 26 | 27 | script: 28 | - source /opt/ros/groovy/setup.bash 29 | - catkin_make 30 | - catkin_make install 31 | - rm -rf build/ install/ devel/ 32 | - catkin_make 33 | - catkin_make install 34 | -------------------------------------------------------------------------------- /AUTHORS.md: -------------------------------------------------------------------------------- 1 | Original Authors 2 | ---------------- 3 | 4 | * Benjamin Pitzer (ben.pitzer@gmail.com) 5 | 6 | Contributors 7 | ------------ 8 | 9 | * [Russell Toris](http://users.wpi.edu/~rctoris/) (rctoris@wpi.edu) 10 | * [Sarah Osentoski](https://sites.google.com/site/sosentos) (sarah.osentoski@us.bosch.com) 11 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mjpeg_server 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.1.2 (2014-10-30) 6 | ------------------ 7 | * ROS_WARN added 8 | * DEPRECATION NOTICE 9 | * Contributors: Russell Toris 10 | 11 | 1.1.1 (2014-04-04) 12 | ------------------ 13 | * cleanup for indigo 14 | * Merge pull request `#6 `_ from garaemon/unregister-nonused-topic 15 | Unregister non-used subscribers 16 | * add documentation 17 | * remove a subscriber in sendStream and sendSnapshot 18 | * unregister Subscriber if the number of subscribers for the topic equals to 0 19 | * Merge pull request `#4 `_ from nus/fixed-timestamp 20 | Fixed timestamp 21 | * Fixed the streaming timestamp 22 | * Fixed the snapshot timestamp 23 | * Merge pull request `#1 `_ from rctoris/groovy-devel 24 | Cleanup/Travis Added 25 | * Header files now installed 26 | * minor cleanup and travis added 27 | * Minor cleanup and change of READMEs and such 28 | * Contributors: Brandon Alexander, Russell Toris, Ryohei Ueda, yota 29 | 30 | 1.1.0 (2013-05-01) 31 | ------------------ 32 | * initial commit 33 | * Contributors: Russell Toris 34 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mjpeg_server) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS roscpp std_msgs roslib cv_bridge image_transport) 8 | find_package(OpenCV REQUIRED) 9 | find_package(Boost REQUIRED COMPONENTS thread) 10 | 11 | ################################################### 12 | ## Declare things to be passed to other projects ## 13 | ################################################### 14 | 15 | ## LIBRARIES: libraries you create in this project that dependent projects also need 16 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 17 | ## DEPENDS: system dependencies of this project that dependent projects also need 18 | catkin_package() 19 | 20 | ########### 21 | ## Build ## 22 | ########### 23 | 24 | ## Specify additional locations of header files 25 | include_directories(include 26 | ${catkin_INCLUDE_DIRS} 27 | ${Boost_INCLUDE_DIRS} 28 | ) 29 | 30 | ## Declare a cpp executable 31 | add_executable(${PROJECT_NAME} src/mjpeg_server.cpp) 32 | 33 | ## Specify libraries to link a library or executable target against 34 | target_link_libraries(${PROJECT_NAME} 35 | ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OpenCV_LIBS} 36 | ) 37 | 38 | ############# 39 | ## Install ## 40 | ############# 41 | 42 | ## Mark executables and/or libraries for installation 43 | install(TARGETS ${PROJECT_NAME} 44 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 45 | ) 46 | 47 | install(DIRECTORY include/ 48 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 49 | FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 50 | ) 51 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2014, Robert Bosch LLC, Worcester Polytechnic Institute 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above 13 | copyright notice, this list of conditions and the following 14 | disclaimer in the documentation and/or other materials provided 15 | with the distribution. 16 | * Neither the name of Robert Bosch LLC, Worcester Polytechnic 17 | Institute, nor the names of its contributors may be used to 18 | endorse or promote products derived from this software without 19 | specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | POSSIBILITY OF SUCH DAMAGE. 33 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## DEPRECATION NOTICE 2 | mjpeg_server has been replaced by [web_video_server](https://github.com/RobotWebTools/web_video_server). 3 | 4 | mjpeg_server [![Build Status](https://api.travis-ci.org/RobotWebTools/mjpeg_server.png)](https://travis-ci.org/RobotWebTools/mjpeg_server) 5 | ============ 6 | 7 | #### A ROS Node to Stream Image Topics Via a MJPEG Server 8 | For full documentation, see [the ROS wiki](http://ros.org/wiki/mjpeg_server). 9 | 10 | [Doxygen](http://docs.ros.org/indigo/api/mjpeg_server/html/) files can be found on the ROS wiki. 11 | 12 | This project is released as part of the [Robot Web Tools](http://robotwebtools.org/) effort. 13 | 14 | ### License 15 | mjpeg_server is released with a BSD license. For full terms and conditions, see the [LICENSE](LICENSE) file. 16 | 17 | ### Authors 18 | See the [AUTHORS](AUTHORS.md) file for a full list of contributors. 19 | -------------------------------------------------------------------------------- /include/mjpeg_server/mjpeg_server.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2014, Robert Bosch LLC. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Robert Bosch nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | #ifndef MJPEG_SERVER_H_ 37 | #define MJPEG_SERVER_H_ 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | 47 | /* 48 | * Maximum number of server sockets (i.e. protocol families) to listen. 49 | */ 50 | #define MAX_NUM_SOCKETS 100 51 | #define IO_BUFFER 256 52 | #define BUFFER_SIZE 1024 53 | 54 | namespace mjpeg_server 55 | { 56 | 57 | /* the webserver determines between these values for an answer */ 58 | typedef enum 59 | { 60 | A_UNKNOWN, A_STREAM, A_SNAPSHOT, 61 | } answer_t; 62 | 63 | /* 64 | * the client sends information with each request 65 | * this structure is used to store the important parts 66 | */ 67 | typedef struct 68 | { 69 | answer_t type; 70 | char *parameter; 71 | char *client; 72 | char *credentials; 73 | } request; 74 | 75 | /* the iobuffer structure is used to read from the HTTP-client */ 76 | typedef struct 77 | { 78 | int level; /* how full is the buffer */ 79 | char buffer[IO_BUFFER]; /* the data */ 80 | } iobuffer; 81 | 82 | /** 83 | * @class ImageBuffer 84 | * @brief 85 | */ 86 | class ImageBuffer 87 | { 88 | public: 89 | ImageBuffer() 90 | { 91 | } 92 | sensor_msgs::Image msg; 93 | boost::condition_variable condition_; 94 | boost::mutex mutex_; 95 | }; 96 | 97 | /** 98 | * @class MJPEGServer 99 | * @brief 100 | */ 101 | class MJPEGServer 102 | { 103 | public: 104 | /** 105 | * @brief Constructor 106 | * @return 107 | */ 108 | MJPEGServer(ros::NodeHandle& node); 109 | 110 | /** 111 | * @brief Destructor - Cleans up 112 | */ 113 | virtual ~MJPEGServer(); 114 | 115 | /** 116 | * @brief Runs whenever a new goal is sent to the move_base 117 | * @param goal The goal to pursue 118 | * @param feedback Feedback that the action gives to a higher-level monitor, in this case, the position of the robot 119 | * @return The result of the execution, ie: Success, Preempted, Aborted, etc. 120 | */ 121 | void execute(); 122 | 123 | /** 124 | * @brief Starts the server and spins 125 | */ 126 | void spin(); 127 | 128 | /** 129 | * @brief Stops the server 130 | */ 131 | void stop(); 132 | 133 | /** 134 | * @brief Closes all client threads 135 | */ 136 | void cleanUp(); 137 | 138 | /** 139 | * @brief Client thread function 140 | * Serve a connected TCP-client. This thread function is called 141 | * for each connect of a HTTP client like a webbrowser. It determines 142 | * if it is a valid HTTP request and dispatches between the different 143 | * response options. 144 | */ 145 | void client(int fd); 146 | 147 | private: 148 | typedef std::map ImageBufferMap; 149 | typedef std::map ImageSubscriberMap; 150 | typedef std::map ImageSubscriberCountMap; 151 | typedef std::map ParameterMap; 152 | 153 | std::string header; 154 | 155 | ImageBuffer* getImageBuffer(const std::string& topic); 156 | 157 | void imageCallback(const sensor_msgs::ImageConstPtr& msg, const std::string& topic); 158 | 159 | /** 160 | * @brief Rotate input image by 180 degrees. 161 | * @param input image 162 | * @param output image 163 | */ 164 | void invertImage(const cv::Mat& input, cv::Mat& output); 165 | 166 | /** 167 | * @brief Send an error message. 168 | * @param fildescriptor fd to send the answer to 169 | * @param error number 170 | * @param error message 171 | */ 172 | void sendError(int fd, int which, const char *message); 173 | 174 | /** 175 | * @brief Send a complete HTTP response and a stream of JPG-frames. 176 | * @param fildescriptor fd to send the answer to 177 | * @param parameter string 178 | */ 179 | void sendStream(int fd, const char *parameter); 180 | 181 | /** 182 | * @brief Send a complete HTTP response and a single JPG-frame. 183 | * @param fildescriptor fd to send the answer to 184 | * @param parameter string 185 | */ 186 | void sendSnapshot(int fd, const char *parameter); 187 | 188 | /** 189 | * @brief Initializes the iobuffer structure properly 190 | * @param Pointer to already allocated iobuffer 191 | */ 192 | void initIOBuffer(iobuffer *iobuf); 193 | 194 | /** 195 | * @brief Initializes the request structure properly 196 | * @param Pointer to already allocated req 197 | */ 198 | void initRequest(request *req); 199 | 200 | /** 201 | * @brief If strings were assigned to the different members free them. 202 | * This will fail if strings are static, so always use strdup(). 203 | * @param req: pointer to request structure 204 | */ 205 | void freeRequest(request *req); 206 | 207 | /** 208 | * @brief read with timeout, implemented without using signals 209 | * tries to read len bytes and returns if enough bytes were read 210 | * or the timeout was triggered. In case of timeout the return 211 | * value may differ from the requested bytes "len". 212 | * @param fd.....: fildescriptor to read from 213 | * @param iobuf..: iobuffer that allows to use this functions from multiple 214 | * threads because the complete context is the iobuffer. 215 | * @param buffer.: The buffer to store values at, will be set to zero 216 | before storing values. 217 | * @param len....: the length of buffer 218 | * @param timeout: seconds to wait for an answer 219 | * @return buffer.: will become filled with bytes read 220 | * @return iobuf..: May get altered to save the context for future calls. 221 | * @return func().: bytes copied to buffer or -1 in case of error 222 | */ 223 | int readWithTimeout(int fd, iobuffer *iobuf, char *buffer, size_t len, int timeout); 224 | 225 | /** 226 | * @brief Read a single line from the provided fildescriptor. 227 | * This funtion will return under two conditions: 228 | * - line end was reached 229 | * - timeout occured 230 | * @param fd.....: fildescriptor to read from 231 | * @param iobuf..: iobuffer that allows to use this functions from multiple 232 | * threads because the complete context is the iobuffer. 233 | * @param buffer.: The buffer to store values at, will be set to zero 234 | before storing values. 235 | * @param len....: the length of buffer 236 | * @param timeout: seconds to wait for an answer 237 | * @return buffer.: will become filled with bytes read 238 | * @return iobuf..: May get altered to save the context for future calls. 239 | * @return func().: bytes copied to buffer or -1 in case of error 240 | */ 241 | int readLineWithTimeout(int fd, iobuffer *iobuf, char *buffer, size_t len, int timeout); 242 | 243 | /** 244 | * @brief Decodes the data and stores the result to the same buffer. 245 | * The buffer will be large enough, because base64 requires more 246 | * space then plain text. 247 | * @param base64 encoded data 248 | * @return plain decoded data 249 | */ 250 | void decodeBase64(char *data); 251 | 252 | /** 253 | * @brief Convert a hexadecimal ASCII character to integer 254 | * @param ASCII character 255 | * @return corresponding value between 0 and 15, or -1 in case of error 256 | */ 257 | int hexCharToInt(char in); 258 | 259 | /** 260 | * @brief Replaces %XX with the character code it represents, URI 261 | * @param string to unescape 262 | * @return 0 if everything is ok, -1 in case of error 263 | */ 264 | int unescape(char *string); 265 | 266 | /** 267 | * @brief Split string into a list of tokens 268 | * @param string to split 269 | * @return vector of tokens 270 | */ 271 | void splitString(const std::string& str, std::vector& tokens, const std::string& delimiter = " "); 272 | 273 | /** 274 | * @brief Convert a string to an integer 275 | * @param string 276 | * @param default return value 277 | * @return corresponding value, default_value in case of error 278 | */ 279 | int stringToInt(const std::string& str, const int default_value = 0); 280 | 281 | /** 282 | * @brief Decodes URI parameters in form of ?parameter=value 283 | * @param URI string 284 | * @return a map of parameter/value pairs 285 | */ 286 | void decodeParameter(const std::string& parameter, ParameterMap& parameter_map); 287 | 288 | /** 289 | * @brief increase the number of the subscribers of the specified topic 290 | * @param topic name string 291 | */ 292 | void decreaseSubscriberCount(const std::string topic); 293 | 294 | /** 295 | * @brief decrease the number of the subscribers of the specified topic 296 | * @param topic name string 297 | */ 298 | void increaseSubscriberCount(const std::string topic); 299 | 300 | /** 301 | * @brief remove ros::Subscriber if the number of the subscribers of the topic is equal to 0 302 | * @param topic name string 303 | */ 304 | void unregisterSubscriberIfPossible(const std::string topic); 305 | 306 | ros::NodeHandle node_; 307 | image_transport::ImageTransport image_transport_; 308 | int port_; 309 | 310 | int sd[MAX_NUM_SOCKETS]; 311 | int sd_len; 312 | 313 | bool stop_requested_; 314 | char* www_folder_; 315 | 316 | ImageBufferMap image_buffers_; 317 | ImageSubscriberMap image_subscribers_; 318 | ImageSubscriberCountMap image_subscribers_count_; 319 | boost::mutex image_maps_mutex_; 320 | 321 | }; 322 | 323 | } 324 | 325 | #endif 326 | 327 | -------------------------------------------------------------------------------- /mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | 4 | \b A ROS Node to Stream Image Topics Via a MJPEG Server 5 | mjpeg_server converts ROS image streams into an MJPEG stream and streams them via a socket connection. 6 | */ 7 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mjpeg_server 4 | 1.1.2 5 | A ROS Node to Stream Image Topics Via a MJPEG Server 6 | 7 | Russell Toris 8 | Benjamin Pitzer 9 | 10 | BSD 11 | 12 | http://ros.org/wiki/mjpeg_server 13 | https://github.com/RobotWebTools/mjpeg_server/issues 14 | https://github.com/RobotWebTools/mjpeg_server 15 | 16 | catkin 17 | 18 | roscpp 19 | std_msgs 20 | roslib 21 | cv_bridge 22 | image_transport 23 | 24 | roscpp 25 | std_msgs 26 | roslib 27 | cv_bridge 28 | image_transport 29 | 30 | -------------------------------------------------------------------------------- /src/mjpeg_server.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2014, Robert Bosch LLC. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Robert Bosch nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | template 54 | inline T ABS(T a) 55 | { 56 | return (a < 0 ? -a : a); 57 | } 58 | 59 | template 60 | inline T min(T a, T b) 61 | { 62 | return (a < b ? a : b); 63 | } 64 | 65 | template 66 | inline T max(T a, T b) 67 | { 68 | return (a > b ? a : b); 69 | } 70 | 71 | template 72 | inline T LENGTH_OF(T x) 73 | { 74 | return (sizeof(x) / sizeof(x[0])); 75 | } 76 | 77 | namespace mjpeg_server 78 | { 79 | 80 | MJPEGServer::MJPEGServer(ros::NodeHandle& node) : 81 | node_(node), image_transport_(node), stop_requested_(false), www_folder_(NULL) 82 | { 83 | ros::NodeHandle private_nh("~"); 84 | private_nh.param("port", port_, 8080); 85 | header = "Connection: close\r\nServer: mjpeg_server\r\n" 86 | "Cache-Control: no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0\r\n" 87 | "Pragma: no-cache\r\n"; 88 | sd_len = 0; 89 | } 90 | 91 | MJPEGServer::~MJPEGServer() 92 | { 93 | cleanUp(); 94 | } 95 | 96 | void MJPEGServer::imageCallback(const sensor_msgs::ImageConstPtr& msg, const std::string& topic) 97 | { 98 | 99 | ImageBuffer* image_buffer = getImageBuffer(topic); 100 | boost::unique_lock lock(image_buffer->mutex_); 101 | // copy image 102 | image_buffer->msg = *msg; 103 | // notify senders 104 | image_buffer->condition_.notify_all(); 105 | } 106 | 107 | void MJPEGServer::splitString(const std::string& str, std::vector& tokens, const std::string& delimiter) 108 | { 109 | // Skip delimiters at beginning. 110 | std::string::size_type lastPos = str.find_first_not_of(delimiter, 0); 111 | // Find first "non-delimiter". 112 | std::string::size_type pos = str.find_first_of(delimiter, lastPos); 113 | 114 | while (std::string::npos != pos || std::string::npos != lastPos) 115 | { 116 | // Found a token, add it to the vector. 117 | tokens.push_back(str.substr(lastPos, pos - lastPos)); 118 | // Skip delimiters. Note the "not_of" 119 | lastPos = str.find_first_not_of(delimiter, pos); 120 | // Find next "non-delimiter" 121 | pos = str.find_first_of(delimiter, lastPos); 122 | } 123 | } 124 | 125 | int MJPEGServer::stringToInt(const std::string& str, const int default_value) 126 | { 127 | int value; 128 | int res; 129 | if (str.length() == 0) 130 | return default_value; 131 | res = sscanf(str.c_str(), "%i", &value); 132 | if (res == 1) 133 | return value; 134 | return default_value; 135 | } 136 | 137 | void MJPEGServer::initIOBuffer(iobuffer *iobuf) 138 | { 139 | memset(iobuf->buffer, 0, sizeof(iobuf->buffer)); 140 | iobuf->level = 0; 141 | } 142 | 143 | void MJPEGServer::initRequest(request *req) 144 | { 145 | req->type = A_UNKNOWN; 146 | req->type = A_UNKNOWN; 147 | req->parameter = NULL; 148 | req->client = NULL; 149 | req->credentials = NULL; 150 | } 151 | 152 | void MJPEGServer::freeRequest(request *req) 153 | { 154 | if (req->parameter != NULL) 155 | free(req->parameter); 156 | if (req->client != NULL) 157 | free(req->client); 158 | if (req->credentials != NULL) 159 | free(req->credentials); 160 | } 161 | 162 | int MJPEGServer::readWithTimeout(int fd, iobuffer *iobuf, char *buffer, size_t len, int timeout) 163 | { 164 | size_t copied = 0; 165 | int rc, i; 166 | fd_set fds; 167 | struct timeval tv; 168 | 169 | memset(buffer, 0, len); 170 | 171 | while ((copied < len)) 172 | { 173 | i = min((size_t)iobuf->level, len - copied); 174 | memcpy(buffer + copied, iobuf->buffer + IO_BUFFER - iobuf->level, i); 175 | 176 | iobuf->level -= i; 177 | copied += i; 178 | if (copied >= len) 179 | return copied; 180 | 181 | /* select will return in case of timeout or new data arrived */ 182 | tv.tv_sec = timeout; 183 | tv.tv_usec = 0; 184 | FD_ZERO(&fds); 185 | FD_SET(fd, &fds); 186 | if ((rc = select(fd + 1, &fds, NULL, NULL, &tv)) <= 0) 187 | { 188 | if (rc < 0) 189 | exit(EXIT_FAILURE); 190 | 191 | /* this must be a timeout */ 192 | return copied; 193 | } 194 | 195 | initIOBuffer(iobuf); 196 | 197 | /* 198 | * there should be at least one byte, because select signalled it. 199 | * But: It may happen (very seldomly), that the socket gets closed remotly between 200 | * the select() and the following read. That is the reason for not relying 201 | * on reading at least one byte. 202 | */ 203 | if ((iobuf->level = read(fd, &iobuf->buffer, IO_BUFFER)) <= 0) 204 | { 205 | /* an error occured */ 206 | return -1; 207 | } 208 | 209 | /* align data to the end of the buffer if less than IO_BUFFER bytes were read */ 210 | memmove(iobuf->buffer + (IO_BUFFER - iobuf->level), iobuf->buffer, iobuf->level); 211 | } 212 | 213 | return 0; 214 | } 215 | 216 | int MJPEGServer::readLineWithTimeout(int fd, iobuffer *iobuf, char *buffer, size_t len, int timeout) 217 | { 218 | char c = '\0', *out = buffer; 219 | unsigned int i; 220 | 221 | memset(buffer, 0, len); 222 | 223 | for (i = 0; i < len && c != '\n'; i++) 224 | { 225 | if (readWithTimeout(fd, iobuf, &c, 1, timeout) <= 0) 226 | { 227 | /* timeout or error occured */ 228 | return -1; 229 | } 230 | *out++ = c; 231 | } 232 | 233 | return i; 234 | } 235 | 236 | void MJPEGServer::decodeBase64(char *data) 237 | { 238 | union 239 | { 240 | int i; 241 | char c[4]; 242 | } buffer; 243 | 244 | char* ptr = data; 245 | unsigned int size = strlen(data); 246 | char* temp = new char[size]; 247 | char* tempptr = temp; 248 | char t; 249 | 250 | for (buffer.i = 0, t = *ptr; ptr; ptr++) 251 | { 252 | if (t >= 'A' && t <= 'Z') 253 | t = t - 'A'; 254 | else if (t >= 'a' && t <= 'z') 255 | t = t - 'a' + 26; 256 | else if (t >= '0' && t <= '9') 257 | t = t - '0' + 52; 258 | else if (t == '+') 259 | t = 62; 260 | else if (t == '/') 261 | t = 63; 262 | else 263 | continue; 264 | 265 | buffer.i = (buffer.i << 6) | t; 266 | 267 | if ((ptr - data + 1) % 4) 268 | { 269 | *tempptr++ = buffer.c[2]; 270 | *tempptr++ = buffer.c[1]; 271 | *tempptr++ = buffer.c[0]; 272 | buffer.i = 0; 273 | } 274 | } 275 | *tempptr = '\0'; 276 | strcpy(data, temp); 277 | delete temp; 278 | } 279 | 280 | int MJPEGServer::hexCharToInt(char in) 281 | { 282 | if (in >= '0' && in <= '9') 283 | return in - '0'; 284 | 285 | if (in >= 'a' && in <= 'f') 286 | return (in - 'a') + 10; 287 | 288 | if (in >= 'A' && in <= 'F') 289 | return (in - 'A') + 10; 290 | 291 | return -1; 292 | } 293 | 294 | int MJPEGServer::unescape(char *string) 295 | { 296 | char *source = string, *destination = string; 297 | int src, dst, length = strlen(string), rc; 298 | 299 | /* iterate over the string */ 300 | for (dst = 0, src = 0; src < length; src++) 301 | { 302 | 303 | /* is it an escape character? */ 304 | if (source[src] != '%') 305 | { 306 | /* no, so just go to the next character */ 307 | destination[dst] = source[src]; 308 | dst++; 309 | continue; 310 | } 311 | 312 | /* yes, it is an escaped character */ 313 | 314 | /* check if there are enough characters */ 315 | if (src + 2 > length) 316 | { 317 | return -1; 318 | break; 319 | } 320 | 321 | /* perform replacement of %## with the corresponding character */ 322 | if ((rc = hexCharToInt(source[src + 1])) == -1) 323 | return -1; 324 | destination[dst] = rc * 16; 325 | if ((rc = hexCharToInt(source[src + 2])) == -1) 326 | return -1; 327 | destination[dst] += rc; 328 | 329 | /* advance pointers, here is the reason why the resulting string is shorter */ 330 | dst++; 331 | src += 2; 332 | } 333 | 334 | /* ensure the string is properly finished with a null-character */ 335 | destination[dst] = '\0'; 336 | 337 | return 0; 338 | } 339 | 340 | void MJPEGServer::sendError(int fd, int which, const char *message) 341 | { 342 | char buffer[BUFFER_SIZE] = {0}; 343 | 344 | if (which == 401) 345 | { 346 | sprintf(buffer, "HTTP/1.0 401 Unauthorized\r\n" 347 | "Content-type: text/plain\r\n" 348 | "%s" 349 | "WWW-Authenticate: Basic realm=\"MJPG-Streamer\"\r\n" 350 | "\r\n" 351 | "401: Not Authenticated!\r\n" 352 | "%s", 353 | header.c_str(), message); 354 | } 355 | else if (which == 404) 356 | { 357 | sprintf(buffer, "HTTP/1.0 404 Not Found\r\n" 358 | "Content-type: text/plain\r\n" 359 | "%s" 360 | "\r\n" 361 | "404: Not Found!\r\n" 362 | "%s", 363 | header.c_str(), message); 364 | } 365 | else if (which == 500) 366 | { 367 | sprintf(buffer, "HTTP/1.0 500 Internal Server Error\r\n" 368 | "Content-type: text/plain\r\n" 369 | "%s" 370 | "\r\n" 371 | "500: Internal Server Error!\r\n" 372 | "%s", 373 | header.c_str(), message); 374 | } 375 | else if (which == 400) 376 | { 377 | sprintf(buffer, "HTTP/1.0 400 Bad Request\r\n" 378 | "Content-type: text/plain\r\n" 379 | "%s" 380 | "\r\n" 381 | "400: Not Found!\r\n" 382 | "%s", 383 | header.c_str(), message); 384 | } 385 | else 386 | { 387 | sprintf(buffer, "HTTP/1.0 501 Not Implemented\r\n" 388 | "Content-type: text/plain\r\n" 389 | "%s" 390 | "\r\n" 391 | "501: Not Implemented!\r\n" 392 | "%s", 393 | header.c_str(), message); 394 | } 395 | 396 | if (write(fd, buffer, strlen(buffer)) < 0) 397 | { 398 | ROS_DEBUG("write failed, done anyway"); 399 | } 400 | } 401 | 402 | void MJPEGServer::decodeParameter(const std::string& parameter, ParameterMap& parameter_map) 403 | { 404 | std::vector parameter_value_pairs; 405 | splitString(parameter, parameter_value_pairs, "?&"); 406 | 407 | for (size_t i = 0; i < parameter_value_pairs.size(); i++) 408 | { 409 | std::vector parameter_value; 410 | splitString(parameter_value_pairs[i], parameter_value, "="); 411 | if (parameter_value.size() == 1) 412 | { 413 | parameter_map.insert(std::make_pair(parameter_value[0], std::string(""))); 414 | } 415 | else if (parameter_value.size() == 2) 416 | { 417 | parameter_map.insert(std::make_pair(parameter_value[0], parameter_value[1])); 418 | } 419 | } 420 | } 421 | 422 | ImageBuffer* MJPEGServer::getImageBuffer(const std::string& topic) 423 | { 424 | boost::unique_lock lock(image_maps_mutex_); 425 | ImageSubscriberMap::iterator it = image_subscribers_.find(topic); 426 | if (it == image_subscribers_.end()) 427 | { 428 | image_subscribers_[topic] = image_transport_.subscribe(topic, 1, 429 | boost::bind(&MJPEGServer::imageCallback, this, _1, topic)); 430 | image_buffers_[topic] = new ImageBuffer(); 431 | ROS_INFO("Subscribing to topic %s", topic.c_str()); 432 | } 433 | ImageBuffer* image_buffer = image_buffers_[topic]; 434 | return image_buffer; 435 | } 436 | 437 | // rotate input image at 180 degrees 438 | void MJPEGServer::invertImage(const cv::Mat& input, cv::Mat& output) 439 | { 440 | 441 | cv::Mat_& input_img = (cv::Mat_&)input; //3 channel pointer to image 442 | cv::Mat_& output_img = (cv::Mat_&)output; //3 channel pointer to image 443 | cv::Size size = input.size(); 444 | 445 | for (int j = 0; j < size.height; ++j) 446 | for (int i = 0; i < size.width; ++i) 447 | { 448 | //outputImage.imageData[size.height*size.width - (i + j*size.width) - 1] = inputImage.imageData[i + j*size.width]; 449 | output_img(size.height - j - 1, size.width - i - 1) = input_img(j, i); 450 | } 451 | return; 452 | } 453 | 454 | void MJPEGServer::sendStream(int fd, const char *parameter) 455 | { 456 | unsigned char *frame = NULL, *tmp = NULL; 457 | int frame_size = 0, max_frame_size = 0; 458 | int tenk = 10 * 1024; 459 | char buffer[BUFFER_SIZE] = {0}; 460 | double timestamp; 461 | //sensor_msgs::CvBridge image_bridge; 462 | //sensor_msgs::cv_bridge image_bridge; 463 | cv_bridge::CvImage image_bridge; 464 | 465 | ROS_DEBUG("Decoding parameter"); 466 | 467 | std::string params = parameter; 468 | 469 | ParameterMap parameter_map; 470 | decodeParameter(params, parameter_map); 471 | 472 | ParameterMap::iterator itp = parameter_map.find("topic"); 473 | if (itp == parameter_map.end()) 474 | return; 475 | 476 | std::string topic = itp->second; 477 | increaseSubscriberCount(topic); 478 | ImageBuffer* image_buffer = getImageBuffer(topic); 479 | 480 | ROS_DEBUG("preparing header"); 481 | sprintf(buffer, "HTTP/1.0 200 OK\r\n" 482 | "%s" 483 | "Content-Type: multipart/x-mixed-replace;boundary=boundarydonotcross \r\n" 484 | "\r\n" 485 | "--boundarydonotcross \r\n", 486 | header.c_str()); 487 | 488 | if (write(fd, buffer, strlen(buffer)) < 0) 489 | { 490 | free(frame); 491 | return; 492 | } 493 | 494 | ROS_DEBUG("Headers send, sending stream now"); 495 | 496 | while (!stop_requested_) 497 | { 498 | { 499 | /* wait for fresh frames */ 500 | boost::unique_lock lock(image_buffer->mutex_); 501 | image_buffer->condition_.wait(lock); 502 | 503 | //IplImage* image; 504 | cv_bridge::CvImagePtr cv_msg; 505 | try 506 | { 507 | if (cv_msg = cv_bridge::toCvCopy(image_buffer->msg, "bgr8")) 508 | { 509 | ; //image = image_bridge.toIpl(); 510 | } 511 | else 512 | { 513 | ROS_ERROR("Unable to convert %s image to bgr8", image_buffer->msg.encoding.c_str()); 514 | return; 515 | } 516 | } 517 | catch (...) 518 | { 519 | ROS_ERROR("Unable to convert %s image to ipl format", image_buffer->msg.encoding.c_str()); 520 | return; 521 | } 522 | 523 | // encode image 524 | cv::Mat img = cv_msg->image; 525 | std::vector encoded_buffer; 526 | std::vector encode_params; 527 | 528 | // invert 529 | //int invert = 0; 530 | if (parameter_map.find("invert") != parameter_map.end()) 531 | { 532 | cv::Mat cloned_image = img.clone(); 533 | invertImage(cloned_image, img); 534 | } 535 | 536 | // quality 537 | int quality = 95; 538 | if (parameter_map.find("quality") != parameter_map.end()) 539 | { 540 | quality = stringToInt(parameter_map["quality"]); 541 | } 542 | encode_params.push_back(CV_IMWRITE_JPEG_QUALITY); 543 | encode_params.push_back(quality); 544 | 545 | // resize image 546 | if (parameter_map.find("width") != parameter_map.end() && parameter_map.find("height") != parameter_map.end()) 547 | { 548 | int width = stringToInt(parameter_map["width"]); 549 | int height = stringToInt(parameter_map["height"]); 550 | if (width > 0 && height > 0) 551 | { 552 | cv::Mat img_resized; 553 | cv::Size new_size(width, height); 554 | cv::resize(img, img_resized, new_size); 555 | cv::imencode(".jpeg", img_resized, encoded_buffer, encode_params); 556 | } 557 | else 558 | { 559 | cv::imencode(".jpeg", img, encoded_buffer, encode_params); 560 | } 561 | } 562 | else 563 | { 564 | cv::imencode(".jpeg", img, encoded_buffer, encode_params); 565 | } 566 | 567 | // copy encoded frame buffer 568 | frame_size = encoded_buffer.size(); 569 | 570 | /* check if frame buffer is large enough, increase it if necessary */ 571 | if (frame_size > max_frame_size) 572 | { 573 | ROS_DEBUG("increasing frame buffer size to %d", frame_size); 574 | 575 | max_frame_size = frame_size + tenk; 576 | if ((tmp = (unsigned char*)realloc(frame, max_frame_size)) == NULL) 577 | { 578 | free(frame); 579 | sendError(fd, 500, "not enough memory"); 580 | return; 581 | } 582 | frame = tmp; 583 | } 584 | 585 | /* copy v4l2_buffer timeval to user space */ 586 | timestamp = ros::Time::now().toSec(); 587 | 588 | memcpy(frame, &encoded_buffer[0], frame_size); 589 | ROS_DEBUG("got frame (size: %d kB)", frame_size / 1024); 590 | } 591 | 592 | /* 593 | * print the individual mimetype and the length 594 | * sending the content-length fixes random stream disruption observed 595 | * with firefox 596 | */ 597 | sprintf(buffer, "Content-Type: image/jpeg\r\n" 598 | "Content-Length: %d\r\n" 599 | "X-Timestamp: %.06lf\r\n" 600 | "\r\n", 601 | frame_size, (double)timestamp); 602 | ROS_DEBUG("sending intemdiate header"); 603 | if (write(fd, buffer, strlen(buffer)) < 0) 604 | break; 605 | 606 | ROS_DEBUG("sending frame"); 607 | if (write(fd, frame, frame_size) < 0) 608 | break; 609 | 610 | ROS_DEBUG("sending boundary"); 611 | sprintf(buffer, "\r\n--boundarydonotcross \r\n"); 612 | if (write(fd, buffer, strlen(buffer)) < 0) 613 | break; 614 | } 615 | 616 | free(frame); 617 | decreaseSubscriberCount(topic); 618 | unregisterSubscriberIfPossible(topic); 619 | 620 | } 621 | 622 | void MJPEGServer::sendSnapshot(int fd, const char *parameter) 623 | { 624 | unsigned char *frame = NULL; 625 | int frame_size = 0; 626 | char buffer[BUFFER_SIZE] = {0}; 627 | double timestamp; 628 | //sensor_msgs::CvBridge image_bridge; 629 | //sensor_msgs::cv_bridge image_bridge; 630 | 631 | std::string params = parameter; 632 | ParameterMap parameter_map; 633 | decodeParameter(params, parameter_map); // http://merry:8080/stream?topic=/remote_lab_cam1/image_raw?invert=1 634 | 635 | ParameterMap::iterator itp = parameter_map.find("topic"); 636 | if (itp == parameter_map.end()) 637 | return; 638 | 639 | std::string topic = itp->second; 640 | increaseSubscriberCount(topic); 641 | ImageBuffer* image_buffer = getImageBuffer(topic); 642 | 643 | /* wait for fresh frames */ 644 | boost::unique_lock lock(image_buffer->mutex_); 645 | image_buffer->condition_.wait(lock); 646 | 647 | //IplImage* image; 648 | cv_bridge::CvImagePtr cv_msg; 649 | try 650 | { 651 | if (cv_msg = cv_bridge::toCvCopy(image_buffer->msg, "bgr8")) 652 | { 653 | ; //image = image_bridge.toIpl(); 654 | } 655 | else 656 | { 657 | ROS_ERROR("Unable to convert %s image to bgr8", image_buffer->msg.encoding.c_str()); 658 | return; 659 | } 660 | } 661 | catch (...) 662 | { 663 | ROS_ERROR("Unable to convert %s image to ipl format", image_buffer->msg.encoding.c_str()); 664 | return; 665 | } 666 | 667 | cv::Mat img = cv_msg->image; 668 | std::vector encoded_buffer; 669 | std::vector encode_params; 670 | 671 | // invert 672 | //int invert = 0; 673 | if (parameter_map.find("invert") != parameter_map.end()) 674 | { 675 | cv::Mat cloned_image = img.clone(); 676 | invertImage(cloned_image, img); 677 | } 678 | 679 | // quality 680 | int quality = 95; 681 | if (parameter_map.find("quality") != parameter_map.end()) 682 | { 683 | quality = stringToInt(parameter_map["quality"]); 684 | } 685 | encode_params.push_back(CV_IMWRITE_JPEG_QUALITY); 686 | encode_params.push_back(quality); 687 | 688 | // resize image 689 | if (parameter_map.find("width") != parameter_map.end() && parameter_map.find("height") != parameter_map.end()) 690 | { 691 | int width = stringToInt(parameter_map["width"]); 692 | int height = stringToInt(parameter_map["height"]); 693 | if (width > 0 && height > 0) 694 | { 695 | cv::Mat img_resized; 696 | cv::Size new_size(width, height); 697 | cv::resize(img, img_resized, new_size); 698 | cv::imencode(".jpeg", img_resized, encoded_buffer, encode_params); 699 | } 700 | else 701 | { 702 | cv::imencode(".jpeg", img, encoded_buffer, encode_params); 703 | } 704 | } 705 | else 706 | { 707 | cv::imencode(".jpeg", img, encoded_buffer, encode_params); 708 | } 709 | 710 | // copy encoded frame buffer 711 | frame_size = encoded_buffer.size(); 712 | 713 | // resize buffer 714 | if ((frame = (unsigned char*)malloc(frame_size)) == NULL) 715 | { 716 | free(frame); 717 | sendError(fd, 500, "not enough memory"); 718 | return; 719 | } 720 | 721 | /* copy v4l2_buffer timeval to user space */ 722 | timestamp = ros::Time::now().toSec(); 723 | 724 | memcpy(frame, &encoded_buffer[0], frame_size); 725 | ROS_DEBUG("got frame (size: %d kB)", frame_size / 1024); 726 | 727 | /* write the response */ 728 | sprintf(buffer, "HTTP/1.0 200 OK\r\n" 729 | "%s" 730 | "Content-type: image/jpeg\r\n" 731 | "X-Timestamp: %.06lf\r\n" 732 | "\r\n", 733 | header.c_str(), (double)timestamp); 734 | 735 | /* send header and image now */ 736 | if (write(fd, buffer, strlen(buffer)) < 0 || write(fd, frame, frame_size) < 0) 737 | { 738 | free(frame); 739 | return; 740 | } 741 | 742 | free(frame); 743 | decreaseSubscriberCount(topic); 744 | unregisterSubscriberIfPossible(topic); 745 | } 746 | 747 | void MJPEGServer::client(int fd) 748 | { 749 | int cnt; 750 | char buffer[BUFFER_SIZE] = {0}, *pb = buffer; 751 | iobuffer iobuf; 752 | request req; 753 | 754 | /* initializes the structures */ 755 | initIOBuffer(&iobuf); 756 | initRequest(&req); 757 | 758 | /* What does the client want to receive? Read the request. */ 759 | memset(buffer, 0, sizeof(buffer)); 760 | if ((cnt = readLineWithTimeout(fd, &iobuf, buffer, sizeof(buffer) - 1, 5)) == -1) 761 | { 762 | close(fd); 763 | return; 764 | } 765 | 766 | /* determine what to deliver */ 767 | if (strstr(buffer, "GET /?") != NULL) 768 | { 769 | req.type = A_STREAM; 770 | 771 | /* advance by the length of known string */ 772 | if ((pb = strstr(buffer, "GET /")) == NULL) 773 | { 774 | ROS_DEBUG("HTTP request seems to be malformed"); 775 | sendError(fd, 400, "Malformed HTTP request"); 776 | close(fd); 777 | return; 778 | } 779 | pb += strlen("GET /"); // a pb points to the string after the first & after command 780 | int len = min(max((int)strspn(pb, "abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ._/-1234567890?="), 0), 100); 781 | req.parameter = (char*)malloc(len + 1); 782 | if (req.parameter == NULL) 783 | { 784 | exit(EXIT_FAILURE); 785 | } 786 | memset(req.parameter, 0, len + 1); 787 | strncpy(req.parameter, pb, len); 788 | 789 | ROS_DEBUG("requested image topic[%d]: \"%s\"", len, req.parameter); 790 | } 791 | else if (strstr(buffer, "GET /stream?") != NULL) 792 | { 793 | req.type = A_STREAM; 794 | 795 | /* advance by the length of known string */ 796 | if ((pb = strstr(buffer, "GET /stream")) == NULL) 797 | { 798 | ROS_DEBUG("HTTP request seems to be malformed"); 799 | sendError(fd, 400, "Malformed HTTP request"); 800 | close(fd); 801 | return; 802 | } 803 | pb += strlen("GET /stream"); // a pb points to the string after the first & after command 804 | int len = min(max((int)strspn(pb, "abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ._/-1234567890?="), 0), 100); 805 | req.parameter = (char*)malloc(len + 1); 806 | if (req.parameter == NULL) 807 | { 808 | exit(EXIT_FAILURE); 809 | } 810 | memset(req.parameter, 0, len + 1); 811 | strncpy(req.parameter, pb, len); 812 | 813 | ROS_DEBUG("requested image topic[%d]: \"%s\"", len, req.parameter); 814 | } 815 | else if (strstr(buffer, "GET /snapshot?") != NULL) 816 | { 817 | req.type = A_SNAPSHOT; 818 | 819 | /* advance by the length of known string */ 820 | if ((pb = strstr(buffer, "GET /snapshot")) == NULL) 821 | { 822 | ROS_DEBUG("HTTP request seems to be malformed"); 823 | sendError(fd, 400, "Malformed HTTP request"); 824 | close(fd); 825 | return; 826 | } 827 | pb += strlen("GET /snapshot"); // a pb points to the string after the first & after command 828 | int len = min(max((int)strspn(pb, "abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ._/-1234567890?="), 0), 100); 829 | req.parameter = (char*)malloc(len + 1); 830 | if (req.parameter == NULL) 831 | { 832 | exit(EXIT_FAILURE); 833 | } 834 | memset(req.parameter, 0, len + 1); 835 | strncpy(req.parameter, pb, len); 836 | 837 | ROS_DEBUG("requested image topic[%d]: \"%s\"", len, req.parameter); 838 | } 839 | 840 | /* 841 | * parse the rest of the HTTP-request 842 | * the end of the request-header is marked by a single, empty line with "\r\n" 843 | */ 844 | do 845 | { 846 | memset(buffer, 0, sizeof(buffer)); 847 | 848 | if ((cnt = readLineWithTimeout(fd, &iobuf, buffer, sizeof(buffer) - 1, 5)) == -1) 849 | { 850 | freeRequest(&req); 851 | close(fd); 852 | return; 853 | } 854 | 855 | if (strstr(buffer, "User-Agent: ") != NULL) 856 | { 857 | req.client = strdup(buffer + strlen("User-Agent: ")); 858 | } 859 | else if (strstr(buffer, "Authorization: Basic ") != NULL) 860 | { 861 | req.credentials = strdup(buffer + strlen("Authorization: Basic ")); 862 | decodeBase64(req.credentials); 863 | ROS_DEBUG("username:password: %s", req.credentials); 864 | } 865 | 866 | } while (cnt > 2 && !(buffer[0] == '\r' && buffer[1] == '\n')); 867 | 868 | /* now it's time to answer */ 869 | switch (req.type) 870 | { 871 | case A_STREAM: 872 | { 873 | ROS_DEBUG("Request for streaming"); 874 | sendStream(fd, req.parameter); 875 | break; 876 | } 877 | case A_SNAPSHOT: 878 | { 879 | ROS_DEBUG("Request for snapshot"); 880 | sendSnapshot(fd, req.parameter); 881 | break; 882 | } 883 | default: 884 | ROS_DEBUG("unknown request"); 885 | } 886 | 887 | close(fd); 888 | freeRequest(&req); 889 | ROS_INFO("Disconnecting HTTP client"); 890 | return; 891 | } 892 | 893 | void MJPEGServer::execute() 894 | { 895 | 896 | ROS_INFO("Starting mjpeg server"); 897 | 898 | struct addrinfo *aip, *aip2; 899 | struct addrinfo hints; 900 | struct sockaddr_storage client_addr; 901 | socklen_t addr_len = sizeof(struct sockaddr_storage); 902 | fd_set selectfds; 903 | int max_fds = 0; 904 | 905 | char name[NI_MAXHOST]; 906 | 907 | bzero(&hints, sizeof(hints)); 908 | hints.ai_family = PF_UNSPEC; 909 | hints.ai_flags = AI_PASSIVE; 910 | hints.ai_socktype = SOCK_STREAM; 911 | 912 | int err; 913 | snprintf(name, sizeof(name), "%d", port_); 914 | if ((err = getaddrinfo(NULL, name, &hints, &aip)) != 0) 915 | { 916 | perror(gai_strerror(err)); 917 | exit(EXIT_FAILURE); 918 | } 919 | 920 | for (int i = 0; i < MAX_NUM_SOCKETS; i++) 921 | sd[i] = -1; 922 | 923 | /* open sockets for server (1 socket / address family) */ 924 | int i = 0; 925 | int on; 926 | for (aip2 = aip; aip2 != NULL; aip2 = aip2->ai_next) 927 | { 928 | if ((sd[i] = socket(aip2->ai_family, aip2->ai_socktype, 0)) < 0) 929 | { 930 | continue; 931 | } 932 | 933 | /* ignore "socket already in use" errors */ 934 | on = 1; 935 | if (setsockopt(sd[i], SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)) < 0) 936 | { 937 | perror("setsockopt(SO_REUSEADDR) failed"); 938 | } 939 | 940 | /* IPv6 socket should listen to IPv6 only, otherwise we will get "socket already in use" */ 941 | on = 1; 942 | if (aip2->ai_family == AF_INET6 && setsockopt(sd[i], IPPROTO_IPV6, IPV6_V6ONLY, (const void *)&on, sizeof(on)) < 0) 943 | { 944 | perror("setsockopt(IPV6_V6ONLY) failed"); 945 | } 946 | 947 | /* perhaps we will use this keep-alive feature oneday */ 948 | /* setsockopt(sd, SOL_SOCKET, SO_KEEPALIVE, &on, sizeof(on)); */ 949 | 950 | if (bind(sd[i], aip2->ai_addr, aip2->ai_addrlen) < 0) 951 | { 952 | perror("bind"); 953 | sd[i] = -1; 954 | continue; 955 | } 956 | 957 | if (listen(sd[i], 10) < 0) 958 | { 959 | perror("listen"); 960 | sd[i] = -1; 961 | } 962 | else 963 | { 964 | i++; 965 | if (i >= MAX_NUM_SOCKETS) 966 | { 967 | ROS_ERROR("Maximum number of server sockets exceeded"); 968 | i--; 969 | break; 970 | } 971 | } 972 | } 973 | 974 | sd_len = i; 975 | 976 | if (sd_len < 1) 977 | { 978 | ROS_ERROR("Bind(%d) failed", port_); 979 | closelog(); 980 | exit(EXIT_FAILURE); 981 | } 982 | else 983 | { 984 | ROS_INFO("Bind(%d) succeeded", port_); 985 | } 986 | 987 | ROS_INFO("waiting for clients to connect"); 988 | 989 | /* create a child for every client that connects */ 990 | while (!stop_requested_) 991 | { 992 | 993 | do 994 | { 995 | FD_ZERO(&selectfds); 996 | 997 | for (i = 0; i < MAX_NUM_SOCKETS; i++) 998 | { 999 | if (sd[i] != -1) 1000 | { 1001 | FD_SET(sd[i], &selectfds); 1002 | 1003 | if (sd[i] > max_fds) 1004 | max_fds = sd[i]; 1005 | } 1006 | } 1007 | 1008 | err = select(max_fds + 1, &selectfds, NULL, NULL, NULL); 1009 | 1010 | if (err < 0 && errno != EINTR) 1011 | { 1012 | perror("select"); 1013 | exit(EXIT_FAILURE); 1014 | } 1015 | } while (err <= 0); 1016 | 1017 | ROS_INFO("Client connected"); 1018 | 1019 | for (i = 0; i < max_fds + 1; i++) 1020 | { 1021 | if (sd[i] != -1 && FD_ISSET(sd[i], &selectfds)) 1022 | { 1023 | int fd = accept(sd[i], (struct sockaddr *)&client_addr, &addr_len); 1024 | 1025 | /* start new thread that will handle this TCP connected client */ 1026 | ROS_DEBUG("create thread to handle client that just established a connection"); 1027 | 1028 | if (getnameinfo((struct sockaddr *)&client_addr, addr_len, name, sizeof(name), NULL, 0, NI_NUMERICHOST) == 0) 1029 | { 1030 | syslog(LOG_INFO, "serving client: %s\n", name); 1031 | } 1032 | 1033 | boost::thread t(boost::bind(&MJPEGServer::client, this, fd)); 1034 | t.detach(); 1035 | } 1036 | } 1037 | } 1038 | 1039 | ROS_INFO("leaving server thread, calling cleanup function now"); 1040 | cleanUp(); 1041 | } 1042 | 1043 | void MJPEGServer::cleanUp() 1044 | { 1045 | ROS_INFO("cleaning up ressources allocated by server thread"); 1046 | 1047 | for (int i = 0; i < MAX_NUM_SOCKETS; i++) 1048 | close(sd[i]); 1049 | } 1050 | 1051 | void MJPEGServer::spin() 1052 | { 1053 | boost::thread t(boost::bind(&MJPEGServer::execute, this)); 1054 | t.detach(); 1055 | ros::spin(); 1056 | ROS_INFO("stop requested"); 1057 | stop(); 1058 | } 1059 | 1060 | void MJPEGServer::stop() 1061 | { 1062 | stop_requested_ = true; 1063 | } 1064 | 1065 | void MJPEGServer::decreaseSubscriberCount(const std::string topic) 1066 | { 1067 | boost::unique_lock lock(image_maps_mutex_); 1068 | ImageSubscriberCountMap::iterator it = image_subscribers_count_.find(topic); 1069 | if (it != image_subscribers_count_.end()) 1070 | { 1071 | if (image_subscribers_count_[topic] == 1) { 1072 | image_subscribers_count_.erase(it); 1073 | ROS_INFO("no subscribers for %s", topic.c_str()); 1074 | } 1075 | else if (image_subscribers_count_[topic] > 0) { 1076 | image_subscribers_count_[topic] = image_subscribers_count_[topic] - 1; 1077 | ROS_INFO("%lu subscribers for %s", image_subscribers_count_[topic], topic.c_str()); 1078 | } 1079 | } 1080 | else 1081 | { 1082 | ROS_INFO("no subscribers counter for %s", topic.c_str()); 1083 | } 1084 | } 1085 | 1086 | void MJPEGServer::increaseSubscriberCount(const std::string topic) 1087 | { 1088 | boost::unique_lock lock(image_maps_mutex_); 1089 | ImageSubscriberCountMap::iterator it = image_subscribers_count_.find(topic); 1090 | if (it == image_subscribers_count_.end()) 1091 | { 1092 | image_subscribers_count_.insert(ImageSubscriberCountMap::value_type(topic, 1)); 1093 | } 1094 | else { 1095 | image_subscribers_count_[topic] = image_subscribers_count_[topic] + 1; 1096 | } 1097 | ROS_INFO("%lu subscribers for %s", image_subscribers_count_[topic], topic.c_str()); 1098 | } 1099 | 1100 | void MJPEGServer::unregisterSubscriberIfPossible(const std::string topic) 1101 | { 1102 | boost::unique_lock lock(image_maps_mutex_); 1103 | ImageSubscriberCountMap::iterator it = image_subscribers_count_.find(topic); 1104 | if (it == image_subscribers_count_.end() || 1105 | image_subscribers_count_[topic] == 0) 1106 | { 1107 | ImageSubscriberMap::iterator sub_it = image_subscribers_.find(topic); 1108 | if (sub_it != image_subscribers_.end()) 1109 | { 1110 | ROS_INFO("Unsubscribing from %s", topic.c_str()); 1111 | image_subscribers_.erase(sub_it); 1112 | } 1113 | } 1114 | } 1115 | } 1116 | 1117 | int main(int argc, char** argv) 1118 | { 1119 | ros::init(argc, argv, "mjpeg_server"); 1120 | 1121 | ROS_WARN("WARNING -- mjpeg_server IS NOW DEPRECATED."); 1122 | ROS_WARN("PLEASE SEE https://github.com/RobotWebTools/web_video_server."); 1123 | 1124 | ros::NodeHandle nh; 1125 | mjpeg_server::MJPEGServer server(nh); 1126 | server.spin(); 1127 | 1128 | return (0); 1129 | } 1130 | 1131 | --------------------------------------------------------------------------------