theora_publisher.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 20012, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include "theora_image_transport/theora_publisher.h"
00036 #include <sensor_msgs/image_encodings.h>
00037 #include <std_msgs/Header.h>
00038 
00039 #include <vector>
00040 #include <cstdio> //for memcpy
00041 
00042 #include <opencv2/imgproc/imgproc.hpp>
00043 
00044 using namespace std;
00045 
00046 namespace theora_image_transport {
00047 
00048 TheoraPublisher::TheoraPublisher()
00049 {
00050   // Initialize info structure fields that don't change
00051   th_info_init(&encoder_setup_);
00052   
00053   encoder_setup_.pic_x = 0;
00054   encoder_setup_.pic_y = 0;
00055   encoder_setup_.colorspace = TH_CS_UNSPECIFIED;
00056   encoder_setup_.pixel_fmt = TH_PF_420; // See bottom of http://www.theora.org/doc/libtheora-1.1beta1/codec_8h.html
00057   encoder_setup_.aspect_numerator = 1;
00058   encoder_setup_.aspect_denominator = 1;
00059   encoder_setup_.fps_numerator = 1; // don't know the frame rate ahead of time
00060   encoder_setup_.fps_denominator = 1;
00061   encoder_setup_.keyframe_granule_shift = 6; // A good default for streaming applications
00062   // Note: target_bitrate and quality set to correct values in configCb
00063   encoder_setup_.target_bitrate = -1;
00064   encoder_setup_.quality = -1;
00065 }
00066 
00067 TheoraPublisher::~TheoraPublisher()
00068 {
00069   th_info_clear(&encoder_setup_);
00070 }
00071 
00072 void TheoraPublisher::advertiseImpl(ros::NodeHandle &nh, const std::string &base_topic, uint32_t queue_size,
00073                                     const image_transport::SubscriberStatusCallback  &user_connect_cb,
00074                                     const image_transport::SubscriberStatusCallback  &user_disconnect_cb,
00075                                     const ros::VoidPtr &tracked_object, bool latch)
00076 {
00077   // queue_size doesn't account for the 3 header packets, so we correct (with a little extra) here.
00078   queue_size += 4;
00079   // Latching doesn't make a lot of sense with this transport. Could try to save the last keyframe,
00080   // but do you then send all following delta frames too?
00081   latch = false;
00082   typedef image_transport::SimplePublisherPlugin<theora_image_transport::Packet> Base;
00083   Base::advertiseImpl(nh, base_topic, queue_size, user_connect_cb, user_disconnect_cb, tracked_object, latch);
00084 
00085   // Set up reconfigure server for this topic
00086   reconfigure_server_ = boost::make_shared<ReconfigureServer>(this->nh());
00087   ReconfigureServer::CallbackType f = boost::bind(&TheoraPublisher::configCb, this, _1, _2);
00088   reconfigure_server_->setCallback(f);
00089 }
00090 
00091 void TheoraPublisher::configCb(Config& config, uint32_t level)
00092 {
00093   // target_bitrate must be 0 if we're using quality.
00094   long bitrate = 0;
00095   if (config.optimize_for == theora_image_transport::TheoraPublisher_Bitrate)
00096     bitrate = config.target_bitrate;
00097   bool update_bitrate = bitrate && encoder_setup_.target_bitrate != bitrate;
00098   bool update_quality = !bitrate && ((encoder_setup_.quality != config.quality) || encoder_setup_.target_bitrate > 0);
00099   encoder_setup_.quality = config.quality;
00100   encoder_setup_.target_bitrate = bitrate;
00101   keyframe_frequency_ = config.keyframe_frequency;
00102   
00103   if (encoding_context_) {
00104     int err = 0;
00105     // libtheora 1.1 lets us change quality or bitrate on the fly, 1.0 does not.
00106 #ifdef TH_ENCCTL_SET_BITRATE
00107     if (update_bitrate) {
00108       err = th_encode_ctl(encoding_context_.get(), TH_ENCCTL_SET_BITRATE, &bitrate, sizeof(long));
00109       if (err)
00110         ROS_ERROR("Failed to update bitrate dynamically");
00111     }
00112 #else
00113     err |= update_bitrate;
00114 #endif
00115 
00116 #ifdef TH_ENCCTL_SET_QUALITY
00117     if (update_quality) {
00118       err = th_encode_ctl(encoding_context_.get(), TH_ENCCTL_SET_QUALITY, &config.quality, sizeof(int));
00119       // In 1.1 above call will fail if a bitrate has previously been set. That restriction may
00120       // be relaxed in a future version. Complain on other failures.
00121       if (err && err != TH_EINVAL)
00122         ROS_ERROR("Failed to update quality dynamically");
00123     }
00124 #else
00125     err |= update_quality;
00126 #endif
00127 
00128     // If unable to change parameters dynamically, just create a new encoding context.
00129     if (err) {
00130       encoding_context_.reset();
00131     }
00132     // Otherwise, do the easy updates and keep going!
00133     else {
00134       updateKeyframeFrequency();
00135       config.keyframe_frequency = keyframe_frequency_; // In case desired value was unattainable
00136     }
00137   }
00138 }
00139 
00140 void TheoraPublisher::connectCallback(const ros::SingleSubscriberPublisher& pub)
00141 {
00142   // Send the header packets to new subscribers
00143   for (unsigned int i = 0; i < stream_header_.size(); i++) {
00144     pub.publish(stream_header_[i]);
00145   }
00146 }
00147 
00148 static void cvToTheoraPlane(cv::Mat& mat, th_img_plane& plane)
00149 {
00150   plane.width  = mat.cols;
00151   plane.height = mat.rows;
00152   plane.stride = mat.step;
00153   plane.data   = mat.data;
00154 }
00155 
00156 void TheoraPublisher::publish(const sensor_msgs::Image& message, const PublishFn& publish_fn) const
00157 {
00158   if (!ensureEncodingContext(message, publish_fn))
00159     return;
00160   //return;
00164 
00165   cv_bridge::CvImageConstPtr cv_image_ptr;
00166   try
00167   {
00168     // conversion necessary
00169     cv_image_ptr = cv_bridge::toCvCopy(message, sensor_msgs::image_encodings::BGR8);
00170   }
00171   catch (cv_bridge::Exception& e)
00172   {
00173     ROS_ERROR("cv_bridge exception: '%s'", e.what());
00174     return;
00175   }
00176   catch (cv::Exception& e)
00177   {
00178     ROS_ERROR("OpenCV exception: '%s'", e.what());
00179     return;
00180   }
00181 
00182   if (cv_image_ptr == 0) {
00183     ROS_ERROR("Unable to convert from '%s' to 'bgr8'", message.encoding.c_str());
00184     return;
00185   }
00186 
00187   const cv::Mat bgr = cv_image_ptr->image;
00188 
00189   cv::Mat bgr_padded;
00190   int frame_width = encoder_setup_.frame_width, frame_height = encoder_setup_.frame_height;
00191   if (frame_width == bgr.cols && frame_height == bgr.rows) {
00192     bgr_padded = bgr;
00193   }
00194   else {
00195     bgr_padded = cv::Mat::zeros(frame_height, frame_width, bgr.type());
00196     cv::Mat pic_roi = bgr_padded(cv::Rect(0, 0, bgr.cols, bgr.rows));
00197     bgr.copyTo(pic_roi);
00198   }
00199 
00200   // Convert image to Y'CbCr color space used by Theora
00201   cv::Mat ycrcb;
00202   cv::cvtColor(bgr_padded, ycrcb, cv::COLOR_BGR2YCrCb);
00203   
00204   // Split channels
00205   cv::Mat ycrcb_planes[3];
00206   cv::split(ycrcb, ycrcb_planes);
00207 
00208   // Use Y as-is but subsample chroma channels
00209   cv::Mat y = ycrcb_planes[0], cr, cb;
00210   cv::pyrDown(ycrcb_planes[1], cr);
00211   cv::pyrDown(ycrcb_planes[2], cb);
00212 
00213   // Construct Theora image buffer
00214   th_ycbcr_buffer ycbcr_buffer;
00215   cvToTheoraPlane(y,  ycbcr_buffer[0]);
00216   cvToTheoraPlane(cb, ycbcr_buffer[1]);
00217   cvToTheoraPlane(cr, ycbcr_buffer[2]);
00218 
00219   // Submit frame to the encoder
00220   int rval = th_encode_ycbcr_in(encoding_context_.get(), ycbcr_buffer);
00221   if (rval == TH_EFAULT) {
00222     ROS_ERROR("[theora] EFAULT in submitting uncompressed frame to encoder");
00223     return;
00224   }
00225   if (rval == TH_EINVAL) {
00226     ROS_ERROR("[theora] EINVAL in submitting uncompressed frame to encoder");
00227     return;
00228   }
00229 
00230   // Retrieve and publish encoded video data packets
00231   ogg_packet oggpacket;
00232   theora_image_transport::Packet output;
00233   while ((rval = th_encode_packetout(encoding_context_.get(), 0, &oggpacket)) > 0) {
00234     oggPacketToMsg(message.header, oggpacket, output);
00235     publish_fn(output);
00236   }
00237   if (rval == TH_EFAULT)
00238     ROS_ERROR("[theora] EFAULT in retrieving encoded video data packets");
00239 }
00240 
00241 void freeContext(th_enc_ctx* context)
00242 {
00243   if (context) th_encode_free(context);
00244 }
00245 
00246 bool TheoraPublisher::ensureEncodingContext(const sensor_msgs::Image& image, const PublishFn& publish_fn) const
00247 {
00249   if (encoding_context_ && encoder_setup_.pic_width == image.width &&
00250       encoder_setup_.pic_height == image.height)
00251     return true;
00252 
00253   // Theora has a divisible-by-sixteen restriction for the encoded frame size, so
00254   // scale the picture size up to the nearest multiple of 16 and calculate offsets.
00255   encoder_setup_.frame_width = (image.width + 15) & ~0xF;
00256   encoder_setup_.frame_height = (image.height + 15) & ~0xF;
00257   encoder_setup_.pic_width = image.width;
00258   encoder_setup_.pic_height = image.height;
00259 
00260   // Allocate encoding context. Smart pointer ensures that th_encode_free gets called.
00261   encoding_context_.reset(th_encode_alloc(&encoder_setup_), freeContext);
00262   if (!encoding_context_) {
00263     ROS_ERROR("[theora] Failed to create encoding context");
00264     return false;
00265   }
00266 
00267   updateKeyframeFrequency();
00268 
00269   th_comment comment;
00270   th_comment_init(&comment);
00271   boost::shared_ptr<th_comment> clear_guard(&comment, th_comment_clear);
00273   comment.vendor = strdup("Willow Garage theora_image_transport");
00274 
00275   // Construct the header and stream it in case anyone is already listening
00277   stream_header_.clear();
00278   ogg_packet oggpacket;
00279   while (th_encode_flushheader(encoding_context_.get(), &comment, &oggpacket) > 0) {
00280     stream_header_.push_back(theora_image_transport::Packet());
00281     oggPacketToMsg(image.header, oggpacket, stream_header_.back());
00282     publish_fn(stream_header_.back());
00283   }
00284   return true;
00285 }
00286 
00287 void TheoraPublisher::oggPacketToMsg(const std_msgs::Header& header, const ogg_packet &oggpacket,
00288                                      theora_image_transport::Packet &msg) const
00289 {
00290   msg.header     = header;
00291   msg.b_o_s      = oggpacket.b_o_s;
00292   msg.e_o_s      = oggpacket.e_o_s;
00293   msg.granulepos = oggpacket.granulepos;
00294   msg.packetno   = oggpacket.packetno;
00295   msg.data.resize(oggpacket.bytes);
00296   memcpy(&msg.data[0], oggpacket.packet, oggpacket.bytes);
00297 }
00298 
00299 void TheoraPublisher::updateKeyframeFrequency() const
00300 {
00301   ogg_uint32_t desired_frequency = keyframe_frequency_;
00302   if (th_encode_ctl(encoding_context_.get(), TH_ENCCTL_SET_KEYFRAME_FREQUENCY_FORCE,
00303                     &keyframe_frequency_, sizeof(ogg_uint32_t)))
00304     ROS_ERROR("Failed to change keyframe frequency");
00305   if (keyframe_frequency_ != desired_frequency)
00306     ROS_WARN("Couldn't set keyframe frequency to %d, actually set to %d",
00307              desired_frequency, keyframe_frequency_);
00308 }
00309 
00310 } //namespace theora_image_transport


theora_image_transport
Author(s): Patrick Mihelich, Ethan Dreyfuss
autogenerated on Tue Jul 2 2019 19:53:25