$search
00001 /********************************************************************* 00002 * 00003 * Software License Agreement (BSD License) 00004 * 00005 * Copyright (c) 2010, Robert Bosch LLC. 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of the Robert Bosch nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 *********************************************************************/ 00036 #ifndef MJPEG_SERVER_H_ 00037 #define MJPEG_SERVER_H_ 00038 00039 #include <ros/ros.h> 00040 #include <sensor_msgs/Image.h> 00041 #include <image_transport/image_transport.h> 00042 #include <cv_bridge/CvBridge.h> 00043 00044 #include <boost/thread/mutex.hpp> 00045 #include <boost/thread/condition.hpp> 00046 00047 /* 00048 * Maximum number of server sockets (i.e. protocol families) to listen. 00049 */ 00050 #define MAX_NUM_SOCKETS 100 00051 #define IO_BUFFER 256 00052 #define BUFFER_SIZE 1024 00053 00054 namespace mjpeg_server { 00055 00056 00057 /* the webserver determines between these values for an answer */ 00058 typedef enum { 00059 A_UNKNOWN, 00060 A_STREAM, 00061 A_SNAPSHOT, 00062 } answer_t; 00063 00064 /* 00065 * the client sends information with each request 00066 * this structure is used to store the important parts 00067 */ 00068 typedef struct { 00069 answer_t type; 00070 char *parameter; 00071 char *client; 00072 char *credentials; 00073 } request; 00074 00075 /* the iobuffer structure is used to read from the HTTP-client */ 00076 typedef struct { 00077 int level; /* how full is the buffer */ 00078 char buffer[IO_BUFFER]; /* the data */ 00079 } iobuffer; 00080 00085 class ImageBuffer { 00086 public: 00087 ImageBuffer() {} 00088 sensor_msgs::Image msg; 00089 boost::condition_variable condition_; 00090 boost::mutex mutex_; 00091 }; 00092 00097 class MJPEGServer { 00098 public: 00103 MJPEGServer(ros::NodeHandle& node); 00104 00108 virtual ~MJPEGServer(); 00109 00116 void execute(); 00117 00121 void spin(); 00122 00126 void stop(); 00127 00131 void cleanUp(); 00132 00140 void client(int fd); 00141 00142 private: 00143 typedef std::map<std::string, ImageBuffer*> ImageBufferMap; 00144 typedef std::map<std::string, image_transport::Subscriber> ImageSubscriberMap; 00145 typedef std::map<std::string, std::string> ParameterMap; 00146 00147 ImageBuffer* getImageBuffer(const std::string& topic); 00148 00149 00150 void imageCallback(const sensor_msgs::ImageConstPtr& msg, const std::string& topic); 00151 00157 void invertImage(const cv::Mat& input, cv::Mat& output); 00158 00165 void sendError(int fd, int which, const char *message); 00166 00172 void sendStream(int fd, const char *parameter); 00173 00179 void sendSnapshot(int fd, const char *parameter); 00180 00185 void initIOBuffer(iobuffer *iobuf); 00186 00191 void initRequest(request *req); 00192 00198 void freeRequest(request *req); 00199 00216 int readWithTimeout(int fd, iobuffer *iobuf, char *buffer, size_t len, int timeout); 00217 00234 int readLineWithTimeout(int fd, iobuffer *iobuf, char *buffer, size_t len, int timeout); 00235 00243 void decodeBase64(char *data); 00244 00250 int hexCharToInt(char in); 00251 00257 int unescape(char *string); 00258 00264 void splitString(const std::string& str, std::vector<std::string>& tokens, const std::string& delimiter = " "); 00265 00272 int stringToInt(const std::string& str, const int default_value = 0); 00273 00279 void decodeParameter(const std::string& parameter, ParameterMap& parameter_map); 00280 00281 00282 ros::NodeHandle node_; 00283 image_transport::ImageTransport image_transport_; 00284 int port_; 00285 00286 int sd[MAX_NUM_SOCKETS]; 00287 int sd_len; 00288 00289 bool stop_requested_; 00290 char* www_folder_; 00291 00292 ImageBufferMap image_buffers_; 00293 ImageSubscriberMap image_subscribers_; 00294 boost::mutex image_maps_mutex_; 00295 00296 }; 00297 00298 } 00299 00300 #endif 00301