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


theora_imagem_transport
Author(s): Ethan Dreyfuss, Cedric Pradalier
autogenerated on Sun Oct 5 2014 23:49:34