Go to the documentation of this file.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/cv_bridge.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 {
00060 A_UNKNOWN, A_STREAM, A_SNAPSHOT,
00061 } answer_t;
00062
00063
00064
00065
00066
00067 typedef struct
00068 {
00069 answer_t type;
00070 char *parameter;
00071 char *client;
00072 char *credentials;
00073 } request;
00074
00075
00076 typedef struct
00077 {
00078 int level;
00079 char buffer[IO_BUFFER];
00080 } iobuffer;
00081
00086 class ImageBuffer
00087 {
00088 public:
00089 ImageBuffer()
00090 {
00091 }
00092 sensor_msgs::Image msg;
00093 boost::condition_variable condition_;
00094 boost::mutex mutex_;
00095 };
00096
00101 class MJPEGServer
00102 {
00103 public:
00108 MJPEGServer(ros::NodeHandle& node);
00109
00113 virtual ~MJPEGServer();
00114
00121 void execute();
00122
00126 void spin();
00127
00131 void stop();
00132
00136 void cleanUp();
00137
00145 void client(int fd);
00146
00147 private:
00148 typedef std::map<std::string, ImageBuffer*> ImageBufferMap;
00149 typedef std::map<std::string, image_transport::Subscriber> ImageSubscriberMap;
00150 typedef std::map<std::string, size_t> ImageSubscriberCountMap;
00151 typedef std::map<std::string, std::string> ParameterMap;
00152
00153 std::string header;
00154
00155 ImageBuffer* getImageBuffer(const std::string& topic);
00156
00157 void imageCallback(const sensor_msgs::ImageConstPtr& msg, const std::string& topic);
00158
00164 void invertImage(const cv::Mat& input, cv::Mat& output);
00165
00172 void sendError(int fd, int which, const char *message);
00173
00179 void sendStream(int fd, const char *parameter);
00180
00186 void sendSnapshot(int fd, const char *parameter);
00187
00192 void initIOBuffer(iobuffer *iobuf);
00193
00198 void initRequest(request *req);
00199
00205 void freeRequest(request *req);
00206
00223 int readWithTimeout(int fd, iobuffer *iobuf, char *buffer, size_t len, int timeout);
00224
00241 int readLineWithTimeout(int fd, iobuffer *iobuf, char *buffer, size_t len, int timeout);
00242
00250 void decodeBase64(char *data);
00251
00257 int hexCharToInt(char in);
00258
00264 int unescape(char *string);
00265
00271 void splitString(const std::string& str, std::vector<std::string>& tokens, const std::string& delimiter = " ");
00272
00279 int stringToInt(const std::string& str, const int default_value = 0);
00280
00286 void decodeParameter(const std::string& parameter, ParameterMap& parameter_map);
00287
00292 void decreaseSubscriberCount(const std::string topic);
00293
00298 void increaseSubscriberCount(const std::string topic);
00299
00304 void unregisterSubscriberIfPossible(const std::string topic);
00305
00306 ros::NodeHandle node_;
00307 image_transport::ImageTransport image_transport_;
00308 int port_;
00309
00310 int sd[MAX_NUM_SOCKETS];
00311 int sd_len;
00312
00313 bool stop_requested_;
00314 char* www_folder_;
00315
00316 ImageBufferMap image_buffers_;
00317 ImageSubscriberMap image_subscribers_;
00318 ImageSubscriberCountMap image_subscribers_count_;
00319 boost::mutex image_maps_mutex_;
00320
00321 };
00322
00323 }
00324
00325 #endif
00326