00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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
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
00058 typedef enum {
00059 A_UNKNOWN,
00060 A_STREAM,
00061 A_SNAPSHOT,
00062 } answer_t;
00063
00064
00065
00066
00067
00068 typedef struct {
00069 answer_t type;
00070 char *parameter;
00071 char *client;
00072 char *credentials;
00073 } request;
00074
00075
00076 typedef struct {
00077 int level;
00078 char buffer[IO_BUFFER];
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