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
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
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_;
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_;
00073 }
00074 return level;
00075 }
00076
00077
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);
00096
00097
00098 if (oggpacket.b_o_s == 1) {
00099
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
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
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;
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
00143 if (rval < 0)
00144 ROS_WARN("[theora] Error code %d when processing header packet", rval);
00145 return;
00146 }
00147 }
00148
00149
00150 received_keyframe_ = received_keyframe_ || (th_packet_iskeyframe(&oggpacket) == 1);
00151 if (!received_keyframe_)
00152 return;
00153
00154
00155 int rval = th_decode_packetin(decoding_context_, &oggpacket, NULL);
00156 switch (rval) {
00157 case 0:
00158 break;
00159 case TH_DUPFRAME:
00160
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
00182 th_ycbcr_buffer ycbcr_buffer;
00183 th_decode_ycbcr_out(decoding_context_, ycbcr_buffer);
00184
00185
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
00192 cv::Mat cb, cr;
00193 cv::pyrUp(cb_sub, cb);
00194 cv::pyrUp(cr_sub, cr);
00195
00196
00197 cv::Mat ycrcb, channels[] = {y, cr, cb};
00198 cv::merge(channels, 3, ycrcb);
00199
00200
00201 cv::Mat bgr, bgr_padded;
00202 cv::cvtColor(ycrcb, bgr_padded, CV_YCrCb2BGR);
00203
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 }