37 #include <std_msgs/Header.h> 42 #include <opencv2/imgproc/imgproc.hpp> 48 TheoraPublisher::TheoraPublisher()
51 th_info_init(&encoder_setup_);
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;
57 encoder_setup_.aspect_numerator = 1;
58 encoder_setup_.aspect_denominator = 1;
59 encoder_setup_.fps_numerator = 1;
60 encoder_setup_.fps_denominator = 1;
61 encoder_setup_.keyframe_granule_shift = 6;
63 encoder_setup_.target_bitrate = -1;
64 encoder_setup_.quality = -1;
67 TheoraPublisher::~TheoraPublisher()
69 th_info_clear(&encoder_setup_);
72 void TheoraPublisher::advertiseImpl(
ros::NodeHandle &nh,
const std::string &base_topic, uint32_t queue_size,
83 Base::advertiseImpl(nh, base_topic, queue_size, user_connect_cb, user_disconnect_cb, tracked_object, latch);
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);
91 void TheoraPublisher::configCb(
Config& config, uint32_t level)
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;
103 if (encoding_context_) {
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));
110 ROS_ERROR(
"Failed to update bitrate dynamically");
113 err |= update_bitrate;
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));
121 if (err && err != TH_EINVAL)
122 ROS_ERROR(
"Failed to update quality dynamically");
125 err |= update_quality;
130 encoding_context_.reset();
134 updateKeyframeFrequency();
135 config.keyframe_frequency = keyframe_frequency_;
143 for (
unsigned int i = 0; i < stream_header_.size(); i++) {
144 pub.
publish(stream_header_[i]);
150 plane.width = mat.cols;
151 plane.height = mat.rows;
152 plane.stride = mat.step;
153 plane.data = mat.data;
156 void TheoraPublisher::publish(
const sensor_msgs::Image& message,
const PublishFn& publish_fn)
const 158 if (!ensureEncodingContext(message, publish_fn))
173 ROS_ERROR(
"cv_bridge exception: '%s'", e.what());
176 catch (cv::Exception& e)
178 ROS_ERROR(
"OpenCV exception: '%s'", e.what());
182 if (cv_image_ptr == 0) {
183 ROS_ERROR(
"Unable to convert from '%s' to 'bgr8'", message.encoding.c_str());
187 const cv::Mat bgr = cv_image_ptr->image;
190 int frame_width = encoder_setup_.frame_width, frame_height = encoder_setup_.frame_height;
191 if (frame_width == bgr.cols && frame_height == bgr.rows) {
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));
202 cv::cvtColor(bgr_padded, ycrcb, cv::COLOR_BGR2YCrCb);
205 cv::Mat ycrcb_planes[3];
206 cv::split(ycrcb, ycrcb_planes);
209 cv::Mat y = ycrcb_planes[0], cr, cb;
210 cv::pyrDown(ycrcb_planes[1], cr);
211 cv::pyrDown(ycrcb_planes[2], cb);
214 th_ycbcr_buffer ycbcr_buffer;
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");
225 if (rval == TH_EINVAL) {
226 ROS_ERROR(
"[theora] EINVAL in submitting uncompressed frame to encoder");
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);
237 if (rval == TH_EFAULT)
238 ROS_ERROR(
"[theora] EFAULT in retrieving encoded video data packets");
243 if (context) th_encode_free(context);
246 bool TheoraPublisher::ensureEncodingContext(
const sensor_msgs::Image& image,
const PublishFn& publish_fn)
const 249 if (encoding_context_ && encoder_setup_.pic_width == image.width &&
250 encoder_setup_.pic_height == image.height)
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;
261 encoding_context_.reset(th_encode_alloc(&encoder_setup_),
freeContext);
262 if (!encoding_context_) {
263 ROS_ERROR(
"[theora] Failed to create encoding context");
267 updateKeyframeFrequency();
270 th_comment_init(&comment);
273 comment.vendor = strdup(
"Willow Garage theora_image_transport");
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());
287 void TheoraPublisher::oggPacketToMsg(
const std_msgs::Header& header,
const ogg_packet &oggpacket,
288 theora_image_transport::Packet &msg)
const 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);
299 void TheoraPublisher::updateKeyframeFrequency()
const 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_);
void publish(const boost::shared_ptr< M const > &message) const
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
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)
void freeContext(th_enc_ctx *context)