From f799beb4fbf86ad8ee1e61e684a4f192585c9186 Mon Sep 17 00:00:00 2001 From: Miguel Oliveira Date: Sun, 10 Apr 2022 19:32:49 +0100 Subject: [PATCH 01/13] Mouse click functionality for ImafeDisplay. --- src/mouse_watcher.cpp | 103 ++++++++++++++++++++++++++++++++++++++++++ src/mouse_watcher.h | 50 ++++++++++++++++++++ 2 files changed, 153 insertions(+) create mode 100644 src/mouse_watcher.cpp create mode 100644 src/mouse_watcher.h diff --git a/src/mouse_watcher.cpp b/src/mouse_watcher.cpp new file mode 100644 index 0000000000..417a5a37c2 --- /dev/null +++ b/src/mouse_watcher.cpp @@ -0,0 +1,103 @@ +#include + +using namespace std; + +namespace atom_rviz +{ + +MouseWatcher::MouseWatcher(QWidget * parent) : QObject(parent) { + has_dimensions = false; + node_handle = new ros::NodeHandle(); + mouse_event_pub = new ros::Publisher(); + } + + +bool MouseWatcher::eventFilter(QObject * obj, QEvent * event) + { + if (event->type() == QEvent::MouseButtonPress) + { + QMouseEvent * me = static_cast(event); + QPointF windowPos = me->windowPos(); + printf("User clicked mouse at %f,%f\n", windowPos.x(), windowPos.y()); + cout << "img_width = " << img_width << endl; + cout << "img_height = " << img_height << endl; + cout << "win_width = " << win_width << endl; + cout << "win_height = " << win_height << endl; + + if (img_width != 0 && img_height != 0 && win_width != 0 && win_height != 0) + { + float img_aspect = float(img_width) / float(img_height); + float win_aspect = float(win_width) / float(win_height); + cout << "img_aspect = " << img_aspect << endl; + cout << "win_aspect = " << win_aspect << endl; + + int pix_x=-1; + int pix_y=-1; + if (img_aspect > win_aspect) // case where the window is taller than the image + { + cout << "black bars on top!" << endl; + pix_x = int(float(windowPos.x()) / float(win_width) * float(img_width)); + + int resized_img_height = int(float(win_width) / float(img_width) * float(img_height)); + int bias = int( ( float(win_height) - float(resized_img_height) )/2.0); + pix_y = (float(windowPos.y()) - bias) / float(resized_img_height) * float(img_height); + cout << "pix_x = " << pix_x << endl; + cout << "resized_img_height = " << resized_img_height << endl; + cout << "bias = " << bias << endl; + cout << "pix_y = " << pix_y << endl; + } + else // case where the window is wider than the image + { + cout << "black bars on side!" << endl; + pix_y = int(float(windowPos.y()) / float(win_height) * float(img_height)); + + int resized_img_width = int(float(win_height) / float(img_height) * float(img_width)); + int bias = int( ( float(win_width) - float(resized_img_width) )/2.0); + pix_x = (float(windowPos.x()) - bias) / float(resized_img_width) * float(img_width); + cout << "pix_x = " << pix_x << endl; + cout << "resized_img_width = " << resized_img_width << endl; + cout << "bias = " << bias << endl; + cout << "pix_y = " << pix_y << endl; + } + + // Check if clicked point is inside the image, publish if so + if (pix_x > 0 && pix_x < img_width && pix_y > 0 && pix_y < img_height) + { + cout << "You clicked inside the image. I should publish ..." << endl; + geometry_msgs::PointStamped point_msgs; + point_msgs.header.stamp = ros::Time::now(); + point_msgs.point.x = pix_x; + point_msgs.point.y = pix_y; + mouse_event_pub->publish(point_msgs); + } + } + } + return QObject::eventFilter(obj, event); + } + + void MouseWatcher::setDimensions(int _img_width, int _img_height, int _win_width, int _win_height) + { + img_width = _img_width; + img_height = _img_height; + win_width = _win_width; + win_height = _win_height; + has_dimensions = true; + } + + void MouseWatcher::setTopic(string image_topic) + { + string tmp_topic = image_topic + "/click"; // build the click full topic name based on the image topic + + if (tmp_topic.compare(image_click_topic) != 0) // if topic changed, reconfigure the publisher + { + image_click_topic = tmp_topic; // set new topic + cout << "Changed pub topic, reconfiguring to publish on topic " << image_click_topic << endl; + + mouse_event_pub->shutdown(); // shutdown current publisher + delete mouse_event_pub; // delete the mem allocation (to avoid mem leaks) + // reconfigure new publisher + mouse_event_pub = new ros::Publisher(node_handle->advertise(image_click_topic, 1000)); + } + } + +} // atom_rviz diff --git a/src/mouse_watcher.h b/src/mouse_watcher.h new file mode 100644 index 0000000000..e15ec556d1 --- /dev/null +++ b/src/mouse_watcher.h @@ -0,0 +1,50 @@ +#ifndef MOUSE_WATCHER_H +#define MOUSE_WATCHER_H + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include "../../../../../../../usr/include/x86_64-linux-gnu/qt5/QtCore/QObject" +#include "../../../../../../../usr/include/OGRE/OgreMaterial.h" +#include "../../../../../../../usr/include/OGRE/OgreRenderTargetListener.h" +#include "../../../../../../../usr/include/OGRE/OgreSharedPtr.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/image/image_display_base.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/image/ros_image_texture.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/render_panel.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/properties/bool_property.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/properties/float_property.h" +#include "../../../../../../../opt/ros/noetic/include/rviz/properties/int_property.h" +#endif + +#include +#include + +#include + +#include "ros/ros.h" +#include "geometry_msgs/PointStamped.h" +#include "std_msgs/String.h" + +using namespace std; + +namespace atom_rviz +{ +class MouseWatcher : public QObject +{ +public: + MouseWatcher(QWidget * parent); + virtual bool eventFilter(QObject * obj, QEvent * event); + void setDimensions(int _img_width, int _img_height, int _win_width, int _win_height); + void setTopic(string image_topic); + +private: + string image_click_topic; + bool has_dimensions; + int img_width; + int img_height; + int win_width; + int win_height; + ros::Publisher* mouse_event_pub; + ros::NodeHandle* node_handle; +}; + +} // namespace rviz +#endif From 8648d55afa3a842fa3cb7f45f7088b7682b6ada0 Mon Sep 17 00:00:00 2001 From: Miguel Oliveira Date: Sun, 10 Apr 2022 19:38:41 +0100 Subject: [PATCH 02/13] MouseClick functionality ... missing files. --- src/mouse_watcher.cpp | 103 ------------------ src/mouse_watcher.h | 50 --------- src/rviz/CMakeLists.txt | 1 + src/rviz/default_plugin/image_display.cpp | 23 ++++ src/rviz/default_plugin/image_display.h | 5 + src/rviz/image/mouse_click.cpp | 127 ++++++++++++++++++++++ src/rviz/image/mouse_click.h | 61 +++++++++++ 7 files changed, 217 insertions(+), 153 deletions(-) delete mode 100644 src/mouse_watcher.cpp delete mode 100644 src/mouse_watcher.h create mode 100644 src/rviz/image/mouse_click.cpp create mode 100644 src/rviz/image/mouse_click.h diff --git a/src/mouse_watcher.cpp b/src/mouse_watcher.cpp deleted file mode 100644 index 417a5a37c2..0000000000 --- a/src/mouse_watcher.cpp +++ /dev/null @@ -1,103 +0,0 @@ -#include - -using namespace std; - -namespace atom_rviz -{ - -MouseWatcher::MouseWatcher(QWidget * parent) : QObject(parent) { - has_dimensions = false; - node_handle = new ros::NodeHandle(); - mouse_event_pub = new ros::Publisher(); - } - - -bool MouseWatcher::eventFilter(QObject * obj, QEvent * event) - { - if (event->type() == QEvent::MouseButtonPress) - { - QMouseEvent * me = static_cast(event); - QPointF windowPos = me->windowPos(); - printf("User clicked mouse at %f,%f\n", windowPos.x(), windowPos.y()); - cout << "img_width = " << img_width << endl; - cout << "img_height = " << img_height << endl; - cout << "win_width = " << win_width << endl; - cout << "win_height = " << win_height << endl; - - if (img_width != 0 && img_height != 0 && win_width != 0 && win_height != 0) - { - float img_aspect = float(img_width) / float(img_height); - float win_aspect = float(win_width) / float(win_height); - cout << "img_aspect = " << img_aspect << endl; - cout << "win_aspect = " << win_aspect << endl; - - int pix_x=-1; - int pix_y=-1; - if (img_aspect > win_aspect) // case where the window is taller than the image - { - cout << "black bars on top!" << endl; - pix_x = int(float(windowPos.x()) / float(win_width) * float(img_width)); - - int resized_img_height = int(float(win_width) / float(img_width) * float(img_height)); - int bias = int( ( float(win_height) - float(resized_img_height) )/2.0); - pix_y = (float(windowPos.y()) - bias) / float(resized_img_height) * float(img_height); - cout << "pix_x = " << pix_x << endl; - cout << "resized_img_height = " << resized_img_height << endl; - cout << "bias = " << bias << endl; - cout << "pix_y = " << pix_y << endl; - } - else // case where the window is wider than the image - { - cout << "black bars on side!" << endl; - pix_y = int(float(windowPos.y()) / float(win_height) * float(img_height)); - - int resized_img_width = int(float(win_height) / float(img_height) * float(img_width)); - int bias = int( ( float(win_width) - float(resized_img_width) )/2.0); - pix_x = (float(windowPos.x()) - bias) / float(resized_img_width) * float(img_width); - cout << "pix_x = " << pix_x << endl; - cout << "resized_img_width = " << resized_img_width << endl; - cout << "bias = " << bias << endl; - cout << "pix_y = " << pix_y << endl; - } - - // Check if clicked point is inside the image, publish if so - if (pix_x > 0 && pix_x < img_width && pix_y > 0 && pix_y < img_height) - { - cout << "You clicked inside the image. I should publish ..." << endl; - geometry_msgs::PointStamped point_msgs; - point_msgs.header.stamp = ros::Time::now(); - point_msgs.point.x = pix_x; - point_msgs.point.y = pix_y; - mouse_event_pub->publish(point_msgs); - } - } - } - return QObject::eventFilter(obj, event); - } - - void MouseWatcher::setDimensions(int _img_width, int _img_height, int _win_width, int _win_height) - { - img_width = _img_width; - img_height = _img_height; - win_width = _win_width; - win_height = _win_height; - has_dimensions = true; - } - - void MouseWatcher::setTopic(string image_topic) - { - string tmp_topic = image_topic + "/click"; // build the click full topic name based on the image topic - - if (tmp_topic.compare(image_click_topic) != 0) // if topic changed, reconfigure the publisher - { - image_click_topic = tmp_topic; // set new topic - cout << "Changed pub topic, reconfiguring to publish on topic " << image_click_topic << endl; - - mouse_event_pub->shutdown(); // shutdown current publisher - delete mouse_event_pub; // delete the mem allocation (to avoid mem leaks) - // reconfigure new publisher - mouse_event_pub = new ros::Publisher(node_handle->advertise(image_click_topic, 1000)); - } - } - -} // atom_rviz diff --git a/src/mouse_watcher.h b/src/mouse_watcher.h deleted file mode 100644 index e15ec556d1..0000000000 --- a/src/mouse_watcher.h +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef MOUSE_WATCHER_H -#define MOUSE_WATCHER_H - -#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 -#include "../../../../../../../usr/include/x86_64-linux-gnu/qt5/QtCore/QObject" -#include "../../../../../../../usr/include/OGRE/OgreMaterial.h" -#include "../../../../../../../usr/include/OGRE/OgreRenderTargetListener.h" -#include "../../../../../../../usr/include/OGRE/OgreSharedPtr.h" -#include "../../../../../../../opt/ros/noetic/include/rviz/image/image_display_base.h" -#include "../../../../../../../opt/ros/noetic/include/rviz/image/ros_image_texture.h" -#include "../../../../../../../opt/ros/noetic/include/rviz/render_panel.h" -#include "../../../../../../../opt/ros/noetic/include/rviz/properties/bool_property.h" -#include "../../../../../../../opt/ros/noetic/include/rviz/properties/float_property.h" -#include "../../../../../../../opt/ros/noetic/include/rviz/properties/int_property.h" -#endif - -#include -#include - -#include - -#include "ros/ros.h" -#include "geometry_msgs/PointStamped.h" -#include "std_msgs/String.h" - -using namespace std; - -namespace atom_rviz -{ -class MouseWatcher : public QObject -{ -public: - MouseWatcher(QWidget * parent); - virtual bool eventFilter(QObject * obj, QEvent * event); - void setDimensions(int _img_width, int _img_height, int _win_width, int _win_height); - void setTopic(string image_topic); - -private: - string image_click_topic; - bool has_dimensions; - int img_width; - int img_height; - int win_width; - int win_height; - ros::Publisher* mouse_event_pub; - ros::NodeHandle* node_handle; -}; - -} // namespace rviz -#endif diff --git a/src/rviz/CMakeLists.txt b/src/rviz/CMakeLists.txt index e131dd5e11..cc29d58a03 100644 --- a/src/rviz/CMakeLists.txt +++ b/src/rviz/CMakeLists.txt @@ -40,6 +40,7 @@ add_library(${PROJECT_NAME} help_panel.cpp image/ros_image_texture.cpp image/image_display_base.cpp + image/mouse_click.cpp loading_dialog.cpp message_filter_display.h mesh_loader.cpp diff --git a/src/rviz/default_plugin/image_display.cpp b/src/rviz/default_plugin/image_display.cpp index c2aa13c31d..a9e6fcf527 100644 --- a/src/rviz/default_plugin/image_display.cpp +++ b/src/rviz/default_plugin/image_display.cpp @@ -72,6 +72,8 @@ ImageDisplay::ImageDisplay() : ImageDisplayBase(), texture_() this, SLOT(updateNormalizeOptions())); got_float_image_ = false; + + mouse_click = new MouseClick(); } void ImageDisplay::onInitialize() @@ -130,6 +132,8 @@ void ImageDisplay::onInitialize() render_panel_->getCamera()->setNearClipDistance(0.01f); updateNormalizeOptions(); + + mouse_click->onInitialize(render_panel_); } ImageDisplay::~ImageDisplay() @@ -145,6 +149,7 @@ ImageDisplay::~ImageDisplay() void ImageDisplay::onEnable() { ImageDisplayBase::subscribe(); + mouse_click->publish(); render_panel_->getRenderWindow()->setActive(true); } @@ -152,6 +157,7 @@ void ImageDisplay::onDisable() { render_panel_->getRenderWindow()->setActive(false); ImageDisplayBase::unsubscribe(); + mouse_click->unpublish(); reset(); } @@ -211,6 +217,8 @@ void ImageDisplay::update(float wall_dt, float ros_dt) } render_panel_->getRenderWindow()->update(); + + mouse_click->setDimensions(img_width, img_height, win_width, win_height); } catch (UnsupportedImageEncoding& e) { @@ -241,6 +249,21 @@ void ImageDisplay::processMessage(const sensor_msgs::Image::ConstPtr& msg) texture_.addMessage(msg); } +void ImageDisplay::setTopic(const QString& topic, const QString& datatype) +{ + std::cout << "ImageDisplay::setTopic called" << std::endl; + ImageDisplayBase::setTopic(topic, datatype); + mouse_click->setTopic(topic); +} + +void ImageDisplay::updateTopic() +{ + std::cout << "ImageDisplay::updateTopic called" << std::endl; + ImageDisplayBase::updateTopic(); + mouse_click->updateTopic(topic_property_->getTopic()); +} + + } // namespace rviz #include diff --git a/src/rviz/default_plugin/image_display.h b/src/rviz/default_plugin/image_display.h index 88492c191e..c62cc236a5 100644 --- a/src/rviz/default_plugin/image_display.h +++ b/src/rviz/default_plugin/image_display.h @@ -39,6 +39,7 @@ #include "rviz/image/image_display_base.h" #include "rviz/image/ros_image_texture.h" +#include "rviz/image/mouse_click.h" #include "rviz/render_panel.h" #include "rviz/properties/bool_property.h" @@ -81,6 +82,8 @@ public Q_SLOTS: /* This is called by incomingMessage(). */ void processMessage(const sensor_msgs::Image::ConstPtr& msg) override; + void setTopic(const QString& topic, const QString& datatype) override; + void updateTopic() override; Ogre::SceneManager* img_scene_manager_; @@ -98,6 +101,8 @@ public Q_SLOTS: FloatProperty* max_property_; IntProperty* median_buffer_size_property_; bool got_float_image_; + + MouseClick* mouse_click; }; } // namespace rviz diff --git a/src/rviz/image/mouse_click.cpp b/src/rviz/image/mouse_click.cpp new file mode 100644 index 0000000000..958df1aaea --- /dev/null +++ b/src/rviz/image/mouse_click.cpp @@ -0,0 +1,127 @@ +#include "rviz/image/mouse_click.h" + +#include + + +namespace rviz +{ + +MouseClick::MouseClick() : QObject() +{ +} + +MouseClick::~MouseClick() +{ +} + + +void MouseClick::onInitialize(QObject* parent) +{ + setParent(parent); + parent->installEventFilter(this); + + has_dimensions_ = false; + is_topic_name_ok_ = false; + node_handle_.reset(new ros::NodeHandle()); +} + +void MouseClick::publish() +{ + if (is_topic_name_ok_) + { + publisher_.reset( + new ros::Publisher(node_handle_->advertise(topic_, 1000))); + } +} + +void MouseClick::unpublish() +{ + publisher_.reset(); +} + + +bool MouseClick::eventFilter(QObject* obj, QEvent* event) +{ + if (has_dimensions_ == false) + return false; // Cannot compute pixel coordinates. + + if (is_topic_name_ok_ == false) + return false; // Cannot publish. + + + if (event->type() == QEvent::MouseButtonPress) + { + QMouseEvent* me = static_cast(event); + QPointF windowPos = me->windowPos(); + + if (img_width_ != 0 && img_height_ != 0 && win_width_ != 0 && win_height_ != 0) + { + float img_aspect = float(img_width_) / float(img_height_); + float win_aspect = float(win_width_) / float(win_height_); + + int pix_x = -1; + int pix_y = -1; + if (img_aspect > win_aspect) // Window is taller than the image: black bars over and under image. + { + pix_x = int(float(windowPos.x()) / float(win_width_) * float(img_width_)); + + int resized_img_height = int(float(win_width_) / float(img_width_) * float(img_height_)); + int bias = int((float(win_height_) - float(resized_img_height)) / 2.0); + pix_y = (float(windowPos.y()) - bias) / float(resized_img_height) * float(img_height_); + } + else // Window wider than the image: black bards on the side. + { + pix_y = int(float(windowPos.y()) / float(win_height_) * float(img_height_)); + + int resized_img_width = int(float(win_height_) / float(img_height_) * float(img_width_)); + int bias = int((float(win_width_) - float(resized_img_width)) / 2.0); + pix_x = (float(windowPos.x()) - bias) / float(resized_img_width) * float(img_width_); + } + + // Publish if clicked point is inside the image. + if (pix_x > 0 && pix_x < img_width_ && pix_y > 0 && pix_y < img_height_) + { + geometry_msgs::PointStamped point_msgs; + point_msgs.header.stamp = ros::Time::now(); + point_msgs.point.x = pix_x; + point_msgs.point.y = pix_y; + publisher_->publish(point_msgs); + } + } + } + return QObject::eventFilter(obj, event); +} + +void MouseClick::setDimensions(int img_width, int img_height, int win_width, int win_height) +{ + img_width_ = img_width; + img_height_ = img_height; + win_width_ = win_width; + win_height_ = win_height; + has_dimensions_ = true; +} + +void MouseClick::setTopic(const QString& image_topic) +{ + // Build the click full topic name based on the image topic. + topic_ = image_topic.toStdString().append("/mouse_click"); + + std::string error; + if (ros::names::validate((const std::string)topic_, error)) + { + is_topic_name_ok_ = true; + } + else + { + is_topic_name_ok_ = false; + } +} + +void MouseClick::updateTopic(const QString& image_topic) +{ + unpublish(); + setTopic(image_topic); + publish(); +} + +} // namespace rviz diff --git a/src/rviz/image/mouse_click.h b/src/rviz/image/mouse_click.h new file mode 100644 index 0000000000..6f645fd8b3 --- /dev/null +++ b/src/rviz/image/mouse_click.h @@ -0,0 +1,61 @@ +#ifndef RVIZ_MOUSE_CLICK_H +#define RVIZ_MOUSE_CLICK_H + +#include + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include +#include +#include + +#include + +#include "ros/ros.h" +#include "geometry_msgs/PointStamped.h" +#include "std_msgs/String.h" + +#include "rviz/rviz_export.h" +#endif + + +namespace rviz +{ +/** @brief Class for capturing mouse clicks. + * + * This class handles mouse clicking functionalities integrated into the ImageDisplay. + * It uses a qt event filter to capture mouse clicks, handles image resizing and checks if the click was + * inside or outside the image. It also scales the pixel coordinates to get them w.r.t. the image (not + * the window) size. Mouse clicks image pixel coordinates are published as ros geometry_msgs + * PointStamped. The z component is ignored. The topic name where the mouse clicks are published is + * defined created after the subscribed image topic as: /mouse_click. + */ + +class RVIZ_EXPORT MouseClick : QObject +{ +public: + MouseClick(); + ~MouseClick(); + + void onInitialize(QObject* parent); + + /** @brief ROS topic management. */ + void publish(); + void unpublish(); + + void setDimensions(int img_width, int img_height, int win_width, int win_height); + void setTopic(const QString& image_topic); + void updateTopic(const QString& image_topic); + +private: + virtual bool eventFilter(QObject* obj, QEvent* event); + + bool has_dimensions_; + int img_width_, img_height_, win_width_, win_height_; // To assess if the clicks are inside the image. + boost::shared_ptr node_handle_; + boost::shared_ptr publisher_; + std::string topic_; + bool is_topic_name_ok_; +}; + +} // namespace rviz +#endif From 20fb106b33ab9905c6b40097c89c82cbf3e7a9a8 Mon Sep 17 00:00:00 2001 From: Miguel Oliveira Date: Sun, 10 Apr 2022 19:42:29 +0100 Subject: [PATCH 03/13] Cleaning up couts ... --- src/rviz/default_plugin/image_display.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rviz/default_plugin/image_display.cpp b/src/rviz/default_plugin/image_display.cpp index a9e6fcf527..e411ae318d 100644 --- a/src/rviz/default_plugin/image_display.cpp +++ b/src/rviz/default_plugin/image_display.cpp @@ -150,6 +150,7 @@ void ImageDisplay::onEnable() { ImageDisplayBase::subscribe(); mouse_click->publish(); + render_panel_->getRenderWindow()->setActive(true); } @@ -158,6 +159,7 @@ void ImageDisplay::onDisable() render_panel_->getRenderWindow()->setActive(false); ImageDisplayBase::unsubscribe(); mouse_click->unpublish(); + reset(); } @@ -251,14 +253,12 @@ void ImageDisplay::processMessage(const sensor_msgs::Image::ConstPtr& msg) void ImageDisplay::setTopic(const QString& topic, const QString& datatype) { - std::cout << "ImageDisplay::setTopic called" << std::endl; ImageDisplayBase::setTopic(topic, datatype); mouse_click->setTopic(topic); } void ImageDisplay::updateTopic() { - std::cout << "ImageDisplay::updateTopic called" << std::endl; ImageDisplayBase::updateTopic(); mouse_click->updateTopic(topic_property_->getTopic()); } From 32f8cb5708c6fbccac29f364a357f6c222eb7df5 Mon Sep 17 00:00:00 2001 From: Miguel Riem de Oliveira Date: Sat, 16 Jul 2022 08:29:12 +0100 Subject: [PATCH 04/13] Update src/rviz/default_plugin/image_display.h Co-authored-by: Robert Haschke --- src/rviz/default_plugin/image_display.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rviz/default_plugin/image_display.h b/src/rviz/default_plugin/image_display.h index c62cc236a5..a6e0a90926 100644 --- a/src/rviz/default_plugin/image_display.h +++ b/src/rviz/default_plugin/image_display.h @@ -102,7 +102,7 @@ public Q_SLOTS: IntProperty* median_buffer_size_property_; bool got_float_image_; - MouseClick* mouse_click; + MouseClick* mouse_click_; }; } // namespace rviz From 6663a9d42fb50038aae073eaac96b293156d3835 Mon Sep 17 00:00:00 2001 From: Miguel Riem de Oliveira Date: Sat, 16 Jul 2022 08:29:42 +0100 Subject: [PATCH 05/13] Update src/rviz/image/mouse_click.cpp Co-authored-by: Robert Haschke --- src/rviz/image/mouse_click.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rviz/image/mouse_click.cpp b/src/rviz/image/mouse_click.cpp index 958df1aaea..736bc6fb7d 100644 --- a/src/rviz/image/mouse_click.cpp +++ b/src/rviz/image/mouse_click.cpp @@ -69,7 +69,7 @@ bool MouseClick::eventFilter(QObject* obj, QEvent* event) int bias = int((float(win_height_) - float(resized_img_height)) / 2.0); pix_y = (float(windowPos.y()) - bias) / float(resized_img_height) * float(img_height_); } - else // Window wider than the image: black bards on the side. + else // Window wider than the image: black bars on the side. { pix_y = int(float(windowPos.y()) / float(win_height_) * float(img_height_)); From 25fc5c665177fecc83ff7e8848451179578a4bd1 Mon Sep 17 00:00:00 2001 From: Miguel Riem de Oliveira Date: Sat, 16 Jul 2022 08:30:01 +0100 Subject: [PATCH 06/13] Update src/rviz/image/mouse_click.cpp Co-authored-by: Robert Haschke --- src/rviz/image/mouse_click.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rviz/image/mouse_click.cpp b/src/rviz/image/mouse_click.cpp index 736bc6fb7d..c911ce4f5d 100644 --- a/src/rviz/image/mouse_click.cpp +++ b/src/rviz/image/mouse_click.cpp @@ -79,7 +79,7 @@ bool MouseClick::eventFilter(QObject* obj, QEvent* event) } // Publish if clicked point is inside the image. - if (pix_x > 0 && pix_x < img_width_ && pix_y > 0 && pix_y < img_height_) + if (pix_x >= 0 && pix_x < img_width_ && pix_y >= 0 && pix_y < img_height_) { geometry_msgs::PointStamped point_msgs; point_msgs.header.stamp = ros::Time::now(); From 61974117adaf5f632800ccc906fe84d3204d5a93 Mon Sep 17 00:00:00 2001 From: Miguel Riem de Oliveira Date: Sat, 16 Jul 2022 08:30:10 +0100 Subject: [PATCH 07/13] Update src/rviz/image/mouse_click.cpp Co-authored-by: Robert Haschke --- src/rviz/image/mouse_click.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rviz/image/mouse_click.cpp b/src/rviz/image/mouse_click.cpp index c911ce4f5d..2461dc031e 100644 --- a/src/rviz/image/mouse_click.cpp +++ b/src/rviz/image/mouse_click.cpp @@ -81,7 +81,7 @@ bool MouseClick::eventFilter(QObject* obj, QEvent* event) // Publish if clicked point is inside the image. if (pix_x >= 0 && pix_x < img_width_ && pix_y >= 0 && pix_y < img_height_) { - geometry_msgs::PointStamped point_msgs; + geometry_msgs::PointStamped point_msg; point_msgs.header.stamp = ros::Time::now(); point_msgs.point.x = pix_x; point_msgs.point.y = pix_y; From 0135a808d7185187f7e3e3d260902358e0c396cf Mon Sep 17 00:00:00 2001 From: Miguel Oliveira Date: Wed, 10 Aug 2022 03:09:48 +0100 Subject: [PATCH 08/13] (#1737) Trying to implement rhaschke's comments. --- src/rviz/default_plugin/image_display.cpp | 15 +++++++------ src/rviz/default_plugin/image_display.h | 2 +- src/rviz/image/mouse_click.cpp | 27 +++++++++++------------ src/rviz/image/mouse_click.h | 5 +++-- 4 files changed, 25 insertions(+), 24 deletions(-) diff --git a/src/rviz/default_plugin/image_display.cpp b/src/rviz/default_plugin/image_display.cpp index e411ae318d..81e1c171c3 100644 --- a/src/rviz/default_plugin/image_display.cpp +++ b/src/rviz/default_plugin/image_display.cpp @@ -73,7 +73,6 @@ ImageDisplay::ImageDisplay() : ImageDisplayBase(), texture_() got_float_image_ = false; - mouse_click = new MouseClick(); } void ImageDisplay::onInitialize() @@ -133,7 +132,9 @@ void ImageDisplay::onInitialize() updateNormalizeOptions(); - mouse_click->onInitialize(render_panel_); + + mouse_click_.reset(new MouseClick(render_panel_, update_nh_)); + mouse_click_->onInitialize(); } ImageDisplay::~ImageDisplay() @@ -149,7 +150,7 @@ ImageDisplay::~ImageDisplay() void ImageDisplay::onEnable() { ImageDisplayBase::subscribe(); - mouse_click->publish(); + mouse_click_->publish(); render_panel_->getRenderWindow()->setActive(true); } @@ -158,7 +159,7 @@ void ImageDisplay::onDisable() { render_panel_->getRenderWindow()->setActive(false); ImageDisplayBase::unsubscribe(); - mouse_click->unpublish(); + mouse_click_->unpublish(); reset(); } @@ -220,7 +221,7 @@ void ImageDisplay::update(float wall_dt, float ros_dt) render_panel_->getRenderWindow()->update(); - mouse_click->setDimensions(img_width, img_height, win_width, win_height); + mouse_click_->setDimensions(img_width, img_height, win_width, win_height); } catch (UnsupportedImageEncoding& e) { @@ -254,13 +255,13 @@ void ImageDisplay::processMessage(const sensor_msgs::Image::ConstPtr& msg) void ImageDisplay::setTopic(const QString& topic, const QString& datatype) { ImageDisplayBase::setTopic(topic, datatype); - mouse_click->setTopic(topic); + mouse_click_->setTopic(topic); } void ImageDisplay::updateTopic() { ImageDisplayBase::updateTopic(); - mouse_click->updateTopic(topic_property_->getTopic()); + mouse_click_->updateTopic(topic_property_->getTopic()); } diff --git a/src/rviz/default_plugin/image_display.h b/src/rviz/default_plugin/image_display.h index a6e0a90926..bb1758d152 100644 --- a/src/rviz/default_plugin/image_display.h +++ b/src/rviz/default_plugin/image_display.h @@ -102,7 +102,7 @@ public Q_SLOTS: IntProperty* median_buffer_size_property_; bool got_float_image_; - MouseClick* mouse_click_; + boost::shared_ptr mouse_click_; }; } // namespace rviz diff --git a/src/rviz/image/mouse_click.cpp b/src/rviz/image/mouse_click.cpp index 2461dc031e..3bf1ef57ee 100644 --- a/src/rviz/image/mouse_click.cpp +++ b/src/rviz/image/mouse_click.cpp @@ -6,23 +6,22 @@ namespace rviz { -MouseClick::MouseClick() : QObject() +MouseClick::MouseClick(QObject* parent, const ros::NodeHandle& nh) : QObject(parent) { + node_handle_.reset(new ros::NodeHandle()); + *node_handle_ = nh; + has_dimensions_ = false; + is_topic_name_ok_ = false; + setParent(parent); + parent->installEventFilter(this); } MouseClick::~MouseClick() { } - -void MouseClick::onInitialize(QObject* parent) +void MouseClick::onInitialize() { - setParent(parent); - parent->installEventFilter(this); - - has_dimensions_ = false; - is_topic_name_ok_ = false; - node_handle_.reset(new ros::NodeHandle()); } void MouseClick::publish() @@ -30,7 +29,7 @@ void MouseClick::publish() if (is_topic_name_ok_) { publisher_.reset( - new ros::Publisher(node_handle_->advertise(topic_, 1000))); + new ros::Publisher(node_handle_->advertise(topic_, 1))); } } @@ -82,10 +81,10 @@ bool MouseClick::eventFilter(QObject* obj, QEvent* event) if (pix_x >= 0 && pix_x < img_width_ && pix_y >= 0 && pix_y < img_height_) { geometry_msgs::PointStamped point_msg; - point_msgs.header.stamp = ros::Time::now(); - point_msgs.point.x = pix_x; - point_msgs.point.y = pix_y; - publisher_->publish(point_msgs); + point_msg.header.stamp = ros::Time::now(); + point_msg.point.x = pix_x; + point_msg.point.y = pix_y; + publisher_->publish(point_msg); } } } diff --git a/src/rviz/image/mouse_click.h b/src/rviz/image/mouse_click.h index 6f645fd8b3..cf570c6b42 100644 --- a/src/rviz/image/mouse_click.h +++ b/src/rviz/image/mouse_click.h @@ -15,6 +15,7 @@ #include "std_msgs/String.h" #include "rviz/rviz_export.h" +#include "rviz/display.h" #endif @@ -33,10 +34,10 @@ namespace rviz class RVIZ_EXPORT MouseClick : QObject { public: - MouseClick(); + MouseClick(QObject* parent, const ros::NodeHandle& nh); ~MouseClick(); - void onInitialize(QObject* parent); + void onInitialize(); /** @brief ROS topic management. */ void publish(); From 22fb3422c53ec4fc99decee759733e3196042cf6 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 25 Apr 2024 10:02:11 +0200 Subject: [PATCH 09/13] Fix clang-format --- src/rviz/default_plugin/image_display.cpp | 2 -- src/rviz/image/mouse_click.cpp | 4 +--- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/src/rviz/default_plugin/image_display.cpp b/src/rviz/default_plugin/image_display.cpp index 81e1c171c3..8ea08a2544 100644 --- a/src/rviz/default_plugin/image_display.cpp +++ b/src/rviz/default_plugin/image_display.cpp @@ -72,7 +72,6 @@ ImageDisplay::ImageDisplay() : ImageDisplayBase(), texture_() this, SLOT(updateNormalizeOptions())); got_float_image_ = false; - } void ImageDisplay::onInitialize() @@ -132,7 +131,6 @@ void ImageDisplay::onInitialize() updateNormalizeOptions(); - mouse_click_.reset(new MouseClick(render_panel_, update_nh_)); mouse_click_->onInitialize(); } diff --git a/src/rviz/image/mouse_click.cpp b/src/rviz/image/mouse_click.cpp index 3bf1ef57ee..42cfea40b3 100644 --- a/src/rviz/image/mouse_click.cpp +++ b/src/rviz/image/mouse_click.cpp @@ -5,7 +5,6 @@ namespace rviz { - MouseClick::MouseClick(QObject* parent, const ros::NodeHandle& nh) : QObject(parent) { node_handle_.reset(new ros::NodeHandle()); @@ -28,8 +27,7 @@ void MouseClick::publish() { if (is_topic_name_ok_) { - publisher_.reset( - new ros::Publisher(node_handle_->advertise(topic_, 1))); + publisher_.reset(new ros::Publisher(node_handle_->advertise(topic_, 1))); } } From 1a4821788321f92f88fda9ed57ce6855d77d00ac Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 25 Apr 2024 11:33:32 +0200 Subject: [PATCH 10/13] Cleanup - Rename variables and functions for more clarity - Drop shared_ptr whereever possible - Drop MouseClick::updateTopic() --- src/rviz/default_plugin/image_display.cpp | 15 ++--- src/rviz/default_plugin/image_display.h | 3 +- src/rviz/image/mouse_click.cpp | 72 +++++++---------------- src/rviz/image/mouse_click.h | 21 +++---- 4 files changed, 34 insertions(+), 77 deletions(-) diff --git a/src/rviz/default_plugin/image_display.cpp b/src/rviz/default_plugin/image_display.cpp index 8ea08a2544..242beba9af 100644 --- a/src/rviz/default_plugin/image_display.cpp +++ b/src/rviz/default_plugin/image_display.cpp @@ -131,8 +131,7 @@ void ImageDisplay::onInitialize() updateNormalizeOptions(); - mouse_click_.reset(new MouseClick(render_panel_, update_nh_)); - mouse_click_->onInitialize(); + mouse_click_ = new MouseClick(render_panel_, update_nh_); } ImageDisplay::~ImageDisplay() @@ -148,7 +147,7 @@ ImageDisplay::~ImageDisplay() void ImageDisplay::onEnable() { ImageDisplayBase::subscribe(); - mouse_click_->publish(); + mouse_click_->enable(); render_panel_->getRenderWindow()->setActive(true); } @@ -157,7 +156,7 @@ void ImageDisplay::onDisable() { render_panel_->getRenderWindow()->setActive(false); ImageDisplayBase::unsubscribe(); - mouse_click_->unpublish(); + mouse_click_->disable(); reset(); } @@ -250,16 +249,10 @@ void ImageDisplay::processMessage(const sensor_msgs::Image::ConstPtr& msg) texture_.addMessage(msg); } -void ImageDisplay::setTopic(const QString& topic, const QString& datatype) -{ - ImageDisplayBase::setTopic(topic, datatype); - mouse_click_->setTopic(topic); -} - void ImageDisplay::updateTopic() { ImageDisplayBase::updateTopic(); - mouse_click_->updateTopic(topic_property_->getTopic()); + mouse_click_->setImageTopic(topic_property_->getTopic()); } diff --git a/src/rviz/default_plugin/image_display.h b/src/rviz/default_plugin/image_display.h index bb1758d152..21a4e1b017 100644 --- a/src/rviz/default_plugin/image_display.h +++ b/src/rviz/default_plugin/image_display.h @@ -82,7 +82,6 @@ public Q_SLOTS: /* This is called by incomingMessage(). */ void processMessage(const sensor_msgs::Image::ConstPtr& msg) override; - void setTopic(const QString& topic, const QString& datatype) override; void updateTopic() override; Ogre::SceneManager* img_scene_manager_; @@ -102,7 +101,7 @@ public Q_SLOTS: IntProperty* median_buffer_size_property_; bool got_float_image_; - boost::shared_ptr mouse_click_; + MouseClick* mouse_click_; }; } // namespace rviz diff --git a/src/rviz/image/mouse_click.cpp b/src/rviz/image/mouse_click.cpp index 42cfea40b3..337d1e88b0 100644 --- a/src/rviz/image/mouse_click.cpp +++ b/src/rviz/image/mouse_click.cpp @@ -1,52 +1,32 @@ #include "rviz/image/mouse_click.h" - +#include #include namespace rviz { -MouseClick::MouseClick(QObject* parent, const ros::NodeHandle& nh) : QObject(parent) -{ - node_handle_.reset(new ros::NodeHandle()); - *node_handle_ = nh; - has_dimensions_ = false; - is_topic_name_ok_ = false; - setParent(parent); - parent->installEventFilter(this); -} - -MouseClick::~MouseClick() -{ -} - -void MouseClick::onInitialize() +MouseClick::MouseClick(QWidget* widget, const ros::NodeHandle& nh) : QObject(widget) { + node_handle_ = nh; + have_dimensions_ = false; + topic_name_ok_ = false; + widget->installEventFilter(this); } -void MouseClick::publish() +void MouseClick::enable() { - if (is_topic_name_ok_) - { - publisher_.reset(new ros::Publisher(node_handle_->advertise(topic_, 1))); - } + if (topic_name_ok_) + publisher_ = node_handle_.advertise(topic_, 1); } -void MouseClick::unpublish() +void MouseClick::disable() { - publisher_.reset(); + publisher_.shutdown(); } - bool MouseClick::eventFilter(QObject* obj, QEvent* event) { - if (has_dimensions_ == false) - return false; // Cannot compute pixel coordinates. - - if (is_topic_name_ok_ == false) - return false; // Cannot publish. - - - if (event->type() == QEvent::MouseButtonPress) + if (have_dimensions_ && publisher_.operator void*() && event->type() == QEvent::MouseButtonPress) { QMouseEvent* me = static_cast(event); QPointF windowPos = me->windowPos(); @@ -82,7 +62,7 @@ bool MouseClick::eventFilter(QObject* obj, QEvent* event) point_msg.header.stamp = ros::Time::now(); point_msg.point.x = pix_x; point_msg.point.y = pix_y; - publisher_->publish(point_msg); + publisher_.publish(point_msg); } } } @@ -95,30 +75,20 @@ void MouseClick::setDimensions(int img_width, int img_height, int win_width, int img_height_ = img_height; win_width_ = win_width; win_height_ = win_height; - has_dimensions_ = true; + have_dimensions_ = true; } -void MouseClick::setTopic(const QString& image_topic) +void MouseClick::setImageTopic(const QString& topic) { - // Build the click full topic name based on the image topic. - topic_ = image_topic.toStdString().append("/mouse_click"); + disable(); + + // Build the click full topic name based on the image topic + topic_ = topic.toStdString().append("/mouse_click"); std::string error; - if (ros::names::validate((const std::string)topic_, error)) - { - is_topic_name_ok_ = true; - } - else - { - is_topic_name_ok_ = false; - } -} + topic_name_ok_ = ros::names::validate(topic_, error); -void MouseClick::updateTopic(const QString& image_topic) -{ - unpublish(); - setTopic(image_topic); - publish(); + enable(); } } // namespace rviz diff --git a/src/rviz/image/mouse_click.h b/src/rviz/image/mouse_click.h index cf570c6b42..aed7a01f28 100644 --- a/src/rviz/image/mouse_click.h +++ b/src/rviz/image/mouse_click.h @@ -34,28 +34,23 @@ namespace rviz class RVIZ_EXPORT MouseClick : QObject { public: - MouseClick(QObject* parent, const ros::NodeHandle& nh); - ~MouseClick(); + MouseClick(QWidget* widget, const ros::NodeHandle& nh); - void onInitialize(); - - /** @brief ROS topic management. */ - void publish(); - void unpublish(); + void enable(); + void disable(); void setDimensions(int img_width, int img_height, int win_width, int win_height); - void setTopic(const QString& image_topic); - void updateTopic(const QString& image_topic); + void setImageTopic(const QString& topic); private: virtual bool eventFilter(QObject* obj, QEvent* event); - bool has_dimensions_; + bool have_dimensions_; int img_width_, img_height_, win_width_, win_height_; // To assess if the clicks are inside the image. - boost::shared_ptr node_handle_; - boost::shared_ptr publisher_; + ros::NodeHandle node_handle_; + ros::Publisher publisher_; std::string topic_; - bool is_topic_name_ok_; + bool topic_name_ok_; }; } // namespace rviz From 8469d187a8de8725da0b0cdf7debbd76558a384c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 25 Apr 2024 11:43:13 +0200 Subject: [PATCH 11/13] Drop have_dimensions_ member --- src/rviz/image/mouse_click.cpp | 5 ++--- src/rviz/image/mouse_click.h | 1 - 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/rviz/image/mouse_click.cpp b/src/rviz/image/mouse_click.cpp index 337d1e88b0..9eb63c0b1f 100644 --- a/src/rviz/image/mouse_click.cpp +++ b/src/rviz/image/mouse_click.cpp @@ -7,8 +7,8 @@ namespace rviz { MouseClick::MouseClick(QWidget* widget, const ros::NodeHandle& nh) : QObject(widget) { + img_width_ = img_height_ = win_width_ = win_height_ = 0; node_handle_ = nh; - have_dimensions_ = false; topic_name_ok_ = false; widget->installEventFilter(this); } @@ -26,7 +26,7 @@ void MouseClick::disable() bool MouseClick::eventFilter(QObject* obj, QEvent* event) { - if (have_dimensions_ && publisher_.operator void*() && event->type() == QEvent::MouseButtonPress) + if (publisher_.operator void*() && event->type() == QEvent::MouseButtonPress) { QMouseEvent* me = static_cast(event); QPointF windowPos = me->windowPos(); @@ -75,7 +75,6 @@ void MouseClick::setDimensions(int img_width, int img_height, int win_width, int img_height_ = img_height; win_width_ = win_width; win_height_ = win_height; - have_dimensions_ = true; } void MouseClick::setImageTopic(const QString& topic) diff --git a/src/rviz/image/mouse_click.h b/src/rviz/image/mouse_click.h index aed7a01f28..e0270c373c 100644 --- a/src/rviz/image/mouse_click.h +++ b/src/rviz/image/mouse_click.h @@ -45,7 +45,6 @@ class RVIZ_EXPORT MouseClick : QObject private: virtual bool eventFilter(QObject* obj, QEvent* event); - bool have_dimensions_; int img_width_, img_height_, win_width_, win_height_; // To assess if the clicks are inside the image. ros::NodeHandle node_handle_; ros::Publisher publisher_; From b55497d5c3929031aa9342bda47588421d09c968 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 25 Apr 2024 11:45:35 +0200 Subject: [PATCH 12/13] Allow continouos dragging --- src/rviz/image/mouse_click.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/rviz/image/mouse_click.cpp b/src/rviz/image/mouse_click.cpp index 9eb63c0b1f..447471b599 100644 --- a/src/rviz/image/mouse_click.cpp +++ b/src/rviz/image/mouse_click.cpp @@ -26,12 +26,14 @@ void MouseClick::disable() bool MouseClick::eventFilter(QObject* obj, QEvent* event) { - if (publisher_.operator void*() && event->type() == QEvent::MouseButtonPress) + if (publisher_.operator void*() && + (event->type() == QEvent::MouseButtonPress || event->type() == QEvent::MouseMove)) { QMouseEvent* me = static_cast(event); QPointF windowPos = me->windowPos(); + bool left_button = me->buttons() == Qt::LeftButton; - if (img_width_ != 0 && img_height_ != 0 && win_width_ != 0 && win_height_ != 0) + if (left_button && img_width_ != 0 && img_height_ != 0 && win_width_ != 0 && win_height_ != 0) { float img_aspect = float(img_width_) / float(img_height_); float win_aspect = float(win_width_) / float(win_height_); From 71c4603d2f3b05c9720730881de28bc3410b4e7d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 25 Apr 2024 12:02:56 +0200 Subject: [PATCH 13/13] Install/Remove event filter on enable()/disable() --- src/rviz/image/mouse_click.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/rviz/image/mouse_click.cpp b/src/rviz/image/mouse_click.cpp index 447471b599..48002e621e 100644 --- a/src/rviz/image/mouse_click.cpp +++ b/src/rviz/image/mouse_click.cpp @@ -10,24 +10,26 @@ MouseClick::MouseClick(QWidget* widget, const ros::NodeHandle& nh) : QObject(wid img_width_ = img_height_ = win_width_ = win_height_ = 0; node_handle_ = nh; topic_name_ok_ = false; - widget->installEventFilter(this); } void MouseClick::enable() { if (topic_name_ok_) + { publisher_ = node_handle_.advertise(topic_, 1); + parent()->installEventFilter(this); + } } void MouseClick::disable() { + parent()->removeEventFilter(this); publisher_.shutdown(); } bool MouseClick::eventFilter(QObject* obj, QEvent* event) { - if (publisher_.operator void*() && - (event->type() == QEvent::MouseButtonPress || event->type() == QEvent::MouseMove)) + if (event->type() == QEvent::MouseButtonPress || event->type() == QEvent::MouseMove) { QMouseEvent* me = static_cast(event); QPointF windowPos = me->windowPos();