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 using namespace std;
00043 
00044 namespace theora_image_transport {
00045 
00046 TheoraPublisher::TheoraPublisher()
00047 {
00048   // Initialize info structure fields that don't change
00049   th_info_init(&encoder_setup_);
00050   
00051   encoder_setup_.pic_x = 0;
00052   encoder_setup_.pic_y = 0;
00053   encoder_setup_.colorspace = TH_CS_UNSPECIFIED;
00054   encoder_setup_.pixel_fmt = TH_PF_420; // See bottom of http://www.theora.org/doc/libtheora-1.1beta1/codec_8h.html
00055   encoder_setup_.aspect_numerator = 1;
00056   encoder_setup_.aspect_denominator = 1;
00057   encoder_setup_.fps_numerator = 1; // don't know the frame rate ahead of time
00058   encoder_setup_.fps_denominator = 1;
00059   encoder_setup_.keyframe_granule_shift = 6; // A good default for streaming applications
00060   // Note: target_bitrate and quality set to correct values in configCb
00061   encoder_setup_.target_bitrate = -1;
00062   encoder_setup_.quality = -1;
00063 }
00064 
00065 TheoraPublisher::~TheoraPublisher()
00066 {
00067   th_info_clear(&encoder_setup_);
00068 }
00069 
00070 void TheoraPublisher::advertiseImpl(ros::NodeHandle &nh, const std::string &base_topic, uint32_t queue_size,
00071                                     const image_transport::SubscriberStatusCallback  &user_connect_cb,
00072                                     const image_transport::SubscriberStatusCallback  &user_disconnect_cb,
00073                                     const ros::VoidPtr &tracked_object, bool latch)
00074 {
00075   // queue_size doesn't account for the 3 header packets, so we correct (with a little extra) here.
00076   queue_size += 4;
00077   // Latching doesn't make a lot of sense with this transport. Could try to save the last keyframe,
00078   // but do you then send all following delta frames too?
00079   latch = false;
00080   typedef image_transport::SimplePublisherPlugin<theora_image_transport::Packet> Base;
00081   Base::advertiseImpl(nh, base_topic, queue_size, user_connect_cb, user_disconnect_cb, tracked_object, latch);
00082 
00083   // Set up reconfigure server for this topic
00084   reconfigure_server_ = boost::make_shared<ReconfigureServer>(this->nh());
00085   ReconfigureServer::CallbackType f = boost::bind(&TheoraPublisher::configCb, this, _1, _2);
00086   reconfigure_server_->setCallback(f);
00087 }
00088 
00089 void TheoraPublisher::configCb(Config& config, uint32_t level)
00090 {
00091   // target_bitrate must be 0 if we're using quality.
00092   long bitrate = 0;
00093   if (config.optimize_for == theora_image_transport::TheoraPublisher_Bitrate)
00094     bitrate = config.target_bitrate;
00095   bool update_bitrate = bitrate && encoder_setup_.target_bitrate != bitrate;
00096   bool update_quality = !bitrate && ((encoder_setup_.quality != config.quality) || encoder_setup_.target_bitrate > 0);
00097   encoder_setup_.quality = config.quality;
00098   encoder_setup_.target_bitrate = bitrate;
00099   keyframe_frequency_ = config.keyframe_frequency;
00100   
00101   if (encoding_context_) {
00102     int err = 0;
00103     // libtheora 1.1 lets us change quality or bitrate on the fly, 1.0 does not.
00104 #ifdef TH_ENCCTL_SET_BITRATE
00105     if (update_bitrate) {
00106       err = th_encode_ctl(encoding_context_.get(), TH_ENCCTL_SET_BITRATE, &bitrate, sizeof(long));
00107       if (err)
00108         ROS_ERROR("Failed to update bitrate dynamically");
00109     }
00110 #else
00111     err |= update_bitrate;
00112 #endif
00113 
00114 #ifdef TH_ENCCTL_SET_QUALITY
00115     if (update_quality) {
00116       err = th_encode_ctl(encoding_context_.get(), TH_ENCCTL_SET_QUALITY, &config.quality, sizeof(int));
00117       // In 1.1 above call will fail if a bitrate has previously been set. That restriction may
00118       // be relaxed in a future version. Complain on other failures.
00119       if (err && err != TH_EINVAL)
00120         ROS_ERROR("Failed to update quality dynamically");
00121     }
00122 #else
00123     err |= update_quality;
00124 #endif
00125 
00126     // If unable to change parameters dynamically, just create a new encoding context.
00127     if (err) {
00128       encoding_context_.reset();
00129     }
00130     // Otherwise, do the easy updates and keep going!
00131     else {
00132       updateKeyframeFrequency();
00133       config.keyframe_frequency = keyframe_frequency_; // In case desired value was unattainable
00134     }
00135   }
00136 }
00137 
00138 void TheoraPublisher::connectCallback(const ros::SingleSubscriberPublisher& pub)
00139 {
00140   // Send the header packets to new subscribers
00141   for (unsigned int i = 0; i < stream_header_.size(); i++) {
00142     pub.publish(stream_header_[i]);
00143   }
00144 }
00145 
00146 static void cvToTheoraPlane(cv::Mat& mat, th_img_plane& plane)
00147 {
00148   plane.width  = mat.cols;
00149   plane.height = mat.rows;
00150   plane.stride = mat.step;
00151   plane.data   = mat.data;
00152 }
00153 
00154 void TheoraPublisher::publish(const sensor_msgs::Image& message, const PublishFn& publish_fn) const
00155 {
00156   if (!ensureEncodingContext(message, publish_fn))
00157     return;
00158   //return;
00162 
00163   cv_bridge::CvImageConstPtr cv_image_ptr;
00164   try
00165   {
00166     // conversion necessary
00167     cv_image_ptr = cv_bridge::toCvCopy(message, sensor_msgs::image_encodings::BGR8);
00168   }
00169   catch (cv_bridge::Exception& e)
00170   {
00171     ROS_ERROR("cv_bridge exception: '%s'", e.what());
00172     return;
00173   }
00174   catch (cv::Exception& e)
00175   {
00176     ROS_ERROR("OpenCV exception: '%s'", e.what());
00177     return;
00178   }
00179 
00180   if (cv_image_ptr == 0) {
00181     ROS_ERROR("Unable to convert from '%s' to 'bgr8'", message.encoding.c_str());
00182     return;
00183   }
00184 
00185   const cv::Mat bgr = cv_image_ptr->image;
00186 
00187   cv::Mat bgr_padded;
00188   int frame_width = encoder_setup_.frame_width, frame_height = encoder_setup_.frame_height;
00189   if (frame_width == bgr.cols && frame_height == bgr.rows) {
00190     bgr_padded = bgr;
00191   }
00192   else {
00193     bgr_padded = cv::Mat::zeros(frame_height, frame_width, bgr.type());
00194     cv::Mat pic_roi = bgr_padded(cv::Rect(0, 0, bgr.cols, bgr.rows));
00195     bgr.copyTo(pic_roi);
00196   }
00197 
00198   // Convert image to Y'CbCr color space used by Theora
00199   cv::Mat ycrcb;
00200   cv::cvtColor(bgr_padded, ycrcb, CV_BGR2YCrCb);
00201   
00202   // Split channels
00203   cv::Mat ycrcb_planes[3];
00204   cv::split(ycrcb, ycrcb_planes);
00205 
00206   // Use Y as-is but subsample chroma channels
00207   cv::Mat y = ycrcb_planes[0], cr, cb;
00208   cv::pyrDown(ycrcb_planes[1], cr);
00209   cv::pyrDown(ycrcb_planes[2], cb);
00210 
00211   // Construct Theora image buffer
00212   th_ycbcr_buffer ycbcr_buffer;
00213   cvToTheoraPlane(y,  ycbcr_buffer[0]);
00214   cvToTheoraPlane(cb, ycbcr_buffer[1]);
00215   cvToTheoraPlane(cr, ycbcr_buffer[2]);
00216 
00217   // Submit frame to the encoder
00218   int rval = th_encode_ycbcr_in(encoding_context_.get(), ycbcr_buffer);
00219   if (rval == TH_EFAULT) {
00220     ROS_ERROR("[theora] EFAULT in submitting uncompressed frame to encoder");
00221     return;
00222   }
00223   if (rval == TH_EINVAL) {
00224     ROS_ERROR("[theora] EINVAL in submitting uncompressed frame to encoder");
00225     return;
00226   }
00227 
00228   // Retrieve and publish encoded video data packets
00229   ogg_packet oggpacket;
00230   theora_image_transport::Packet output;
00231   while ((rval = th_encode_packetout(encoding_context_.get(), 0, &oggpacket)) > 0) {
00232     oggPacketToMsg(message.header, oggpacket, output);
00233     publish_fn(output);
00234   }
00235   if (rval == TH_EFAULT)
00236     ROS_ERROR("[theora] EFAULT in retrieving encoded video data packets");
00237 }
00238 
00239 void freeContext(th_enc_ctx* context)
00240 {
00241   if (context) th_encode_free(context);
00242 }
00243 
00244 bool TheoraPublisher::ensureEncodingContext(const sensor_msgs::Image& image, const PublishFn& publish_fn) const
00245 {
00247   if (encoding_context_ && encoder_setup_.pic_width == image.width &&
00248       encoder_setup_.pic_height == image.height)
00249     return true;
00250 
00251   // Theora has a divisible-by-sixteen restriction for the encoded frame size, so
00252   // scale the picture size up to the nearest multiple of 16 and calculate offsets.
00253   encoder_setup_.frame_width = (image.width + 15) & ~0xF;
00254   encoder_setup_.frame_height = (image.height + 15) & ~0xF;
00255   encoder_setup_.pic_width = image.width;
00256   encoder_setup_.pic_height = image.height;
00257 
00258   // Allocate encoding context. Smart pointer ensures that th_encode_free gets called.
00259   encoding_context_.reset(th_encode_alloc(&encoder_setup_), freeContext);
00260   if (!encoding_context_) {
00261     ROS_ERROR("[theora] Failed to create encoding context");
00262     return false;
00263   }
00264 
00265   updateKeyframeFrequency();
00266 
00267   th_comment comment;
00268   th_comment_init(&comment);
00269   boost::shared_ptr<th_comment> clear_guard(&comment, th_comment_clear);
00271   comment.vendor = strdup("Willow Garage theora_image_transport");
00272 
00273   // Construct the header and stream it in case anyone is already listening
00275   stream_header_.clear();
00276   ogg_packet oggpacket;
00277   while (th_encode_flushheader(encoding_context_.get(), &comment, &oggpacket) > 0) {
00278     stream_header_.push_back(theora_image_transport::Packet());
00279     oggPacketToMsg(image.header, oggpacket, stream_header_.back());
00280     publish_fn(stream_header_.back());
00281   }
00282   return true;
00283 }
00284 
00285 void TheoraPublisher::oggPacketToMsg(const std_msgs::Header& header, const ogg_packet &oggpacket,
00286                                      theora_image_transport::Packet &msg) const
00287 {
00288   msg.header     = header;
00289   msg.b_o_s      = oggpacket.b_o_s;
00290   msg.e_o_s      = oggpacket.e_o_s;
00291   msg.granulepos = oggpacket.granulepos;
00292   msg.packetno   = oggpacket.packetno;
00293   msg.data.resize(oggpacket.bytes);
00294   memcpy(&msg.data[0], oggpacket.packet, oggpacket.bytes);
00295 }
00296 
00297 void TheoraPublisher::updateKeyframeFrequency() const
00298 {
00299   ogg_uint32_t desired_frequency = keyframe_frequency_;
00300   if (th_encode_ctl(encoding_context_.get(), TH_ENCCTL_SET_KEYFRAME_FREQUENCY_FORCE,
00301                     &keyframe_frequency_, sizeof(ogg_uint32_t)))
00302     ROS_ERROR("Failed to change keyframe frequency");
00303   if (keyframe_frequency_ != desired_frequency)
00304     ROS_WARN("Couldn't set keyframe frequency to %d, actually set to %d",
00305              desired_frequency, keyframe_frequency_);
00306 }
00307 
00308 } //namespace theora_image_transport


theora_image_transport
Author(s): Patrick Mihelich, Ethan Dreyfuss
autogenerated on Fri Aug 28 2015 11:08:26