From 3c885a04573966b030bf66479945b658e6994ea3 Mon Sep 17 00:00:00 2001
From: Franco Cipollone <53065142+francocipollone@users.noreply.github.com>
Date: Thu, 29 Apr 2021 18:14:15 -0300
Subject: [PATCH] MaliputViewerPlugin: Provides ray-cast support (#388)

---
 .../visualizer/layout2_maliput_viewer.config  |  5 +-
 .../visualizer/maliput_viewer_plugin.cc       | 77 ++++++++++++++++++-
 .../visualizer/maliput_viewer_plugin.hh       | 47 ++++++++++-
 3 files changed, 121 insertions(+), 8 deletions(-)

diff --git a/delphyne_gui/visualizer/layout2_maliput_viewer.config b/delphyne_gui/visualizer/layout2_maliput_viewer.config
index 51f47ddb..7cb7e895 100644
--- a/delphyne_gui/visualizer/layout2_maliput_viewer.config
+++ b/delphyne_gui/visualizer/layout2_maliput_viewer.config
@@ -79,6 +79,7 @@
 
 <plugin filename="Scene3D">
   <ignition-gui>
+    <title>Main3DScene</title>
     <property type="bool" key="showTitleBar">false</property>
     <property type="bool" key="showCollapseButton">false</property>
     <property type="bool" key="showDockButton">false</property>
@@ -104,4 +105,6 @@
   <scene>scene</scene>
 </plugin>
 
-<plugin filename="MaliputViewerPlugin"/>
+<plugin filename="MaliputViewerPlugin">
+  <main_scene_plugin_title>Main3DScene</main_scene_plugin_title>
+</plugin>
diff --git a/delphyne_gui/visualizer/maliput_viewer_plugin.cc b/delphyne_gui/visualizer/maliput_viewer_plugin.cc
index abd4cc7b..53b99e55 100644
--- a/delphyne_gui/visualizer/maliput_viewer_plugin.cc
+++ b/delphyne_gui/visualizer/maliput_viewer_plugin.cc
@@ -1,8 +1,10 @@
 // Copyright 2021 Toyota Research Institute
-
 #include "maliput_viewer_plugin.hh"
 
+#include <algorithm>
+
 #include <ignition/common/Console.hh>
+#include <ignition/gui/Conversions.hh>
 #include <ignition/plugin/Register.hh>
 #include <ignition/rendering/RenderEngine.hh>
 #include <ignition/rendering/RenderingIface.hh>
@@ -125,7 +127,7 @@ void MaliputViewerPlugin::timerEvent(QTimerEvent* _event) {
     return;
   }
   timer.stop();
-  ConfigurateScene();
+  Initialize();
   RenderMeshes();
 }
 
@@ -301,6 +303,10 @@ void MaliputViewerPlugin::LoadConfig(const tinyxml2::XMLElement* _pluginElem) {
     ignerr << "Error reading plugin XML element " << std::endl;
   }
 
+  if (auto elem = _pluginElem->FirstChildElement("main_scene_plugin_title")) {
+    mainScene3dPluginTitle = elem->GetText();
+  }
+
   // Get the render engine.
   // Note: we don't support other engines than Ogre.
   auto engine = ignition::rendering::engine(kEngineName);
@@ -316,16 +322,22 @@ void MaliputViewerPlugin::LoadConfig(const tinyxml2::XMLElement* _pluginElem) {
     timer.start(kTimerPeriodInMs, this);
     return;
   }
-  ConfigurateScene();
+  Initialize();
   RenderMeshes();
 }
 
