Image display with mouse clicking functionality

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

@@ -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,13 +149,17 @@ ImageDisplay::~ImageDisplay()
 void ImageDisplay::onEnable()
 {
   ImageDisplayBase::subscribe();
+  mouse_click->publish();
+
   render_panel_->getRenderWindow()->setActive(true);
 }
 
 void ImageDisplay::onDisable()
 {
   render_panel_->getRenderWindow()->setActive(false);
   ImageDisplayBase::unsubscribe();
+  mouse_click->unpublish();
+
   reset();
 }
 
@@ -211,6 +219,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 +251,19 @@ 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());
+}
+
+
 } // 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,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

src/rviz/image/mouse_click.cpp

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

src/rviz/image/mouse_click.h

@@ -0,0 +1,61 @@
+#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"
+#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();
+  ~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<ros::NodeHandle> node_handle_;
+  boost::shared_ptr<ros::Publisher> publisher_;
+  std::string topic_;
+  bool is_topic_name_ok_;
+};
+
+} // namespace rviz
+#endif