VisualizationManager.cpp
Go to the documentation of this file.
00001 
00033 #include "ros/ros.h"
00034 #include "sensor_msgs/Image.h"
00035 #include "image_transport/image_transport.h"
00036 #include "cv_bridge/CvBridge.h"
00037 #include <opencv/cv.h>
00038 #include "VisualizationManager.h"
00039 
00040 #include "DUtils.h"
00041 
00042 // --------------------------------------------------------------------------
00043 
00044 VisualizationManager::VisualizationManager(ros::Publisher &pub):
00045   m_publisher(pub)
00046 {
00047 }
00048 
00049 // --------------------------------------------------------------------------
00050 
00051 VisualizationManager::~VisualizationManager()
00052 {
00053 }
00054 
00055 // --------------------------------------------------------------------------
00056 
00057 void VisualizationManager::show(const cv::Mat &image, 
00058   const std::string &text, bool hold)
00059 {
00060   const float HOLDING_TIME = 2.f; // seconds
00061   const cv::Scalar HOLDING_COLOR(0, 255, 0);
00062   const cv::Scalar NORMAL_COLOR(255, 255, 255);
00063 
00064   // check if we can update the current image
00065   bool update_image = false;
00066   
00067   DUtils::Timestamp current_time(DUtils::Timestamp::CURRENT_TIME);
00068   
00069   if(hold)
00070   {
00071     // the given image has a higher priority
00072     m_holding = true;
00073     m_unfreeze_time = current_time + HOLDING_TIME;
00074     update_image = true;
00075   }
00076   else if(m_holding)
00077   {
00078     // check current time
00079     if(current_time >= m_unfreeze_time)
00080     {
00081       m_holding = false;
00082       update_image = true;
00083     }
00084   }
00085   else
00086     update_image = true;
00087 
00088 
00089   if(update_image)
00090   {
00091     cv::Mat img;
00092     
00093     if(image.channels() == 3)
00094       img = image.clone();
00095     else
00096       cvtColor(image, img, CV_GRAY2BGR);
00097     
00098     if(!text.empty()) putText(img, text, 
00099       (hold ? HOLDING_COLOR : NORMAL_COLOR));
00100     
00101     IplImage iplimg(img);
00102     
00103           try{
00104                   sensor_msgs::Image::Ptr ros_img_ptr = 
00105                     m_bridge.cvToImgMsg(&iplimg, "passthrough");
00106                   sensor_msgs::Image msg = sensor_msgs::Image(*ros_img_ptr);
00107       
00108       m_publisher.publish(msg);
00109       
00110           }catch (sensor_msgs::CvBridgeException error){
00111                   ROS_WARN("Error sending image for visualization");
00112           }
00113         }
00114 
00115 }
00116 
00117 // --------------------------------------------------------------------------
00118 
00119 void VisualizationManager::putText(cv::Mat &image, const std::string &text,
00120   const cv::Scalar &color) const
00121 { 
00122   const int face = cv::FONT_HERSHEY_COMPLEX;
00123   const double scale = 2.f;
00124   const int thickness = 2;
00125   const int linetype = 8;
00126   const bool bottomleftorigin = false;
00127   cv::Point origin;
00128   int baseline = 0;
00129   
00130   cv::Size size = cv::getTextSize(text, face, scale, thickness, &baseline);
00131   
00132   origin.y = image.rows - size.height;
00133   origin.x = (image.cols - size.width)/2;
00134   
00135   cv::putText(image, text, origin, face, scale, color, thickness, linetype,
00136     bottomleftorigin);
00137 }
00138 
00139 // --------------------------------------------------------------------------
00140 


re_vision
Author(s): Dorian Galvez-Lopez
autogenerated on Sun Jan 5 2014 11:33:43