compressed_image_transport: compressed_image_transport: compressed

00001 #include "compressed_image_transport/compressed_publisher.h"
00002 #include <sensor_msgs/image_encodings.h>
00003 #include <cv_bridge/CvBridge.h>
00004 #include <opencv/highgui.h>
00005 #include <boost/make_shared.hpp>
00006 
00007 namespace compressed_image_transport {
00008 
00009 void CompressedPublisher::advertiseImpl(ros::NodeHandle &nh, const std::string &base_topic, uint32_t queue_size,
00010                                         const image_transport::SubscriberStatusCallback  &user_connect_cb,
00011                                         const image_transport::SubscriberStatusCallback  &user_disconnect_cb,
00012                                         const ros::VoidPtr &tracked_object, bool latch)
00013 {
00014   typedef image_transport::SimplePublisherPlugin<sensor_msgs::CompressedImage> Base;
00015   Base::advertiseImpl(nh, base_topic, queue_size, user_connect_cb, user_disconnect_cb, tracked_object, latch);
00016 
00017   // Set up reconfigure server for this topic
00018   reconfigure_server_ = boost::make_shared<ReconfigureServer>(this->nh());
00019   ReconfigureServer::CallbackType f = boost::bind(&CompressedPublisher::configCb, this, _1, _2);
00020   reconfigure_server_->setCallback(f);
00021 }
00022 
00023 void CompressedPublisher::configCb(Config& config, uint32_t level)
00024 {
00025   config_ = config;
00026 }
00027 
00028 void CompressedPublisher::publish(const sensor_msgs::Image& message,
00029                                   const PublishFn& publish_fn) const
00030 {
00031   // View/convert as mono or RGB
00032   sensor_msgs::CvBridge bridge;
00036   if (bridge.encoding_as_fmt(message.encoding) == "GRAY") {
00037     if (!bridge.fromImage(message, sensor_msgs::image_encodings::MONO8)) {
00038       ROS_ERROR("Could not convert image from %s to mono8", message.encoding.c_str());
00039       return;
00040     }
00041   }
00042   else if (!bridge.fromImage(message, sensor_msgs::image_encodings::BGR8)) {
00043     ROS_ERROR("Could not convert image from %s to bgr8", message.encoding.c_str());
00044     return;
00045   }
00046 
00047   // Fill compression settings
00048   int params[3] = {0};
00049   if (config_.format == "jpeg") {
00050     params[0] = CV_IMWRITE_JPEG_QUALITY;
00051     params[1] = config_.jpeg_quality;
00052   }
00053   else if (config_.format == "png") {
00054     params[0] = CV_IMWRITE_PNG_COMPRESSION;
00055     params[1] = config_.png_level;
00056   }
00057   else {
00058     ROS_ERROR("Unknown compression type '%s', valid options are 'jpeg' and 'png'",
00059               config_.format.c_str());
00060     return;
00061   }
00062   std::string extension = '.' + config_.format;
00063 
00064   // Compress image
00065   const IplImage* image = bridge.toIpl();
00067   CvMat* buf = cvEncodeImage(extension.c_str(), image, params);
00068 
00069   // Set up message and publish
00070   sensor_msgs::CompressedImage compressed;
00071   compressed.header = message.header;
00072   compressed.format = config_.format;
00073   compressed.data.resize(buf->width);
00074   memcpy(&compressed.data[0], buf->data.ptr, buf->width);
00075   cvReleaseMat(&buf);
00076   
00077   publish_fn(compressed);
00078 }
00079 
00080 } //namespace compressed_image_transport
compressed_publisher.cpp
