VisualizationManager.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "sensor_msgs/Image.h"
#include "image_transport/image_transport.h"
#include "cv_bridge/CvBridge.h"
#include <opencv/cv.h>
#include "VisualizationManager.h"

#include "DUtils.h"

// --------------------------------------------------------------------------

VisualizationManager::VisualizationManager(ros::Publisher &pub):
  m_publisher(pub)
{
}

// --------------------------------------------------------------------------

VisualizationManager::~VisualizationManager()
{
}

// --------------------------------------------------------------------------

void VisualizationManager::show(const cv::Mat &image, 
  const std::string &text, bool hold)
{
  const float HOLDING_TIME = 2.f; // seconds
  const cv::Scalar HOLDING_COLOR(0, 255, 0);
  const cv::Scalar NORMAL_COLOR(255, 255, 255);

  // check if we can update the current image
  bool update_image = false;
  
  DUtils::Timestamp current_time(DUtils::Timestamp::CURRENT_TIME);
  
  if(hold)
  {
    // the given image has a higher priority
    m_holding = true;
    m_unfreeze_time = current_time + HOLDING_TIME;
    update_image = true;
  }
  else if(m_holding)
  {
    // check current time
    if(current_time >= m_unfreeze_time)
    {
      m_holding = false;
      update_image = true;
    }
  }
  else
    update_image = true;


  if(update_image)
  {
    cv::Mat img;
    
    if(image.channels() == 3)
      img = image.clone();
    else
      cvtColor(image, img, CV_GRAY2BGR);
    
    if(!text.empty()) putText(img, text, 
      (hold ? HOLDING_COLOR : NORMAL_COLOR));
    
    IplImage iplimg(img);
    
          try{
                  sensor_msgs::Image::Ptr ros_img_ptr = 
                    m_bridge.cvToImgMsg(&iplimg, "passthrough");
                  sensor_msgs::Image msg = sensor_msgs::Image(*ros_img_ptr);
      
      m_publisher.publish(msg);
      
          }catch (sensor_msgs::CvBridgeException error){
                  ROS_WARN("Error sending image for visualization");
          }
        }

}

// --------------------------------------------------------------------------

void VisualizationManager::putText(cv::Mat &image, const std::string &text,
  const cv::Scalar &color) const
{ 
  const int face = cv::FONT_HERSHEY_COMPLEX;
  const double scale = 2.f;
  const int thickness = 2;
  const int linetype = 8;
  const bool bottomleftorigin = false;
  cv::Point origin;
  int baseline = 0;
  
  cv::Size size = cv::getTextSize(text, face, scale, thickness, &baseline);
  
  origin.y = image.rows - size.height;
  origin.x = (image.cols - size.width)/2;
  
  cv::putText(image, text, origin, face, scale, color, thickness, linetype,
    bottomleftorigin);
}

// --------------------------------------------------------------------------