Tape Measure Plugin (#456)

Signed-off-by: John Shepherd <[email protected]>

CMakeLists.txt

@@ -75,7 +75,7 @@ set(IGN_FUEL_TOOLS_VER ${ignition-fuel_tools4_VERSION_MAJOR})
 
 #--------------------------------------
 # Find ignition-gui
-ign_find_package(ignition-gui3 REQUIRED VERSION 3.1)
+ign_find_package(ignition-gui3 REQUIRED VERSION 3.4)
 set(IGN_GUI_VER ${ignition-gui3_VERSION_MAJOR})
 ign_find_package (Qt5
   COMPONENTS

src/gui/plugins/CMakeLists.txt

@@ -89,6 +89,7 @@ add_subdirectory(playback_scrubber)
 add_subdirectory(resource_spawner)
 add_subdirectory(scene3d)
 add_subdirectory(shapes)
+add_subdirectory(tape_measure)
 add_subdirectory(transform_control)
 add_subdirectory(video_recorder)
 add_subdirectory(view_angle)

src/gui/plugins/scene3d/Scene3D.cc

@@ -55,6 +55,7 @@
 #include <ignition/transport/Node.hh>
 
 #include <ignition/gui/Conversions.hh>
+#include <ignition/gui/GuiEvents.hh>
 #include <ignition/gui/Application.hh>
 #include <ignition/gui/MainWindow.hh>
 
@@ -767,12 +768,44 @@ Entity IgnRenderer::UniqueId()
 void IgnRenderer::HandleMouseEvent()
 {
   std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+  this->BroadcastHoverPos();
+  this->BroadcastLeftClick();
   this->HandleMouseContextMenu();
   this->HandleModelPlacement();
   this->HandleMouseTransformControl();
   this->HandleMouseViewControl();
 }
 
+/////////////////////////////////////////////////
+void IgnRenderer::BroadcastHoverPos()
+{
+  if (this->dataPtr->hoverDirty)
+  {
+    math::Vector3d pos = this->ScreenToScene(this->dataPtr->mouseHoverPos);
+
+    ignition::gui::events::HoverToScene hoverToSceneEvent(pos);
+    ignition::gui::App()->sendEvent(
+        ignition::gui::App()->findChild<ignition::gui::MainWindow *>(),
+        &hoverToSceneEvent);
+  }
+}
+
+/////////////////////////////////////////////////
+void IgnRenderer::BroadcastLeftClick()
+{
+  if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT &&
+      this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE &&
+      !this->dataPtr->mouseEvent.Dragging() && this->dataPtr->mouseDirty)
+  {
+    math::Vector3d pos = this->ScreenToScene(this->dataPtr->mouseEvent.Pos());
+
+    ignition::gui::events::LeftClickToScene leftClickToSceneEvent(pos);
+    ignition::gui::App()->sendEvent(
+        ignition::gui::App()->findChild<ignition::gui::MainWindow *>(),
+        &leftClickToSceneEvent);
+  }
+}
+
 /////////////////////////////////////////////////
 void IgnRenderer::HandleMouseContextMenu()
 {

src/gui/plugins/scene3d/Scene3D.hh

@@ -351,6 +351,12 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
     /// \brief Handle model placement requests
     private: void HandleModelPlacement();
 
+    /// \brief Broadcasts the currently hovered 3d scene location.
+    private: void BroadcastHoverPos();
+
+    /// \brief Broadcasts a left click within the scene
+    private: void BroadcastLeftClick();
+
     /// \brief Generate a unique entity id.
     /// \return The unique entity id
     private: Entity UniqueId();

src/gui/plugins/tape_measure/CMakeLists.txt

@@ -0,0 +1,6 @@
+gz_add_gui_plugin(TapeMeasure
+  SOURCES TapeMeasure.cc
+  QT_HEADERS TapeMeasure.hh
+  PRIVATE_LINK_LIBS
+    ${IGNITION-RENDERING_LIBRARIES}
+)

src/gui/plugins/tape_measure/TapeMeasure.cc

@@ -0,0 +1,301 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "ignition/gazebo/gui/GuiEvents.hh"
+
+#include "TapeMeasure.hh"
+
+#include <iostream>
+#include <unordered_set>
+#include <string>
+#include <memory>
+
+#include <ignition/common/Console.hh>
+#include <ignition/gui/Application.hh>
+#include <ignition/gui/GuiEvents.hh>
+#include <ignition/gui/MainWindow.hh>
+#include <ignition/msgs/marker.pb.h>
+#include <ignition/msgs/Utility.hh>
+#include <ignition/plugin/Register.hh>
+#include <ignition/transport/Node.hh>
+#include <ignition/transport/Publisher.hh>
+
+namespace ignition::gazebo
+{
+  class TapeMeasurePrivate
+  {
+    /// \brief Ignition communication node.
+    public: transport::Node node;
+
+    /// \brief True if currently measuring, else false.
+    public: bool measure = false;
+
+    /// \brief The id of the start point marker.
+    public: const int kStartPointId = 1;
+
+    /// \brief The id of the end point marker.
+    public: const int kEndPointId = 2;
+
+    /// \brief The id of the line marker.
+    public: const int kLineId = 3;
+
+    /// \brief The id of the start or end point marker that is currently
+    /// being placed. This is primarily used to track the state machine of
+    /// the plugin.
+    public: int currentId = kStartPointId;
+
+    /// \brief The location of the placed starting point of the tape measure
+    /// tool, only set when the user clicks to set the point.
+    public: ignition::math::Vector3d startPoint =
+            ignition::math::Vector3d::Zero;
+
+    /// \brief The location of the placed ending point of the tape measure
+    /// tool, only set when the user clicks to set the point.
+    public: ignition::math::Vector3d endPoint = ignition::math::Vector3d::Zero;
+
+    /// \brief The color to set the marker when hovering the mouse over the
+    /// scene.
+    public: ignition::math::Color
+            hoverColor{ignition::math::Color(0.2, 0.2, 0.2, 0.5)};
+
+    /// \brief The color to draw the marker when the user clicks to confirm
+    /// its location.
+    public: ignition::math::Color
+            drawColor{ignition::math::Color(0.2, 0.2, 0.2, 1.0)};
+
+    /// \brief A set of the currently placed markers.  Used to make sure a
+    /// non-existent marker is not deleted.
+    public: std::unordered_set<int> placedMarkers;
+
+    /// \brief The current distance between the two points.  This distance
+    /// is updated as the user hovers the end point as well.
+    public: double distance = 0.0;
+
+    /// \brief The namespace that the markers for this plugin are placed in.
+    public: std::string ns = "tape_measure";
+  };
+}
+
+using namespace ignition;
+using namespace gazebo;
+
+/////////////////////////////////////////////////
+TapeMeasure::TapeMeasure()
+  : ignition::gui::Plugin(),
+  dataPtr(std::make_unique<TapeMeasurePrivate>())
+{
+}
+
+/////////////////////////////////////////////////
+TapeMeasure::~TapeMeasure() = default;
+
+/////////////////////////////////////////////////
+void TapeMeasure::LoadConfig(const tinyxml2::XMLElement *)
+{
+  if (this->title.empty())
+    this->title = "Tape measure";
+
+  ignition::gui::App()->findChild<ignition::gui::MainWindow *>
+      ()->installEventFilter(this);
+  ignition::gui::App()->findChild<ignition::gui::MainWindow *>
+      ()->QuickWindow()->installEventFilter(this);
+}
+
+/////////////////////////////////////////////////
+void TapeMeasure::OnMeasure()
+{
+  this->Measure();
+}
+
+/////////////////////////////////////////////////
+void TapeMeasure::Measure()
+{
+  this->Reset();
+  this->dataPtr->measure = true;
+  QGuiApplication::setOverrideCursor(Qt::CrossCursor);
+}
+
+/////////////////////////////////////////////////
+void TapeMeasure::OnReset()
+{
+  this->Reset();
+}
+
+/////////////////////////////////////////////////
+void TapeMeasure::Reset()
+{
+  this->DeleteMarker(this->dataPtr->kStartPointId);
+  this->DeleteMarker(this->dataPtr->kEndPointId);
+  this->DeleteMarker(this->dataPtr->kLineId);
+
+  this->dataPtr->currentId = this->dataPtr->kStartPointId;
+  this->dataPtr->startPoint = ignition::math::Vector3d::Zero;
+  this->dataPtr->endPoint = ignition::math::Vector3d::Zero;
+  this->dataPtr->distance = 0.0;
+  this->dataPtr->measure = false;
+  this->newDistance();
+  QGuiApplication::restoreOverrideCursor();
+}
+
+/////////////////////////////////////////////////
+double TapeMeasure::Distance()
+{
+  return this->dataPtr->distance;
+}
+
+/////////////////////////////////////////////////
+void TapeMeasure::DeleteMarker(int _id)
+{
+  if (this->dataPtr->placedMarkers.find(_id) ==
+      this->dataPtr->placedMarkers.end())
+    return;
+
+  // Delete the previously created marker
+  ignition::msgs::Marker markerMsg;
+  markerMsg.set_ns(this->dataPtr->ns);
+  markerMsg.set_id(_id);
+  markerMsg.set_action(ignition::msgs::Marker::DELETE_MARKER);
+  this->dataPtr->node.Request("/marker", markerMsg);
+  this->dataPtr->placedMarkers.erase(_id);
+}
+
+/////////////////////////////////////////////////
+void TapeMeasure::DrawPoint(int _id,
+    ignition::math::Vector3d &_point, ignition::math::Color &_color)
+{
+  this->DeleteMarker(_id);
+
+  ignition::msgs::Marker markerMsg;
+  markerMsg.set_ns(this->dataPtr->ns);
+  markerMsg.set_id(_id);
+  markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY);
+  markerMsg.set_type(ignition::msgs::Marker::SPHERE);
+  ignition::msgs::Set(markerMsg.mutable_material()->mutable_ambient(), _color);
+  ignition::msgs::Set(markerMsg.mutable_material()->mutable_diffuse(), _color);
+  ignition::msgs::Set(markerMsg.mutable_scale(),
+    ignition::math::Vector3d(0.1, 0.1, 0.1));
+  ignition::msgs::Set(markerMsg.mutable_pose(),
+    ignition::math::Pose3d(_point.X(), _point.Y(), _point.Z(), 0, 0, 0));
+
+  this->dataPtr->node.Request("/marker", markerMsg);
+  this->dataPtr->placedMarkers.insert(_id);
+}
+
+/////////////////////////////////////////////////
+void TapeMeasure::DrawLine(int _id, ignition::math::Vector3d &_startPoint,
+    ignition::math::Vector3d &_endPoint, ignition::math::Color &_color)
+{
+  this->DeleteMarker(_id);
+
+  ignition::msgs::Marker markerMsg;
+  markerMsg.set_ns(this->dataPtr->ns);
+  markerMsg.set_id(_id);
+  markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY);
+  markerMsg.set_type(ignition::msgs::Marker::LINE_LIST);
+  ignition::msgs::Set(markerMsg.mutable_material()->mutable_ambient(), _color);
+  ignition::msgs::Set(markerMsg.mutable_material()->mutable_diffuse(), _color);
+  ignition::msgs::Set(markerMsg.add_point(), _startPoint);
+  ignition::msgs::Set(markerMsg.add_point(), _endPoint);
+
+  this->dataPtr->node.Request("/marker", markerMsg);
+  this->dataPtr->placedMarkers.insert(_id);
+}
+
+/////////////////////////////////////////////////
+bool TapeMeasure::eventFilter(QObject *_obj, QEvent *_event)
+{
+  if (_event->type() == ignition::gui::events::HoverToScene::kType)
+  {
+    auto hoverToSceneEvent =
+        reinterpret_cast<ignition::gui::events::HoverToScene *>(_event);
+
+    // This event is called in Scene3d's RenderThread, so it's safe to make
+    // rendering calls here
+    if (this->dataPtr->measure && hoverToSceneEvent)
+    {
+      ignition::math::Vector3d point = hoverToSceneEvent->Point();
+      this->DrawPoint(this->dataPtr->currentId, point,
+        this->dataPtr->hoverColor);
+
+      // If the user is currently choosing the end point, draw the connecting
+      // line and update the new distance.
+      if (this->dataPtr->currentId == this->dataPtr->kEndPointId)
+      {
+        this->DrawLine(this->dataPtr->kLineId, this->dataPtr->startPoint,
+          point, this->dataPtr->hoverColor);
+        this->dataPtr->distance = this->dataPtr->startPoint.Distance(point);
+        this->newDistance();
+      }
+    }
+  }
+  else if (_event->type() == ignition::gui::events::LeftClickToScene::kType)
+  {
+    auto leftClickToSceneEvent =
+        reinterpret_cast<ignition::gui::events::LeftClickToScene *>(_event);
+
+    // This event is called in Scene3d's RenderThread, so it's safe to make
+    // rendering calls here
+    if (this->dataPtr->measure && leftClickToSceneEvent)
+    {
+      ignition::math::Vector3d point = leftClickToSceneEvent->Point();
+      this->DrawPoint(this->dataPtr->currentId, point,
+        this->dataPtr->drawColor);
+      // If the user is placing the start point, update its position
+      if (this->dataPtr->currentId == this->dataPtr->kStartPointId)
+      {
+        this->dataPtr->startPoint = point;
+      }
+      // If the user is placing the end point, update the end position,
+      // end the measurement state, and update the draw line and distance
+      else
+      {
+        this->dataPtr->endPoint = point;
+        this->dataPtr->measure = false;
+        this->DrawLine(this->dataPtr->kLineId, this->dataPtr->startPoint,
+          this->dataPtr->endPoint, this->dataPtr->drawColor);
+        this->dataPtr->distance =
+          this->dataPtr->startPoint.Distance(this->dataPtr->endPoint);
+        this->newDistance();
+        QGuiApplication::restoreOverrideCursor();
+      }
+      this->dataPtr->currentId = this->dataPtr->kEndPointId;
+    }
+  }
+  else if (_event->type() == QEvent::KeyPress)
+  {
+    QKeyEvent *keyEvent = static_cast<QKeyEvent*>(_event);
+    if (keyEvent && keyEvent->key() == Qt::Key_M)
+    {
+      this->Reset();
+      this->Measure();
+    }
+  }
+  else if (_event->type() == QEvent::KeyRelease)
+  {
+    QKeyEvent *keyEvent = static_cast<QKeyEvent*>(_event);
+    if (keyEvent && keyEvent->key() == Qt::Key_Escape &&
+        this->dataPtr->measure)
+    {
+      this->Reset();
+    }
+  }
+  return QObject::eventFilter(_obj, _event);
+}
+
+// Register this plugin
+IGNITION_ADD_PLUGIN(ignition::gazebo::TapeMeasure,
+                    ignition::gui::Plugin)