diff --git a/src/rviz/CMakeLists.txt b/src/rviz/CMakeLists.txt index e131dd5e1..cc29d58a0 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 8cf5b1fe0..e460a8db1 100644 --- a/src/rviz/default_plugin/image_display.cpp +++ b/src/rviz/default_plugin/image_display.cpp @@ -138,6 +138,8 @@ void ImageDisplay::onInitialize() render_panel_->getCamera()->setNearClipDistance(0.01f); updateNormalizeOptions(); + + mouse_click_ = new MouseClick(render_panel_, update_nh_); } ImageDisplay::~ImageDisplay() @@ -153,6 +155,8 @@ ImageDisplay::~ImageDisplay() void ImageDisplay::onEnable() { ImageDisplayBase::subscribe(); + mouse_click_->enable(); + render_panel_->getRenderWindow()->setActive(true); } @@ -160,6 +164,8 @@ void ImageDisplay::onDisable() { render_panel_->getRenderWindow()->setActive(false); ImageDisplayBase::unsubscribe(); + mouse_click_->disable(); + reset(); } @@ -219,6 +225,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) { @@ -249,6 +257,13 @@ void ImageDisplay::processMessage(const sensor_msgs::Image::ConstPtr& msg) texture_.addMessage(msg); } +void ImageDisplay::updateTopic() +{ + ImageDisplayBase::updateTopic(); + mouse_click_->setImageTopic(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 88492c191..21a4e1b01 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,7 @@ public Q_SLOTS: /* This is called by incomingMessage(). */ void processMessage(const sensor_msgs::Image::ConstPtr& msg) override; + void updateTopic() override; Ogre::SceneManager* img_scene_manager_; @@ -98,6 +100,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 000000000..48002e621 --- /dev/null +++ b/src/rviz/image/mouse_click.cpp @@ -0,0 +1,97 @@ +#include "rviz/image/mouse_click.h" +#include +#include + + +namespace rviz +{ +MouseClick::MouseClick(QWidget* widget, const ros::NodeHandle& nh) : QObject(widget) +{ + img_width_ = img_height_ = win_width_ = win_height_ = 0; + node_handle_ = nh; + topic_name_ok_ = false; +} + +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 (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 (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_); + + 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 bars 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_msg; + point_msg.header.stamp = ros::Time::now(); + point_msg.point.x = pix_x; + point_msg.point.y = pix_y; + publisher_.publish(point_msg); + } + } + } + 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; +} + +void MouseClick::setImageTopic(const QString& topic) +{ + disable(); + + // Build the click full topic name based on the image topic + topic_ = topic.toStdString().append("/mouse_click"); + + std::string error; + topic_name_ok_ = ros::names::validate(topic_, error); + + enable(); +} + +} // namespace rviz diff --git a/src/rviz/image/mouse_click.h b/src/rviz/image/mouse_click.h new file mode 100644 index 000000000..e0270c373 --- /dev/null +++ b/src/rviz/image/mouse_click.h @@ -0,0 +1,56 @@ +#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" +#include "rviz/display.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(QWidget* widget, const ros::NodeHandle& nh); + + void enable(); + void disable(); + + void setDimensions(int img_width, int img_height, int win_width, int win_height); + void setImageTopic(const QString& topic); + +private: + virtual bool eventFilter(QObject* obj, QEvent* event); + + 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_; + std::string topic_; + bool topic_name_ok_; +}; + +} // namespace rviz +#endif