format_controllers.hpp

Go to the documentation of this file.

#ifndef USB_CAM_CONTROLLERS_FORMAT_CONTROLLERS
#define USB_CAM_CONTROLLERS_FORMAT_CONTROLLERS
 
#include <string>
 
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <image_transport/publisher.h>
#include <ros/console.h>
#include <ros/duration.h>
#include <ros/node_handle.h>
#include <ros/time.h>
#include <sensor_msgs/image_encodings.h>
#include <usb_cam_controllers/simple_packet_controller.hpp>
#include <usb_cam_hardware_interface/packet_interface.hpp>
 
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
 
namespace usb_cam_controllers {
 
template < cv::ColorConversionCodes ConversionCode, const std::string *DstEncoding >
class FormatController : public SimplePacketController {
public:
  FormatController() {}
 
  virtual ~FormatController() {}
 
protected:
  virtual bool initImpl(usb_cam_hardware_interface::PacketInterface *hw, ros::NodeHandle &root_nh,
                        ros::NodeHandle &controller_nh) {
    width_ = controller_nh.param("image_width", 640);
    height_ = controller_nh.param("image_height", 480);
 
    // init publisher for decoded images
    publisher_ = image_transport::ImageTransport(controller_nh).advertise("image", 1);
 
    return true;
  }
 
  virtual void startingImpl(const ros::Time &time) {
    // nothing to do
  }
 
  virtual void updateImpl(const ros::Time &time, const ros::Duration &period) {
    // allocate output message
    cv_bridge::CvImage out;
    out.header.stamp = packet_iface_.getStamp();
    out.encoding = *DstEncoding;
 
    // convert pixel formats
    try {
      cv::cvtColor(cv::Mat(height_, width_, CV_8UC(packet_iface_.getLength() / (height_ * width_)),
                           const_cast< uint8_t * >(packet_iface_.getStartAs< uint8_t >())),
                   out.image, ConversionCode);
    } catch (const cv::Exception &ex) {
      ROS_ERROR_STREAM(ex.what());
      return;
    }
 
    publisher_.publish(out.toImageMsg());
  }
 
  virtual void stoppingImpl(const ros::Time &time) {
    // nothing to do
  }
 
private:
  int height_, width_;
 
  image_transport::Publisher publisher_;
};
 
typedef FormatController< cv::COLOR_RGB2BGR, &sensor_msgs::image_encodings::BGR8 >
    RGB24Controller;
typedef FormatController< cv::COLOR_YUV2BGR_UYVY, &sensor_msgs::image_encodings::BGR8 >
    UYVYController;
typedef FormatController< cv::COLOR_YUV2BGR_YUYV, &sensor_msgs::image_encodings::BGR8 >
    YUYVController;
 
} // namespace usb_cam_controllers
 
#endif