00001 #include "web_video_server/libav_streamer.h"
00002 #include "async_web_server_cpp/http_reply.hpp"
00003
00004
00005 #define AV_CODEC_FLAG_GLOBAL_HEADER (1 << 22)
00006 #define CODEC_FLAG_GLOBAL_HEADER AV_CODEC_FLAG_GLOBAL_HEADER
00007
00008 namespace web_video_server
00009 {
00010
00011 static int ffmpeg_boost_mutex_lock_manager(void **mutex, enum AVLockOp op)
00012 {
00013 if (NULL == mutex)
00014 return -1;
00015
00016 switch (op)
00017 {
00018 case AV_LOCK_CREATE:
00019 {
00020 *mutex = NULL;
00021 boost::mutex *m = new boost::mutex();
00022 *mutex = static_cast<void *>(m);
00023 break;
00024 }
00025 case AV_LOCK_OBTAIN:
00026 {
00027 boost::mutex *m = static_cast<boost::mutex *>(*mutex);
00028 m->lock();
00029 break;
00030 }
00031 case AV_LOCK_RELEASE:
00032 {
00033 boost::mutex *m = static_cast<boost::mutex *>(*mutex);
00034 m->unlock();
00035 break;
00036 }
00037 case AV_LOCK_DESTROY:
00038 {
00039 boost::mutex *m = static_cast<boost::mutex *>(*mutex);
00040 m->lock();
00041 m->unlock();
00042 delete m;
00043 break;
00044 }
00045 default:
00046 break;
00047 }
00048 return 0;
00049 }
00050
00051 LibavStreamer::LibavStreamer(const async_web_server_cpp::HttpRequest &request,
00052 async_web_server_cpp::HttpConnectionPtr connection, ros::NodeHandle& nh,
00053 const std::string &format_name, const std::string &codec_name,
00054 const std::string &content_type) :
00055 ImageTransportImageStreamer(request, connection, nh), output_format_(0), format_context_(0), codec_(0), codec_context_(0), video_stream_(
00056 0), frame_(0), sws_context_(0), first_image_timestamp_(0), format_name_(
00057 format_name), codec_name_(codec_name), content_type_(content_type), opt_(0), io_buffer_(0)
00058 {
00059
00060 bitrate_ = request.get_query_param_value_or_default<int>("bitrate", 100000);
00061 qmin_ = request.get_query_param_value_or_default<int>("qmin", 10);
00062 qmax_ = request.get_query_param_value_or_default<int>("qmax", 42);
00063 gop_ = request.get_query_param_value_or_default<int>("gop", 250);
00064
00065 av_lockmgr_register(&ffmpeg_boost_mutex_lock_manager);
00066 av_register_all();
00067 }
00068
00069 LibavStreamer::~LibavStreamer()
00070 {
00071 if (codec_context_)
00072 avcodec_close(codec_context_);
00073 if (frame_)
00074 {
00075 #if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1)
00076 av_free(frame_);
00077 frame_ = NULL;
00078 #else
00079 av_frame_free(&frame_);
00080 #endif
00081 }
00082 if (io_buffer_)
00083 delete io_buffer_;
00084 if (format_context_) {
00085 if (format_context_->pb)
00086 av_free(format_context_->pb);
00087 avformat_free_context(format_context_);
00088 }
00089 if (sws_context_)
00090 sws_freeContext(sws_context_);
00091 }
00092
00093
00094 static int dispatch_output_packet(void* opaque, uint8_t* buffer, int buffer_size)
00095 {
00096 async_web_server_cpp::HttpConnectionPtr connection = *((async_web_server_cpp::HttpConnectionPtr*) opaque);
00097 std::vector<uint8_t> encoded_frame;
00098 encoded_frame.assign(buffer, buffer + buffer_size);
00099 connection->write_and_clear(encoded_frame);
00100 return 0;
00101 }
00102
00103 void LibavStreamer::initialize(const cv::Mat &img)
00104 {
00105
00106 format_context_ = avformat_alloc_context();
00107 if (!format_context_)
00108 {
00109 async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_,
00110 connection_,
00111 NULL, NULL);
00112 throw std::runtime_error("Error allocating ffmpeg format context");
00113 }
00114 output_format_ = av_guess_format(format_name_.c_str(), NULL, NULL);
00115 if (!output_format_)
00116 {
00117 async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_,
00118 connection_,
00119 NULL, NULL);
00120 throw std::runtime_error("Error looking up output format");
00121 }
00122 format_context_->oformat = output_format_;
00123
00124
00125 size_t io_buffer_size = 3 * 1024;
00126 io_buffer_ = new unsigned char[io_buffer_size];
00127 AVIOContext* io_ctx = avio_alloc_context(io_buffer_, io_buffer_size, AVIO_FLAG_WRITE, &connection_, NULL, dispatch_output_packet, NULL);
00128 if (!io_ctx)
00129 {
00130 async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_,
00131 connection_,
00132 NULL, NULL);
00133 throw std::runtime_error("Error setting up IO context");
00134 }
00135 io_ctx->seekable = 0;
00136 format_context_->pb = io_ctx;
00137 output_format_->flags |= AVFMT_FLAG_CUSTOM_IO;
00138 output_format_->flags |= AVFMT_NOFILE;
00139
00140
00141 if (codec_name_.empty())
00142 codec_ = avcodec_find_encoder(output_format_->video_codec);
00143 else
00144 codec_ = avcodec_find_encoder_by_name(codec_name_.c_str());
00145 if (!codec_)
00146 {
00147 async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_,
00148 connection_,
00149 NULL, NULL);
00150 throw std::runtime_error("Error looking up codec");
00151 }
00152 video_stream_ = avformat_new_stream(format_context_, codec_);
00153 if (!video_stream_)
00154 {
00155 async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_,
00156 connection_,
00157 NULL, NULL);
00158 throw std::runtime_error("Error creating video stream");
00159 }
00160 codec_context_ = video_stream_->codec;
00161
00162
00163 avcodec_get_context_defaults3(codec_context_, codec_);
00164
00165 codec_context_->codec_id = codec_->id;
00166 codec_context_->bit_rate = bitrate_;
00167
00168 codec_context_->width = output_width_;
00169 codec_context_->height = output_height_;
00170 codec_context_->delay = 0;
00171
00172 video_stream_->time_base.num = 1;
00173 video_stream_->time_base.den = 1000;
00174
00175 codec_context_->time_base.num = 1;
00176 codec_context_->time_base.den = 1;
00177 codec_context_->gop_size = gop_;
00178 codec_context_->pix_fmt = AV_PIX_FMT_YUV420P;
00179 codec_context_->max_b_frames = 0;
00180
00181
00182 codec_context_->qmin = qmin_;
00183 codec_context_->qmax = qmax_;
00184
00185 initializeEncoder();
00186
00187
00188 if (format_context_->oformat->flags & AVFMT_GLOBALHEADER)
00189 codec_context_->flags |= CODEC_FLAG_GLOBAL_HEADER;
00190
00191
00192 if (avcodec_open2(codec_context_, codec_, NULL) < 0)
00193 {
00194 async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_,
00195 connection_,
00196 NULL, NULL);
00197 throw std::runtime_error("Could not open video codec");
00198 }
00199
00200
00201 #if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1)
00202 frame_ = avcodec_alloc_frame();
00203 #else
00204 frame_ = av_frame_alloc();
00205 #endif
00206 av_image_alloc(frame_->data, frame_->linesize, output_width_, output_height_,
00207 codec_context_->pix_fmt, 1);
00208
00209 frame_->width = output_width_;
00210 frame_->height = output_height_;
00211 frame_->format = codec_context_->pix_fmt;
00212 output_format_->flags |= AVFMT_NOFILE;
00213
00214
00215 std::vector<uint8_t> header_buffer;
00216 std::size_t header_size;
00217 uint8_t *header_raw_buffer;
00218
00219 av_dict_set(&format_context_->metadata, "author", "ROS web_video_server", 0);
00220 av_dict_set(&format_context_->metadata, "title", topic_.c_str(), 0);
00221
00222
00223 async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok).header("Connection", "close").header(
00224 "Server", "web_video_server").header("Cache-Control",
00225 "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0").header(
00226 "Pragma", "no-cache").header("Expires", "0").header("Max-Age", "0").header("Trailer", "Expires").header(
00227 "Content-type", content_type_).header("Access-Control-Allow-Origin", "*").write(connection_);
00228
00229
00230 if (avformat_write_header(format_context_, &opt_) < 0)
00231 {
00232 async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_,
00233 connection_,
00234 NULL, NULL);
00235 throw std::runtime_error("Error openning dynamic buffer");
00236 }
00237 }
00238
00239 void LibavStreamer::initializeEncoder()
00240 {
00241 }
00242
00243 void LibavStreamer::sendImage(const cv::Mat &img, const ros::Time &time)
00244 {
00245 boost::mutex::scoped_lock lock(encode_mutex_);
00246 if (first_image_timestamp_.isZero())
00247 {
00248 first_image_timestamp_ = time;
00249 }
00250 std::vector<uint8_t> encoded_frame;
00251 #if (LIBAVUTIL_VERSION_MAJOR < 53)
00252 PixelFormat input_coding_format = PIX_FMT_BGR24;
00253 #else
00254 AVPixelFormat input_coding_format = AV_PIX_FMT_BGR24;
00255 #endif
00256
00257 #if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1)
00258 AVPicture *raw_frame = new AVPicture;
00259 avpicture_fill(raw_frame, img.data, input_coding_format, output_width_, output_height_);
00260 #else
00261 AVFrame *raw_frame = av_frame_alloc();
00262 av_image_fill_arrays(raw_frame->data, raw_frame->linesize,
00263 img.data, input_coding_format, output_width_, output_height_, 1);
00264 #endif
00265
00266
00267
00268
00269 if (!sws_context_)
00270 {
00271 static int sws_flags = SWS_BICUBIC;
00272 sws_context_ = sws_getContext(output_width_, output_height_, input_coding_format, output_width_, output_height_,
00273 codec_context_->pix_fmt, sws_flags, NULL, NULL, NULL);
00274 if (!sws_context_)
00275 {
00276 throw std::runtime_error("Could not initialize the conversion context");
00277 }
00278 }
00279
00280
00281 int ret = sws_scale(sws_context_,
00282 (const uint8_t * const *)raw_frame->data, raw_frame->linesize, 0,
00283 output_height_, frame_->data, frame_->linesize);
00284
00285 #if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1)
00286 delete raw_frame;
00287 #else
00288 av_frame_free(&raw_frame);
00289 #endif
00290
00291
00292 AVPacket pkt;
00293 int got_packet;
00294 av_init_packet(&pkt);
00295
00296 #if (LIBAVCODEC_VERSION_MAJOR < 54)
00297 int buf_size = 6 * output_width_ * output_height_;
00298 pkt.data = (uint8_t*)av_malloc(buf_size);
00299 pkt.size = avcodec_encode_video(codec_context_, pkt.data, buf_size, frame_);
00300 got_packet = pkt.size > 0;
00301 #elif (LIBAVCODEC_VERSION_MAJOR < 57)
00302 pkt.data = NULL;
00303 pkt.size = 0;
00304 if (avcodec_encode_video2(codec_context_, &pkt, frame_, &got_packet) < 0)
00305 {
00306 throw std::runtime_error("Error encoding video frame");
00307 }
00308 #else
00309 pkt.data = NULL;
00310 pkt.size = 0;
00311 if (avcodec_send_frame(codec_context_, frame_) < 0)
00312 {
00313 throw std::runtime_error("Error encoding video frame");
00314 }
00315 if (avcodec_receive_packet(codec_context_, &pkt) < 0)
00316 {
00317 throw std::runtime_error("Error retrieving encoded packet");
00318 }
00319 #endif
00320
00321 if (got_packet)
00322 {
00323 std::size_t size;
00324 uint8_t *output_buf;
00325
00326 double seconds = (time - first_image_timestamp_).toSec();
00327
00328 pkt.pts = (int64_t)(seconds / av_q2d(video_stream_->time_base) * 0.95);
00329 if (pkt.pts <= 0)
00330 pkt.pts = 1;
00331 pkt.dts = AV_NOPTS_VALUE;
00332
00333 if (pkt.flags&AV_PKT_FLAG_KEY)
00334 pkt.flags |= AV_PKT_FLAG_KEY;
00335
00336 pkt.stream_index = video_stream_->index;
00337
00338 if (av_write_frame(format_context_, &pkt))
00339 {
00340 throw std::runtime_error("Error when writing frame");
00341 }
00342 }
00343 else
00344 {
00345 encoded_frame.clear();
00346 }
00347 #if LIBAVCODEC_VERSION_INT < 54
00348 av_free(pkt.data);
00349 #endif
00350
00351 #if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1)
00352 av_free_packet(&pkt);
00353 #else
00354 av_packet_unref(&pkt);
00355 #endif
00356
00357 connection_->write_and_clear(encoded_frame);
00358 }
00359
00360 LibavStreamerType::LibavStreamerType(const std::string &format_name, const std::string &codec_name,
00361 const std::string &content_type) :
00362 format_name_(format_name), codec_name_(codec_name), content_type_(content_type)
00363 {
00364 }
00365
00366 boost::shared_ptr<ImageStreamer> LibavStreamerType::create_streamer(const async_web_server_cpp::HttpRequest &request,
00367 async_web_server_cpp::HttpConnectionPtr connection,
00368 ros::NodeHandle& nh)
00369 {
00370 return boost::shared_ptr<ImageStreamer>(
00371 new LibavStreamer(request, connection, nh, format_name_, codec_name_, content_type_));
00372 }
00373
00374 std::string LibavStreamerType::create_viewer(const async_web_server_cpp::HttpRequest &request)
00375 {
00376 std::stringstream ss;
00377 ss << "<video src=\"/stream?";
00378 ss << request.query;
00379 ss << "\" autoplay=\"true\" preload=\"none\"></video>";
00380 return ss.str();
00381 }
00382
00383 }