ImageDisplay: publish mouse clicks (ros-visualization#1737)

Publish mouse clicks (or dragging) on <image_topic>/mouse_click

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>

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

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,13 +155,17 @@ ImageDisplay::~ImageDisplay()
 void ImageDisplay::onEnable()
 {
   ImageDisplayBase::subscribe();
+  mouse_click_->enable();
+
   render_panel_->getRenderWindow()->setActive(true);
 }
 
 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 <pluginlib/class_list_macros.hpp>

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

src/rviz/image/mouse_click.cpp

@@ -0,0 +1,97 @@
+#include "rviz/image/mouse_click.h"
+#include <QWidget>
+#include <ros/names.h>
+
+
+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<geometry_msgs::PointStamped>(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<QMouseEvent*>(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

src/rviz/image/mouse_click.h

@@ -0,0 +1,56 @@
+#ifndef RVIZ_MOUSE_CLICK_H
+#define RVIZ_MOUSE_CLICK_H
+
+#include <QObject>
+
+#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829
+#include <iostream>
+#include <string>
+#include <boost/shared_ptr.hpp>
+
+#include <QMouseEvent>
+
+#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: <image_topic>/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