88namespace web_video_server
99{
1010
11- static int ffmpeg_boost_mutex_lock_manager (void **mutex, enum AVLockOp op)
12- {
13- if (NULL == mutex)
14- return -1 ;
15-
16- switch (op)
17- {
18- case AV_LOCK_CREATE:
19- {
20- *mutex = NULL ;
21- boost::mutex *m = new boost::mutex ();
22- *mutex = static_cast <void *>(m);
23- break ;
24- }
25- case AV_LOCK_OBTAIN:
26- {
27- boost::mutex *m = static_cast <boost::mutex *>(*mutex);
28- m->lock ();
29- break ;
30- }
31- case AV_LOCK_RELEASE:
32- {
33- boost::mutex *m = static_cast <boost::mutex *>(*mutex);
34- m->unlock ();
35- break ;
36- }
37- case AV_LOCK_DESTROY:
38- {
39- boost::mutex *m = static_cast <boost::mutex *>(*mutex);
40- m->lock ();
41- m->unlock ();
42- delete m;
43- break ;
44- }
45- default :
46- break ;
47- }
48- return 0 ;
49- }
50-
5111LibavStreamer::LibavStreamer (const async_web_server_cpp::HttpRequest &request,
5212 async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh,
5313 const std::string &format_name, const std::string &codec_name,
5414 const std::string &content_type) :
55- ImageTransportImageStreamer (request, connection, nh), output_format_( 0 ), format_context_(0 ), codec_(0 ), codec_context_(0 ), video_stream_(
15+ ImageTransportImageStreamer (request, connection, nh), format_context_(0 ), codec_(0 ), codec_context_(0 ), video_stream_(
5616 0 ), frame_(0 ), sws_context_(0 ), first_image_timestamp_(0 ), format_name_(
5717 format_name), codec_name_(codec_name), content_type_(content_type), opt_(0 ), io_buffer_(0 )
5818{
5919
6020 bitrate_ = request.get_query_param_value_or_default <int >(" bitrate" , 100000 );
6121 qmin_ = request.get_query_param_value_or_default <int >(" qmin" , 10 );
6222 qmax_ = request.get_query_param_value_or_default <int >(" qmax" , 42 );
63- gop_ = request.get_query_param_value_or_default <int >(" gop" , 250 );
64-
65- av_lockmgr_register (&ffmpeg_boost_mutex_lock_manager);
66- av_register_all ();
23+ gop_ = request.get_query_param_value_or_default <int >(" gop" , 25 );
6724}
6825
6926LibavStreamer::~LibavStreamer ()
7027{
7128 if (codec_context_)
72- avcodec_close (codec_context_);
29+ {
30+ avcodec_free_context (&codec_context_);
31+ }
7332 if (frame_)
7433 {
75- #if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1)
76- av_free (frame_);
77- frame_ = NULL ;
78- #else
7934 av_frame_free (&frame_);
80- #endif
8135 }
8236 if (io_buffer_)
8337 delete io_buffer_;
@@ -111,15 +65,15 @@ void LibavStreamer::initialize(const cv::Mat &img)
11165 NULL , NULL );
11266 throw std::runtime_error (" Error allocating ffmpeg format context" );
11367 }
114- output_format_ = av_guess_format (format_name_.c_str (), NULL , NULL );
115- if (!output_format_)
68+
69+ format_context_->oformat = av_guess_format (format_name_.c_str (), NULL , NULL );
70+ if (!format_context_->oformat )
11671 {
11772 async_web_server_cpp::HttpReply::stock_reply (async_web_server_cpp::HttpReply::internal_server_error)(request_,
11873 connection_,
11974 NULL , NULL );
12075 throw std::runtime_error (" Error looking up output format" );
12176 }
122- format_context_->oformat = output_format_;
12377
12478 // Set up custom IO callback.
12579 size_t io_buffer_size = 3 * 1024 ; // 3M seen elsewhere and adjudged good
@@ -134,12 +88,11 @@ void LibavStreamer::initialize(const cv::Mat &img)
13488 }
13589 io_ctx->seekable = 0 ; // no seeking, it's a stream
13690 format_context_->pb = io_ctx;
137- output_format_->flags |= AVFMT_FLAG_CUSTOM_IO;
138- output_format_->flags |= AVFMT_NOFILE;
91+ format_context_->max_interleave_delta = 0 ;
13992
14093 // Load codec
14194 if (codec_name_.empty ()) // use default codec if none specified
142- codec_ = avcodec_find_encoder (output_format_ ->video_codec );
95+ codec_ = avcodec_find_encoder (format_context_-> oformat ->video_codec );
14396 else
14497 codec_ = avcodec_find_encoder_by_name (codec_name_.c_str ());
14598 if (!codec_)
@@ -157,11 +110,10 @@ void LibavStreamer::initialize(const cv::Mat &img)
157110 NULL , NULL );
158111 throw std::runtime_error (" Error creating video stream" );
159112 }
160- codec_context_ = video_stream_->codec ;
161113
162- // Set options
163- avcodec_get_context_defaults3 (codec_context_, codec_);
114+ codec_context_ = avcodec_alloc_context3 (codec_);
164115
116+ // Set options
165117 codec_context_->codec_id = codec_->id ;
166118 codec_context_->bit_rate = bitrate_;
167119
@@ -182,11 +134,11 @@ void LibavStreamer::initialize(const cv::Mat &img)
182134 codec_context_->qmin = qmin_;
183135 codec_context_->qmax = qmax_;
184136
137+ codec_context_->flags |= AV_CODEC_FLAG_LOW_DELAY;
138+
185139 initializeEncoder ();
186140
187- // Some formats want stream headers to be separate
188- if (format_context_->oformat ->flags & AVFMT_GLOBALHEADER)
189- codec_context_->flags |= CODEC_FLAG_GLOBAL_HEADER;
141+ avcodec_parameters_from_context (video_stream_->codecpar , codec_context_);
190142
191143 // Open Codec
192144 if (avcodec_open2 (codec_context_, codec_, NULL ) < 0 )
@@ -198,23 +150,15 @@ void LibavStreamer::initialize(const cv::Mat &img)
198150 }
199151
200152 // Allocate frame buffers
201- #if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1)
202- frame_ = avcodec_alloc_frame ();
203- #else
204153 frame_ = av_frame_alloc ();
205- # endif
154+
206155 av_image_alloc (frame_->data , frame_->linesize , output_width_, output_height_,
207156 codec_context_->pix_fmt , 1 );
208157
209158 frame_->width = output_width_;
210159 frame_->height = output_height_;
211160 frame_->format = codec_context_->pix_fmt ;
212- output_format_->flags |= AVFMT_NOFILE;
213161
214- // Generate header
215- std::vector<uint8_t > header_buffer;
216- std::size_t header_size;
217- uint8_t *header_raw_buffer;
218162 // define meta data
219163 av_dict_set (&format_context_->metadata , " author" , " ROS web_video_server" , 0 );
220164 av_dict_set (&format_context_->metadata , " title" , topic_.c_str (), 0 );
@@ -247,23 +191,12 @@ void LibavStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time)
247191 {
248192 first_image_timestamp_ = time;
249193 }
250- std::vector<uint8_t > encoded_frame;
251- #if (LIBAVUTIL_VERSION_MAJOR < 53)
252- PixelFormat input_coding_format = PIX_FMT_BGR24;
253- #else
194+
254195 AVPixelFormat input_coding_format = AV_PIX_FMT_BGR24;
255- #endif
256196
257- #if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1)
258- AVPicture *raw_frame = new AVPicture;
259- avpicture_fill (raw_frame, img.data , input_coding_format, output_width_, output_height_);
260- #else
261197 AVFrame *raw_frame = av_frame_alloc ();
262198 av_image_fill_arrays (raw_frame->data , raw_frame->linesize ,
263199 img.data , input_coding_format, output_width_, output_height_, 1 );
264- #endif
265-
266-
267200
268201 // Convert from opencv to libav
269202 if (!sws_context_)
@@ -282,41 +215,36 @@ void LibavStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time)
282215 (const uint8_t * const *)raw_frame->data , raw_frame->linesize , 0 ,
283216 output_height_, frame_->data , frame_->linesize );
284217
285- #if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1)
286- delete raw_frame;
287- #else
288218 av_frame_free (&raw_frame);
289- #endif
290219
291220 // Encode the frame
292- AVPacket pkt;
293- int got_packet;
294- av_init_packet (&pkt);
295-
296- #if (LIBAVCODEC_VERSION_MAJOR < 54)
297- int buf_size = 6 * output_width_ * output_height_;
298- pkt.data = (uint8_t *)av_malloc (buf_size);
299- pkt.size = avcodec_encode_video (codec_context_, pkt.data , buf_size, frame_);
300- got_packet = pkt.size > 0 ;
301- #elif (LIBAVCODEC_VERSION_MAJOR < 57)
302- pkt.data = NULL ; // packet data will be allocated by the encoder
303- pkt.size = 0 ;
304- if (avcodec_encode_video2 (codec_context_, &pkt, frame_, &got_packet) < 0 )
221+ AVPacket* pkt = av_packet_alloc ();
222+
223+ ret = avcodec_send_frame (codec_context_, frame_);
224+ if (ret == AVERROR_EOF)
305225 {
306- throw std::runtime_error ( " Error encoding video frame " );
226+ RCLCPP_DEBUG_STREAM (nh_-> get_logger (), " avcodec_send_frame() encoder flushed \n " );
307227 }
308- #else
309- pkt.data = NULL ; // packet data will be allocated by the encoder
310- pkt.size = 0 ;
311- if (avcodec_send_frame (codec_context_, frame_) < 0 )
228+ else if (ret == AVERROR (EAGAIN))
229+ {
230+ RCLCPP_DEBUG_STREAM (nh_->get_logger (), " avcodec_send_frame() need output read out\n " );
231+ }
232+ if (ret < 0 )
312233 {
313234 throw std::runtime_error (" Error encoding video frame" );
314235 }
315- if (avcodec_receive_packet (codec_context_, &pkt) < 0 )
236+
237+ ret = avcodec_receive_packet (codec_context_, pkt);
238+ bool got_packet = pkt->size > 0 ;
239+ if (ret == AVERROR_EOF)
240+ {
241+ RCLCPP_DEBUG_STREAM (nh_->get_logger (), " avcodec_receive_packet() encoder flushed\n " );
242+ }
243+ else if (ret == AVERROR (EAGAIN))
316244 {
317- throw std::runtime_error (" Error retrieving encoded packet" );
245+ RCLCPP_DEBUG_STREAM (nh_->get_logger (), " avcodec_receive_packet() needs more input\n " );
246+ got_packet = false ;
318247 }
319- #endif
320248
321249 if (got_packet)
322250 {
@@ -325,36 +253,23 @@ void LibavStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time)
325253
326254 double seconds = (time - first_image_timestamp_).seconds ();
327255 // Encode video at 1/0.95 to minimize delay
328- pkt. pts = (int64_t )(seconds / av_q2d (video_stream_->time_base ) * 0.95 );
329- if (pkt. pts <= 0 )
330- pkt. pts = 1 ;
331- pkt. dts = AV_NOPTS_VALUE ;
256+ pkt-> pts = (int64_t )(seconds / av_q2d (video_stream_->time_base ) * 0.95 );
257+ if (pkt-> pts <= 0 )
258+ pkt-> pts = 1 ;
259+ pkt-> dts = pkt-> pts ;
332260
333- if (pkt. flags &AV_PKT_FLAG_KEY)
334- pkt. flags |= AV_PKT_FLAG_KEY;
261+ if (pkt-> flags &AV_PKT_FLAG_KEY)
262+ pkt-> flags |= AV_PKT_FLAG_KEY;
335263
336- pkt. stream_index = video_stream_->index ;
264+ pkt-> stream_index = video_stream_->index ;
337265
338- if (av_write_frame (format_context_, & pkt))
266+ if (av_write_frame (format_context_, pkt))
339267 {
340268 throw std::runtime_error (" Error when writing frame" );
341269 }
342270 }
343- else
344- {
345- encoded_frame.clear ();
346- }
347- #if LIBAVCODEC_VERSION_INT < 54
348- av_free (pkt.data );
349- #endif
350-
351- #if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1)
352- av_free_packet (&pkt);
353- #else
354- av_packet_unref (&pkt);
355- #endif
356271
357- connection_-> write_and_clear (encoded_frame );
272+ av_packet_unref (pkt );
358273}
359274
360275LibavStreamerType::LibavStreamerType (const std::string &format_name, const std::string &codec_name,
0 commit comments