image_cropper.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_cropping/image_cropper.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/opencv.hpp>
00042 
00043 #include <QMessageBox>
00044 #include <QPainter>
00045 
00046 #include <algorithm>
00047 
00048 namespace rqt_image_cropping {
00049 
00050 ImageCropper::ImageCropper()
00051   : rqt_gui_cpp::Plugin()
00052   , widget_(0)
00053   , selected_(false)
00054 {
00055   setObjectName("ImageCropper");
00056 }
00057 
00058 void ImageCropper::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 
00073   ui_.topics_combo_box->setCurrentIndex(ui_.topics_combo_box->findText(""));
00074   connect(ui_.topics_combo_box, SIGNAL(currentIndexChanged(int)), this, SLOT(onInTopicChanged(int)));
00075 
00076   ui_.out_topic_line_edit->setText("/cropped");
00077   connect(ui_.out_topic_line_edit, SIGNAL(editingFinished()), this, SLOT(onOutTopicChanged()));
00078   onOutTopicChanged();
00079 
00080   ui_.refresh_topics_push_button->setIcon(QIcon::fromTheme("view-refresh"));
00081   connect(ui_.refresh_topics_push_button, SIGNAL(pressed()), this, SLOT(updateTopicList()));
00082   
00083   connect(ui_.dynamic_range_check_box, SIGNAL(toggled(bool)), this, SLOT(onDynamicRange(bool)));
00084 
00085   connect(ui_.image_frame, SIGNAL(rightMouseButtonClicked()), this, SLOT(onRemoveSelection()));
00086   connect(ui_.image_frame, SIGNAL(selectionInProgress(QPoint,QPoint)), this, SLOT(onSelectionInProgress(QPoint,QPoint)));
00087   connect(ui_.image_frame, SIGNAL(selectionFinished(QPoint,QPoint)), this, SLOT(onSelectionFinished(QPoint,QPoint)));
00088 }
00089 
00090 bool ImageCropper::eventFilter(QObject* watched, QEvent* event)
00091 {
00092   if (watched == ui_.image_frame && event->type() == QEvent::Paint)
00093   {
00094     QPainter painter(ui_.image_frame);
00095     if (!qimage_.isNull())
00096     {
00097       ui_.image_frame->resizeToFitAspectRatio();
00098       // TODO: check if full draw is really necessary
00099       //QPaintEvent* paint_event = dynamic_cast<QPaintEvent*>(event);
00100       //painter.drawImage(paint_event->rect(), qimage_);
00101       qimage_mutex_.lock();
00102       painter.drawImage(ui_.image_frame->contentsRect(), qimage_);
00103       qimage_mutex_.unlock();
00104     } else {
00105       // default image with gradient
00106       QLinearGradient gradient(0, 0, ui_.image_frame->frameRect().width(), ui_.image_frame->frameRect().height());
00107       gradient.setColorAt(0, Qt::white);
00108       gradient.setColorAt(1, Qt::black);
00109       painter.setBrush(gradient);
00110       painter.drawRect(0, 0, ui_.image_frame->frameRect().width() + 1, ui_.image_frame->frameRect().height() + 1);
00111     }
00112 
00113     if(selected_)
00114     {
00115         selection_ = QRectF(selection_top_left_rect_, selection_size_rect_);
00116 
00117         painter.setPen(Qt::red);
00118         painter.drawRect(selection_);
00119     }
00120 
00121     ui_.image_frame->update();
00122 
00123     return false;
00124   }
00125 
00126   return QObject::eventFilter(watched, event);
00127 }
00128 
00129 void ImageCropper::shutdownPlugin()
00130 {
00131   subscriber_.shutdown();
00132   publisher_.shutdown();
00133 }
00134 
00135 void ImageCropper::saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const
00136 {
00137   QString topic = ui_.topics_combo_box->currentText();
00138   //qDebug("ImageCropper::saveSettings() topic '%s'", topic.toStdString().c_str());
00139   instance_settings.setValue("topic", topic);
00140   instance_settings.setValue("dynamic_range", ui_.dynamic_range_check_box->isChecked());
00141   instance_settings.setValue("max_range", ui_.max_range_double_spin_box->value());
00142 }
00143 
00144 void ImageCropper::restoreSettings(const qt_gui_cpp::Settings& plugin_settings, const qt_gui_cpp::Settings& instance_settings)
00145 {
00146   bool dynamic_range_checked = instance_settings.value("dynamic_range", false).toBool();
00147   ui_.dynamic_range_check_box->setChecked(dynamic_range_checked);
00148 
00149   double max_range = instance_settings.value("max_range", ui_.max_range_double_spin_box->value()).toDouble();
00150   ui_.max_range_double_spin_box->setValue(max_range);
00151 
00152   QString topic = instance_settings.value("topic", "").toString();
00153   //qDebug("ImageCropper::restoreSettings() topic '%s'", topic.toStdString().c_str());
00154   selectTopic(topic);
00155 }
00156 
00157 void ImageCropper::updateTopicList()
00158 {
00159   QSet<QString> message_types;
00160   message_types.insert("sensor_msgs/Image");
00161 
00162   // get declared transports
00163   QList<QString> transports;
00164   image_transport::ImageTransport it(getNodeHandle());
00165   std::vector<std::string> declared = it.getDeclaredTransports();
00166   for (std::vector<std::string>::const_iterator it = declared.begin(); it != declared.end(); it++)
00167   {
00168     //qDebug("ImageCropper::updateTopicList() declared transport '%s'", it->c_str());
00169     QString transport = it->c_str();
00170 
00171     // strip prefix from transport name
00172     QString prefix = "image_transport/";
00173     if (transport.startsWith(prefix))
00174     {
00175       transport = transport.mid(prefix.length());
00176     }
00177     transports.append(transport);
00178   }
00179 
00180   QString selected = ui_.topics_combo_box->currentText();
00181 
00182   // fill combo box
00183   QList<QString> topics = getTopicList(message_types, transports);
00184   topics.append("");
00185   qSort(topics);
00186   ui_.topics_combo_box->clear();
00187   for (QList<QString>::const_iterator it = topics.begin(); it != topics.end(); it++)
00188   {
00189     QString label(*it);
00190     label.replace(" ", "/");
00191     ui_.topics_combo_box->addItem(label, QVariant(*it));
00192   }
00193 
00194   // restore previous selection
00195   selectTopic(selected);
00196 }
00197 
00198 QList<QString> ImageCropper::getTopicList(const QSet<QString>& message_types, const QList<QString>& transports)
00199 {
00200   ros::master::V_TopicInfo topic_info;
00201   ros::master::getTopics(topic_info);
00202 
00203   QSet<QString> all_topics;
00204   for (ros::master::V_TopicInfo::const_iterator it = topic_info.begin(); it != topic_info.end(); it++)
00205   {
00206     all_topics.insert(it->name.c_str());
00207   }
00208 
00209   QList<QString> topics;
00210   for (ros::master::V_TopicInfo::const_iterator it = topic_info.begin(); it != topic_info.end(); it++)
00211   {
00212     if (message_types.contains(it->datatype.c_str()))
00213     {
00214       QString topic = it->name.c_str();
00215 
00216       // add raw topic
00217       topics.append(topic);
00218       //qDebug("ImageCropper::getTopicList() raw topic '%s'", topic.toStdString().c_str());
00219       
00220       // add transport specific sub-topics
00221       for (QList<QString>::const_iterator jt = transports.begin(); jt != transports.end(); jt++)
00222       {
00223         if (all_topics.contains(topic + "/" + *jt))
00224         {
00225           QString sub = topic + " " + *jt;
00226           topics.append(sub);
00227           //qDebug("ImageCropper::getTopicList() transport specific sub-topic '%s'", sub.toStdString().c_str());
00228         }
00229       }
00230     }
00231   }
00232   return topics;
00233 }
00234 
00235 void ImageCropper::selectTopic(const QString& topic)
00236 {
00237   int index = ui_.topics_combo_box->findText(topic);
00238   if (index == -1)
00239   {
00240     index = ui_.topics_combo_box->findText("");
00241   }
00242   ui_.topics_combo_box->setCurrentIndex(index);
00243 }
00244 
00245 void ImageCropper::onInTopicChanged(int index)
00246 {
00247   subscriber_.shutdown();
00248 
00249   // reset image on topic change
00250   qimage_ = QImage();
00251   ui_.image_frame->update();
00252 
00253   QStringList parts = ui_.topics_combo_box->itemData(index).toString().split(" ");
00254   QString topic = parts.first();
00255   QString transport = parts.length() == 2 ? parts.last() : "raw";
00256 
00257   if (!topic.isEmpty())
00258   {
00259     image_transport::ImageTransport it(getNodeHandle());
00260     image_transport::TransportHints hints(transport.toStdString());
00261     try {
00262       subscriber_ = it.subscribeCamera(topic.toStdString(), 1, &ImageCropper::callbackImage, this, hints);
00263       //qDebug("ImageCropper::onInTopicChanged() to topic '%s' with transport '%s'", topic.toStdString().c_str(), subscriber_.getTransport().c_str());
00264     } catch (image_transport::TransportLoadException& e) {
00265       QMessageBox::warning(widget_, tr("Loading image transport plugin failed"), e.what());
00266     }
00267   }
00268 
00269   selected_ = false;
00270 }
00271 
00272 void ImageCropper::onOutTopicChanged()
00273 {
00274     publisher_.shutdown();
00275 
00276     QString topic = ui_.out_topic_line_edit->text();
00277 
00278     if(!topic.isEmpty())
00279     {
00280         image_transport::ImageTransport it(getNodeHandle());
00281 
00282         try {
00283             publisher_ = it.advertiseCamera(topic.toStdString(), 1, true);
00284         } catch (image_transport::TransportLoadException& e) {
00285             QMessageBox::warning(widget_, tr("Loading image transport plugin failed"), e.what());
00286         }
00287     }
00288 
00289     selected_ = false;
00290 }
00291 
00292 void ImageCropper::onSelectionInProgress(QPoint p1, QPoint p2)
00293 {
00294     enforceSelectionConstraints(p1);
00295     enforceSelectionConstraints(p2);
00296 
00297     int tl_x = std::min(p1.x(), p2.x());
00298     int tl_y = std::min(p1.y(), p2.y());
00299 
00300     selection_top_left_rect_ = QPointF(tl_x,tl_y);
00301     selection_size_rect_ = QSizeF(abs(p1.x() - p2.x()), abs(p1.y() - p2.y()));
00302 
00303     //ROS_DEBUG_STREAM << "p1: " << p1.x() << " " << p1.y() << " p2: " << p2.x() << " " << p2.y();
00304 
00305     selected_ = true;
00306 }
00307 
00308 void ImageCropper::onSelectionFinished(QPoint p1, QPoint p2)
00309 {
00310     enforceSelectionConstraints(p1);
00311     enforceSelectionConstraints(p2);
00312 
00313     int tl_x = p1.x() < p2.x() ? p1.x() : p2.x();
00314     int tl_y = p1.y() < p2.y() ? p1.y() : p2.y();
00315 
00316     selection_top_left_rect_ = QPointF(tl_x,tl_y);
00317     selection_size_rect_ = QSizeF(abs(p1.x() - p2.x()), abs(p1.y() - p2.y()));
00318 
00319     selection_top_left_ = QPointF(tl_x,tl_y);
00320     selection_size_ = QSizeF(abs(p1.x() - p2.x()), abs(p1.y() - p2.y()));
00321 
00322     selection_top_left_ *= (double)qimage_.width() / (double)ui_.image_frame->contentsRect().width();// width();
00323     selection_size_ *= (double)qimage_.width() / (double)ui_.image_frame->width();
00324 
00325     // crop image from cv image
00326     cv::Mat roi = cv::Mat(conversion_mat_, cv::Rect(selection_top_left_.x(), selection_top_left_.y(), selection_size_.width(), selection_size_.height()));
00327 
00328 //    std::cout << "sle height: " << selection_size_.height() << " width: " << selection_size_.width() << std::endl;
00329 //    std::cout << "roi rows: " << roi.rows << " cols: " << roi.cols << std::endl;
00330 
00331 //    cv_bridge::CvImage crop;
00332 //    crop.header = sens_msg_image_->header;
00333 //    crop.encoding = sensor_msgs::image_encodings::RGB8;
00334 //    crop.image = roi;
00335 
00336     // adapt camera info
00337     // Create updated CameraInfo message
00338     sensor_msgs::CameraInfoPtr out_info = boost::make_shared<sensor_msgs::CameraInfo>(*camera_info_);
00339     int binning_x = std::max((int)camera_info_->binning_x, 1);
00340     int binning_y = std::max((int)camera_info_->binning_y, 1);
00341     out_info->binning_x = binning_x * 1;
00342     out_info->binning_y = binning_y * 1;
00343     out_info->roi.x_offset += selection_top_left_.x() * binning_x;
00344     out_info->roi.y_offset += selection_top_left_.y() * binning_y;
00345     out_info->roi.height = selection_size_.height() * binning_y;
00346     out_info->roi.width = selection_size_.width() * binning_x;
00347 
00348 
00349     if(publisher_.getNumSubscribers())
00350     {
00351         publisher_.publish(sens_msg_image_,out_info);
00352     }
00353 }
00354 
00355 void ImageCropper::onRemoveSelection()
00356 {
00357     selected_region_ = QImage();
00358     selected_ = false;
00359 }
00360 
00361 void ImageCropper::enforceSelectionConstraints(QPoint & p)
00362 {
00363     int min_x = 1;
00364     int max_x = ui_.image_frame->width() - 2 * ui_.image_frame->frameWidth();
00365 
00366     int min_y = 1;
00367     int max_y = ui_.image_frame->height() - 2 * ui_.image_frame->frameWidth();
00368 
00369     p.setX(std::min(std::max(p.x(),min_x),max_x));
00370     p.setY(std::min(std::max(p.y(),min_y),max_y));
00371 }
00372 
00373 void ImageCropper::onDynamicRange(bool checked)
00374 {
00375   ui_.max_range_double_spin_box->setEnabled(!checked);
00376 }
00377 
00378 void ImageCropper::callbackImage(const sensor_msgs::Image::ConstPtr& img, const sensor_msgs::CameraInfoConstPtr& ci)
00379 {
00380     if(!selected_)
00381     {
00382 
00383         sens_msg_image_ = img;
00384         camera_info_ = ci;
00385 
00386         try
00387         {
00388             // First let cv_bridge do its magic
00389             cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(img, sensor_msgs::image_encodings::RGB8);
00390             conversion_mat_ = cv_ptr->image;
00391         }
00392         catch (cv_bridge::Exception& e)
00393         {
00394             // If we're here, there is no conversion that makes sense, but let's try to imagine a few first
00395             cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(img);
00396             if (img->encoding == "CV_8UC3")
00397             {
00398                 // assuming it is rgb
00399                 conversion_mat_ = cv_ptr->image;
00400             } else if (img->encoding == "8UC1") {
00401                 // convert gray to rgb
00402                 cv::cvtColor(cv_ptr->image, conversion_mat_, CV_GRAY2RGB);
00403             } else if (img->encoding == "16UC1" || img->encoding == "32FC1") {
00404                 // scale / quantify
00405                 image_min_value_ = 0;
00406                 image_max_value_ = ui_.max_range_double_spin_box->value();
00407                 if (img->encoding == "16UC1") image_max_value_ *= 1000;
00408                 if (ui_.dynamic_range_check_box->isChecked())
00409                 {
00410                     // dynamically adjust range based on min/max in image
00411                     cv::minMaxLoc(cv_ptr->image, &image_min_value_, &image_max_value_);
00412                     if (image_min_value_ == image_max_value_) {
00413                         // completely homogeneous images are displayed in gray
00414                         image_min_value_ = 0;
00415                         image_max_value_ = 2;
00416                     }
00417                 }
00418                 cv::Mat img_scaled_8u;
00419                 cv::Mat(cv_ptr->image-image_min_value_).convertTo(img_scaled_8u, CV_8UC1, 255. / (image_max_value_ - image_min_value_));
00420                 cv::cvtColor(img_scaled_8u, conversion_mat_, CV_GRAY2RGB);
00421             } else {
00422                 qWarning("ImageCropper.callback_image() could not convert image from '%s' to 'rgb8' (%s)", img->encoding.c_str(), e.what());
00423                 qimage_ = QImage();
00424                 return;
00425             }
00426         }
00427 
00428         // copy temporary image as it uses the conversion_mat_ for storage which is asynchronously overwritten in the next callback invocation
00429         QImage image(conversion_mat_.data, conversion_mat_.cols, conversion_mat_.rows, QImage::Format_RGB888);
00430         qimage_mutex_.lock();
00431         qimage_ = image.copy();
00432         qimage_mutex_.unlock();
00433 
00434         ui_.image_frame->setAspectRatio(qimage_.width(), qimage_.height());
00435         //onZoom1(false);
00436 
00437         ui_.image_frame->setInnerFrameMinimumSize(QSize(80, 60));
00438         ui_.image_frame->setMaximumSize(QSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX));
00439         widget_->setMinimumSize(QSize(80, 60));
00440         widget_->setMaximumSize(QSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX));
00441     }
00442 }
00443 
00444 void ImageCropper::publishCrop()
00445 {
00446 
00447 }
00448 
00449 }
00450 
00451 PLUGINLIB_EXPORT_CLASS(rqt_image_cropping::ImageCropper, rqt_gui_cpp::Plugin)


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