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


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