ffmpeg_encoder.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2014, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  *
00030  * ffmpeg_encoder.cpp
00031  *
00032  *  Created on: Oct 30, 2012
00033  *      Author: jkammerl
00034  */
00035 
00036 #include "boost/date_time/posix_time/posix_time.hpp"
00037 
00038 #include <boost/bind.hpp>
00039 
00040 #include "ffmpeg_encoder.h"
00041 #include "image_subscriber.h"
00042 
00043 #include "boost/date_time/posix_time/posix_time.hpp" //include all types plus i/o
00044 
00045 #define MAX_DEPTH 4.0
00046 #define RAW_DEPTH_IMAGE_RESOLUTION 512
00047 
00048 namespace enc = sensor_msgs::image_encodings;
00049 
00050 namespace ros_http_video_streamer
00051 {
00052 
00053 FFMPEGEncoder::FFMPEGEncoder(const std::string& refID, const std::string& topic, const ServerConfiguration& config) :
00054     doEncoding_(true),
00055     refID_(refID),
00056     topic_(topic),
00057     config_(config),
00058     subscriber_(topic, config.ros_transport_),
00059     frameID_(0),
00060     init_(false),
00061     ffmpeg_(0)
00062 {
00063   start();
00064 }
00065 
00066 FFMPEGEncoder::~FFMPEGEncoder()
00067 {
00068   stop();
00069 }
00070 
00071 void FFMPEGEncoder::start()
00072 {
00073 }
00074 
00075 void FFMPEGEncoder::stop()
00076 {
00077 
00078   if (doEncoding_)
00079   {
00080     doEncoding_ = false;
00081 
00082     if (ffmpeg_)
00083     {
00084       ffmpeg_->shutdown();
00085       delete (ffmpeg_);
00086       ffmpeg_ = 0;
00087     }
00088   }
00089 
00090 }
00091 
00092 const std::string& FFMPEGEncoder::getRefID()
00093 {
00094   return refID_;
00095 }
00096 
00097 void FFMPEGEncoder::convertFloatingPointImageToMono8(float* input, const std::size_t width, const std::size_t height,
00098                                                      std::vector<uint8_t>& output)
00099 {
00100   std::size_t image_size, i;
00101   float* input_ptr;
00102   uint8_t* output_ptr;
00103 
00104   // prepare output vector
00105   image_size = width * height;
00106   output.resize(image_size);
00107 
00108   // Find min. and max. pixel value
00109   float minValue = std::numeric_limits<float>::max();
00110   float maxValue = std::numeric_limits<float>::min();
00111   input_ptr = input;
00112   output_ptr = &output[0];
00113   bool valid_image = false;
00114   for (i = 0; i < image_size; ++i)
00115   {
00116     if (*input_ptr == *input_ptr) // check for NaN
00117     {
00118       minValue = std::min(minValue, *input_ptr);
00119       maxValue = std::max(maxValue, *input_ptr);
00120       valid_image = true;
00121     }
00122     input_ptr++;
00123   }
00124 
00125   // reset data pointer
00126   input_ptr = input;
00127   output_ptr = &output[0];
00128 
00129   // Rescale floating point image and convert it to 8-bit
00130   float dynamic_range = maxValue - minValue;
00131   if (valid_image && (dynamic_range > 0.0f))
00132   {
00133     // Rescale and quantize
00134     for (i = 0; i < image_size; ++i, ++input_ptr, ++output_ptr)
00135     {
00136       if (*input_ptr == *input_ptr) // check for NaN
00137       {
00138         *output_ptr = ((*input_ptr - minValue) / dynamic_range) * 255u;
00139       }
00140       else
00141       {
00142         *output_ptr = 0u;
00143       }
00144     }
00145 
00146   }
00147   else
00148   {
00149     // clear output buffer
00150     memset(output_ptr, 0, image_size * sizeof(uint8_t));
00151   }
00152 }
00153 
00154 int FFMPEGEncoder::initEncoding(std::vector<uint8_t>& header)
00155 {
00156   sensor_msgs::ImageConstPtr frame;
00157 
00158   boost::posix_time::ptime start = boost::posix_time::microsec_clock::local_time();;
00159 
00160   while (!init_ && (boost::posix_time::time_duration(boost::posix_time::microsec_clock::local_time()-start).total_seconds()<5.0))
00161   {
00162     // get frame from ROS iamge subscriber
00163     subscriber_.getImageFromQueue(frame);
00164 
00165     if ((frame) && (!ffmpeg_))
00166     {
00167       // ffmpeg wrapper has not been initialized yet
00168       ffmpeg_ = new FFMPEG_Wrapper();
00169 
00170       // first input frame defines resolution
00171       if (ffmpeg_ && ffmpeg_->init(frame->width, frame->height, config_)>=0 )
00172       {
00173 
00174         // retrieve header data from ffmpeg wrapper
00175         ffmpeg_->get_header(header);
00176 
00177         ROS_DEBUG("Codec header received: %d bytes", (int)header.size());
00178 
00179         subscriber_.emptyQueue();
00180 
00181         // FFMPEG initialized
00182         init_ = true;
00183       }
00184 
00185     }
00186     else
00187     {
00188       boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00189     }
00190   }
00191 
00192   return init_?0:-1;
00193 }
00194 
00195 void FFMPEGEncoder::getVideoPacket(std::vector<uint8_t>& buf)
00196 {
00197   buf.clear();
00198 
00199   if (!init_)
00200   {
00201     ROS_ERROR("Video encoding not initialized.");
00202     return;
00203   }
00204 
00205   while (buf.size() == 0)
00206   {
00207     // time stamps used to control the encoder rate
00208     unsigned int milisec_used;
00209     const unsigned int milisec_per_frame = 1000 / config_.framerate_;
00210 
00211     sensor_msgs::ImageConstPtr frame;
00212 
00213     // get frame from ROS iamge subscriber
00214     subscriber_.getImageFromQueue(frame);
00215 
00216     ROS_DEBUG("Encoding triggered..");
00217 
00218     if (frame)
00219     {
00220 
00221       std::vector<uint8_t> frame_buf_temp;
00222 
00223       bool valid_format = true;
00224 
00225       // Bit depth of image frame
00226       unsigned int num_channels = enc::numChannels(frame->encoding);
00227       switch (num_channels)
00228       {
00229         case 1:
00230           // monochrome 8-bit encoded frame
00231           if ((!frame->encoding.compare("mono8")) || (!frame->encoding.compare("8UC1")))
00232           {
00233             ffmpeg_->encode_mono8_frame((uint8_t*)&frame->data[0], buf);
00234           }
00235           else if ((!frame->encoding.compare("mono16")) || (!frame->encoding.compare("16UC1")))
00236           {
00237             // monochrome 16-bit encoded frame
00238 
00239             ffmpeg_->encode_mono16_frame((uint8_t*)&frame->data[0], buf);
00240 
00241           }
00242           else if (!frame->encoding.compare("32FC1"))
00243           {
00244             // floating-point encoded frame
00245 
00246             std::vector<uint8_t> normalized_image;
00247 
00248             convertFloatingPointImageToMono8((float*)&frame->data[0], frame->width, frame->height, normalized_image);
00249 
00250             ffmpeg_->encode_mono8_frame(&normalized_image[0], buf);
00251 
00252           }
00253           else
00254           {
00255             valid_format = false;
00256           }
00257           break;
00258         case 3:
00259           if (frame->encoding.find("rgb") != std::string::npos)
00260           {
00261             // rgb encoded frame
00262 
00263             ffmpeg_->encode_rgb_frame((uint8_t*)&frame->data[0], buf);
00264           }
00265           else if (frame->encoding.find("bgr") != std::string::npos)
00266           {
00267             // bgr encoded frame
00268 
00269             ffmpeg_->encode_bgr_frame((uint8_t*)&frame->data[0], buf);
00270           }
00271           else
00272           {
00273             valid_format = false;
00274           }
00275           break;
00276         default:
00277           valid_format = false;
00278           break;
00279       }
00280 
00281       if (!valid_format)
00282       {
00283         ROS_ERROR("Invalid image format");
00284       }
00285 
00286     }
00287 
00288     // calculate encoding time
00289     boost::posix_time::ptime now = boost::posix_time::microsec_clock::local_time();
00290     boost::posix_time::time_duration diff = now - last_tick;
00291     last_tick = now;
00292     milisec_used = diff.total_milliseconds();
00293 
00294     if (milisec_used < milisec_per_frame)
00295     {
00296       // if encoder worked faster than the desired frame rate -> go sleeping
00297       boost::this_thread::sleep(boost::posix_time::milliseconds(milisec_per_frame - milisec_used));
00298     }
00299   }
00300 
00301   ROS_DEBUG("Frame received: %d bytes", (int)buf.size());
00302 
00303 }
00304 
00305 } // ros_http_video_streamer


ros_web_video
Author(s): Julius Kammer
autogenerated on Fri Aug 28 2015 12:38:36