theora_publisher.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 20012, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
37 #include <std_msgs/Header.h>
38 
39 #include <vector>
40 #include <cstdio> //for memcpy
41 
42 #include <opencv2/imgproc/imgproc.hpp>
43 
44 using namespace std;
45 
46 namespace theora_image_transport {
47 
48 TheoraPublisher::TheoraPublisher()
49 {
50  // Initialize info structure fields that don't change
51  th_info_init(&encoder_setup_);
52 
53  encoder_setup_.pic_x = 0;
54  encoder_setup_.pic_y = 0;
55  encoder_setup_.colorspace = TH_CS_UNSPECIFIED;
56  encoder_setup_.pixel_fmt = TH_PF_420; // See bottom of http://www.theora.org/doc/libtheora-1.1beta1/codec_8h.html
57  encoder_setup_.aspect_numerator = 1;
58  encoder_setup_.aspect_denominator = 1;
59  encoder_setup_.fps_numerator = 1; // don't know the frame rate ahead of time
60  encoder_setup_.fps_denominator = 1;
61  encoder_setup_.keyframe_granule_shift = 6; // A good default for streaming applications
62  // Note: target_bitrate and quality set to correct values in configCb
63  encoder_setup_.target_bitrate = -1;
64  encoder_setup_.quality = -1;
65 }
66 
67 TheoraPublisher::~TheoraPublisher()
68 {
69  th_info_clear(&encoder_setup_);
70 }
71 
72 void TheoraPublisher::advertiseImpl(ros::NodeHandle &nh, const std::string &base_topic, uint32_t queue_size,
73  const image_transport::SubscriberStatusCallback &user_connect_cb,
74  const image_transport::SubscriberStatusCallback &user_disconnect_cb,
75  const ros::VoidPtr &tracked_object, bool latch)
76 {
77  // queue_size doesn't account for the 3 header packets, so we correct (with a little extra) here.
78  queue_size += 4;
79  // Latching doesn't make a lot of sense with this transport. Could try to save the last keyframe,
80  // but do you then send all following delta frames too?
81  latch = false;
83  Base::advertiseImpl(nh, base_topic, queue_size, user_connect_cb, user_disconnect_cb, tracked_object, latch);
84 
85  // Set up reconfigure server for this topic
86  reconfigure_server_ = boost::make_shared<ReconfigureServer>(this->nh());
87  ReconfigureServer::CallbackType f = boost::bind(&TheoraPublisher::configCb, this, _1, _2);
88  reconfigure_server_->setCallback(f);
89 }
90 
91 void TheoraPublisher::configCb(Config& config, uint32_t level)
92 {
93  // target_bitrate must be 0 if we're using quality.
94  long bitrate = 0;
95  if (config.optimize_for == theora_image_transport::TheoraPublisher_Bitrate)
96  bitrate = config.target_bitrate;
97  bool update_bitrate = bitrate && encoder_setup_.target_bitrate != bitrate;
98  bool update_quality = !bitrate && ((encoder_setup_.quality != config.quality) || encoder_setup_.target_bitrate > 0);
99  encoder_setup_.quality = config.quality;
100  encoder_setup_.target_bitrate = bitrate;
101  keyframe_frequency_ = config.keyframe_frequency;
102 
103  if (encoding_context_) {
104  int err = 0;
105  // libtheora 1.1 lets us change quality or bitrate on the fly, 1.0 does not.
106 #ifdef TH_ENCCTL_SET_BITRATE
107  if (update_bitrate) {
108  err = th_encode_ctl(encoding_context_.get(), TH_ENCCTL_SET_BITRATE, &bitrate, sizeof(long));
109  if (err)
110  ROS_ERROR("Failed to update bitrate dynamically");
111  }
112 #else
113  err |= update_bitrate;
114 #endif
115 
116 #ifdef TH_ENCCTL_SET_QUALITY
117  if (update_quality) {
118  err = th_encode_ctl(encoding_context_.get(), TH_ENCCTL_SET_QUALITY, &config.quality, sizeof(int));
119  // In 1.1 above call will fail if a bitrate has previously been set. That restriction may
120  // be relaxed in a future version. Complain on other failures.
121  if (err && err != TH_EINVAL)
122  ROS_ERROR("Failed to update quality dynamically");
123  }
124 #else
125  err |= update_quality;
126 #endif
127 
128  // If unable to change parameters dynamically, just create a new encoding context.
129  if (err) {
130  encoding_context_.reset();
131  }
132  // Otherwise, do the easy updates and keep going!
133  else {
134  updateKeyframeFrequency();
135  config.keyframe_frequency = keyframe_frequency_; // In case desired value was unattainable
136  }
137  }
138 }
139 
140 void TheoraPublisher::connectCallback(const ros::SingleSubscriberPublisher& pub)
141 {
142  // Send the header packets to new subscribers
143  for (unsigned int i = 0; i < stream_header_.size(); i++) {
144  pub.publish(stream_header_[i]);
145  }
146 }
147 
148 static void cvToTheoraPlane(cv::Mat& mat, th_img_plane& plane)
149 {
150  plane.width = mat.cols;
151  plane.height = mat.rows;
152  plane.stride = mat.step;
153  plane.data = mat.data;
154 }
155 
156 void TheoraPublisher::publish(const sensor_msgs::Image& message, const PublishFn& publish_fn) const
157 {
158  if (!ensureEncodingContext(message, publish_fn))
159  return;
160  //return;
164 
165  cv_bridge::CvImageConstPtr cv_image_ptr;
166  try
167  {
168  // conversion necessary
170  }
171  catch (cv_bridge::Exception& e)
172  {
173  ROS_ERROR("cv_bridge exception: '%s'", e.what());
174  return;
175  }
176  catch (cv::Exception& e)
177  {
178  ROS_ERROR("OpenCV exception: '%s'", e.what());
179  return;
180  }
181 
182  if (cv_image_ptr == 0) {
183  ROS_ERROR("Unable to convert from '%s' to 'bgr8'", message.encoding.c_str());
184  return;
185  }
186 
187  const cv::Mat bgr = cv_image_ptr->image;
188 
189  cv::Mat bgr_padded;
190  int frame_width = encoder_setup_.frame_width, frame_height = encoder_setup_.frame_height;
191  if (frame_width == bgr.cols && frame_height == bgr.rows) {
192  bgr_padded = bgr;
193  }
194  else {
195  bgr_padded = cv::Mat::zeros(frame_height, frame_width, bgr.type());
196  cv::Mat pic_roi = bgr_padded(cv::Rect(0, 0, bgr.cols, bgr.rows));
197  bgr.copyTo(pic_roi);
198  }
199 
200  // Convert image to Y'CbCr color space used by Theora
201  cv::Mat ycrcb;
202  cv::cvtColor(bgr_padded, ycrcb, cv::COLOR_BGR2YCrCb);
203 
204  // Split channels
205  cv::Mat ycrcb_planes[3];
206  cv::split(ycrcb, ycrcb_planes);
207 
208  // Use Y as-is but subsample chroma channels
209  cv::Mat y = ycrcb_planes[0], cr, cb;
210  cv::pyrDown(ycrcb_planes[1], cr);
211  cv::pyrDown(ycrcb_planes[2], cb);
212 
213  // Construct Theora image buffer
214  th_ycbcr_buffer ycbcr_buffer;
215  cvToTheoraPlane(y, ycbcr_buffer[0]);
216  cvToTheoraPlane(cb, ycbcr_buffer[1]);
217  cvToTheoraPlane(cr, ycbcr_buffer[2]);
218 
219  // Submit frame to the encoder
220  int rval = th_encode_ycbcr_in(encoding_context_.get(), ycbcr_buffer);
221  if (rval == TH_EFAULT) {
222  ROS_ERROR("[theora] EFAULT in submitting uncompressed frame to encoder");
223  return;
224  }
225  if (rval == TH_EINVAL) {
226  ROS_ERROR("[theora] EINVAL in submitting uncompressed frame to encoder");
227  return;
228  }
229 
230  // Retrieve and publish encoded video data packets
231  ogg_packet oggpacket;
232  theora_image_transport::Packet output;
233  while ((rval = th_encode_packetout(encoding_context_.get(), 0, &oggpacket)) > 0) {
234  oggPacketToMsg(message.header, oggpacket, output);
235  publish_fn(output);
236  }
237  if (rval == TH_EFAULT)
238  ROS_ERROR("[theora] EFAULT in retrieving encoded video data packets");
239 }
240 
241 void freeContext(th_enc_ctx* context)
242 {
243  if (context) th_encode_free(context);
244 }
245 
246 bool TheoraPublisher::ensureEncodingContext(const sensor_msgs::Image& image, const PublishFn& publish_fn) const
247 {
249  if (encoding_context_ && encoder_setup_.pic_width == image.width &&
250  encoder_setup_.pic_height == image.height)
251  return true;
252 
253  // Theora has a divisible-by-sixteen restriction for the encoded frame size, so
254  // scale the picture size up to the nearest multiple of 16 and calculate offsets.
255  encoder_setup_.frame_width = (image.width + 15) & ~0xF;
256  encoder_setup_.frame_height = (image.height + 15) & ~0xF;
257  encoder_setup_.pic_width = image.width;
258  encoder_setup_.pic_height = image.height;
259 
260  // Allocate encoding context. Smart pointer ensures that th_encode_free gets called.
261  encoding_context_.reset(th_encode_alloc(&encoder_setup_), freeContext);
262  if (!encoding_context_) {
263  ROS_ERROR("[theora] Failed to create encoding context");
264  return false;
265  }
266 
267  updateKeyframeFrequency();
268 
269  th_comment comment;
270  th_comment_init(&comment);
271  boost::shared_ptr<th_comment> clear_guard(&comment, th_comment_clear);
273  comment.vendor = strdup("Willow Garage theora_image_transport");
274 
275  // Construct the header and stream it in case anyone is already listening
277  stream_header_.clear();
278  ogg_packet oggpacket;
279  while (th_encode_flushheader(encoding_context_.get(), &comment, &oggpacket) > 0) {
280  stream_header_.push_back(theora_image_transport::Packet());
281  oggPacketToMsg(image.header, oggpacket, stream_header_.back());
282  publish_fn(stream_header_.back());
283  }
284  return true;
285 }
286 
287 void TheoraPublisher::oggPacketToMsg(const std_msgs::Header& header, const ogg_packet &oggpacket,
288  theora_image_transport::Packet &msg) const
289 {
290  msg.header = header;
291  msg.b_o_s = oggpacket.b_o_s;
292  msg.e_o_s = oggpacket.e_o_s;
293  msg.granulepos = oggpacket.granulepos;
294  msg.packetno = oggpacket.packetno;
295  msg.data.resize(oggpacket.bytes);
296  memcpy(&msg.data[0], oggpacket.packet, oggpacket.bytes);
297 }
298 
299 void TheoraPublisher::updateKeyframeFrequency() const
300 {
301  ogg_uint32_t desired_frequency = keyframe_frequency_;
302  if (th_encode_ctl(encoding_context_.get(), TH_ENCCTL_SET_KEYFRAME_FREQUENCY_FORCE,
303  &keyframe_frequency_, sizeof(ogg_uint32_t)))
304  ROS_ERROR("Failed to change keyframe frequency");
305  if (keyframe_frequency_ != desired_frequency)
306  ROS_WARN("Couldn't set keyframe frequency to %d, actually set to %d",
307  desired_frequency, keyframe_frequency_);
308 }
309 
310 } //namespace theora_image_transport
f
void publish(const boost::shared_ptr< M const > &message) const
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
#define ROS_WARN(...)
boost::function< void(const theora_image_transport::Packet &)> PublishFn
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
theora_image_transport::TheoraPublisherConfig Config
static void cvToTheoraPlane(cv::Mat &mat, th_img_plane &plane)
#define ROS_ERROR(...)
void freeContext(th_enc_ctx *context)


theora_image_transport
Author(s): Patrick Mihelich, Ethan Dreyfuss
autogenerated on Fri Sep 20 2019 03:32:16