theora_subscriber.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_subscriber.h"
00036 #include <cv_bridge/cv_bridge.h>
00037 #include <sensor_msgs/image_encodings.h>
00038 #include <opencv/cvwimage.h>
00039 #include <opencv2/imgproc/imgproc.hpp>
00040 #include <boost/scoped_array.hpp>
00041 #include <vector>
00042 
00043 using namespace std;
00044 
00045 namespace theora_image_transport {
00046 
00047 TheoraSubscriber::TheoraSubscriber()
00048   : pplevel_(0),
00049     received_header_(false),
00050     received_keyframe_(false),
00051     decoding_context_(NULL),
00052     setup_info_(NULL)
00053 {
00054   th_info_init(&header_info_);
00055   th_comment_init(&header_comment_);
00056 }
00057 
00058 TheoraSubscriber::~TheoraSubscriber()
00059 {
00060   if (decoding_context_) th_decode_free(decoding_context_);
00061   th_setup_free(setup_info_);
00062   th_info_clear(&header_info_);
00063   th_comment_clear(&header_comment_);
00064 }
00065 
00066 void TheoraSubscriber::subscribeImpl(ros::NodeHandle &nh, const std::string &base_topic, uint32_t queue_size,
00067                                      const Callback &callback, const ros::VoidPtr &tracked_object,
00068                                      const image_transport::TransportHints &transport_hints)
00069 {
00070   // queue_size doesn't account for the 3 header packets, so we correct (with a little extra) here.
00071   queue_size += 4;
00072   typedef image_transport::SimpleSubscriberPlugin<theora_image_transport::Packet> Base;
00073   Base::subscribeImpl(nh, base_topic, queue_size, callback, tracked_object, transport_hints);
00074 
00075   // Set up reconfigure server for this topic
00076   reconfigure_server_ = boost::make_shared<ReconfigureServer>(this->nh());
00077   ReconfigureServer::CallbackType f = boost::bind(&TheoraSubscriber::configCb, this, _1, _2);
00078   reconfigure_server_->setCallback(f);
00079 }
00080 
00081 void TheoraSubscriber::configCb(Config& config, uint32_t level)
00082 {
00083   if (decoding_context_ && pplevel_ != config.post_processing_level) {
00084     pplevel_ = updatePostProcessingLevel(config.post_processing_level);
00085     config.post_processing_level = pplevel_; // In case more than PPLEVEL_MAX
00086   }
00087   else
00088     pplevel_ = config.post_processing_level;
00089 }
00090 
00091 int TheoraSubscriber::updatePostProcessingLevel(int level)
00092 {
00093   int pplevel_max;
00094   int err = th_decode_ctl(decoding_context_, TH_DECCTL_GET_PPLEVEL_MAX, &pplevel_max, sizeof(int));
00095   if (err)
00096     ROS_WARN("Failed to get maximum post-processing level, error code %d", err);
00097   else if (level > pplevel_max) {
00098     ROS_WARN("Post-processing level %d is above the maximum, clamping to %d", level, pplevel_max);
00099     level = pplevel_max;
00100   }
00101 
00102   err = th_decode_ctl(decoding_context_, TH_DECCTL_SET_PPLEVEL, &level, sizeof(int));
00103   if (err) {
00104     ROS_ERROR("Failed to set post-processing level, error code %d", err);
00105     return pplevel_; // old value
00106   }
00107   return level;
00108 }
00109 
00110 //When using this caller is responsible for deleting oggpacket.packet!!
00111 void TheoraSubscriber::msgToOggPacket(const theora_image_transport::Packet &msg, ogg_packet &ogg)
00112 {
00113   ogg.bytes      = msg.data.size();
00114   ogg.b_o_s      = msg.b_o_s;
00115   ogg.e_o_s      = msg.e_o_s;
00116   ogg.granulepos = msg.granulepos;
00117   ogg.packetno   = msg.packetno;
00118   ogg.packet = new unsigned char[ogg.bytes];
00119   memcpy(ogg.packet, &msg.data[0], ogg.bytes);
00120 }
00121 
00122 void TheoraSubscriber::internalCallback(const theora_image_transport::PacketConstPtr& message, const Callback& callback)
00123 {
00125   ogg_packet oggpacket;
00126   msgToOggPacket(*message, oggpacket);
00127   boost::scoped_array<unsigned char> packet_guard(oggpacket.packet); // Make sure packet memory gets deleted
00128 
00129   // Beginning of logical stream flag means we're getting new headers
00130   if (oggpacket.b_o_s == 1) {
00131     // Clear all state, everything we knew is wrong
00132     received_header_ = false;
00133     received_keyframe_ = false;
00134     if (decoding_context_) {
00135       th_decode_free(decoding_context_);
00136       decoding_context_ = NULL;
00137     }
00138     th_setup_free(setup_info_);
00139     setup_info_ = NULL;
00140     th_info_clear(&header_info_);
00141     th_info_init(&header_info_);
00142     th_comment_clear(&header_comment_);
00143     th_comment_init(&header_comment_);
00144     latest_image_.reset();
00145   }
00146 
00147   // Decode header packets until we get the first video packet
00148   if (received_header_ == false) {
00149     int rval = th_decode_headerin(&header_info_, &header_comment_, &setup_info_, &oggpacket);
00150     switch (rval) {
00151       case 0:
00152         // We've received the full header; this is the first video packet.
00153         decoding_context_ = th_decode_alloc(&header_info_, setup_info_);
00154         if (!decoding_context_) {
00155           ROS_ERROR("[theora] Decoding parameters were invalid");
00156           return;
00157         }
00158         received_header_ = true;
00159         pplevel_ = updatePostProcessingLevel(pplevel_);
00160         break; // Continue on the video decoding
00161       case TH_EFAULT:
00162         ROS_WARN("[theora] EFAULT when processing header packet");
00163         return;
00164       case TH_EBADHEADER:
00165         ROS_WARN("[theora] Bad header packet");
00166         return;
00167       case TH_EVERSION:
00168         ROS_WARN("[theora] Header packet not decodable with this version of libtheora");
00169         return;
00170       case TH_ENOTFORMAT:
00171         ROS_WARN("[theora] Packet was not a Theora header");
00172         return;
00173       default:
00174         // If rval > 0, we successfully received a header packet.
00175         if (rval < 0)
00176           ROS_WARN("[theora] Error code %d when processing header packet", rval);
00177         return;
00178     }
00179   }
00180 
00181   // Wait for a keyframe if we haven't received one yet - delta frames are useless to us in that case
00182   received_keyframe_ = received_keyframe_ || (th_packet_iskeyframe(&oggpacket) == 1);
00183   if (!received_keyframe_)
00184     return;
00185   
00186   // We have a video packet we can handle, let's decode it
00187   int rval = th_decode_packetin(decoding_context_, &oggpacket, NULL);
00188   switch (rval) {
00189     case 0:
00190       break; // Yay, we got a frame. Carry on below.
00191     case TH_DUPFRAME:
00192       // Video data hasn't changed, so we update the timestamp and reuse the last received frame.
00193       ROS_DEBUG("[theora] Got a duplicate frame");
00194       if (latest_image_) {
00195         latest_image_->header = message->header;
00196         callback(latest_image_);
00197       }
00198       return;
00199     case TH_EFAULT:
00200       ROS_WARN("[theora] EFAULT processing video packet");
00201       return;
00202     case TH_EBADPACKET:
00203       ROS_WARN("[theora] Packet does not contain encoded video data");
00204       return;
00205     case TH_EIMPL:
00206       ROS_WARN("[theora] The video data uses bitstream features not supported by this version of libtheora");
00207       return;
00208     default:
00209       ROS_WARN("[theora] Error code %d when decoding video packet", rval);
00210       return;
00211   }
00212 
00213   // We have a new decoded frame available
00214   th_ycbcr_buffer ycbcr_buffer;
00215   th_decode_ycbcr_out(decoding_context_, ycbcr_buffer);
00216 
00217   // Wrap YCbCr channel data into OpenCV format
00218   th_img_plane &y_plane = ycbcr_buffer[0], &cb_plane = ycbcr_buffer[1], &cr_plane = ycbcr_buffer[2];
00219   cv::Mat y(y_plane.height, y_plane.width, CV_8UC1, y_plane.data, y_plane.stride);
00220   cv::Mat cb_sub(cb_plane.height, cb_plane.width, CV_8UC1, cb_plane.data, cb_plane.stride);
00221   cv::Mat cr_sub(cr_plane.height, cr_plane.width, CV_8UC1, cr_plane.data, cr_plane.stride);
00222 
00223   // Upsample chroma channels
00224   cv::Mat cb, cr;
00225   cv::pyrUp(cb_sub, cb);
00226   cv::pyrUp(cr_sub, cr);
00227 
00228   // Merge into interleaved image. Note OpenCV uses YCrCb, so we swap the chroma channels.
00229   cv::Mat ycrcb, channels[] = {y, cr, cb};
00230   cv::merge(channels, 3, ycrcb);
00231 
00232   // Convert to BGR color
00233   cv::Mat bgr, bgr_padded;
00234   cv::cvtColor(ycrcb, bgr_padded, CV_YCrCb2BGR);
00235   // Pull out original (non-padded) image region
00236   bgr = bgr_padded(cv::Rect(header_info_.pic_x, header_info_.pic_y,
00237                             header_info_.pic_width, header_info_.pic_height));
00238 
00239   latest_image_ = cv_bridge::CvImage(message->header, sensor_msgs::image_encodings::BGR8, bgr).toImageMsg();
00240   latest_image_->__connection_header = message->__connection_header;
00242   callback(latest_image_);
00243 }
00244 
00245 } //namespace theora_image_transport


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