image_enhancer.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Dirk Thomas, TU Darmstadt
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions
00007  * are met:
00008  *
00009  *   * Redistributions of source code must retain the above copyright
00010  *     notice, this list of conditions and the following disclaimer.
00011  *   * Redistributions in binary form must reproduce the above
00012  *     copyright notice, this list of conditions and the following
00013  *     disclaimer in the documentation and/or other materials provided
00014  *     with the distribution.
00015  *   * Neither the name of the TU Darmstadt nor the names of its
00016  *     contributors may be used to endorse or promote products derived
00017  *     from this software without specific prior written permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00020  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00021  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00022  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00023  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00024  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00025  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00026  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00027  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00028  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00029  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00030  * POSSIBILITY OF SUCH DAMAGE.
00031  */
00032 
00033 #include <rqt_image_enhancer/image_enhancer.h>
00034 
00035 #include <pluginlib/class_list_macros.h>
00036 #include <ros/master.h>
00037 #include <ros/console.h>
00038 #include <sensor_msgs/image_encodings.h>
00039 
00040 #include <cv_bridge/cv_bridge.h>
00041 #include <opencv2/imgproc/imgproc.hpp>
00042 
00043 #include <QMessageBox>
00044 #include <QPainter>
00045 
00046 #include <algorithm>
00047 
00048 namespace rqt_image_enhancer {
00049 
00050 ImageEnhancer::ImageEnhancer()
00051   : rqt_gui_cpp::Plugin()
00052   , widget_(0)
00053   , selected_(false)
00054 {
00055   setObjectName("ImageEnhancer");
00056 }
00057 
00058 void ImageEnhancer::initPlugin(qt_gui_cpp::PluginContext& context)
00059 {
00060   widget_ = new QWidget();
00061   ui_.setupUi(widget_);
00062 
00063   if (context.serialNumber() > 1)
00064   {
00065     widget_->setWindowTitle(widget_->windowTitle() + " (" + QString::number(context.serialNumber()) + ")");
00066   }
00067   context.addWidget(widget_);
00068 
00069   ui_.image_frame->installEventFilter(this);
00070 
00071   updateTopicList();
00072   ui_.topics_combo_box->setCurrentIndex(ui_.topics_combo_box->findText(""));
00073   connect(ui_.topics_combo_box, SIGNAL(currentIndexChanged(int)), this, SLOT(onTopicChanged(int)));
00074 
00075   ui_.refresh_topics_push_button->setIcon(QIcon::fromTheme("view-refresh"));
00076   connect(ui_.refresh_topics_push_button, SIGNAL(pressed()), this, SLOT(updateTopicList()));
00077   
00078   connect(ui_.dynamic_range_check_box, SIGNAL(toggled(bool)), this, SLOT(onDynamicRange(bool)));
00079 
00080   connect(ui_.image_frame, SIGNAL(rightMouseButtonClicked()), this, SLOT(onRemoveSelection()));
00081   connect(ui_.image_frame, SIGNAL(selectionInProgress(QPoint,QPoint)), this, SLOT(onSelectionInProgress(QPoint,QPoint)));
00082   connect(ui_.image_frame, SIGNAL(selectionFinished(QPoint,QPoint)), this, SLOT(onSelectionFinished(QPoint,QPoint)));
00083 }
00084 
00085 bool ImageEnhancer::eventFilter(QObject* watched, QEvent* event)
00086 {
00087   if (watched == ui_.image_frame && event->type() == QEvent::Paint)
00088   {
00089     QPainter painter(ui_.image_frame);
00090     if (!qimage_.isNull())
00091     {
00092       ui_.image_frame->resizeToFitAspectRatio();
00093       // TODO: check if full draw is really necessary
00094       //QPaintEvent* paint_event = dynamic_cast<QPaintEvent*>(event);
00095       //painter.drawImage(paint_event->rect(), qimage_);
00096       qimage_mutex_.lock();
00097       painter.drawImage(ui_.image_frame->contentsRect(), qimage_);
00098       qimage_mutex_.unlock();
00099     } else {
00100       // default image with gradient
00101       QLinearGradient gradient(0, 0, ui_.image_frame->frameRect().width(), ui_.image_frame->frameRect().height());
00102       gradient.setColorAt(0, Qt::white);
00103       gradient.setColorAt(1, Qt::black);
00104       painter.setBrush(gradient);
00105       painter.drawRect(0, 0, ui_.image_frame->frameRect().width() + 1, ui_.image_frame->frameRect().height() + 1);
00106     }
00107 
00108     if(selected_)
00109     {
00110         QRect rect(selection_top_left_, selection_size_);
00111         painter.setPen(Qt::red);
00112         painter.drawRect(rect);
00113     }
00114 
00115     ui_.image_frame->update();
00116 
00117     return false;
00118   }
00119 
00120   return QObject::eventFilter(watched, event);
00121 }
00122 
00123 void ImageEnhancer::shutdownPlugin()
00124 {
00125   subscriber_.shutdown();
00126 }
00127 
00128 void ImageEnhancer::saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const
00129 {
00130   QString topic = ui_.topics_combo_box->currentText();
00131   //qDebug("ImageEnhancer::saveSettings() topic '%s'", topic.toStdString().c_str());
00132   instance_settings.setValue("topic", topic);
00133   instance_settings.setValue("dynamic_range", ui_.dynamic_range_check_box->isChecked());
00134   instance_settings.setValue("max_range", ui_.max_range_double_spin_box->value());
00135 }
00136 
00137 void ImageEnhancer::restoreSettings(const qt_gui_cpp::Settings& plugin_settings, const qt_gui_cpp::Settings& instance_settings)
00138 {
00139   bool dynamic_range_checked = instance_settings.value("dynamic_range", false).toBool();
00140   ui_.dynamic_range_check_box->setChecked(dynamic_range_checked);
00141 
00142   double max_range = instance_settings.value("max_range", ui_.max_range_double_spin_box->value()).toDouble();
00143   ui_.max_range_double_spin_box->setValue(max_range);
00144 
00145   QString topic = instance_settings.value("topic", "").toString();
00146   //qDebug("ImageEnhancer::restoreSettings() topic '%s'", topic.toStdString().c_str());
00147   selectTopic(topic);
00148 }
00149 
00150 void ImageEnhancer::updateTopicList()
00151 {
00152   QSet<QString> message_types;
00153   message_types.insert("sensor_msgs/Image");
00154 
00155   // get declared transports
00156   QList<QString> transports;
00157   image_transport::ImageTransport it(getNodeHandle());
00158   std::vector<std::string> declared = it.getDeclaredTransports();
00159   for (std::vector<std::string>::const_iterator it = declared.begin(); it != declared.end(); it++)
00160   {
00161     //qDebug("ImageEnhancer::updateTopicList() declared transport '%s'", it->c_str());
00162     QString transport = it->c_str();
00163 
00164     // strip prefix from transport name
00165     QString prefix = "image_transport/";
00166     if (transport.startsWith(prefix))
00167     {
00168       transport = transport.mid(prefix.length());
00169     }
00170     transports.append(transport);
00171   }
00172 
00173   QString selected = ui_.topics_combo_box->currentText();
00174 
00175   // fill combo box
00176   QList<QString> topics = getTopicList(message_types, transports);
00177   topics.append("");
00178   qSort(topics);
00179   ui_.topics_combo_box->clear();
00180   for (QList<QString>::const_iterator it = topics.begin(); it != topics.end(); it++)
00181   {
00182     QString label(*it);
00183     label.replace(" ", "/");
00184     ui_.topics_combo_box->addItem(label, QVariant(*it));
00185   }
00186 
00187   // restore previous selection
00188   selectTopic(selected);
00189 }
00190 
00191 QList<QString> ImageEnhancer::getTopicList(const QSet<QString>& message_types, const QList<QString>& transports)
00192 {
00193   ros::master::V_TopicInfo topic_info;
00194   ros::master::getTopics(topic_info);
00195 
00196   QSet<QString> all_topics;
00197   for (ros::master::V_TopicInfo::const_iterator it = topic_info.begin(); it != topic_info.end(); it++)
00198   {
00199     all_topics.insert(it->name.c_str());
00200   }
00201 
00202   QList<QString> topics;
00203   for (ros::master::V_TopicInfo::const_iterator it = topic_info.begin(); it != topic_info.end(); it++)
00204   {
00205     if (message_types.contains(it->datatype.c_str()))
00206     {
00207       QString topic = it->name.c_str();
00208 
00209       // add raw topic
00210       topics.append(topic);
00211       //qDebug("ImageEnhancer::getTopicList() raw topic '%s'", topic.toStdString().c_str());
00212       
00213       // add transport specific sub-topics
00214       for (QList<QString>::const_iterator jt = transports.begin(); jt != transports.end(); jt++)
00215       {
00216         if (all_topics.contains(topic + "/" + *jt))
00217         {
00218           QString sub = topic + " " + *jt;
00219           topics.append(sub);
00220           //qDebug("ImageEnhancer::getTopicList() transport specific sub-topic '%s'", sub.toStdString().c_str());
00221         }
00222       }
00223     }
00224   }
00225   return topics;
00226 }
00227 
00228 void ImageEnhancer::selectTopic(const QString& topic)
00229 {
00230   int index = ui_.topics_combo_box->findText(topic);
00231   if (index == -1)
00232   {
00233     index = ui_.topics_combo_box->findText("");
00234   }
00235   ui_.topics_combo_box->setCurrentIndex(index);
00236 }
00237 
00238 void ImageEnhancer::onTopicChanged(int index)
00239 {
00240   subscriber_.shutdown();
00241 
00242   // reset image on topic change
00243   qimage_ = QImage();
00244   ui_.image_frame->update();
00245 
00246   QStringList parts = ui_.topics_combo_box->itemData(index).toString().split(" ");
00247   QString topic = parts.first();
00248   QString transport = parts.length() == 2 ? parts.last() : "raw";
00249 
00250   if (!topic.isEmpty())
00251   {
00252     image_transport::ImageTransport it(getNodeHandle());
00253     image_transport::TransportHints hints(transport.toStdString());
00254     try {
00255       subscriber_ = it.subscribe(topic.toStdString(), 1, &ImageEnhancer::callbackImage, this, hints);
00256       //qDebug("ImageEnhancer::onTopicChanged() to topic '%s' with transport '%s'", topic.toStdString().c_str(), subscriber_.getTransport().c_str());
00257     } catch (image_transport::TransportLoadException& e) {
00258       QMessageBox::warning(widget_, tr("Loading image transport plugin failed"), e.what());
00259     }
00260   }
00261 }
00262 
00263 void ImageEnhancer::onZoom1(bool checked)
00264 {
00265   if (checked)
00266   {
00267     if (qimage_.isNull())
00268     {
00269       return;
00270     }
00271     ui_.image_frame->setInnerFrameFixedSize(qimage_.size());
00272     widget_->resize(ui_.image_frame->size());
00273     widget_->setMinimumSize(widget_->sizeHint());
00274     widget_->setMaximumSize(widget_->sizeHint());
00275   } else {
00276     ui_.image_frame->setInnerFrameMinimumSize(QSize(80, 60));
00277     ui_.image_frame->setMaximumSize(QSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX));
00278     widget_->setMinimumSize(QSize(80, 60));
00279     widget_->setMaximumSize(QSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX));
00280   }
00281 }
00282 
00283 void ImageEnhancer::onSelectionInProgress(QPoint p1, QPoint p2)
00284 {
00285     enforceSelectionConstraints(p1);
00286     enforceSelectionConstraints(p2);
00287 
00288     int tl_x = std::min(p1.x(), p2.x());
00289     int tl_y = std::min(p1.y(), p2.y());
00290 
00291     selection_top_left_ = QPoint(tl_x,tl_y);
00292     selection_size_ = QSize(abs(p1.x() - p2.x()), abs(p1.y() - p2.y()));
00293 
00294     //ROS_DEBUG_STREAM << "p1: " << p1.x() << " " << p1.y() << " p2: " << p2.x() << " " << p2.y();
00295 
00296     selected_ = true;
00297 }
00298 
00299 void ImageEnhancer::onSelectionFinished(QPoint p1, QPoint p2)
00300 {
00301     enforceSelectionConstraints(p1);
00302     enforceSelectionConstraints(p2);
00303 
00304     int tl_x = p1.x() < p2.x() ? p1.x() : p2.x();
00305     int tl_y = p1.y() < p2.y() ? p1.y() : p2.y();
00306 
00307     selection_top_left_ = QPoint(tl_x,tl_y);
00308     selection_size_ = QSize(abs(p1.x() - p2.x()), abs(p1.y() - p2.y()));
00309 
00310 
00311 }
00312 
00313 void ImageEnhancer::onRemoveSelection()
00314 {
00315     selected_ = false;
00316 }
00317 
00318 void ImageEnhancer::enforceSelectionConstraints(QPoint & p)
00319 {
00320     int min_x = 0;
00321     int max_x = ui_.image_frame->width();
00322 
00323     int min_y = 0;
00324     int max_y = ui_.image_frame->height();
00325 
00326     p.setX(std::min(std::max(p.x(),min_x),max_x));
00327     p.setY(std::min(std::max(p.y(),min_y),max_y));
00328 }
00329 
00330 void ImageEnhancer::onDynamicRange(bool checked)
00331 {
00332   ui_.max_range_double_spin_box->setEnabled(!checked);
00333 }
00334 
00335 void ImageEnhancer::callbackImage(const sensor_msgs::Image::ConstPtr& msg)
00336 {
00337     if(!selected_)
00338     {
00339         try
00340         {
00341             // First let cv_bridge do its magic
00342             cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::RGB8);
00343             conversion_mat_ = cv_ptr->image;
00344         }
00345         catch (cv_bridge::Exception& e)
00346         {
00347             // If we're here, there is no conversion that makes sense, but let's try to imagine a few first
00348             cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg);
00349             if (msg->encoding == "CV_8UC3")
00350             {
00351                 // assuming it is rgb
00352                 conversion_mat_ = cv_ptr->image;
00353             } else if (msg->encoding == "8UC1") {
00354                 // convert gray to rgb
00355                 cv::cvtColor(cv_ptr->image, conversion_mat_, CV_GRAY2RGB);
00356             } else if (msg->encoding == "16UC1" || msg->encoding == "32FC1") {
00357                 // scale / quantify
00358                 double min = 0;
00359                 double max = ui_.max_range_double_spin_box->value();
00360                 if (msg->encoding == "16UC1") max *= 1000;
00361                 if (ui_.dynamic_range_check_box->isChecked())
00362                 {
00363                     // dynamically adjust range based on min/max in image
00364                     cv::minMaxLoc(cv_ptr->image, &min, &max);
00365                     if (min == max) {
00366                         // completely homogeneous images are displayed in gray
00367                         min = 0;
00368                         max = 2;
00369                     }
00370                 }
00371                 cv::Mat img_scaled_8u;
00372                 cv::Mat(cv_ptr->image-min).convertTo(img_scaled_8u, CV_8UC1, 255. / (max - min));
00373                 cv::cvtColor(img_scaled_8u, conversion_mat_, CV_GRAY2RGB);
00374             } else {
00375                 qWarning("ImageEnhancer.callback_image() could not convert image from '%s' to 'rgb8' (%s)", msg->encoding.c_str(), e.what());
00376                 qimage_ = QImage();
00377                 return;
00378             }
00379         }
00380 
00381         // copy temporary image as it uses the conversion_mat_ for storage which is asynchronously overwritten in the next callback invocation
00382         QImage image(conversion_mat_.data, conversion_mat_.cols, conversion_mat_.rows, QImage::Format_RGB888);
00383         qimage_mutex_.lock();
00384         qimage_ = image.copy();
00385         qimage_mutex_.unlock();
00386 
00387         ui_.image_frame->setAspectRatio(qimage_.width(), qimage_.height());
00388         //onZoom1(false);
00389 
00390         ui_.image_frame->setInnerFrameMinimumSize(QSize(80, 60));
00391         ui_.image_frame->setMaximumSize(QSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX));
00392         widget_->setMinimumSize(QSize(80, 60));
00393         widget_->setMaximumSize(QSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX));
00394     }
00395 }
00396 
00397 }
00398 
00399 PLUGINLIB_EXPORT_CLASS(rqt_image_enhancer::ImageEnhancer, rqt_gui_cpp::Plugin)


hector_rqt_plugins
Author(s): Dirk Thomas, Thorsten Graber, Gregor Gebhardt
autogenerated on Mon Oct 6 2014 00:35:46