00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 #include <rqt_image_view/image_view.h>
00034
00035 #include <pluginlib/class_list_macros.h>
00036 #include <ros/master.h>
00037 #include <sensor_msgs/image_encodings.h>
00038
00039 #include <cv_bridge/cv_bridge.h>
00040 #include <opencv2/imgproc/imgproc.hpp>
00041
00042 #include <QFileDialog>
00043 #include <QMessageBox>
00044 #include <QPainter>
00045
00046 namespace rqt_image_view {
00047
00048 ImageView::ImageView()
00049 : rqt_gui_cpp::Plugin()
00050 , widget_(0)
00051 {
00052 setObjectName("ImageView");
00053 }
00054
00055 void ImageView::initPlugin(qt_gui_cpp::PluginContext& context)
00056 {
00057 widget_ = new QWidget();
00058 ui_.setupUi(widget_);
00059
00060 if (context.serialNumber() > 1)
00061 {
00062 widget_->setWindowTitle(widget_->windowTitle() + " (" + QString::number(context.serialNumber()) + ")");
00063 }
00064 context.addWidget(widget_);
00065
00066 updateTopicList();
00067 ui_.topics_combo_box->setCurrentIndex(ui_.topics_combo_box->findText(""));
00068 connect(ui_.topics_combo_box, SIGNAL(currentIndexChanged(int)), this, SLOT(onTopicChanged(int)));
00069
00070 ui_.refresh_topics_push_button->setIcon(QIcon::fromTheme("view-refresh"));
00071 connect(ui_.refresh_topics_push_button, SIGNAL(pressed()), this, SLOT(updateTopicList()));
00072
00073 ui_.zoom_1_push_button->setIcon(QIcon::fromTheme("zoom-original"));
00074 connect(ui_.zoom_1_push_button, SIGNAL(toggled(bool)), this, SLOT(onZoom1(bool)));
00075
00076 connect(ui_.dynamic_range_check_box, SIGNAL(toggled(bool)), this, SLOT(onDynamicRange(bool)));
00077
00078 ui_.save_as_image_push_button->setIcon(QIcon::fromTheme("image-x-generic"));
00079 connect(ui_.save_as_image_push_button, SIGNAL(pressed()), this, SLOT(saveImage()));
00080
00081
00082 const QStringList& argv = context.argv();
00083 if (!argv.empty()) {
00084 arg_topic_name = argv[0];
00085 selectTopic(arg_topic_name);
00086 }
00087 pub_topic_custom_ = false;
00088
00089 ui_.image_frame->setOuterLayout(ui_.image_layout);
00090
00091 QRegExp rx("([a-zA-Z/][a-zA-Z0-9_/]*)?");
00092 ui_.publish_click_location_topic_line_edit->setValidator(new QRegExpValidator(rx, this));
00093 connect(ui_.publish_click_location_check_box, SIGNAL(toggled(bool)), this, SLOT(onMousePublish(bool)));
00094 connect(ui_.image_frame, SIGNAL(mouseLeft(int, int)), this, SLOT(onMouseLeft(int, int)));
00095 connect(ui_.publish_click_location_topic_line_edit, SIGNAL(editingFinished()), this, SLOT(onPubTopicChanged()));
00096
00097 connect(ui_.smooth_image_check_box, SIGNAL(toggled(bool)), ui_.image_frame, SLOT(onSmoothImageChanged(bool)));
00098
00099 hide_toolbar_action_ = new QAction(tr("Hide toolbar"), this);
00100 hide_toolbar_action_->setCheckable(true);
00101 ui_.image_frame->addAction(hide_toolbar_action_);
00102 connect(hide_toolbar_action_, SIGNAL(toggled(bool)), this, SLOT(onHideToolbarChanged(bool)));
00103 }
00104
00105 void ImageView::shutdownPlugin()
00106 {
00107 subscriber_.shutdown();
00108 pub_mouse_left_.shutdown();
00109 }
00110
00111 void ImageView::saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const
00112 {
00113 QString topic = ui_.topics_combo_box->currentText();
00114
00115 instance_settings.setValue("topic", topic);
00116 instance_settings.setValue("zoom1", ui_.zoom_1_push_button->isChecked());
00117 instance_settings.setValue("dynamic_range", ui_.dynamic_range_check_box->isChecked());
00118 instance_settings.setValue("max_range", ui_.max_range_double_spin_box->value());
00119 instance_settings.setValue("publish_click_location", ui_.publish_click_location_check_box->isChecked());
00120 instance_settings.setValue("mouse_pub_topic", ui_.publish_click_location_topic_line_edit->text());
00121 instance_settings.setValue("toolbar_hidden", hide_toolbar_action_->isChecked());
00122 }
00123
00124 void ImageView::restoreSettings(const qt_gui_cpp::Settings& plugin_settings, const qt_gui_cpp::Settings& instance_settings)
00125 {
00126 bool zoom1_checked = instance_settings.value("zoom1", false).toBool();
00127 ui_.zoom_1_push_button->setChecked(zoom1_checked);
00128
00129 bool dynamic_range_checked = instance_settings.value("dynamic_range", false).toBool();
00130 ui_.dynamic_range_check_box->setChecked(dynamic_range_checked);
00131
00132 double max_range = instance_settings.value("max_range", ui_.max_range_double_spin_box->value()).toDouble();
00133 ui_.max_range_double_spin_box->setValue(max_range);
00134
00135 QString topic = instance_settings.value("topic", "").toString();
00136
00137 if (!arg_topic_name.isEmpty())
00138 {
00139 arg_topic_name = "";
00140 }
00141 else
00142 {
00143
00144 selectTopic(topic);
00145 }
00146
00147 bool publish_click_location = instance_settings.value("publish_click_location", false).toBool();
00148 ui_.publish_click_location_check_box->setChecked(publish_click_location);
00149
00150 QString pub_topic = instance_settings.value("mouse_pub_topic", "").toString();
00151 ui_.publish_click_location_topic_line_edit->setText(pub_topic);
00152
00153 bool toolbar_hidden = instance_settings.value("toolbar_hidden", false).toBool();
00154 hide_toolbar_action_->setChecked(toolbar_hidden);
00155 }
00156
00157 void ImageView::updateTopicList()
00158 {
00159 QSet<QString> message_types;
00160 message_types.insert("sensor_msgs/Image");
00161 QSet<QString> message_sub_types;
00162 message_sub_types.insert("sensor_msgs/CompressedImage");
00163
00164
00165 QList<QString> transports;
00166 image_transport::ImageTransport it(getNodeHandle());
00167 std::vector<std::string> declared = it.getDeclaredTransports();
00168 for (std::vector<std::string>::const_iterator it = declared.begin(); it != declared.end(); it++)
00169 {
00170
00171 QString transport = it->c_str();
00172
00173
00174 QString prefix = "image_transport/";
00175 if (transport.startsWith(prefix))
00176 {
00177 transport = transport.mid(prefix.length());
00178 }
00179 transports.append(transport);
00180 }
00181
00182 QString selected = ui_.topics_combo_box->currentText();
00183
00184
00185 QList<QString> topics = getTopics(message_types, message_sub_types, transports).values();
00186 topics.append("");
00187 qSort(topics);
00188 ui_.topics_combo_box->clear();
00189 for (QList<QString>::const_iterator it = topics.begin(); it != topics.end(); it++)
00190 {
00191 QString label(*it);
00192 label.replace(" ", "/");
00193 ui_.topics_combo_box->addItem(label, QVariant(*it));
00194 }
00195
00196
00197 selectTopic(selected);
00198 }
00199
00200 QList<QString> ImageView::getTopicList(const QSet<QString>& message_types, const QList<QString>& transports)
00201 {
00202 QSet<QString> message_sub_types;
00203 return getTopics(message_types, message_sub_types, transports).values();
00204 }
00205
00206 QSet<QString> ImageView::getTopics(const QSet<QString>& message_types, const QSet<QString>& message_sub_types, const QList<QString>& transports)
00207 {
00208 ros::master::V_TopicInfo topic_info;
00209 ros::master::getTopics(topic_info);
00210
00211 QSet<QString> all_topics;
00212 for (ros::master::V_TopicInfo::const_iterator it = topic_info.begin(); it != topic_info.end(); it++)
00213 {
00214 all_topics.insert(it->name.c_str());
00215 }
00216
00217 QSet<QString> topics;
00218 for (ros::master::V_TopicInfo::const_iterator it = topic_info.begin(); it != topic_info.end(); it++)
00219 {
00220 if (message_types.contains(it->datatype.c_str()))
00221 {
00222 QString topic = it->name.c_str();
00223
00224
00225 topics.insert(topic);
00226
00227
00228
00229 for (QList<QString>::const_iterator jt = transports.begin(); jt != transports.end(); jt++)
00230 {
00231 if (all_topics.contains(topic + "/" + *jt))
00232 {
00233 QString sub = topic + " " + *jt;
00234 topics.insert(sub);
00235
00236 }
00237 }
00238 }
00239 if (message_sub_types.contains(it->datatype.c_str()))
00240 {
00241 QString topic = it->name.c_str();
00242 int index = topic.lastIndexOf("/");
00243 if (index != -1)
00244 {
00245 topic.replace(index, 1, " ");
00246 topics.insert(topic);
00247
00248 }
00249 }
00250 }
00251 return topics;
00252 }
00253
00254 void ImageView::selectTopic(const QString& topic)
00255 {
00256 int index = ui_.topics_combo_box->findText(topic);
00257 if (index == -1)
00258 {
00259
00260 QString label(topic);
00261 label.replace(" ", "/");
00262 ui_.topics_combo_box->addItem(label, QVariant(topic));
00263 index = ui_.topics_combo_box->findText(topic);
00264 }
00265 ui_.topics_combo_box->setCurrentIndex(index);
00266 }
00267
00268 void ImageView::onTopicChanged(int index)
00269 {
00270 subscriber_.shutdown();
00271
00272
00273 ui_.image_frame->setImage(QImage());
00274
00275 QStringList parts = ui_.topics_combo_box->itemData(index).toString().split(" ");
00276 QString topic = parts.first();
00277 QString transport = parts.length() == 2 ? parts.last() : "raw";
00278
00279 if (!topic.isEmpty())
00280 {
00281 image_transport::ImageTransport it(getNodeHandle());
00282 image_transport::TransportHints hints(transport.toStdString());
00283 try {
00284 subscriber_ = it.subscribe(topic.toStdString(), 1, &ImageView::callbackImage, this, hints);
00285
00286 } catch (image_transport::TransportLoadException& e) {
00287 QMessageBox::warning(widget_, tr("Loading image transport plugin failed"), e.what());
00288 }
00289 }
00290
00291 onMousePublish(ui_.publish_click_location_check_box->isChecked());
00292 }
00293
00294 void ImageView::onZoom1(bool checked)
00295 {
00296 if (checked)
00297 {
00298 if (ui_.image_frame->getImage().isNull())
00299 {
00300 return;
00301 }
00302 ui_.image_frame->setInnerFrameFixedSize(ui_.image_frame->getImage().size());
00303 } else {
00304 ui_.image_frame->setInnerFrameMinimumSize(QSize(80, 60));
00305 ui_.image_frame->setMaximumSize(QSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX));
00306 widget_->setMinimumSize(QSize(80, 60));
00307 widget_->setMaximumSize(QSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX));
00308 }
00309 }
00310
00311 void ImageView::onDynamicRange(bool checked)
00312 {
00313 ui_.max_range_double_spin_box->setEnabled(!checked);
00314 }
00315
00316 void ImageView::saveImage()
00317 {
00318
00319 QImage img = ui_.image_frame->getImageCopy();
00320
00321 QString file_name = QFileDialog::getSaveFileName(widget_, tr("Save as image"), "image.png", tr("Image (*.bmp *.jpg *.png *.tiff)"));
00322 if (file_name.isEmpty())
00323 {
00324 return;
00325 }
00326
00327 img.save(file_name);
00328 }
00329
00330 void ImageView::onMousePublish(bool checked)
00331 {
00332 std::string topicName;
00333 if(pub_topic_custom_)
00334 {
00335 topicName = ui_.publish_click_location_topic_line_edit->text().toStdString();
00336 } else {
00337 if(!subscriber_.getTopic().empty())
00338 {
00339 topicName = subscriber_.getTopic()+"_mouse_left";
00340 } else {
00341 topicName = "mouse_left";
00342 }
00343 ui_.publish_click_location_topic_line_edit->setText(QString::fromStdString(topicName));
00344 }
00345
00346 if(checked)
00347 {
00348 pub_mouse_left_ = getNodeHandle().advertise<geometry_msgs::Point>(topicName, 1000);
00349 } else {
00350 pub_mouse_left_.shutdown();
00351 }
00352 }
00353
00354 void ImageView::onMouseLeft(int x, int y)
00355 {
00356 if(ui_.publish_click_location_check_box->isChecked() && !ui_.image_frame->getImage().isNull())
00357 {
00358 geometry_msgs::Point clickLocation;
00359
00360 clickLocation.x = round((double)x/(double)ui_.image_frame->width()*(double)ui_.image_frame->getImage().width());
00361 clickLocation.y = round((double)y/(double)ui_.image_frame->height()*(double)ui_.image_frame->getImage().height());
00362 clickLocation.z = 0;
00363 pub_mouse_left_.publish(clickLocation);
00364 }
00365 }
00366
00367 void ImageView::onPubTopicChanged()
00368 {
00369 pub_topic_custom_ = !(ui_.publish_click_location_topic_line_edit->text().isEmpty());
00370 onMousePublish(ui_.publish_click_location_check_box->isChecked());
00371 }
00372
00373 void ImageView::onHideToolbarChanged(bool hide)
00374 {
00375 ui_.toolbar_widget->setVisible(!hide);
00376 }
00377
00378
00379 void ImageView::callbackImage(const sensor_msgs::Image::ConstPtr& msg)
00380 {
00381 try
00382 {
00383
00384 cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::RGB8);
00385 conversion_mat_ = cv_ptr->image;
00386 }
00387 catch (cv_bridge::Exception& e)
00388 {
00389 try
00390 {
00391
00392 cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg);
00393 if (msg->encoding == "CV_8UC3")
00394 {
00395
00396 conversion_mat_ = cv_ptr->image;
00397 } else if (msg->encoding == "8UC1") {
00398
00399 cv::cvtColor(cv_ptr->image, conversion_mat_, CV_GRAY2RGB);
00400 } else if (msg->encoding == "16UC1" || msg->encoding == "32FC1") {
00401
00402 double min = 0;
00403 double max = ui_.max_range_double_spin_box->value();
00404 if (msg->encoding == "16UC1") max *= 1000;
00405 if (ui_.dynamic_range_check_box->isChecked())
00406 {
00407
00408 cv::minMaxLoc(cv_ptr->image, &min, &max);
00409 if (min == max) {
00410
00411 min = 0;
00412 max = 2;
00413 }
00414 }
00415 cv::Mat img_scaled_8u;
00416 cv::Mat(cv_ptr->image-min).convertTo(img_scaled_8u, CV_8UC1, 255. / (max - min));
00417 cv::cvtColor(img_scaled_8u, conversion_mat_, CV_GRAY2RGB);
00418 } else {
00419 qWarning("ImageView.callback_image() could not convert image from '%s' to 'rgb8' (%s)", msg->encoding.c_str(), e.what());
00420 ui_.image_frame->setImage(QImage());
00421 return;
00422 }
00423 }
00424 catch (cv_bridge::Exception& e)
00425 {
00426 qWarning("ImageView.callback_image() while trying to convert image from '%s' to 'rgb8' an exception was thrown (%s)", msg->encoding.c_str(), e.what());
00427 ui_.image_frame->setImage(QImage());
00428 return;
00429 }
00430 }
00431
00432
00433 QImage image(conversion_mat_.data, conversion_mat_.cols, conversion_mat_.rows, conversion_mat_.step[0], QImage::Format_RGB888);
00434 ui_.image_frame->setImage(image);
00435
00436 if (!ui_.zoom_1_push_button->isEnabled())
00437 {
00438 ui_.zoom_1_push_button->setEnabled(true);
00439 }
00440
00441
00442 onZoom1(ui_.zoom_1_push_button->isChecked());
00443 }
00444 }
00445
00446 PLUGINLIB_EXPORT_CLASS(rqt_image_view::ImageView, rqt_gui_cpp::Plugin)