├── .gitignore
├── mainpage.dox
├── AUTHORS.md
├── README.md
├── .travis.yml
├── package.xml
├── CHANGELOG.rst
├── LICENSE
├── CMakeLists.txt
├── include
└── mjpeg_server
│ └── mjpeg_server.h
└── src
└── mjpeg_server.cpp
/.gitignore:
--------------------------------------------------------------------------------
1 | .cproject
2 | .project
3 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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 [](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 |
--------------------------------------------------------------------------------
/.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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------