From 1353f4e1d10878b48af495add13bd572c51eff83 Mon Sep 17 00:00:00 2001 From: authaldo <45536968+authaldo@users.noreply.github.com> Date: Thu, 10 Jun 2021 21:54:55 +0200 Subject: [PATCH 1/5] added .gitignore --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..8b4cebe --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +build +install +log From 7b43dcc65bc755a9c9b99278336cc9b6f066f7e8 Mon Sep 17 00:00:00 2001 From: authaldo <45536968+authaldo@users.noreply.github.com> Date: Thu, 10 Jun 2021 21:56:34 +0200 Subject: [PATCH 2/5] autoformatted files --- include/rqt_image_view/image_view.h | 107 +- include/rqt_image_view/ratio_layouted_frame.h | 64 +- src/rqt_image_view/image_view.cpp | 1004 ++++++++--------- src/rqt_image_view/ratio_layouted_frame.cpp | 289 +++-- 4 files changed, 696 insertions(+), 768 deletions(-) diff --git a/include/rqt_image_view/image_view.h b/include/rqt_image_view/image_view.h index 946d966..c67006b 100644 --- a/include/rqt_image_view/image_view.h +++ b/include/rqt_image_view/image_view.h @@ -57,100 +57,103 @@ namespace rqt_image_view { -class ImageView - : public rqt_gui_cpp::Plugin -{ + class ImageView : public rqt_gui_cpp::Plugin { - Q_OBJECT + Q_OBJECT -public: + public: - ImageView(); + ImageView(); - virtual void initPlugin(qt_gui_cpp::PluginContext& context); + virtual void initPlugin(qt_gui_cpp::PluginContext &context); - virtual void shutdownPlugin(); + virtual void shutdownPlugin(); - virtual void saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const; + virtual void + saveSettings(qt_gui_cpp::Settings &plugin_settings, qt_gui_cpp::Settings &instance_settings) const; - virtual void restoreSettings(const qt_gui_cpp::Settings& plugin_settings, const qt_gui_cpp::Settings& instance_settings); + virtual void + restoreSettings(const qt_gui_cpp::Settings &plugin_settings, const qt_gui_cpp::Settings &instance_settings); -protected slots: + protected slots: - virtual void updateTopicList(); + virtual void updateTopicList(); -protected: + protected: - virtual QSet getTopics(const QSet& message_types, const QSet& message_sub_types, const QList& transports); + virtual QSet + getTopics(const QSet &message_types, const QSet &message_sub_types, + const QList &transports); - virtual void selectTopic(const QString& topic); + virtual void selectTopic(const QString &topic); -protected slots: + protected slots: - virtual void onTopicChanged(int index); + virtual void onTopicChanged(int index); - virtual void onZoom1(bool checked); + virtual void onZoom1(bool checked); - virtual void onDynamicRange(bool checked); + virtual void onDynamicRange(bool checked); - virtual void saveImage(); + virtual void saveImage(); - virtual void updateNumGridlines(); + virtual void updateNumGridlines(); - virtual void onMousePublish(bool checked); + virtual void onMousePublish(bool checked); - virtual void onMouseLeft(int x, int y); + virtual void onMouseLeft(int x, int y); - virtual void onPubTopicChanged(); + virtual void onPubTopicChanged(); - virtual void onHideToolbarChanged(bool hide); + virtual void onHideToolbarChanged(bool hide); - virtual void onRotateLeft(); - virtual void onRotateRight(); + virtual void onRotateLeft(); -protected: + virtual void onRotateRight(); - virtual void callbackImage(const sensor_msgs::msg::Image::ConstSharedPtr& msg); + protected: - virtual void invertPixels(int x, int y); + virtual void callbackImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg); - QList getGridIndices(int size) const; + virtual void invertPixels(int x, int y); - virtual void overlayGrid(); + QList getGridIndices(int size) const; - Ui::ImageViewWidget ui_; + virtual void overlayGrid(); - QWidget* widget_; + Ui::ImageViewWidget ui_; - image_transport::Subscriber subscriber_; + QWidget *widget_; - cv::Mat conversion_mat_; + image_transport::Subscriber subscriber_; -private: + cv::Mat conversion_mat_; - enum RotateState { - ROTATE_0 = 0, - ROTATE_90 = 1, - ROTATE_180 = 2, - ROTATE_270 = 3, + private: - ROTATE_STATE_COUNT - }; + enum RotateState { + ROTATE_0 = 0, + ROTATE_90 = 1, + ROTATE_180 = 2, + ROTATE_270 = 3, - void syncRotateLabel(); + ROTATE_STATE_COUNT + }; - QString arg_topic_name; + void syncRotateLabel(); - rclcpp::Publisher::SharedPtr pub_mouse_left_; + QString arg_topic_name; - bool pub_topic_custom_; + rclcpp::Publisher::SharedPtr pub_mouse_left_; - QAction* hide_toolbar_action_; + bool pub_topic_custom_; - int num_gridlines_; + QAction *hide_toolbar_action_; - RotateState rotate_state_; -}; + int num_gridlines_; + + RotateState rotate_state_; + }; } diff --git a/include/rqt_image_view/ratio_layouted_frame.h b/include/rqt_image_view/ratio_layouted_frame.h index 4dc3761..58cd117 100644 --- a/include/rqt_image_view/ratio_layouted_frame.h +++ b/include/rqt_image_view/ratio_layouted_frame.h @@ -48,67 +48,65 @@ namespace rqt_image_view { * RatioLayoutedFrame is a layout containing a single frame with a fixed aspect ratio. * The default aspect ratio is 4:3. */ -class RatioLayoutedFrame - : public QFrame -{ + class RatioLayoutedFrame : public QFrame { - Q_OBJECT + Q_OBJECT -public: + public: - RatioLayoutedFrame(QWidget* parent, Qt::WindowFlags flags = 0); + RatioLayoutedFrame(QWidget *parent, Qt::WindowFlags flags = 0); - virtual ~RatioLayoutedFrame(); + virtual ~RatioLayoutedFrame(); - const QImage& getImage() const; + const QImage &getImage() const; - QImage getImageCopy() const; + QImage getImageCopy() const; - void setImage(const QImage& image); + void setImage(const QImage &image); - QRect getAspectRatioCorrectPaintArea(); + QRect getAspectRatioCorrectPaintArea(); - void resizeToFitAspectRatio(); + void resizeToFitAspectRatio(); - void setOuterLayout(QHBoxLayout* outer_layout); + void setOuterLayout(QHBoxLayout *outer_layout); - void setInnerFrameMinimumSize(const QSize& size); + void setInnerFrameMinimumSize(const QSize &size); - void setInnerFrameMaximumSize(const QSize& size); + void setInnerFrameMaximumSize(const QSize &size); - void setInnerFrameFixedSize(const QSize& size); + void setInnerFrameFixedSize(const QSize &size); -signals: + signals: - void delayed_update(); + void delayed_update(); - void mouseLeft(int x, int y); + void mouseLeft(int x, int y); -protected slots: + protected slots: - void onSmoothImageChanged(bool checked); + void onSmoothImageChanged(bool checked); -protected: + protected: - void setAspectRatio(unsigned short width, unsigned short height); + void setAspectRatio(unsigned short width, unsigned short height); - void paintEvent(QPaintEvent* event); + void paintEvent(QPaintEvent *event); -private: + private: - static int greatestCommonDivisor(int a, int b); + static int greatestCommonDivisor(int a, int b); - void mousePressEvent(QMouseEvent * mouseEvent); + void mousePressEvent(QMouseEvent *mouseEvent); - QHBoxLayout* outer_layout_; + QHBoxLayout *outer_layout_; - QSize aspect_ratio_; + QSize aspect_ratio_; - QImage qimage_; - mutable QMutex qimage_mutex_; + QImage qimage_; + mutable QMutex qimage_mutex_; - bool smoothImage_; -}; + bool smoothImage_; + }; } diff --git a/src/rqt_image_view/image_view.cpp b/src/rqt_image_view/image_view.cpp index 9eaf21a..aa91226 100644 --- a/src/rqt_image_view/image_view.cpp +++ b/src/rqt_image_view/image_view.cpp @@ -44,594 +44,546 @@ namespace rqt_image_view { -ImageView::ImageView() - : rqt_gui_cpp::Plugin() - , widget_(0) - , num_gridlines_(0) - , rotate_state_(ROTATE_0) -{ - setObjectName("ImageView"); -} + ImageView::ImageView() + : rqt_gui_cpp::Plugin(), widget_(0), num_gridlines_(0), rotate_state_(ROTATE_0) { + setObjectName("ImageView"); + } -void ImageView::initPlugin(qt_gui_cpp::PluginContext& context) -{ - widget_ = new QWidget(); - ui_.setupUi(widget_); + void ImageView::initPlugin(qt_gui_cpp::PluginContext &context) { + widget_ = new QWidget(); + ui_.setupUi(widget_); - if (context.serialNumber() > 1) - { - widget_->setWindowTitle(widget_->windowTitle() + " (" + QString::number(context.serialNumber()) + ")"); - } - context.addWidget(widget_); + if (context.serialNumber() > 1) { + widget_->setWindowTitle(widget_->windowTitle() + " (" + QString::number(context.serialNumber()) + ")"); + } + context.addWidget(widget_); - updateTopicList(); - ui_.topics_combo_box->setCurrentIndex(ui_.topics_combo_box->findText("")); - connect(ui_.topics_combo_box, SIGNAL(currentIndexChanged(int)), this, SLOT(onTopicChanged(int))); + updateTopicList(); + ui_.topics_combo_box->setCurrentIndex(ui_.topics_combo_box->findText("")); + connect(ui_.topics_combo_box, SIGNAL(currentIndexChanged(int)), this, SLOT(onTopicChanged(int))); - ui_.refresh_topics_push_button->setIcon(QIcon::fromTheme("view-refresh")); - connect(ui_.refresh_topics_push_button, SIGNAL(pressed()), this, SLOT(updateTopicList())); + ui_.refresh_topics_push_button->setIcon(QIcon::fromTheme("view-refresh")); + connect(ui_.refresh_topics_push_button, SIGNAL(pressed()), this, SLOT(updateTopicList())); - ui_.zoom_1_push_button->setIcon(QIcon::fromTheme("zoom-original")); - connect(ui_.zoom_1_push_button, SIGNAL(toggled(bool)), this, SLOT(onZoom1(bool))); + ui_.zoom_1_push_button->setIcon(QIcon::fromTheme("zoom-original")); + connect(ui_.zoom_1_push_button, SIGNAL(toggled(bool)), this, SLOT(onZoom1(bool))); - connect(ui_.dynamic_range_check_box, SIGNAL(toggled(bool)), this, SLOT(onDynamicRange(bool))); + connect(ui_.dynamic_range_check_box, SIGNAL(toggled(bool)), this, SLOT(onDynamicRange(bool))); - ui_.save_as_image_push_button->setIcon(QIcon::fromTheme("document-save-as")); - connect(ui_.save_as_image_push_button, SIGNAL(pressed()), this, SLOT(saveImage())); + ui_.save_as_image_push_button->setIcon(QIcon::fromTheme("document-save-as")); + connect(ui_.save_as_image_push_button, SIGNAL(pressed()), this, SLOT(saveImage())); - connect(ui_.num_gridlines_spin_box, SIGNAL(valueChanged(int)), this, SLOT(updateNumGridlines())); + connect(ui_.num_gridlines_spin_box, SIGNAL(valueChanged(int)), this, SLOT(updateNumGridlines())); - // set topic name if passed in as argument - const QStringList& argv = context.argv(); - if (!argv.empty()) { - arg_topic_name = argv[0]; - selectTopic(arg_topic_name); - } - pub_topic_custom_ = false; + // set topic name if passed in as argument + const QStringList &argv = context.argv(); + if (!argv.empty()) { + arg_topic_name = argv[0]; + selectTopic(arg_topic_name); + } + pub_topic_custom_ = false; - ui_.image_frame->setOuterLayout(ui_.image_layout); + ui_.image_frame->setOuterLayout(ui_.image_layout); - QRegExp rx("([a-zA-Z/][a-zA-Z0-9_/]*)?"); //see http://www.ros.org/wiki/ROS/Concepts#Names.Valid_Names (but also accept an empty field) - ui_.publish_click_location_topic_line_edit->setValidator(new QRegExpValidator(rx, this)); - connect(ui_.publish_click_location_check_box, SIGNAL(toggled(bool)), this, SLOT(onMousePublish(bool))); - connect(ui_.image_frame, SIGNAL(mouseLeft(int, int)), this, SLOT(onMouseLeft(int, int))); - connect(ui_.publish_click_location_topic_line_edit, SIGNAL(editingFinished()), this, SLOT(onPubTopicChanged())); + QRegExp rx( + "([a-zA-Z/][a-zA-Z0-9_/]*)?"); //see http://www.ros.org/wiki/ROS/Concepts#Names.Valid_Names (but also accept an empty field) + ui_.publish_click_location_topic_line_edit->setValidator(new QRegExpValidator(rx, this)); + connect(ui_.publish_click_location_check_box, SIGNAL(toggled(bool)), this, SLOT(onMousePublish(bool))); + connect(ui_.image_frame, SIGNAL(mouseLeft(int, int)), this, SLOT(onMouseLeft(int, int))); + connect(ui_.publish_click_location_topic_line_edit, SIGNAL(editingFinished()), this, SLOT(onPubTopicChanged())); - connect(ui_.smooth_image_check_box, SIGNAL(toggled(bool)), ui_.image_frame, SLOT(onSmoothImageChanged(bool))); + connect(ui_.smooth_image_check_box, SIGNAL(toggled(bool)), ui_.image_frame, SLOT(onSmoothImageChanged(bool))); - connect(ui_.rotate_left_push_button, SIGNAL(clicked(bool)), this, SLOT(onRotateLeft())); - connect(ui_.rotate_right_push_button, SIGNAL(clicked(bool)), this, SLOT(onRotateRight())); + connect(ui_.rotate_left_push_button, SIGNAL(clicked(bool)), this, SLOT(onRotateLeft())); + connect(ui_.rotate_right_push_button, SIGNAL(clicked(bool)), this, SLOT(onRotateRight())); - // Make sure we have enough space for "XXX °" + // Make sure we have enough space for "XXX °" #if QT_VERSION >= QT_VERSION_CHECK(5, 11, 0) - // QFontMetrics::width(QChar) is deprecated starting from qt version 5.11.0 - // https://doc.qt.io/qt-5/qfontmetrics.html#horizontalAdvance-1 - ui_.rotate_label->setMinimumWidth( - ui_.rotate_label->fontMetrics().horizontalAdvance("XXX°") - ); + // QFontMetrics::width(QChar) is deprecated starting from qt version 5.11.0 + // https://doc.qt.io/qt-5/qfontmetrics.html#horizontalAdvance-1 + ui_.rotate_label->setMinimumWidth( + ui_.rotate_label->fontMetrics().horizontalAdvance("XXX°") + ); #else - ui_.rotate_label->setMinimumWidth( - ui_.rotate_label->fontMetrics().width("XXX°") - ); + ui_.rotate_label->setMinimumWidth( + ui_.rotate_label->fontMetrics().width("XXX°") + ); #endif - hide_toolbar_action_ = new QAction(tr("Hide toolbar"), this); - hide_toolbar_action_->setCheckable(true); - ui_.image_frame->addAction(hide_toolbar_action_); - connect(hide_toolbar_action_, SIGNAL(toggled(bool)), this, SLOT(onHideToolbarChanged(bool))); -} + hide_toolbar_action_ = new QAction(tr("Hide toolbar"), this); + hide_toolbar_action_->setCheckable(true); + ui_.image_frame->addAction(hide_toolbar_action_); + connect(hide_toolbar_action_, SIGNAL(toggled(bool)), this, SLOT(onHideToolbarChanged(bool))); + } -void ImageView::shutdownPlugin() -{ - subscriber_.shutdown(); - pub_mouse_left_.reset(); -} + void ImageView::shutdownPlugin() { + subscriber_.shutdown(); + pub_mouse_left_.reset(); + } -void ImageView::saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const -{ - (void)plugin_settings; - QString topic = ui_.topics_combo_box->currentText(); - //qDebug("ImageView::saveSettings() topic '%s'", topic.toStdString().c_str()); - instance_settings.setValue("topic", topic); - instance_settings.setValue("zoom1", ui_.zoom_1_push_button->isChecked()); - instance_settings.setValue("dynamic_range", ui_.dynamic_range_check_box->isChecked()); - instance_settings.setValue("max_range", ui_.max_range_double_spin_box->value()); - instance_settings.setValue("publish_click_location", ui_.publish_click_location_check_box->isChecked()); - instance_settings.setValue("mouse_pub_topic", ui_.publish_click_location_topic_line_edit->text()); - instance_settings.setValue("toolbar_hidden", hide_toolbar_action_->isChecked()); - instance_settings.setValue("num_gridlines", ui_.num_gridlines_spin_box->value()); - instance_settings.setValue("smooth_image", ui_.smooth_image_check_box->isChecked()); - instance_settings.setValue("rotate", rotate_state_); -} + void ImageView::saveSettings(qt_gui_cpp::Settings &plugin_settings, qt_gui_cpp::Settings &instance_settings) const { + (void) plugin_settings; + QString topic = ui_.topics_combo_box->currentText(); + //qDebug("ImageView::saveSettings() topic '%s'", topic.toStdString().c_str()); + instance_settings.setValue("topic", topic); + instance_settings.setValue("zoom1", ui_.zoom_1_push_button->isChecked()); + instance_settings.setValue("dynamic_range", ui_.dynamic_range_check_box->isChecked()); + instance_settings.setValue("max_range", ui_.max_range_double_spin_box->value()); + instance_settings.setValue("publish_click_location", ui_.publish_click_location_check_box->isChecked()); + instance_settings.setValue("mouse_pub_topic", ui_.publish_click_location_topic_line_edit->text()); + instance_settings.setValue("toolbar_hidden", hide_toolbar_action_->isChecked()); + instance_settings.setValue("num_gridlines", ui_.num_gridlines_spin_box->value()); + instance_settings.setValue("smooth_image", ui_.smooth_image_check_box->isChecked()); + instance_settings.setValue("rotate", rotate_state_); + } -void ImageView::restoreSettings(const qt_gui_cpp::Settings& plugin_settings, const qt_gui_cpp::Settings& instance_settings) -{ - (void)plugin_settings; - bool zoom1_checked = instance_settings.value("zoom1", false).toBool(); - ui_.zoom_1_push_button->setChecked(zoom1_checked); - - bool dynamic_range_checked = instance_settings.value("dynamic_range", false).toBool(); - ui_.dynamic_range_check_box->setChecked(dynamic_range_checked); - - double max_range = instance_settings.value("max_range", ui_.max_range_double_spin_box->value()).toDouble(); - ui_.max_range_double_spin_box->setValue(max_range); - - num_gridlines_ = instance_settings.value("num_gridlines", ui_.num_gridlines_spin_box->value()).toInt(); - ui_.num_gridlines_spin_box->setValue(num_gridlines_); - - QString topic = instance_settings.value("topic", "").toString(); - // don't overwrite topic name passed as command line argument - if (!arg_topic_name.isEmpty()) - { - arg_topic_name = ""; - } - else - { - //qDebug("ImageView::restoreSettings() topic '%s'", topic.toStdString().c_str()); - selectTopic(topic); - } - - bool publish_click_location = instance_settings.value("publish_click_location", false).toBool(); - ui_.publish_click_location_check_box->setChecked(publish_click_location); - - QString pub_topic = instance_settings.value("mouse_pub_topic", "").toString(); - ui_.publish_click_location_topic_line_edit->setText(pub_topic); - - bool toolbar_hidden = instance_settings.value("toolbar_hidden", false).toBool(); - hide_toolbar_action_->setChecked(toolbar_hidden); - - bool smooth_image_checked = instance_settings.value("smooth_image", false).toBool(); - ui_.smooth_image_check_box->setChecked(smooth_image_checked); - - rotate_state_ = static_cast(instance_settings.value("rotate", 0).toInt()); - if(rotate_state_ >= ROTATE_STATE_COUNT) - rotate_state_ = ROTATE_0; - syncRotateLabel(); -} + void ImageView::restoreSettings(const qt_gui_cpp::Settings &plugin_settings, + const qt_gui_cpp::Settings &instance_settings) { + (void) plugin_settings; + bool zoom1_checked = instance_settings.value("zoom1", false).toBool(); + ui_.zoom_1_push_button->setChecked(zoom1_checked); -void ImageView::updateTopicList() -{ - QSet message_types; - message_types.insert("sensor_msgs/Image"); - message_types.insert("sensor_msgs/msg/Image"); - QSet message_sub_types; - message_sub_types.insert("sensor_msgs/CompressedImage"); - message_sub_types.insert("sensor_msgs/msg/CompressedImage"); - - // get declared transports - QList transports; - image_transport::ImageTransport it(node_); - std::vector declared = it.getDeclaredTransports(); - for (std::vector::const_iterator it = declared.begin(); it != declared.end(); it++) - { - //qDebug("ImageView::updateTopicList() declared transport '%s'", it->c_str()); - QString transport = it->c_str(); - - // strip prefix from transport name - QString prefix = "image_transport/"; - if (transport.startsWith(prefix)) - { - transport = transport.mid(prefix.length()); - } - transports.append(transport); - } - - QString selected = ui_.topics_combo_box->currentText(); - - // fill combo box - QList topics = getTopics(message_types, message_sub_types, transports).values(); - topics.append(""); - std::sort(topics.begin(), topics.end()); - ui_.topics_combo_box->clear(); - for (QList::const_iterator it = topics.begin(); it != topics.end(); it++) - { - QString label(*it); - label.replace(" ", "/"); - ui_.topics_combo_box->addItem(label, QVariant(*it)); - } - - // restore previous selection - selectTopic(selected); -} + bool dynamic_range_checked = instance_settings.value("dynamic_range", false).toBool(); + ui_.dynamic_range_check_box->setChecked(dynamic_range_checked); -QSet ImageView::getTopics(const QSet& message_types, const QSet& message_sub_types, const QList& transports) -{ - std::map> topic_info = node_->get_topic_names_and_types(); - - QSet all_topics; - for (std::map>::iterator it = topic_info.begin(); it != topic_info.end(); ++it) - { - all_topics.insert(it->first.c_str()); - } - - QSet topics; - for (std::map>::iterator it = topic_info.begin(); it != topic_info.end(); ++it) - { - for (std::vector::const_iterator msg_type_it = it->second.begin(); msg_type_it != it->second.end(); ++msg_type_it) - { - if (message_types.contains(msg_type_it->c_str())) - { - QString topic = it->first.c_str(); - - // add raw topic - topics.insert(topic); - //qDebug("ImageView::getTopics() raw topic '%s'", topic.toStdString().c_str()); - - // add transport specific sub-topics - for (QList::const_iterator jt = transports.begin(); jt != transports.end(); jt++) - { - if (all_topics.contains(topic + "/" + *jt)) - { - QString sub = topic + " " + *jt; - topics.insert(sub); - //qDebug("ImageView::getTopics() transport specific sub-topic '%s'", sub.toStdString().c_str()); - } - } - } - if (message_sub_types.contains(msg_type_it->c_str())) - { - QString topic = it->first.c_str(); - int index = topic.lastIndexOf("/"); - if (index != -1) - { - topic.replace(index, 1, " "); - topics.insert(topic); - //qDebug("ImageView::getTopics() transport specific sub-topic '%s'", topic.toStdString().c_str()); + double max_range = instance_settings.value("max_range", ui_.max_range_double_spin_box->value()).toDouble(); + ui_.max_range_double_spin_box->setValue(max_range); + + num_gridlines_ = instance_settings.value("num_gridlines", ui_.num_gridlines_spin_box->value()).toInt(); + ui_.num_gridlines_spin_box->setValue(num_gridlines_); + + QString topic = instance_settings.value("topic", "").toString(); + // don't overwrite topic name passed as command line argument + if (!arg_topic_name.isEmpty()) { + arg_topic_name = ""; + } else { + //qDebug("ImageView::restoreSettings() topic '%s'", topic.toStdString().c_str()); + selectTopic(topic); } - } - } - } - return topics; -} -void ImageView::selectTopic(const QString& topic) -{ - int index = ui_.topics_combo_box->findText(topic); - if (index == -1) - { - // add topic name to list if not yet in - QString label(topic); - label.replace(" ", "/"); - ui_.topics_combo_box->addItem(label, QVariant(topic)); - index = ui_.topics_combo_box->findText(topic); - } - ui_.topics_combo_box->setCurrentIndex(index); -} + bool publish_click_location = instance_settings.value("publish_click_location", false).toBool(); + ui_.publish_click_location_check_box->setChecked(publish_click_location); -void ImageView::onTopicChanged(int index) -{ - conversion_mat_.release(); + QString pub_topic = instance_settings.value("mouse_pub_topic", "").toString(); + ui_.publish_click_location_topic_line_edit->setText(pub_topic); - subscriber_.shutdown(); + bool toolbar_hidden = instance_settings.value("toolbar_hidden", false).toBool(); + hide_toolbar_action_->setChecked(toolbar_hidden); - // reset image on topic change - ui_.image_frame->setImage(QImage()); + bool smooth_image_checked = instance_settings.value("smooth_image", false).toBool(); + ui_.smooth_image_check_box->setChecked(smooth_image_checked); - QStringList parts = ui_.topics_combo_box->itemData(index).toString().split(" "); - QString topic = parts.first(); - QString transport = parts.length() == 2 ? parts.last() : "raw"; + rotate_state_ = static_cast(instance_settings.value("rotate", 0).toInt()); + if (rotate_state_ >= ROTATE_STATE_COUNT) + rotate_state_ = ROTATE_0; + syncRotateLabel(); + } - if (!topic.isEmpty()) - { - image_transport::ImageTransport it(node_); - const image_transport::TransportHints hints(node_.get(), transport.toStdString()); - try { - subscriber_ = it.subscribe(topic.toStdString(), 1, &ImageView::callbackImage, this, &hints); - qDebug("ImageView::onTopicChanged() to topic '%s' with transport '%s'", topic.toStdString().c_str(), subscriber_.getTransport().c_str()); - } catch (image_transport::TransportLoadException& e) { - QMessageBox::warning(widget_, tr("Loading image transport plugin failed"), e.what()); + void ImageView::updateTopicList() { + QSet message_types; + message_types.insert("sensor_msgs/Image"); + message_types.insert("sensor_msgs/msg/Image"); + QSet message_sub_types; + message_sub_types.insert("sensor_msgs/CompressedImage"); + message_sub_types.insert("sensor_msgs/msg/CompressedImage"); + + // get declared transports + QList transports; + image_transport::ImageTransport it(node_); + std::vector declared = it.getDeclaredTransports(); + for (std::vector::const_iterator it = declared.begin(); it != declared.end(); it++) { + //qDebug("ImageView::updateTopicList() declared transport '%s'", it->c_str()); + QString transport = it->c_str(); + + // strip prefix from transport name + QString prefix = "image_transport/"; + if (transport.startsWith(prefix)) { + transport = transport.mid(prefix.length()); + } + transports.append(transport); + } + + QString selected = ui_.topics_combo_box->currentText(); + + // fill combo box + QList topics = getTopics(message_types, message_sub_types, transports).values(); + topics.append(""); + std::sort(topics.begin(), topics.end()); + ui_.topics_combo_box->clear(); + for (QList::const_iterator it = topics.begin(); it != topics.end(); it++) { + QString label(*it); + label.replace(" ", "/"); + ui_.topics_combo_box->addItem(label, QVariant(*it)); + } + + // restore previous selection + selectTopic(selected); } - } - onMousePublish(ui_.publish_click_location_check_box->isChecked()); -} + QSet ImageView::getTopics(const QSet &message_types, const QSet &message_sub_types, + const QList &transports) { + std::map> topic_info = node_->get_topic_names_and_types(); + + QSet all_topics; + for (std::map>::iterator it = topic_info.begin(); + it != topic_info.end(); ++it) { + all_topics.insert(it->first.c_str()); + } -void ImageView::onZoom1(bool checked) -{ - if (checked) - { - if (ui_.image_frame->getImage().isNull()) - { - return; + QSet topics; + for (std::map>::iterator it = topic_info.begin(); + it != topic_info.end(); ++it) { + for (std::vector::const_iterator msg_type_it = it->second.begin(); + msg_type_it != it->second.end(); ++msg_type_it) { + if (message_types.contains(msg_type_it->c_str())) { + QString topic = it->first.c_str(); + + // add raw topic + topics.insert(topic); + //qDebug("ImageView::getTopics() raw topic '%s'", topic.toStdString().c_str()); + + // add transport specific sub-topics + for (QList::const_iterator jt = transports.begin(); jt != transports.end(); jt++) { + if (all_topics.contains(topic + "/" + *jt)) { + QString sub = topic + " " + *jt; + topics.insert(sub); + //qDebug("ImageView::getTopics() transport specific sub-topic '%s'", sub.toStdString().c_str()); + } + } + } + if (message_sub_types.contains(msg_type_it->c_str())) { + QString topic = it->first.c_str(); + int index = topic.lastIndexOf("/"); + if (index != -1) { + topic.replace(index, 1, " "); + topics.insert(topic); + //qDebug("ImageView::getTopics() transport specific sub-topic '%s'", topic.toStdString().c_str()); + } + } + } + } + return topics; } - ui_.image_frame->setInnerFrameFixedSize(ui_.image_frame->getImage().size()); - } else { - ui_.image_frame->setInnerFrameMinimumSize(QSize(80, 60)); - ui_.image_frame->setMaximumSize(QSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX)); - widget_->setMinimumSize(QSize(80, 60)); - widget_->setMaximumSize(QSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX)); - } -} -void ImageView::onDynamicRange(bool checked) -{ - ui_.max_range_double_spin_box->setEnabled(!checked); -} + void ImageView::selectTopic(const QString &topic) { + int index = ui_.topics_combo_box->findText(topic); + if (index == -1) { + // add topic name to list if not yet in + QString label(topic); + label.replace(" ", "/"); + ui_.topics_combo_box->addItem(label, QVariant(topic)); + index = ui_.topics_combo_box->findText(topic); + } + ui_.topics_combo_box->setCurrentIndex(index); + } -void ImageView::updateNumGridlines() -{ - num_gridlines_ = ui_.num_gridlines_spin_box->value(); -} + void ImageView::onTopicChanged(int index) { + conversion_mat_.release(); -void ImageView::saveImage() -{ - // take a snapshot before asking for the filename - QImage img = ui_.image_frame->getImageCopy(); + subscriber_.shutdown(); - QString file_name = QFileDialog::getSaveFileName(widget_, tr("Save as image"), "image.png", tr("Image (*.bmp *.jpg *.png *.tiff)")); - if (file_name.isEmpty()) - { - return; - } + // reset image on topic change + ui_.image_frame->setImage(QImage()); - img.save(file_name); -} + QStringList parts = ui_.topics_combo_box->itemData(index).toString().split(" "); + QString topic = parts.first(); + QString transport = parts.length() == 2 ? parts.last() : "raw"; + + if (!topic.isEmpty()) { + image_transport::ImageTransport it(node_); + const image_transport::TransportHints hints(node_.get(), transport.toStdString()); + try { + subscriber_ = it.subscribe(topic.toStdString(), 1, &ImageView::callbackImage, this, &hints); + qDebug("ImageView::onTopicChanged() to topic '%s' with transport '%s'", topic.toStdString().c_str(), + subscriber_.getTransport().c_str()); + } catch (image_transport::TransportLoadException &e) { + QMessageBox::warning(widget_, tr("Loading image transport plugin failed"), e.what()); + } + } -void ImageView::onMousePublish(bool checked) -{ - std::string topicName; - if(pub_topic_custom_) - { - topicName = ui_.publish_click_location_topic_line_edit->text().toStdString(); - } else { - if(!subscriber_.getTopic().empty()) - { - topicName = subscriber_.getTopic()+"_mouse_left"; - } else { - topicName = "mouse_left"; + onMousePublish(ui_.publish_click_location_check_box->isChecked()); } - ui_.publish_click_location_topic_line_edit->setText(QString::fromStdString(topicName)); - } - - if(checked) - { - pub_mouse_left_ = node_->create_publisher(topicName, 1000); - } else { - pub_mouse_left_.reset(); - } -} -void ImageView::onMouseLeft(int x, int y) -{ - if(ui_.publish_click_location_check_box->isChecked() && !ui_.image_frame->getImage().isNull()) - { - geometry_msgs::msg::Point clickCanvasLocation; - // Publish click location in pixel coordinates - clickCanvasLocation.x = round((double)x/(double)ui_.image_frame->width()*(double)ui_.image_frame->getImage().width()); - clickCanvasLocation.y = round((double)y/(double)ui_.image_frame->height()*(double)ui_.image_frame->getImage().height()); - clickCanvasLocation.z = 0; - - geometry_msgs::msg::Point clickLocation = clickCanvasLocation; - - switch(rotate_state_) - { - case ROTATE_90: - clickLocation.x = clickCanvasLocation.y; - clickLocation.y = ui_.image_frame->getImage().width() - clickCanvasLocation.x; - break; - case ROTATE_180: - clickLocation.x = ui_.image_frame->getImage().width() - clickCanvasLocation.x; - clickLocation.y = ui_.image_frame->getImage().height() - clickCanvasLocation.y; - break; - case ROTATE_270: - clickLocation.x = ui_.image_frame->getImage().height() - clickCanvasLocation.y; - clickLocation.y = clickCanvasLocation.x; - break; - default: - break; + void ImageView::onZoom1(bool checked) { + if (checked) { + if (ui_.image_frame->getImage().isNull()) { + return; + } + ui_.image_frame->setInnerFrameFixedSize(ui_.image_frame->getImage().size()); + } else { + ui_.image_frame->setInnerFrameMinimumSize(QSize(80, 60)); + ui_.image_frame->setMaximumSize(QSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX)); + widget_->setMinimumSize(QSize(80, 60)); + widget_->setMaximumSize(QSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX)); + } } - pub_mouse_left_->publish(clickLocation); - } -} + void ImageView::onDynamicRange(bool checked) { + ui_.max_range_double_spin_box->setEnabled(!checked); + } -void ImageView::onPubTopicChanged() -{ - pub_topic_custom_ = !(ui_.publish_click_location_topic_line_edit->text().isEmpty()); - onMousePublish(ui_.publish_click_location_check_box->isChecked()); -} + void ImageView::updateNumGridlines() { + num_gridlines_ = ui_.num_gridlines_spin_box->value(); + } -void ImageView::onHideToolbarChanged(bool hide) -{ - ui_.toolbar_widget->setVisible(!hide); -} + void ImageView::saveImage() { + // take a snapshot before asking for the filename + QImage img = ui_.image_frame->getImageCopy(); -void ImageView::onRotateLeft() -{ - int m = rotate_state_ - 1; - if(m < 0) - m = ROTATE_STATE_COUNT-1; + QString file_name = QFileDialog::getSaveFileName(widget_, tr("Save as image"), "image.png", + tr("Image (*.bmp *.jpg *.png *.tiff)")); + if (file_name.isEmpty()) { + return; + } - rotate_state_ = static_cast(m); - syncRotateLabel(); -} + img.save(file_name); + } -void ImageView::onRotateRight() -{ - rotate_state_ = static_cast((rotate_state_ + 1) % ROTATE_STATE_COUNT); - syncRotateLabel(); -} + void ImageView::onMousePublish(bool checked) { + std::string topicName; + if (pub_topic_custom_) { + topicName = ui_.publish_click_location_topic_line_edit->text().toStdString(); + } else { + if (!subscriber_.getTopic().empty()) { + topicName = subscriber_.getTopic() + "_mouse_left"; + } else { + topicName = "mouse_left"; + } + ui_.publish_click_location_topic_line_edit->setText(QString::fromStdString(topicName)); + } -void ImageView::syncRotateLabel() -{ - switch(rotate_state_) - { - default: - case ROTATE_0: ui_.rotate_label->setText("0°"); break; - case ROTATE_90: ui_.rotate_label->setText("90°"); break; - case ROTATE_180: ui_.rotate_label->setText("180°"); break; - case ROTATE_270: ui_.rotate_label->setText("270°"); break; - } -} + if (checked) { + pub_mouse_left_ = node_->create_publisher(topicName, 1000); + } else { + pub_mouse_left_.reset(); + } + } -void ImageView::invertPixels(int x, int y) -{ - // Could do 255-conversion_mat_.at(cv::Point(x,y))[i], but that doesn't work well on gray - cv::Vec3b & pixel = conversion_mat_.at(cv::Point(x, y)); - if (pixel[0] + pixel[1] + pixel[2] > 3 * 127) - pixel = cv::Vec3b(0,0,0); - else - pixel = cv::Vec3b(255,255,255); -} + void ImageView::onMouseLeft(int x, int y) { + if (ui_.publish_click_location_check_box->isChecked() && !ui_.image_frame->getImage().isNull()) { + geometry_msgs::msg::Point clickCanvasLocation; + // Publish click location in pixel coordinates + clickCanvasLocation.x = round( + (double) x / (double) ui_.image_frame->width() * (double) ui_.image_frame->getImage().width()); + clickCanvasLocation.y = round( + (double) y / (double) ui_.image_frame->height() * (double) ui_.image_frame->getImage().height()); + clickCanvasLocation.z = 0; + + geometry_msgs::msg::Point clickLocation = clickCanvasLocation; + + switch (rotate_state_) { + case ROTATE_90: + clickLocation.x = clickCanvasLocation.y; + clickLocation.y = ui_.image_frame->getImage().width() - clickCanvasLocation.x; + break; + case ROTATE_180: + clickLocation.x = ui_.image_frame->getImage().width() - clickCanvasLocation.x; + clickLocation.y = ui_.image_frame->getImage().height() - clickCanvasLocation.y; + break; + case ROTATE_270: + clickLocation.x = ui_.image_frame->getImage().height() - clickCanvasLocation.y; + clickLocation.y = clickCanvasLocation.x; + break; + default: + break; + } + + pub_mouse_left_->publish(clickLocation); + } + } -QList ImageView::getGridIndices(int size) const -{ - QList indices; - - // the spacing between adjacent grid lines - float grid_width = 1.0f * size / (num_gridlines_ + 1); - - // select grid line(s) closest to the center - float index; - if (num_gridlines_ % 2) // odd - { - indices.append(size / 2); - // make the center line 2px wide in case of an even resolution - if (size % 2 == 0) // even - indices.append(size / 2 - 1); - index = 1.0f * (size - 1) / 2; - } - else // even - { - index = grid_width * (num_gridlines_ / 2); - // one grid line before the center - indices.append(round(index)); - // one grid line after the center - indices.append(size - 1 - round(index)); - } - - // add additional grid lines from the center to the border of the image - int lines = (num_gridlines_ - 1) / 2; - while (lines > 0) - { - index -= grid_width; - indices.append(round(index)); - indices.append(size - 1 - round(index)); - lines--; - } - - return indices; -} + void ImageView::onPubTopicChanged() { + pub_topic_custom_ = !(ui_.publish_click_location_topic_line_edit->text().isEmpty()); + onMousePublish(ui_.publish_click_location_check_box->isChecked()); + } -void ImageView::overlayGrid() -{ - // vertical gridlines - QList columns = getGridIndices(conversion_mat_.cols); - for (QList::const_iterator x = columns.begin(); x != columns.end(); ++x) - { - for (int y = 0; y < conversion_mat_.rows; ++y) - { - invertPixels(*x, y); + void ImageView::onHideToolbarChanged(bool hide) { + ui_.toolbar_widget->setVisible(!hide); } - } - - // horizontal gridlines - QList rows = getGridIndices(conversion_mat_.rows); - for (QList::const_iterator y = rows.begin(); y != rows.end(); ++y) - { - for (int x = 0; x < conversion_mat_.cols; ++x) - { - invertPixels(x, *y); + + void ImageView::onRotateLeft() { + int m = rotate_state_ - 1; + if (m < 0) + m = ROTATE_STATE_COUNT - 1; + + rotate_state_ = static_cast(m); + syncRotateLabel(); } - } -} -void ImageView::callbackImage(const sensor_msgs::msg::Image::ConstSharedPtr& msg) -{ - try - { - // First let cv_bridge do its magic - cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::RGB8); - conversion_mat_ = cv_ptr->image; - - if (num_gridlines_ > 0) - overlayGrid(); - } - catch (cv_bridge::Exception& e) - { - try - { - // If we're here, there is no conversion that makes sense, but let's try to imagine a few first - cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg); - if (msg->encoding == "CV_8UC3") - { - // assuming it is rgb - conversion_mat_ = cv_ptr->image; - } else if (msg->encoding == "8UC1") { - // convert gray to rgb - cv::cvtColor(cv_ptr->image, conversion_mat_, CV_GRAY2RGB); - } else if (msg->encoding == "16UC1" || msg->encoding == "32FC1") { - // scale / quantify - double min = 0; - double max = ui_.max_range_double_spin_box->value(); - if (msg->encoding == "16UC1") max *= 1000; - if (ui_.dynamic_range_check_box->isChecked()) - { - // dynamically adjust range based on min/max in image - cv::minMaxLoc(cv_ptr->image, &min, &max); - if (min == max) { - // completely homogeneous images are displayed in gray - min = 0; - max = 2; - } + void ImageView::onRotateRight() { + rotate_state_ = static_cast((rotate_state_ + 1) % ROTATE_STATE_COUNT); + syncRotateLabel(); + } + + void ImageView::syncRotateLabel() { + switch (rotate_state_) { + default: + case ROTATE_0: + ui_.rotate_label->setText("0°"); + break; + case ROTATE_90: + ui_.rotate_label->setText("90°"); + break; + case ROTATE_180: + ui_.rotate_label->setText("180°"); + break; + case ROTATE_270: + ui_.rotate_label->setText("270°"); + break; } - cv::Mat img_scaled_8u; - cv::Mat(cv_ptr->image-min).convertTo(img_scaled_8u, CV_8UC1, 255. / (max - min)); - cv::cvtColor(img_scaled_8u, conversion_mat_, CV_GRAY2RGB); - } else { - qWarning("ImageView.callback_image() could not convert image from '%s' to 'rgb8' (%s)", msg->encoding.c_str(), e.what()); - ui_.image_frame->setImage(QImage()); - return; - } } - catch (cv_bridge::Exception& e) - { - qWarning("ImageView.callback_image() while trying to convert image from '%s' to 'rgb8' an exception was thrown (%s)", msg->encoding.c_str(), e.what()); - ui_.image_frame->setImage(QImage()); - return; + + void ImageView::invertPixels(int x, int y) { + // Could do 255-conversion_mat_.at(cv::Point(x,y))[i], but that doesn't work well on gray + cv::Vec3b &pixel = conversion_mat_.at(cv::Point(x, y)); + if (pixel[0] + pixel[1] + pixel[2] > 3 * 127) + pixel = cv::Vec3b(0, 0, 0); + else + pixel = cv::Vec3b(255, 255, 255); } - } - - // Handle rotation - switch(rotate_state_) - { - case ROTATE_90: - { - cv::Mat tmp; - cv::transpose(conversion_mat_, tmp); - cv::flip(tmp, conversion_mat_, 1); - break; + + QList ImageView::getGridIndices(int size) const { + QList indices; + + // the spacing between adjacent grid lines + float grid_width = 1.0f * size / (num_gridlines_ + 1); + + // select grid line(s) closest to the center + float index; + if (num_gridlines_ % 2) // odd + { + indices.append(size / 2); + // make the center line 2px wide in case of an even resolution + if (size % 2 == 0) // even + indices.append(size / 2 - 1); + index = 1.0f * (size - 1) / 2; + } else // even + { + index = grid_width * (num_gridlines_ / 2); + // one grid line before the center + indices.append(round(index)); + // one grid line after the center + indices.append(size - 1 - round(index)); + } + + // add additional grid lines from the center to the border of the image + int lines = (num_gridlines_ - 1) / 2; + while (lines > 0) { + index -= grid_width; + indices.append(round(index)); + indices.append(size - 1 - round(index)); + lines--; + } + + return indices; } - case ROTATE_180: - { - cv::Mat tmp; - cv::flip(conversion_mat_, tmp, -1); - conversion_mat_ = tmp; - break; + + void ImageView::overlayGrid() { + // vertical gridlines + QList columns = getGridIndices(conversion_mat_.cols); + for (QList::const_iterator x = columns.begin(); x != columns.end(); ++x) { + for (int y = 0; y < conversion_mat_.rows; ++y) { + invertPixels(*x, y); + } + } + + // horizontal gridlines + QList rows = getGridIndices(conversion_mat_.rows); + for (QList::const_iterator y = rows.begin(); y != rows.end(); ++y) { + for (int x = 0; x < conversion_mat_.cols; ++x) { + invertPixels(x, *y); + } + } } - case ROTATE_270: - { - cv::Mat tmp; - cv::transpose(conversion_mat_, tmp); - cv::flip(tmp, conversion_mat_, 0); - break; + + void ImageView::callbackImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg) { + try { + // First let cv_bridge do its magic + cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::RGB8); + conversion_mat_ = cv_ptr->image; + + if (num_gridlines_ > 0) + overlayGrid(); + } + catch (cv_bridge::Exception &e) { + try { + // If we're here, there is no conversion that makes sense, but let's try to imagine a few first + cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg); + if (msg->encoding == "CV_8UC3") { + // assuming it is rgb + conversion_mat_ = cv_ptr->image; + } else if (msg->encoding == "8UC1") { + // convert gray to rgb + cv::cvtColor(cv_ptr->image, conversion_mat_, CV_GRAY2RGB); + } else if (msg->encoding == "16UC1" || msg->encoding == "32FC1") { + // scale / quantify + double min = 0; + double max = ui_.max_range_double_spin_box->value(); + if (msg->encoding == "16UC1") max *= 1000; + if (ui_.dynamic_range_check_box->isChecked()) { + // dynamically adjust range based on min/max in image + cv::minMaxLoc(cv_ptr->image, &min, &max); + if (min == max) { + // completely homogeneous images are displayed in gray + min = 0; + max = 2; + } + } + cv::Mat img_scaled_8u; + cv::Mat(cv_ptr->image - min).convertTo(img_scaled_8u, CV_8UC1, 255. / (max - min)); + cv::cvtColor(img_scaled_8u, conversion_mat_, CV_GRAY2RGB); + } else { + qWarning("ImageView.callback_image() could not convert image from '%s' to 'rgb8' (%s)", + msg->encoding.c_str(), e.what()); + ui_.image_frame->setImage(QImage()); + return; + } + } + catch (cv_bridge::Exception &e) { + qWarning( + "ImageView.callback_image() while trying to convert image from '%s' to 'rgb8' an exception was thrown (%s)", + msg->encoding.c_str(), e.what()); + ui_.image_frame->setImage(QImage()); + return; + } + } + + // Handle rotation + switch (rotate_state_) { + case ROTATE_90: { + cv::Mat tmp; + cv::transpose(conversion_mat_, tmp); + cv::flip(tmp, conversion_mat_, 1); + break; + } + case ROTATE_180: { + cv::Mat tmp; + cv::flip(conversion_mat_, tmp, -1); + conversion_mat_ = tmp; + break; + } + case ROTATE_270: { + cv::Mat tmp; + cv::transpose(conversion_mat_, tmp); + cv::flip(tmp, conversion_mat_, 0); + break; + } + default: + break; + } + + // image must be copied since it uses the conversion_mat_ for storage which is asynchronously overwritten in the next callback invocation + QImage image(conversion_mat_.data, conversion_mat_.cols, conversion_mat_.rows, conversion_mat_.step[0], + QImage::Format_RGB888); + ui_.image_frame->setImage(image); + + if (!ui_.zoom_1_push_button->isEnabled()) { + ui_.zoom_1_push_button->setEnabled(true); + } + // Need to update the zoom 1 every new image in case the image aspect ratio changed, + // though could check and see if the aspect ratio changed or not. + onZoom1(ui_.zoom_1_push_button->isChecked()); } - default: - break; - } - - // image must be copied since it uses the conversion_mat_ for storage which is asynchronously overwritten in the next callback invocation - QImage image(conversion_mat_.data, conversion_mat_.cols, conversion_mat_.rows, conversion_mat_.step[0], QImage::Format_RGB888); - ui_.image_frame->setImage(image); - - if (!ui_.zoom_1_push_button->isEnabled()) - { - ui_.zoom_1_push_button->setEnabled(true); - } - // Need to update the zoom 1 every new image in case the image aspect ratio changed, - // though could check and see if the aspect ratio changed or not. - onZoom1(ui_.zoom_1_push_button->isChecked()); -} } PLUGINLIB_EXPORT_CLASS(rqt_image_view::ImageView, rqt_gui_cpp::Plugin) diff --git a/src/rqt_image_view/ratio_layouted_frame.cpp b/src/rqt_image_view/ratio_layouted_frame.cpp index 4bdda81..7292acd 100644 --- a/src/rqt_image_view/ratio_layouted_frame.cpp +++ b/src/rqt_image_view/ratio_layouted_frame.cpp @@ -37,177 +37,152 @@ namespace rqt_image_view { -RatioLayoutedFrame::RatioLayoutedFrame(QWidget* parent, Qt::WindowFlags flags) - : QFrame() - , outer_layout_(NULL) - , aspect_ratio_(4, 3) - , smoothImage_(false) -{ - (void)parent; - (void)flags; - connect(this, SIGNAL(delayed_update()), this, SLOT(update()), Qt::QueuedConnection); -} + RatioLayoutedFrame::RatioLayoutedFrame(QWidget *parent, Qt::WindowFlags flags) + : QFrame(), outer_layout_(NULL), aspect_ratio_(4, 3), smoothImage_(false) { + (void) parent; + (void) flags; + connect(this, SIGNAL(delayed_update()), this, SLOT(update()), Qt::QueuedConnection); + } -RatioLayoutedFrame::~RatioLayoutedFrame() -{ -} + RatioLayoutedFrame::~RatioLayoutedFrame() { + } -const QImage& RatioLayoutedFrame::getImage() const -{ - return qimage_; -} + const QImage &RatioLayoutedFrame::getImage() const { + return qimage_; + } -QImage RatioLayoutedFrame::getImageCopy() const -{ - QImage img; - qimage_mutex_.lock(); - img = qimage_.copy(); - qimage_mutex_.unlock(); - return img; -} + QImage RatioLayoutedFrame::getImageCopy() const { + QImage img; + qimage_mutex_.lock(); + img = qimage_.copy(); + qimage_mutex_.unlock(); + return img; + } -void RatioLayoutedFrame::setImage(const QImage& image)//, QMutex* image_mutex) -{ - qimage_mutex_.lock(); - qimage_ = image.copy(); - setAspectRatio(qimage_.width(), qimage_.height()); - qimage_mutex_.unlock(); - emit delayed_update(); -} + void RatioLayoutedFrame::setImage(const QImage &image)//, QMutex* image_mutex) + { + qimage_mutex_.lock(); + qimage_ = image.copy(); + setAspectRatio(qimage_.width(), qimage_.height()); + qimage_mutex_.unlock(); + emit delayed_update(); + } -void RatioLayoutedFrame::resizeToFitAspectRatio() -{ - QRect rect = contentsRect(); - - // reduce longer edge to aspect ration - double width; - double height; - - if (outer_layout_) - { - width = outer_layout_->contentsRect().width(); - height = outer_layout_->contentsRect().height(); - } - else - { - // if outer layout isn't available, this will use the old - // width and height, but this can shrink the display image if the - // aspect ratio changes. - width = rect.width(); - height = rect.height(); - } - - double layout_ar = width / height; - const double image_ar = double(aspect_ratio_.width()) / double(aspect_ratio_.height()); - if (layout_ar > image_ar) - { - // too large width - width = height * image_ar; - } - else - { - // too large height - height = width / image_ar; - } - rect.setWidth(int(width + 0.5)); - rect.setHeight(int(height + 0.5)); - - // resize taking the border line into account - int border = lineWidth(); - resize(rect.width() + 2 * border, rect.height() + 2 * border); -} + void RatioLayoutedFrame::resizeToFitAspectRatio() { + QRect rect = contentsRect(); + + // reduce longer edge to aspect ration + double width; + double height; + + if (outer_layout_) { + width = outer_layout_->contentsRect().width(); + height = outer_layout_->contentsRect().height(); + } else { + // if outer layout isn't available, this will use the old + // width and height, but this can shrink the display image if the + // aspect ratio changes. + width = rect.width(); + height = rect.height(); + } + + double layout_ar = width / height; + const double image_ar = double(aspect_ratio_.width()) / double(aspect_ratio_.height()); + if (layout_ar > image_ar) { + // too large width + width = height * image_ar; + } else { + // too large height + height = width / image_ar; + } + rect.setWidth(int(width + 0.5)); + rect.setHeight(int(height + 0.5)); + + // resize taking the border line into account + int border = lineWidth(); + resize(rect.width() + 2 * border, rect.height() + 2 * border); + } -void RatioLayoutedFrame::setOuterLayout(QHBoxLayout* outer_layout) -{ - outer_layout_ = outer_layout; -} + void RatioLayoutedFrame::setOuterLayout(QHBoxLayout *outer_layout) { + outer_layout_ = outer_layout; + } -void RatioLayoutedFrame::setInnerFrameMinimumSize(const QSize& size) -{ - int border = lineWidth(); - QSize new_size = size; - new_size += QSize(2 * border, 2 * border); - setMinimumSize(new_size); - emit delayed_update(); -} + void RatioLayoutedFrame::setInnerFrameMinimumSize(const QSize &size) { + int border = lineWidth(); + QSize new_size = size; + new_size += QSize(2 * border, 2 * border); + setMinimumSize(new_size); + emit delayed_update(); + } -void RatioLayoutedFrame::setInnerFrameMaximumSize(const QSize& size) -{ - int border = lineWidth(); - QSize new_size = size; - new_size += QSize(2 * border, 2 * border); - setMaximumSize(new_size); - emit delayed_update(); -} + void RatioLayoutedFrame::setInnerFrameMaximumSize(const QSize &size) { + int border = lineWidth(); + QSize new_size = size; + new_size += QSize(2 * border, 2 * border); + setMaximumSize(new_size); + emit delayed_update(); + } -void RatioLayoutedFrame::setInnerFrameFixedSize(const QSize& size) -{ - setInnerFrameMinimumSize(size); - setInnerFrameMaximumSize(size); -} + void RatioLayoutedFrame::setInnerFrameFixedSize(const QSize &size) { + setInnerFrameMinimumSize(size); + setInnerFrameMaximumSize(size); + } -void RatioLayoutedFrame::setAspectRatio(unsigned short width, unsigned short height) -{ - int divisor = greatestCommonDivisor(width, height); - if (divisor != 0) { - aspect_ratio_.setWidth(width / divisor); - aspect_ratio_.setHeight(height / divisor); - } -} + void RatioLayoutedFrame::setAspectRatio(unsigned short width, unsigned short height) { + int divisor = greatestCommonDivisor(width, height); + if (divisor != 0) { + aspect_ratio_.setWidth(width / divisor); + aspect_ratio_.setHeight(height / divisor); + } + } -void RatioLayoutedFrame::paintEvent(QPaintEvent* event) -{ - (void)event; - QPainter painter(this); - qimage_mutex_.lock(); - if (!qimage_.isNull()) - { - resizeToFitAspectRatio(); - // TODO: check if full draw is really necessary - //QPaintEvent* paint_event = dynamic_cast(event); - //painter.drawImage(paint_event->rect(), qimage_); - if (!smoothImage_) { - painter.drawImage(contentsRect(), qimage_); - } else { - if (contentsRect().width() == qimage_.width()) { - painter.drawImage(contentsRect(), qimage_); - } else { - QImage image = qimage_.scaled(contentsRect().width(), contentsRect().height(), - Qt::KeepAspectRatio, Qt::SmoothTransformation); - painter.drawImage(contentsRect(), image); - } + void RatioLayoutedFrame::paintEvent(QPaintEvent *event) { + (void) event; + QPainter painter(this); + qimage_mutex_.lock(); + if (!qimage_.isNull()) { + resizeToFitAspectRatio(); + // TODO: check if full draw is really necessary + //QPaintEvent* paint_event = dynamic_cast(event); + //painter.drawImage(paint_event->rect(), qimage_); + if (!smoothImage_) { + painter.drawImage(contentsRect(), qimage_); + } else { + if (contentsRect().width() == qimage_.width()) { + painter.drawImage(contentsRect(), qimage_); + } else { + QImage image = qimage_.scaled(contentsRect().width(), contentsRect().height(), + Qt::KeepAspectRatio, Qt::SmoothTransformation); + painter.drawImage(contentsRect(), image); + } + } + } else { + // default image with gradient + QLinearGradient gradient(0, 0, frameRect().width(), frameRect().height()); + gradient.setColorAt(0, Qt::white); + gradient.setColorAt(1, Qt::black); + painter.setBrush(gradient); + painter.drawRect(0, 0, frameRect().width() + 1, frameRect().height() + 1); + } + qimage_mutex_.unlock(); } - } else { - // default image with gradient - QLinearGradient gradient(0, 0, frameRect().width(), frameRect().height()); - gradient.setColorAt(0, Qt::white); - gradient.setColorAt(1, Qt::black); - painter.setBrush(gradient); - painter.drawRect(0, 0, frameRect().width() + 1, frameRect().height() + 1); - } - qimage_mutex_.unlock(); -} -int RatioLayoutedFrame::greatestCommonDivisor(int a, int b) -{ - if (b==0) - { - return a; - } - return greatestCommonDivisor(b, a % b); -} + int RatioLayoutedFrame::greatestCommonDivisor(int a, int b) { + if (b == 0) { + return a; + } + return greatestCommonDivisor(b, a % b); + } -void RatioLayoutedFrame::mousePressEvent(QMouseEvent * mouseEvent) -{ - if(mouseEvent->button() == Qt::LeftButton) - { - emit mouseLeft(mouseEvent->x(), mouseEvent->y()); - } - QFrame::mousePressEvent(mouseEvent); -} + void RatioLayoutedFrame::mousePressEvent(QMouseEvent *mouseEvent) { + if (mouseEvent->button() == Qt::LeftButton) { + emit mouseLeft(mouseEvent->x(), mouseEvent->y()); + } + QFrame::mousePressEvent(mouseEvent); + } -void RatioLayoutedFrame::onSmoothImageChanged(bool checked) { - smoothImage_ = checked; -} + void RatioLayoutedFrame::onSmoothImageChanged(bool checked) { + smoothImage_ = checked; + } } From 188bbd1b400f2a64d710966515637bd0bb301f4a Mon Sep 17 00:00:00 2001 From: authaldo <45536968+authaldo@users.noreply.github.com> Date: Thu, 10 Jun 2021 22:04:14 +0200 Subject: [PATCH 3/5] spatzenhirn adaptions: replaced image transport with normal subscription + tab unsubscription --- include/rqt_image_view/image_view.h | 114 +++--- include/rqt_image_view/ratio_layouted_frame.h | 67 ++-- src/rqt_image_view/image_view.cpp | 335 ++++++++++-------- src/rqt_image_view/ratio_layouted_frame.cpp | 31 +- 4 files changed, 298 insertions(+), 249 deletions(-) diff --git a/include/rqt_image_view/image_view.h b/include/rqt_image_view/image_view.h index c67006b..c0b9c5c 100644 --- a/include/rqt_image_view/image_view.h +++ b/include/rqt_image_view/image_view.h @@ -37,20 +37,18 @@ #include -#include -#include - -#include #include +#include +#include #include #include #include #include -#include #include #include +#include #include #include @@ -61,100 +59,102 @@ namespace rqt_image_view { Q_OBJECT - public: + public: + ImageView(); - ImageView(); + virtual void initPlugin(qt_gui_cpp::PluginContext &context); - virtual void initPlugin(qt_gui_cpp::PluginContext &context); + virtual void shutdownPlugin(); - virtual void shutdownPlugin(); + virtual void saveSettings(qt_gui_cpp::Settings &plugin_settings, + qt_gui_cpp::Settings &instance_settings) const; - virtual void - saveSettings(qt_gui_cpp::Settings &plugin_settings, qt_gui_cpp::Settings &instance_settings) const; + virtual void restoreSettings(const qt_gui_cpp::Settings &plugin_settings, + const qt_gui_cpp::Settings &instance_settings); - virtual void - restoreSettings(const qt_gui_cpp::Settings &plugin_settings, const qt_gui_cpp::Settings &instance_settings); - protected slots: + protected: + virtual QSet getTopics(const QSet &message_types); - virtual void updateTopicList(); + virtual void selectTopic(const QString &topic); - protected: + protected slots: - virtual QSet - getTopics(const QSet &message_types, const QSet &message_sub_types, - const QList &transports); + virtual void updateTopicList(); - virtual void selectTopic(const QString &topic); + virtual void onTopicChanged(int index); - protected slots: + virtual void onZoom1(bool checked); - virtual void onTopicChanged(int index); + virtual void onDynamicRange(bool checked); - virtual void onZoom1(bool checked); + virtual void saveImage(); - virtual void onDynamicRange(bool checked); + virtual void updateNumGridlines(); - virtual void saveImage(); + virtual void onMousePublish(bool checked); - virtual void updateNumGridlines(); + virtual void onMouseLeft(int x, int y); - virtual void onMousePublish(bool checked); + virtual void onPubTopicChanged(); - virtual void onMouseLeft(int x, int y); + virtual void onHideToolbarChanged(bool hide); - virtual void onPubTopicChanged(); + virtual void onRotateLeft(); - virtual void onHideToolbarChanged(bool hide); + virtual void onRotateRight(); - virtual void onRotateLeft(); + virtual void onVisibilityChanged(bool visible); - virtual void onRotateRight(); + virtual void subscribe(); - protected: + virtual void unsubscribe(); - virtual void callbackImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg); + protected: + virtual void + callbackImage(sensor_msgs::msg::Image::ConstSharedPtr msg); - virtual void invertPixels(int x, int y); + virtual void invertPixels(int x, int y); - QList getGridIndices(int size) const; + QList getGridIndices(int size) const; - virtual void overlayGrid(); + virtual void overlayGrid(); - Ui::ImageViewWidget ui_; + Ui::ImageViewWidget ui_; - QWidget *widget_; + QWidget *widget_; - image_transport::Subscriber subscriber_; + rclcpp::Subscription::SharedPtr subscriber_; - cv::Mat conversion_mat_; + cv::Mat conversion_mat_; - private: + private: + enum RotateState { + ROTATE_0 = 0, + ROTATE_90 = 1, + ROTATE_180 = 2, + ROTATE_270 = 3, - enum RotateState { - ROTATE_0 = 0, - ROTATE_90 = 1, - ROTATE_180 = 2, - ROTATE_270 = 3, + ROTATE_STATE_COUNT + }; - ROTATE_STATE_COUNT - }; + void syncRotateLabel(); - void syncRotateLabel(); + QString arg_topic_name; - QString arg_topic_name; + rclcpp::Publisher::SharedPtr pub_mouse_left_; - rclcpp::Publisher::SharedPtr pub_mouse_left_; + bool pub_topic_custom_; - bool pub_topic_custom_; + QAction *hide_toolbar_action_; - QAction *hide_toolbar_action_; + int num_gridlines_; - int num_gridlines_; + RotateState rotate_state_; - RotateState rotate_state_; + std::string cur_topic_name_; }; -} +} // namespace rqt_image_view #endif // rqt_image_view__ImageView_H diff --git a/include/rqt_image_view/ratio_layouted_frame.h b/include/rqt_image_view/ratio_layouted_frame.h index 58cd117..0aa26f6 100644 --- a/include/rqt_image_view/ratio_layouted_frame.h +++ b/include/rqt_image_view/ratio_layouted_frame.h @@ -45,69 +45,68 @@ namespace rqt_image_view { /** - * RatioLayoutedFrame is a layout containing a single frame with a fixed aspect ratio. - * The default aspect ratio is 4:3. + * RatioLayoutedFrame is a layout containing a single frame with a fixed aspect + * ratio. The default aspect ratio is 4:3. */ class RatioLayoutedFrame : public QFrame { Q_OBJECT - public: + public: + RatioLayoutedFrame(QWidget *parent, Qt::WindowFlags flags = 0); - RatioLayoutedFrame(QWidget *parent, Qt::WindowFlags flags = 0); + virtual ~RatioLayoutedFrame(); - virtual ~RatioLayoutedFrame(); + const QImage &getImage() const; - const QImage &getImage() const; + QImage getImageCopy() const; - QImage getImageCopy() const; + void setImage(const QImage &image); - void setImage(const QImage &image); + QRect getAspectRatioCorrectPaintArea(); - QRect getAspectRatioCorrectPaintArea(); + void resizeToFitAspectRatio(); - void resizeToFitAspectRatio(); + void setOuterLayout(QHBoxLayout *outer_layout); - void setOuterLayout(QHBoxLayout *outer_layout); + void setInnerFrameMinimumSize(const QSize &size); - void setInnerFrameMinimumSize(const QSize &size); + void setInnerFrameMaximumSize(const QSize &size); - void setInnerFrameMaximumSize(const QSize &size); + void setInnerFrameFixedSize(const QSize &size); - void setInnerFrameFixedSize(const QSize &size); + signals: - signals: + void delayed_update(); - void delayed_update(); + void mouseLeft(int x, int y); - void mouseLeft(int x, int y); + protected + slots: - protected slots: + void onSmoothImageChanged(bool + checked); - void onSmoothImageChanged(bool checked); + protected: + void setAspectRatio(unsigned short width, unsigned short height); - protected: + void paintEvent(QPaintEvent *event); - void setAspectRatio(unsigned short width, unsigned short height); + private: + static int greatestCommonDivisor(int a, int b); - void paintEvent(QPaintEvent *event); + void mousePressEvent(QMouseEvent *mouseEvent); - private: + QHBoxLayout *outer_layout_; - static int greatestCommonDivisor(int a, int b); + QSize aspect_ratio_; - void mousePressEvent(QMouseEvent *mouseEvent); + QImage qimage_; + mutable QMutex qimage_mutex_; - QHBoxLayout *outer_layout_; - - QSize aspect_ratio_; - - QImage qimage_; - mutable QMutex qimage_mutex_; - - bool smoothImage_; + bool smoothImage_; }; -} +} // namespace rqt_image_view #endif // rqt_image_view__RatioLayoutedFrame_H diff --git a/src/rqt_image_view/image_view.cpp b/src/rqt_image_view/image_view.cpp index aa91226..c1a079c 100644 --- a/src/rqt_image_view/image_view.cpp +++ b/src/rqt_image_view/image_view.cpp @@ -38,6 +38,7 @@ #include #include +#include #include #include #include @@ -45,7 +46,8 @@ namespace rqt_image_view { ImageView::ImageView() - : rqt_gui_cpp::Plugin(), widget_(0), num_gridlines_(0), rotate_state_(ROTATE_0) { + : rqt_gui_cpp::Plugin(), widget_(0), num_gridlines_(0), + rotate_state_(ROTATE_0) { setObjectName("ImageView"); } @@ -54,26 +56,33 @@ namespace rqt_image_view { ui_.setupUi(widget_); if (context.serialNumber() > 1) { - widget_->setWindowTitle(widget_->windowTitle() + " (" + QString::number(context.serialNumber()) + ")"); + widget_->setWindowTitle(widget_->windowTitle() + " (" + + QString::number(context.serialNumber()) + ")"); } context.addWidget(widget_); updateTopicList(); ui_.topics_combo_box->setCurrentIndex(ui_.topics_combo_box->findText("")); - connect(ui_.topics_combo_box, SIGNAL(currentIndexChanged(int)), this, SLOT(onTopicChanged(int))); + connect(ui_.topics_combo_box, SIGNAL(currentIndexChanged(int)), this, + SLOT(onTopicChanged(int))); ui_.refresh_topics_push_button->setIcon(QIcon::fromTheme("view-refresh")); - connect(ui_.refresh_topics_push_button, SIGNAL(pressed()), this, SLOT(updateTopicList())); + connect(ui_.refresh_topics_push_button, SIGNAL(pressed()), this, + SLOT(updateTopicList())); ui_.zoom_1_push_button->setIcon(QIcon::fromTheme("zoom-original")); - connect(ui_.zoom_1_push_button, SIGNAL(toggled(bool)), this, SLOT(onZoom1(bool))); + connect(ui_.zoom_1_push_button, SIGNAL(toggled(bool)), this, + SLOT(onZoom1(bool))); - connect(ui_.dynamic_range_check_box, SIGNAL(toggled(bool)), this, SLOT(onDynamicRange(bool))); + connect(ui_.dynamic_range_check_box, SIGNAL(toggled(bool)), this, + SLOT(onDynamicRange(bool))); ui_.save_as_image_push_button->setIcon(QIcon::fromTheme("document-save-as")); - connect(ui_.save_as_image_push_button, SIGNAL(pressed()), this, SLOT(saveImage())); + connect(ui_.save_as_image_push_button, SIGNAL(pressed()), this, + SLOT(saveImage())); - connect(ui_.num_gridlines_spin_box, SIGNAL(valueChanged(int)), this, SLOT(updateNumGridlines())); + connect(ui_.num_gridlines_spin_box, SIGNAL(valueChanged(int)), this, + SLOT(updateNumGridlines())); // set topic name if passed in as argument const QStringList &argv = context.argv(); @@ -85,55 +94,79 @@ namespace rqt_image_view { ui_.image_frame->setOuterLayout(ui_.image_layout); - QRegExp rx( - "([a-zA-Z/][a-zA-Z0-9_/]*)?"); //see http://www.ros.org/wiki/ROS/Concepts#Names.Valid_Names (but also accept an empty field) - ui_.publish_click_location_topic_line_edit->setValidator(new QRegExpValidator(rx, this)); - connect(ui_.publish_click_location_check_box, SIGNAL(toggled(bool)), this, SLOT(onMousePublish(bool))); - connect(ui_.image_frame, SIGNAL(mouseLeft(int, int)), this, SLOT(onMouseLeft(int, int))); - connect(ui_.publish_click_location_topic_line_edit, SIGNAL(editingFinished()), this, SLOT(onPubTopicChanged())); - - connect(ui_.smooth_image_check_box, SIGNAL(toggled(bool)), ui_.image_frame, SLOT(onSmoothImageChanged(bool))); - - connect(ui_.rotate_left_push_button, SIGNAL(clicked(bool)), this, SLOT(onRotateLeft())); - connect(ui_.rotate_right_push_button, SIGNAL(clicked(bool)), this, SLOT(onRotateRight())); + QRegExp rx("([a-zA-Z/][a-zA-Z0-9_/]*)?"); // see + // http://www.ros.org/wiki/ROS/Concepts#Names.Valid_Names + // (but also accept an empty field) + ui_.publish_click_location_topic_line_edit->setValidator( + new QRegExpValidator(rx, this)); + connect(ui_.publish_click_location_check_box, SIGNAL(toggled(bool)), this, + SLOT(onMousePublish(bool))); + connect(ui_.image_frame, SIGNAL(mouseLeft(int, int)), this, + SLOT(onMouseLeft(int, int))); + connect(ui_.publish_click_location_topic_line_edit, SIGNAL(editingFinished()), + this, SLOT(onPubTopicChanged())); + + connect(ui_.smooth_image_check_box, SIGNAL(toggled(bool)), ui_.image_frame, + SLOT(onSmoothImageChanged(bool))); + + connect(ui_.rotate_left_push_button, SIGNAL(clicked(bool)), this, + SLOT(onRotateLeft())); + connect(ui_.rotate_right_push_button, SIGNAL(clicked(bool)), this, + SLOT(onRotateRight())); // Make sure we have enough space for "XXX °" #if QT_VERSION >= QT_VERSION_CHECK(5, 11, 0) // QFontMetrics::width(QChar) is deprecated starting from qt version 5.11.0 // https://doc.qt.io/qt-5/qfontmetrics.html#horizontalAdvance-1 ui_.rotate_label->setMinimumWidth( - ui_.rotate_label->fontMetrics().horizontalAdvance("XXX°") - ); + ui_.rotate_label->fontMetrics().horizontalAdvance("XXX°")); #else ui_.rotate_label->setMinimumWidth( - ui_.rotate_label->fontMetrics().width("XXX°") - ); + ui_.rotate_label->fontMetrics().width("XXX°")); #endif hide_toolbar_action_ = new QAction(tr("Hide toolbar"), this); hide_toolbar_action_->setCheckable(true); ui_.image_frame->addAction(hide_toolbar_action_); - connect(hide_toolbar_action_, SIGNAL(toggled(bool)), this, SLOT(onHideToolbarChanged(bool))); + connect(hide_toolbar_action_, SIGNAL(toggled(bool)), this, + SLOT(onHideToolbarChanged(bool))); + + /* + * This plugin will be placed within a QDockWidget, thus we can get some information on whether + * it is visible or not via the parentWidget function + */ + auto *parent = dynamic_cast(widget_->parentWidget()); + + connect(parent, SIGNAL(visibilityChanged(bool)), this, SLOT(onVisibilityChanged(bool))); } void ImageView::shutdownPlugin() { - subscriber_.shutdown(); + unsubscribe(); pub_mouse_left_.reset(); } - void ImageView::saveSettings(qt_gui_cpp::Settings &plugin_settings, qt_gui_cpp::Settings &instance_settings) const { + void ImageView::saveSettings(qt_gui_cpp::Settings &plugin_settings, + qt_gui_cpp::Settings &instance_settings) const { (void) plugin_settings; QString topic = ui_.topics_combo_box->currentText(); - //qDebug("ImageView::saveSettings() topic '%s'", topic.toStdString().c_str()); + // qDebug("ImageView::saveSettings() topic '%s'", + // topic.toStdString().c_str()); instance_settings.setValue("topic", topic); instance_settings.setValue("zoom1", ui_.zoom_1_push_button->isChecked()); - instance_settings.setValue("dynamic_range", ui_.dynamic_range_check_box->isChecked()); - instance_settings.setValue("max_range", ui_.max_range_double_spin_box->value()); - instance_settings.setValue("publish_click_location", ui_.publish_click_location_check_box->isChecked()); - instance_settings.setValue("mouse_pub_topic", ui_.publish_click_location_topic_line_edit->text()); - instance_settings.setValue("toolbar_hidden", hide_toolbar_action_->isChecked()); - instance_settings.setValue("num_gridlines", ui_.num_gridlines_spin_box->value()); - instance_settings.setValue("smooth_image", ui_.smooth_image_check_box->isChecked()); + instance_settings.setValue("dynamic_range", + ui_.dynamic_range_check_box->isChecked()); + instance_settings.setValue("max_range", + ui_.max_range_double_spin_box->value()); + instance_settings.setValue("publish_click_location", + ui_.publish_click_location_check_box->isChecked()); + instance_settings.setValue( + "mouse_pub_topic", ui_.publish_click_location_topic_line_edit->text()); + instance_settings.setValue("toolbar_hidden", + hide_toolbar_action_->isChecked()); + instance_settings.setValue("num_gridlines", + ui_.num_gridlines_spin_box->value()); + instance_settings.setValue("smooth_image", + ui_.smooth_image_check_box->isChecked()); instance_settings.setValue("rotate", rotate_state_); } @@ -143,13 +176,20 @@ namespace rqt_image_view { bool zoom1_checked = instance_settings.value("zoom1", false).toBool(); ui_.zoom_1_push_button->setChecked(zoom1_checked); - bool dynamic_range_checked = instance_settings.value("dynamic_range", false).toBool(); + bool dynamic_range_checked = + instance_settings.value("dynamic_range", false).toBool(); ui_.dynamic_range_check_box->setChecked(dynamic_range_checked); - double max_range = instance_settings.value("max_range", ui_.max_range_double_spin_box->value()).toDouble(); + double max_range = + instance_settings + .value("max_range", ui_.max_range_double_spin_box->value()) + .toDouble(); ui_.max_range_double_spin_box->setValue(max_range); - num_gridlines_ = instance_settings.value("num_gridlines", ui_.num_gridlines_spin_box->value()).toInt(); + num_gridlines_ = + instance_settings + .value("num_gridlines", ui_.num_gridlines_spin_box->value()) + .toInt(); ui_.num_gridlines_spin_box->setValue(num_gridlines_); QString topic = instance_settings.value("topic", "").toString(); @@ -157,23 +197,28 @@ namespace rqt_image_view { if (!arg_topic_name.isEmpty()) { arg_topic_name = ""; } else { - //qDebug("ImageView::restoreSettings() topic '%s'", topic.toStdString().c_str()); + // qDebug("ImageView::restoreSettings() topic '%s'", + // topic.toStdString().c_str()); selectTopic(topic); } - bool publish_click_location = instance_settings.value("publish_click_location", false).toBool(); + bool publish_click_location = + instance_settings.value("publish_click_location", false).toBool(); ui_.publish_click_location_check_box->setChecked(publish_click_location); QString pub_topic = instance_settings.value("mouse_pub_topic", "").toString(); ui_.publish_click_location_topic_line_edit->setText(pub_topic); - bool toolbar_hidden = instance_settings.value("toolbar_hidden", false).toBool(); + bool toolbar_hidden = + instance_settings.value("toolbar_hidden", false).toBool(); hide_toolbar_action_->setChecked(toolbar_hidden); - bool smooth_image_checked = instance_settings.value("smooth_image", false).toBool(); + bool smooth_image_checked = + instance_settings.value("smooth_image", false).toBool(); ui_.smooth_image_check_box->setChecked(smooth_image_checked); - rotate_state_ = static_cast(instance_settings.value("rotate", 0).toInt()); + rotate_state_ = + static_cast(instance_settings.value("rotate", 0).toInt()); if (rotate_state_ >= ROTATE_STATE_COUNT) rotate_state_ = ROTATE_0; syncRotateLabel(); @@ -183,34 +228,16 @@ namespace rqt_image_view { QSet message_types; message_types.insert("sensor_msgs/Image"); message_types.insert("sensor_msgs/msg/Image"); - QSet message_sub_types; - message_sub_types.insert("sensor_msgs/CompressedImage"); - message_sub_types.insert("sensor_msgs/msg/CompressedImage"); - - // get declared transports - QList transports; - image_transport::ImageTransport it(node_); - std::vector declared = it.getDeclaredTransports(); - for (std::vector::const_iterator it = declared.begin(); it != declared.end(); it++) { - //qDebug("ImageView::updateTopicList() declared transport '%s'", it->c_str()); - QString transport = it->c_str(); - - // strip prefix from transport name - QString prefix = "image_transport/"; - if (transport.startsWith(prefix)) { - transport = transport.mid(prefix.length()); - } - transports.append(transport); - } QString selected = ui_.topics_combo_box->currentText(); // fill combo box - QList topics = getTopics(message_types, message_sub_types, transports).values(); + QList topics = getTopics(message_types).values(); topics.append(""); std::sort(topics.begin(), topics.end()); ui_.topics_combo_box->clear(); - for (QList::const_iterator it = topics.begin(); it != topics.end(); it++) { + for (QList::const_iterator it = topics.begin(); it != topics.end(); + it++) { QString label(*it); label.replace(" ", "/"); ui_.topics_combo_box->addItem(label, QVariant(*it)); @@ -220,45 +247,30 @@ namespace rqt_image_view { selectTopic(selected); } - QSet ImageView::getTopics(const QSet &message_types, const QSet &message_sub_types, - const QList &transports) { + QSet ImageView::getTopics(const QSet &message_types) { std::map> topic_info = node_->get_topic_names_and_types(); QSet all_topics; - for (std::map>::iterator it = topic_info.begin(); + for (std::map>::iterator it = + topic_info.begin(); it != topic_info.end(); ++it) { all_topics.insert(it->first.c_str()); } QSet topics; - for (std::map>::iterator it = topic_info.begin(); + for (std::map>::iterator it = + topic_info.begin(); it != topic_info.end(); ++it) { - for (std::vector::const_iterator msg_type_it = it->second.begin(); + for (std::vector::const_iterator msg_type_it = + it->second.begin(); msg_type_it != it->second.end(); ++msg_type_it) { if (message_types.contains(msg_type_it->c_str())) { QString topic = it->first.c_str(); // add raw topic topics.insert(topic); - //qDebug("ImageView::getTopics() raw topic '%s'", topic.toStdString().c_str()); - - // add transport specific sub-topics - for (QList::const_iterator jt = transports.begin(); jt != transports.end(); jt++) { - if (all_topics.contains(topic + "/" + *jt)) { - QString sub = topic + " " + *jt; - topics.insert(sub); - //qDebug("ImageView::getTopics() transport specific sub-topic '%s'", sub.toStdString().c_str()); - } - } - } - if (message_sub_types.contains(msg_type_it->c_str())) { - QString topic = it->first.c_str(); - int index = topic.lastIndexOf("/"); - if (index != -1) { - topic.replace(index, 1, " "); - topics.insert(topic); - //qDebug("ImageView::getTopics() transport specific sub-topic '%s'", topic.toStdString().c_str()); - } + // qDebug("ImageView::getTopics() raw topic '%s'", + // topic.toStdString().c_str()); } } } @@ -280,26 +292,18 @@ namespace rqt_image_view { void ImageView::onTopicChanged(int index) { conversion_mat_.release(); - subscriber_.shutdown(); + unsubscribe(); // reset image on topic change ui_.image_frame->setImage(QImage()); - QStringList parts = ui_.topics_combo_box->itemData(index).toString().split(" "); + QStringList parts = + ui_.topics_combo_box->itemData(index).toString().split(" "); QString topic = parts.first(); - QString transport = parts.length() == 2 ? parts.last() : "raw"; - if (!topic.isEmpty()) { - image_transport::ImageTransport it(node_); - const image_transport::TransportHints hints(node_.get(), transport.toStdString()); - try { - subscriber_ = it.subscribe(topic.toStdString(), 1, &ImageView::callbackImage, this, &hints); - qDebug("ImageView::onTopicChanged() to topic '%s' with transport '%s'", topic.toStdString().c_str(), - subscriber_.getTransport().c_str()); - } catch (image_transport::TransportLoadException &e) { - QMessageBox::warning(widget_, tr("Loading image transport plugin failed"), e.what()); - } - } + cur_topic_name_ = topic.toStdString(); + + subscribe(); onMousePublish(ui_.publish_click_location_check_box->isChecked()); } @@ -330,8 +334,9 @@ namespace rqt_image_view { // take a snapshot before asking for the filename QImage img = ui_.image_frame->getImageCopy(); - QString file_name = QFileDialog::getSaveFileName(widget_, tr("Save as image"), "image.png", - tr("Image (*.bmp *.jpg *.png *.tiff)")); + QString file_name = + QFileDialog::getSaveFileName(widget_, tr("Save as image"), "image.png", + tr("Image (*.bmp *.jpg *.png *.tiff)")); if (file_name.isEmpty()) { return; } @@ -342,31 +347,37 @@ namespace rqt_image_view { void ImageView::onMousePublish(bool checked) { std::string topicName; if (pub_topic_custom_) { - topicName = ui_.publish_click_location_topic_line_edit->text().toStdString(); + topicName = + ui_.publish_click_location_topic_line_edit->text().toStdString(); } else { - if (!subscriber_.getTopic().empty()) { - topicName = subscriber_.getTopic() + "_mouse_left"; + auto topicStr = std::string(subscriber_->get_topic_name()); + if (!topicStr.empty()) { + topicName = topicStr + "_mouse_left"; } else { topicName = "mouse_left"; } - ui_.publish_click_location_topic_line_edit->setText(QString::fromStdString(topicName)); + ui_.publish_click_location_topic_line_edit->setText( + QString::fromStdString(topicName)); } if (checked) { - pub_mouse_left_ = node_->create_publisher(topicName, 1000); + pub_mouse_left_ = + node_->create_publisher(topicName, 1000); } else { pub_mouse_left_.reset(); } } void ImageView::onMouseLeft(int x, int y) { - if (ui_.publish_click_location_check_box->isChecked() && !ui_.image_frame->getImage().isNull()) { + if (ui_.publish_click_location_check_box->isChecked() && + !ui_.image_frame->getImage().isNull()) { geometry_msgs::msg::Point clickCanvasLocation; // Publish click location in pixel coordinates - clickCanvasLocation.x = round( - (double) x / (double) ui_.image_frame->width() * (double) ui_.image_frame->getImage().width()); - clickCanvasLocation.y = round( - (double) y / (double) ui_.image_frame->height() * (double) ui_.image_frame->getImage().height()); + clickCanvasLocation.x = round((double) x / (double) ui_.image_frame->width() * + (double) ui_.image_frame->getImage().width()); + clickCanvasLocation.y = + round((double) y / (double) ui_.image_frame->height() * + (double) ui_.image_frame->getImage().height()); clickCanvasLocation.z = 0; geometry_msgs::msg::Point clickLocation = clickCanvasLocation; @@ -374,14 +385,18 @@ namespace rqt_image_view { switch (rotate_state_) { case ROTATE_90: clickLocation.x = clickCanvasLocation.y; - clickLocation.y = ui_.image_frame->getImage().width() - clickCanvasLocation.x; + clickLocation.y = + ui_.image_frame->getImage().width() - clickCanvasLocation.x; break; case ROTATE_180: - clickLocation.x = ui_.image_frame->getImage().width() - clickCanvasLocation.x; - clickLocation.y = ui_.image_frame->getImage().height() - clickCanvasLocation.y; + clickLocation.x = + ui_.image_frame->getImage().width() - clickCanvasLocation.x; + clickLocation.y = + ui_.image_frame->getImage().height() - clickCanvasLocation.y; break; case ROTATE_270: - clickLocation.x = ui_.image_frame->getImage().height() - clickCanvasLocation.y; + clickLocation.x = + ui_.image_frame->getImage().height() - clickCanvasLocation.y; clickLocation.y = clickCanvasLocation.x; break; default: @@ -393,7 +408,8 @@ namespace rqt_image_view { } void ImageView::onPubTopicChanged() { - pub_topic_custom_ = !(ui_.publish_click_location_topic_line_edit->text().isEmpty()); + pub_topic_custom_ = + !(ui_.publish_click_location_topic_line_edit->text().isEmpty()); onMousePublish(ui_.publish_click_location_check_box->isChecked()); } @@ -411,7 +427,8 @@ namespace rqt_image_view { } void ImageView::onRotateRight() { - rotate_state_ = static_cast((rotate_state_ + 1) % ROTATE_STATE_COUNT); + rotate_state_ = + static_cast((rotate_state_ + 1) % ROTATE_STATE_COUNT); syncRotateLabel(); } @@ -434,7 +451,8 @@ namespace rqt_image_view { } void ImageView::invertPixels(int x, int y) { - // Could do 255-conversion_mat_.at(cv::Point(x,y))[i], but that doesn't work well on gray + // Could do 255-conversion_mat_.at(cv::Point(x,y))[i], but that + // doesn't work well on gray cv::Vec3b &pixel = conversion_mat_.at(cv::Point(x, y)); if (pixel[0] + pixel[1] + pixel[2] > 3 * 127) pixel = cv::Vec3b(0, 0, 0); @@ -450,14 +468,14 @@ namespace rqt_image_view { // select grid line(s) closest to the center float index; - if (num_gridlines_ % 2) // odd + if (num_gridlines_ % 2) // odd { indices.append(size / 2); // make the center line 2px wide in case of an even resolution - if (size % 2 == 0) // even + if (size % 2 == 0) // even indices.append(size / 2 - 1); index = 1.0f * (size - 1) / 2; - } else // even + } else // even { index = grid_width * (num_gridlines_ / 2); // one grid line before the center @@ -481,7 +499,8 @@ namespace rqt_image_view { void ImageView::overlayGrid() { // vertical gridlines QList columns = getGridIndices(conversion_mat_.cols); - for (QList::const_iterator x = columns.begin(); x != columns.end(); ++x) { + for (QList::const_iterator x = columns.begin(); x != columns.end(); + ++x) { for (int y = 0; y < conversion_mat_.rows; ++y) { invertPixels(*x, y); } @@ -496,18 +515,19 @@ namespace rqt_image_view { } } - void ImageView::callbackImage(const sensor_msgs::msg::Image::ConstSharedPtr &msg) { + void ImageView::callbackImage(sensor_msgs::msg::Image::ConstSharedPtr msg) { try { // First let cv_bridge do its magic - cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::RGB8); + cv_bridge::CvImageConstPtr cv_ptr = + cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::RGB8); conversion_mat_ = cv_ptr->image; if (num_gridlines_ > 0) overlayGrid(); - } - catch (cv_bridge::Exception &e) { + } catch (cv_bridge::Exception &e) { try { - // If we're here, there is no conversion that makes sense, but let's try to imagine a few first + // If we're here, there is no conversion that makes sense, but let's try + // to imagine a few first cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg); if (msg->encoding == "CV_8UC3") { // assuming it is rgb @@ -519,7 +539,8 @@ namespace rqt_image_view { // scale / quantify double min = 0; double max = ui_.max_range_double_spin_box->value(); - if (msg->encoding == "16UC1") max *= 1000; + if (msg->encoding == "16UC1") + max *= 1000; if (ui_.dynamic_range_check_box->isChecked()) { // dynamically adjust range based on min/max in image cv::minMaxLoc(cv_ptr->image, &min, &max); @@ -530,19 +551,20 @@ namespace rqt_image_view { } } cv::Mat img_scaled_8u; - cv::Mat(cv_ptr->image - min).convertTo(img_scaled_8u, CV_8UC1, 255. / (max - min)); + cv::Mat(cv_ptr->image - min) + .convertTo(img_scaled_8u, CV_8UC1, 255. / (max - min)); cv::cvtColor(img_scaled_8u, conversion_mat_, CV_GRAY2RGB); } else { - qWarning("ImageView.callback_image() could not convert image from '%s' to 'rgb8' (%s)", + qWarning("ImageView.callback_image() could not convert image from '%s' " + "to 'rgb8' (%s)", msg->encoding.c_str(), e.what()); ui_.image_frame->setImage(QImage()); return; } - } - catch (cv_bridge::Exception &e) { - qWarning( - "ImageView.callback_image() while trying to convert image from '%s' to 'rgb8' an exception was thrown (%s)", - msg->encoding.c_str(), e.what()); + } catch (cv_bridge::Exception &e) { + qWarning("ImageView.callback_image() while trying to convert image from " + "'%s' to 'rgb8' an exception was thrown (%s)", + msg->encoding.c_str(), e.what()); ui_.image_frame->setImage(QImage()); return; } @@ -572,18 +594,45 @@ namespace rqt_image_view { break; } - // image must be copied since it uses the conversion_mat_ for storage which is asynchronously overwritten in the next callback invocation - QImage image(conversion_mat_.data, conversion_mat_.cols, conversion_mat_.rows, conversion_mat_.step[0], - QImage::Format_RGB888); + // image must be copied since it uses the conversion_mat_ for storage which is + // asynchronously overwritten in the next callback invocation + QImage image(conversion_mat_.data, conversion_mat_.cols, conversion_mat_.rows, + conversion_mat_.step[0], QImage::Format_RGB888); ui_.image_frame->setImage(image); if (!ui_.zoom_1_push_button->isEnabled()) { ui_.zoom_1_push_button->setEnabled(true); } - // Need to update the zoom 1 every new image in case the image aspect ratio changed, - // though could check and see if the aspect ratio changed or not. + // Need to update the zoom 1 every new image in case the image aspect ratio + // changed, though could check and see if the aspect ratio changed or not. onZoom1(ui_.zoom_1_push_button->isChecked()); } -} + + void ImageView::onVisibilityChanged(bool visible) { + if (!visible) { + unsubscribe(); + } else if (subscriber_.get() == nullptr) { + subscribe(); + } + } + + void ImageView::subscribe() { + if (!cur_topic_name_.empty()) { + try { + subscriber_ = node_->create_subscription( + cur_topic_name_, rclcpp::QoS(1).best_effort(), + std::bind(&ImageView::callbackImage, this, std::placeholders::_1)); + qDebug("ImageView::onTopicChanged() to topic '%s'", cur_topic_name_.c_str()); + } catch (...) { + QMessageBox::warning(widget_, tr("Loading image transport plugin failed"), "Subscription failed!"); + } + } + } + + void ImageView::unsubscribe() { + subscriber_ = nullptr; + ui_.image_frame->setImage(QImage()); + } +} // namespace rqt_image_view PLUGINLIB_EXPORT_CLASS(rqt_image_view::ImageView, rqt_gui_cpp::Plugin) diff --git a/src/rqt_image_view/ratio_layouted_frame.cpp b/src/rqt_image_view/ratio_layouted_frame.cpp index 7292acd..95c2f1f 100644 --- a/src/rqt_image_view/ratio_layouted_frame.cpp +++ b/src/rqt_image_view/ratio_layouted_frame.cpp @@ -32,8 +32,8 @@ #include -#include #include +#include namespace rqt_image_view { @@ -41,15 +41,13 @@ namespace rqt_image_view { : QFrame(), outer_layout_(NULL), aspect_ratio_(4, 3), smoothImage_(false) { (void) parent; (void) flags; - connect(this, SIGNAL(delayed_update()), this, SLOT(update()), Qt::QueuedConnection); + connect(this, SIGNAL(delayed_update()), this, SLOT(update()), + Qt::QueuedConnection); } - RatioLayoutedFrame::~RatioLayoutedFrame() { - } + RatioLayoutedFrame::~RatioLayoutedFrame() {} - const QImage &RatioLayoutedFrame::getImage() const { - return qimage_; - } + const QImage &RatioLayoutedFrame::getImage() const { return qimage_; } QImage RatioLayoutedFrame::getImageCopy() const { QImage img; @@ -59,7 +57,7 @@ namespace rqt_image_view { return img; } - void RatioLayoutedFrame::setImage(const QImage &image)//, QMutex* image_mutex) + void RatioLayoutedFrame::setImage(const QImage &image) //, QMutex* image_mutex) { qimage_mutex_.lock(); qimage_ = image.copy(); @@ -87,7 +85,8 @@ namespace rqt_image_view { } double layout_ar = width / height; - const double image_ar = double(aspect_ratio_.width()) / double(aspect_ratio_.height()); + const double image_ar = + double(aspect_ratio_.width()) / double(aspect_ratio_.height()); if (layout_ar > image_ar) { // too large width width = height * image_ar; @@ -128,7 +127,8 @@ namespace rqt_image_view { setInnerFrameMaximumSize(size); } - void RatioLayoutedFrame::setAspectRatio(unsigned short width, unsigned short height) { + void RatioLayoutedFrame::setAspectRatio(unsigned short width, + unsigned short height) { int divisor = greatestCommonDivisor(width, height); if (divisor != 0) { aspect_ratio_.setWidth(width / divisor); @@ -143,16 +143,17 @@ namespace rqt_image_view { if (!qimage_.isNull()) { resizeToFitAspectRatio(); // TODO: check if full draw is really necessary - //QPaintEvent* paint_event = dynamic_cast(event); - //painter.drawImage(paint_event->rect(), qimage_); + // QPaintEvent* paint_event = dynamic_cast(event); + // painter.drawImage(paint_event->rect(), qimage_); if (!smoothImage_) { painter.drawImage(contentsRect(), qimage_); } else { if (contentsRect().width() == qimage_.width()) { painter.drawImage(contentsRect(), qimage_); } else { - QImage image = qimage_.scaled(contentsRect().width(), contentsRect().height(), - Qt::KeepAspectRatio, Qt::SmoothTransformation); + QImage image = + qimage_.scaled(contentsRect().width(), contentsRect().height(), + Qt::KeepAspectRatio, Qt::SmoothTransformation); painter.drawImage(contentsRect(), image); } } @@ -185,4 +186,4 @@ namespace rqt_image_view { smoothImage_ = checked; } -} +} // namespace rqt_image_view From 0f6f68e2d972f5e87d48d75668c3c1d4ba42aadc Mon Sep 17 00:00:00 2001 From: Dominik Date: Tue, 20 Jul 2021 22:17:44 +0200 Subject: [PATCH 4/5] revert changes in ratio layouted frame --- include/rqt_image_view/ratio_layouted_frame.h | 75 ++-- src/rqt_image_view/ratio_layouted_frame.cpp | 324 ++++++++++-------- 2 files changed, 213 insertions(+), 186 deletions(-) diff --git a/include/rqt_image_view/ratio_layouted_frame.h b/include/rqt_image_view/ratio_layouted_frame.h index 0aa26f6..4dc3761 100644 --- a/include/rqt_image_view/ratio_layouted_frame.h +++ b/include/rqt_image_view/ratio_layouted_frame.h @@ -45,68 +45,71 @@ namespace rqt_image_view { /** - * RatioLayoutedFrame is a layout containing a single frame with a fixed aspect - * ratio. The default aspect ratio is 4:3. + * RatioLayoutedFrame is a layout containing a single frame with a fixed aspect ratio. + * The default aspect ratio is 4:3. */ - class RatioLayoutedFrame : public QFrame { +class RatioLayoutedFrame + : public QFrame +{ - Q_OBJECT + Q_OBJECT - public: - RatioLayoutedFrame(QWidget *parent, Qt::WindowFlags flags = 0); +public: - virtual ~RatioLayoutedFrame(); + RatioLayoutedFrame(QWidget* parent, Qt::WindowFlags flags = 0); - const QImage &getImage() const; + virtual ~RatioLayoutedFrame(); - QImage getImageCopy() const; + const QImage& getImage() const; - void setImage(const QImage &image); + QImage getImageCopy() const; - QRect getAspectRatioCorrectPaintArea(); + void setImage(const QImage& image); - void resizeToFitAspectRatio(); + QRect getAspectRatioCorrectPaintArea(); - void setOuterLayout(QHBoxLayout *outer_layout); + void resizeToFitAspectRatio(); - void setInnerFrameMinimumSize(const QSize &size); + void setOuterLayout(QHBoxLayout* outer_layout); - void setInnerFrameMaximumSize(const QSize &size); + void setInnerFrameMinimumSize(const QSize& size); - void setInnerFrameFixedSize(const QSize &size); + void setInnerFrameMaximumSize(const QSize& size); - signals: + void setInnerFrameFixedSize(const QSize& size); - void delayed_update(); +signals: - void mouseLeft(int x, int y); + void delayed_update(); - protected - slots: + void mouseLeft(int x, int y); - void onSmoothImageChanged(bool - checked); +protected slots: - protected: - void setAspectRatio(unsigned short width, unsigned short height); + void onSmoothImageChanged(bool checked); - void paintEvent(QPaintEvent *event); +protected: - private: - static int greatestCommonDivisor(int a, int b); + void setAspectRatio(unsigned short width, unsigned short height); - void mousePressEvent(QMouseEvent *mouseEvent); + void paintEvent(QPaintEvent* event); - QHBoxLayout *outer_layout_; +private: - QSize aspect_ratio_; + static int greatestCommonDivisor(int a, int b); - QImage qimage_; - mutable QMutex qimage_mutex_; + void mousePressEvent(QMouseEvent * mouseEvent); - bool smoothImage_; - }; + QHBoxLayout* outer_layout_; -} // namespace rqt_image_view + QSize aspect_ratio_; + + QImage qimage_; + mutable QMutex qimage_mutex_; + + bool smoothImage_; +}; + +} #endif // rqt_image_view__RatioLayoutedFrame_H diff --git a/src/rqt_image_view/ratio_layouted_frame.cpp b/src/rqt_image_view/ratio_layouted_frame.cpp index 95c2f1f..4bdda81 100644 --- a/src/rqt_image_view/ratio_layouted_frame.cpp +++ b/src/rqt_image_view/ratio_layouted_frame.cpp @@ -32,158 +32,182 @@ #include -#include #include +#include namespace rqt_image_view { - RatioLayoutedFrame::RatioLayoutedFrame(QWidget *parent, Qt::WindowFlags flags) - : QFrame(), outer_layout_(NULL), aspect_ratio_(4, 3), smoothImage_(false) { - (void) parent; - (void) flags; - connect(this, SIGNAL(delayed_update()), this, SLOT(update()), - Qt::QueuedConnection); - } - - RatioLayoutedFrame::~RatioLayoutedFrame() {} - - const QImage &RatioLayoutedFrame::getImage() const { return qimage_; } - - QImage RatioLayoutedFrame::getImageCopy() const { - QImage img; - qimage_mutex_.lock(); - img = qimage_.copy(); - qimage_mutex_.unlock(); - return img; - } - - void RatioLayoutedFrame::setImage(const QImage &image) //, QMutex* image_mutex) - { - qimage_mutex_.lock(); - qimage_ = image.copy(); - setAspectRatio(qimage_.width(), qimage_.height()); - qimage_mutex_.unlock(); - emit delayed_update(); +RatioLayoutedFrame::RatioLayoutedFrame(QWidget* parent, Qt::WindowFlags flags) + : QFrame() + , outer_layout_(NULL) + , aspect_ratio_(4, 3) + , smoothImage_(false) +{ + (void)parent; + (void)flags; + connect(this, SIGNAL(delayed_update()), this, SLOT(update()), Qt::QueuedConnection); +} + +RatioLayoutedFrame::~RatioLayoutedFrame() +{ +} + +const QImage& RatioLayoutedFrame::getImage() const +{ + return qimage_; +} + +QImage RatioLayoutedFrame::getImageCopy() const +{ + QImage img; + qimage_mutex_.lock(); + img = qimage_.copy(); + qimage_mutex_.unlock(); + return img; +} + +void RatioLayoutedFrame::setImage(const QImage& image)//, QMutex* image_mutex) +{ + qimage_mutex_.lock(); + qimage_ = image.copy(); + setAspectRatio(qimage_.width(), qimage_.height()); + qimage_mutex_.unlock(); + emit delayed_update(); +} + +void RatioLayoutedFrame::resizeToFitAspectRatio() +{ + QRect rect = contentsRect(); + + // reduce longer edge to aspect ration + double width; + double height; + + if (outer_layout_) + { + width = outer_layout_->contentsRect().width(); + height = outer_layout_->contentsRect().height(); + } + else + { + // if outer layout isn't available, this will use the old + // width and height, but this can shrink the display image if the + // aspect ratio changes. + width = rect.width(); + height = rect.height(); + } + + double layout_ar = width / height; + const double image_ar = double(aspect_ratio_.width()) / double(aspect_ratio_.height()); + if (layout_ar > image_ar) + { + // too large width + width = height * image_ar; + } + else + { + // too large height + height = width / image_ar; + } + rect.setWidth(int(width + 0.5)); + rect.setHeight(int(height + 0.5)); + + // resize taking the border line into account + int border = lineWidth(); + resize(rect.width() + 2 * border, rect.height() + 2 * border); +} + +void RatioLayoutedFrame::setOuterLayout(QHBoxLayout* outer_layout) +{ + outer_layout_ = outer_layout; +} + +void RatioLayoutedFrame::setInnerFrameMinimumSize(const QSize& size) +{ + int border = lineWidth(); + QSize new_size = size; + new_size += QSize(2 * border, 2 * border); + setMinimumSize(new_size); + emit delayed_update(); +} + +void RatioLayoutedFrame::setInnerFrameMaximumSize(const QSize& size) +{ + int border = lineWidth(); + QSize new_size = size; + new_size += QSize(2 * border, 2 * border); + setMaximumSize(new_size); + emit delayed_update(); +} + +void RatioLayoutedFrame::setInnerFrameFixedSize(const QSize& size) +{ + setInnerFrameMinimumSize(size); + setInnerFrameMaximumSize(size); +} + +void RatioLayoutedFrame::setAspectRatio(unsigned short width, unsigned short height) +{ + int divisor = greatestCommonDivisor(width, height); + if (divisor != 0) { + aspect_ratio_.setWidth(width / divisor); + aspect_ratio_.setHeight(height / divisor); + } +} + +void RatioLayoutedFrame::paintEvent(QPaintEvent* event) +{ + (void)event; + QPainter painter(this); + qimage_mutex_.lock(); + if (!qimage_.isNull()) + { + resizeToFitAspectRatio(); + // TODO: check if full draw is really necessary + //QPaintEvent* paint_event = dynamic_cast(event); + //painter.drawImage(paint_event->rect(), qimage_); + if (!smoothImage_) { + painter.drawImage(contentsRect(), qimage_); + } else { + if (contentsRect().width() == qimage_.width()) { + painter.drawImage(contentsRect(), qimage_); + } else { + QImage image = qimage_.scaled(contentsRect().width(), contentsRect().height(), + Qt::KeepAspectRatio, Qt::SmoothTransformation); + painter.drawImage(contentsRect(), image); + } } - - void RatioLayoutedFrame::resizeToFitAspectRatio() { - QRect rect = contentsRect(); - - // reduce longer edge to aspect ration - double width; - double height; - - if (outer_layout_) { - width = outer_layout_->contentsRect().width(); - height = outer_layout_->contentsRect().height(); - } else { - // if outer layout isn't available, this will use the old - // width and height, but this can shrink the display image if the - // aspect ratio changes. - width = rect.width(); - height = rect.height(); - } - - double layout_ar = width / height; - const double image_ar = - double(aspect_ratio_.width()) / double(aspect_ratio_.height()); - if (layout_ar > image_ar) { - // too large width - width = height * image_ar; - } else { - // too large height - height = width / image_ar; - } - rect.setWidth(int(width + 0.5)); - rect.setHeight(int(height + 0.5)); - - // resize taking the border line into account - int border = lineWidth(); - resize(rect.width() + 2 * border, rect.height() + 2 * border); - } - - void RatioLayoutedFrame::setOuterLayout(QHBoxLayout *outer_layout) { - outer_layout_ = outer_layout; - } - - void RatioLayoutedFrame::setInnerFrameMinimumSize(const QSize &size) { - int border = lineWidth(); - QSize new_size = size; - new_size += QSize(2 * border, 2 * border); - setMinimumSize(new_size); - emit delayed_update(); - } - - void RatioLayoutedFrame::setInnerFrameMaximumSize(const QSize &size) { - int border = lineWidth(); - QSize new_size = size; - new_size += QSize(2 * border, 2 * border); - setMaximumSize(new_size); - emit delayed_update(); - } - - void RatioLayoutedFrame::setInnerFrameFixedSize(const QSize &size) { - setInnerFrameMinimumSize(size); - setInnerFrameMaximumSize(size); - } - - void RatioLayoutedFrame::setAspectRatio(unsigned short width, - unsigned short height) { - int divisor = greatestCommonDivisor(width, height); - if (divisor != 0) { - aspect_ratio_.setWidth(width / divisor); - aspect_ratio_.setHeight(height / divisor); - } - } - - void RatioLayoutedFrame::paintEvent(QPaintEvent *event) { - (void) event; - QPainter painter(this); - qimage_mutex_.lock(); - if (!qimage_.isNull()) { - resizeToFitAspectRatio(); - // TODO: check if full draw is really necessary - // QPaintEvent* paint_event = dynamic_cast(event); - // painter.drawImage(paint_event->rect(), qimage_); - if (!smoothImage_) { - painter.drawImage(contentsRect(), qimage_); - } else { - if (contentsRect().width() == qimage_.width()) { - painter.drawImage(contentsRect(), qimage_); - } else { - QImage image = - qimage_.scaled(contentsRect().width(), contentsRect().height(), - Qt::KeepAspectRatio, Qt::SmoothTransformation); - painter.drawImage(contentsRect(), image); - } - } - } else { - // default image with gradient - QLinearGradient gradient(0, 0, frameRect().width(), frameRect().height()); - gradient.setColorAt(0, Qt::white); - gradient.setColorAt(1, Qt::black); - painter.setBrush(gradient); - painter.drawRect(0, 0, frameRect().width() + 1, frameRect().height() + 1); - } - qimage_mutex_.unlock(); - } - - int RatioLayoutedFrame::greatestCommonDivisor(int a, int b) { - if (b == 0) { - return a; - } - return greatestCommonDivisor(b, a % b); - } - - void RatioLayoutedFrame::mousePressEvent(QMouseEvent *mouseEvent) { - if (mouseEvent->button() == Qt::LeftButton) { - emit mouseLeft(mouseEvent->x(), mouseEvent->y()); - } - QFrame::mousePressEvent(mouseEvent); - } - - void RatioLayoutedFrame::onSmoothImageChanged(bool checked) { - smoothImage_ = checked; - } - -} // namespace rqt_image_view + } else { + // default image with gradient + QLinearGradient gradient(0, 0, frameRect().width(), frameRect().height()); + gradient.setColorAt(0, Qt::white); + gradient.setColorAt(1, Qt::black); + painter.setBrush(gradient); + painter.drawRect(0, 0, frameRect().width() + 1, frameRect().height() + 1); + } + qimage_mutex_.unlock(); +} + +int RatioLayoutedFrame::greatestCommonDivisor(int a, int b) +{ + if (b==0) + { + return a; + } + return greatestCommonDivisor(b, a % b); +} + +void RatioLayoutedFrame::mousePressEvent(QMouseEvent * mouseEvent) +{ + if(mouseEvent->button() == Qt::LeftButton) + { + emit mouseLeft(mouseEvent->x(), mouseEvent->y()); + } + QFrame::mousePressEvent(mouseEvent); +} + +void RatioLayoutedFrame::onSmoothImageChanged(bool checked) { + smoothImage_ = checked; +} + +} From d0ec59cd7f5840e7f1f6f9497dcd4b17d7f8d58c Mon Sep 17 00:00:00 2001 From: Dominik Date: Tue, 20 Jul 2021 22:32:29 +0200 Subject: [PATCH 5/5] adaptions based on ottojo's comments --- src/rqt_image_view/image_view.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/rqt_image_view/image_view.cpp b/src/rqt_image_view/image_view.cpp index c1a079c..dd04d24 100644 --- a/src/rqt_image_view/image_view.cpp +++ b/src/rqt_image_view/image_view.cpp @@ -132,8 +132,18 @@ namespace rqt_image_view { SLOT(onHideToolbarChanged(bool))); /* - * This plugin will be placed within a QDockWidget, thus we can get some information on whether - * it is visible or not via the parentWidget function + * Note: + * The following visibility check is used to improve the efficiency of the plugin. + * By default, the plugin will always subscribe to the selected image (even if it is hidden by other + * image views). Thus, a rqt perspective having a plugin for each image (even the debug ones) will always cause + * the subscription and hence the calculation of all images. This is extremely costly, mainly because of the + * debug images. + * + * In order to avoid these costs and still provide the option to easily switch between the images (without + * having to create a perspective for each image) the plugin has been modified to subscribe only while the + * view is rendered. + * Hereby, we exploit that the plugin is placed within a QDockWidget, which allows us to check whether it is + * visible via the parentWidget function. */ auto *parent = dynamic_cast(widget_->parentWidget()); @@ -624,7 +634,7 @@ namespace rqt_image_view { std::bind(&ImageView::callbackImage, this, std::placeholders::_1)); qDebug("ImageView::onTopicChanged() to topic '%s'", cur_topic_name_.c_str()); } catch (...) { - QMessageBox::warning(widget_, tr("Loading image transport plugin failed"), "Subscription failed!"); + QMessageBox::warning(widget_, tr("Creating the subscription has failed!"), "Subscription failed!"); } } }