-void MaliputViewerPlugin::ConfigurateScene() {
+void MaliputViewerPlugin::Initialize() {
+  rayQuery = scene->CreateRayQuery();
   rootVisual = scene->RootVisual();
   if (!rootVisual) {
     ignerr << "Failed to find the root visual" << std::endl;
     return;
   }
+  camera = std::dynamic_pointer_cast<ignition::rendering::Camera>(rootVisual->ChildByIndex(0));
+  if (!camera) {
+    ignerr << "Failed to find the camera" << std::endl;
+    return;
+  }
   // Lights.
   const double lightRed = 0.88;
   const double lightGreen = 0.88;
@@ -340,6 +352,63 @@ void MaliputViewerPlugin::ConfigurateScene() {
   directionalLight->SetDiffuseColor(lightRed, lightGreen, lightBlue);
   directionalLight->SetSpecularColor(lightRed, lightGreen, lightBlue);
   rootVisual->AddChild(directionalLight);
+
+  // Install event filter to get mouse event from the main scene.
+  const ignition::gui::Plugin* scene3D = FilterPluginsByTitle(mainScene3dPluginTitle);
+  if (!scene3D) {
+    const std::string msg{"Scene3D plugin titled '" + mainScene3dPluginTitle + "' wasn't found"};
+    ignerr << msg << std::endl;
+    MALIPUT_THROW_MESSAGE(msg);
+  }
+  auto renderWindowItem = scene3D->PluginItem()->findChild<QQuickItem*>();
+  if (!renderWindowItem) {
+    const std::string msg{"Scene3D's renderWindowItem child isn't found"};
+    ignerr << msg << std::endl;
+    MALIPUT_THROW_MESSAGE(msg);
+  }
+  renderWindowItem->installEventFilter(this);
+}
+
+ignition::gui::Plugin* MaliputViewerPlugin::FilterPluginsByTitle(const std::string& _pluginTitle) {
+  QList<ignition::gui::Plugin*> plugins = parent()->findChildren<ignition::gui::Plugin*>();
+  auto plugin = std::find_if(std::begin(plugins), std::end(plugins), [&_pluginTitle](ignition::gui::Plugin* _plugin) {
+    return _plugin->Title() == _pluginTitle;
+  });
+  return plugin == plugins.end() ? nullptr : *plugin;
+}
+
+bool MaliputViewerPlugin::eventFilter(QObject* _obj, QEvent* _event) {
+  if (_event->type() == QEvent::Type::MouseButtonPress) {
+    const QMouseEvent* mouseEvent = static_cast<QMouseEvent*>(_event);
+    if (mouseEvent && mouseEvent->button() == Qt::LeftButton) {
+      MouseClickHandler(mouseEvent);
+    }
+  }
+  // Standard event processing
+  return QObject::eventFilter(_obj, _event);
+}
+
+void MaliputViewerPlugin::MouseClickHandler(const QMouseEvent* _mouseEvent) {
+  const auto rayQueryResult = ScreenToScene(_mouseEvent->x(), _mouseEvent->y());
+  if (rayQueryResult.distance >= 0) {
+    const maliput::api::Lane* lane = model->GetLaneFromWorldPosition(rayQueryResult.point);
+    if (lane) {
+      ignmsg << "Clicked lane ID: " << lane->id().string() << std::endl;
+    }
+  }
+}
+
+ignition::rendering::RayQueryResult MaliputViewerPlugin::ScreenToScene(int _screenX, int _screenY) const {
+  // Normalize point on the image
+  const double width = camera->ImageWidth();
+  const double height = camera->ImageHeight();
+
+  const double nx = 2.0 * _screenX / width - 1.0;
+  const double ny = 1.0 - 2.0 * _screenY / height;
+
+  // Make a ray query
+  rayQuery->SetFromCamera(camera, {nx, ny});
+  return rayQuery->ClosestPoint();
 }
 
 }  // namespace gui
diff --git a/delphyne_gui/visualizer/maliput_viewer_plugin.hh b/delphyne_gui/visualizer/maliput_viewer_plugin.hh
index 8a0b4fe3..cbd4f4f4 100644
--- a/delphyne_gui/visualizer/maliput_viewer_plugin.hh
+++ b/delphyne_gui/visualizer/maliput_viewer_plugin.hh
@@ -6,7 +6,9 @@
 
 #include <maliput/api/road_geometry.h>
 
+#include <ignition/common/MouseEvent.hh>
 #include <ignition/gui/Plugin.hh>
+#include <ignition/rendering/RayQuery.hh>
 #include <ignition/rendering/RenderTypes.hh>
 #include <ignition/rendering/Scene.hh>
 
@@ -17,6 +19,11 @@ namespace gui {
 
 /// \brief Loads a road geometry out of a xodr file or a yaml file.
 ///        Meshes are created and displayed in the scene.
+///
+/// ## Configuration
+///
+/// * \<main_scene_plugin_title\> : Title of the Scene3D plugin instance that manages the main scene.
+///                                 Defaults to '3D Scene'.
 class MaliputViewerPlugin : public ignition::gui::Plugin {
   Q_OBJECT
 
@@ -52,10 +59,15 @@ class MaliputViewerPlugin : public ignition::gui::Plugin {
   void LabelCheckboxesChanged();
 
  protected:
-  /// @brief Timer event callback which handles the logic to load the meshes when
+  /// \brief Timer event callback which handles the logic to load the meshes when
   ///        the scene is not ready yet.
   void timerEvent(QTimerEvent* _event) override;
 
+  /// \brief Filters QMouseEvents from a Scene3D plugin whose title matches with <main_scene_plugin_title>.
+  /// \details To make this method be called by Qt Event System, install the event filter in target object.
+  ///          \see QObject::installEventFilter() method.
+  bool eventFilter(QObject* _obj, QEvent* _event) override;
+
  protected slots:
   /// \brief Clears the visualizer, loads a RoadNetwork and update the GUI with meshes and labels.
   /// \param[in] _mapFile The path to the map file to load and visualize.
@@ -82,6 +94,9 @@ class MaliputViewerPlugin : public ignition::gui::Plugin {
   /// @brief The scene name.
   static constexpr char const* kSceneName = "scene";
 
+  /// @brief The Scene3D instance holding the main scene.
+  static constexpr char const* kMainScene3dPlugin = "Main3DScene";
+
   /// @brief The rendering engine name.
   static constexpr char const* kEngineName = "ogre";
 
@@ -116,8 +131,25 @@ class MaliputViewerPlugin : public ignition::gui::Plugin {
   /// \brief Clears all the references to text labels, meshes and the scene.
   void Clear();
 
-  /// \brief Configurate scene.
-  void ConfigurateScene();
+  /// \brief Configurate scene and install event filter for filtering QMouseEvents.
+  /// \details To install the event filter the Scene3D plugin hosting the scene
+  ///          is expected to be titled as #kMainScene3dPlugin.
+  void Initialize();
+
+  /// \brief Handles the left click mouse event.
+  /// @param[in] _mouseEvent QMouseEvent pointer.
+  void MouseClickHandler(const QMouseEvent* _mouseEvent);
+
+  /// \brief Performs a raycast on the screen.
+  /// \param[in] screenX X screen's coordinate.
+  /// \param[in] screenY Y screen's coordinate.
+  /// \return A ignition::rendering::RayQueryResult.
+  ignition::rendering::RayQueryResult ScreenToScene(int _screenX, int _screenY) const;
+
+  /// \brief Filters by title all the children of the parent plugin.
+  /// \param _pluginTitle Title of the ignition::gui::plugin.
+  /// \return A pointer to the plugin if found, nullptr otherwise.
+  ignition::gui::Plugin* FilterPluginsByTitle(const std::string& _pluginTitle);
 
   /// \brief Holds the map file path.
   std::string mapFile{""};
@@ -131,6 +163,9 @@ class MaliputViewerPlugin : public ignition::gui::Plugin {
   /// \brief Holds the phase ring book file path.
   std::string phaseRingBookFile{""};
 
+  /// \brief Holds the title of the main Scene3D plugin.
+  std::string mainScene3dPluginTitle{"3D Scene"};
+
   /// @brief Triggers an event every `kTimerPeriodInMs` to try to get the scene.
   QBasicTimer timer;
 
@@ -146,6 +181,12 @@ class MaliputViewerPlugin : public ignition::gui::Plugin {
   /// \brief Holds a pointer to the scene.
   ignition::rendering::ScenePtr scene{nullptr};
 
+  /// \brief Holds a pointer to a ray query.
+  ignition::rendering::RayQueryPtr rayQuery{nullptr};
+
+  /// \brief Holds a pointer to the camera.
+  ignition::rendering::CameraPtr camera{};
+
   /// \brief Model that holds the meshes and the visualization status.
   std::unique_ptr<MaliputViewerModel> model{};
 };