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;
00061 const cv::Scalar HOLDING_COLOR(0, 255, 0);
00062 const cv::Scalar NORMAL_COLOR(255, 255, 255);
00063
00064
00065 bool update_image = false;
00066
00067 DUtils::Timestamp current_time(DUtils::Timestamp::CURRENT_TIME);
00068
00069 if(hold)
00070 {
00071
00072 m_holding = true;
00073 m_unfreeze_time = current_time + HOLDING_TIME;
00074 update_image = true;
00075 }
00076 else if(m_holding)
00077 {
00078
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