Add arrow to selected lanes (#223)

* Create arrow class and move it to selected lane

delphyne-gui/visualizer/CMakeLists.txt

@@ -47,6 +47,7 @@ add_library(${maliput_viewer_widget} SHARED
   maliput_viewer_model.cc
   orbit_view_control.cc
   render_maliput_widget.cc
+  arrow_mesh.cc
   ${${maliput_viewer_widget}_MOC}
   ${LayerSelectionWidget_MOC}
   ${RenderMaliputWidget_MOC}

delphyne-gui/visualizer/arrow_mesh.cc

@@ -0,0 +1,58 @@
+// Copyright 2018 Toyota Research Institute
+
+#include "arrow_mesh.hh"
+#include <ignition/common/MeshManager.hh>
+#include <ignition/math/Helpers.hh>
+#include <ignition/rendering/Material.hh>
+#include <ignition/rendering/Mesh.hh>
+#include <ignition/rendering/Scene.hh>
+
+namespace delphyne {
+namespace gui {
+
+ArrowMesh::ArrowMesh(ignition::rendering::ScenePtr& _scene, double _zOffset, double _scaleFactor)
+    : zOffset(_zOffset),
+      scaleFactor(_scaleFactor),
+      distanceToMove(zOffset / 4.0),
+      step(0.005),
+      totalTicks(distanceToMove / step),
+      currentTick(0),
+      currentDirection(-1) {
+  ignition::rendering::MaterialPtr material = _scene->CreateMaterial();
+  material->SetDiffuse(255.0, 0.0, 0.0, 1.0);
+  material->SetAmbient(255.0, 0.0, 0.0, 1.0);
+  this->arrow = _scene->CreateVisual();
+  this->arrow->AddGeometry(_scene->CreateCone());
+  this->arrow->SetMaterial(material);
+  this->arrow->SetVisible(false);
+  this->arrow->SetWorldPosition(0., 0., 0.);
+  this->arrow->SetWorldRotation(0, IGN_PI, 0.);
+  _scene->RootVisual()->AddChild(this->arrow);
+  ignition::common::MeshManager* meshManager = ignition::common::MeshManager::Instance();
+  const ignition::common::Mesh* unitConeMesh = meshManager->MeshByName("unit_cone");
+  minArrowBoundingBox = unitConeMesh->Min();
+}
+
+void ArrowMesh::SelectAt(double _distanceFromCamera, const ignition::math::Vector3d& _worldPosition) {
+  const double scaleIncrement = 1.0 + scaleFactor * _distanceFromCamera;
+  const double newMinArrowBBZAxis = scaleIncrement * minArrowBoundingBox.Z();
+  this->arrow->SetWorldScale(scaleIncrement, scaleIncrement, scaleIncrement);
+  this->arrow->SetWorldPosition(_worldPosition.X(), _worldPosition.Y(),
+                                _worldPosition.Z() + std::abs(newMinArrowBBZAxis) + zOffset);
+  currentDirection = -1;
+  currentTick = 0;
+}
+
+void ArrowMesh::SetVisibility(bool _visible) { this->arrow->SetVisible(_visible); }
+
+void ArrowMesh::Update() {
+  ignition::math::Vector3d worldPosition = this->arrow->WorldPosition();
+  this->arrow->SetWorldPosition(worldPosition.X(), worldPosition.Y(), worldPosition.Z() + currentDirection * step);
+  if (currentTick++ == totalTicks) {
+    currentDirection = currentDirection * -1;
+    currentTick = 0;
+  }
+}
+
+}  // namespace gui
+}  // namespace delphyne

delphyne-gui/visualizer/arrow_mesh.hh

@@ -0,0 +1,62 @@
+// Copyright 2018 Toyota Research Institute
+
+#ifndef DELPHYNE_GUI_ARROW_MESH_HH
+#define DELPHYNE_GUI_ARROW_MESH_HH
+
+#include <ignition/math/Vector3.hh>
+#include <ignition/rendering/Scene.hh>
+
+namespace delphyne {
+namespace gui {
+
+/// \brief Renders a cone which will act as a pointing arrow when the user clicks over a lane
+class ArrowMesh {
+ public:
+  /// \brief Creates a cone and adds it as a child to the RootVisual of the scene, which will move
+  /// upwards and downwards for a fixed amount of ticks.
+  /// \param[in] _scene Scene pointer to create the cone.
+  /// \param[in] _zOffset Units above the pointed object that the cone tip should be.
+  /// \param[in] _scaleFactor Factor to increase/decrease the scale of the arrow based on the distance from the camera
+  /// to the clicked position.
+  ArrowMesh(ignition::rendering::ScenePtr& _scene, double _zOffset = 2.0, double _scaleFactor = 0.025);
+  /// \brief Destructor.
+  ~ArrowMesh() = default;
+
+  /// \brief Moves the arrow to a given world position and resets the downwards movement.
+  /// \param[in] _distanceFromCamera How far the camera is from the clicked point.
+  /// \param[in] _worldPosition World position of the cursor ray cast click.
+  void SelectAt(double _distanceFromCamera, const ignition::math::Vector3d& _worldPosition);
+
+  /// \brief Toggles the visibility of the arrow.
+  /// \param[in] _visible Boolean that determines if the arrow should be visible or not.
+  void SetVisibility(bool _visible);
+
+  /// \brief Updates the position of the arrow moving slightly the z axis.
+  void Update();
+
+ private:
+  /// \brief Bounding box of the cone in construction time.
+  ignition::math::Vector3d minArrowBoundingBox;
+
+  /// \brief Visual pointer of the created cone.
+  ignition::rendering::VisualPtr arrow;
+
+  /// \brief Units that the tip of the cone should be from the clicked position.
+  const double zOffset;
+  /// \brief Factor to increase/decrease the size of the cone.
+  const double scaleFactor;
+  /// \brief Distance to move downwards/upwards.
+  const double distanceToMove;
+  /// \brief How much units should the arrow move downwards/upwards per tick.
+  const double step;
+  /// \brief How much ticks required to move the arrow from down to up or up to down.
+  const int totalTicks;
+  /// \brief How many ticks passed since it started moving.
+  int currentTick;
+  /// \brief Tells the arrow if it should move upwards (1) or downwards (-1).
+  int currentDirection;
+};
+}  // namespace gui
+}  // namespace delphyne
+
+#endif  // DELPHYNE_GUI_ARROW_MESH_HH

delphyne-gui/visualizer/maliput_viewer_widget.cc

@@ -71,6 +71,7 @@ void MaliputViewerWidget::OnNewMultilaneFile(const std::string& filePath) {
     this->rulesVisualizerWiget->AddLaneId(lane_ids[i]);
   }
 
+  this->renderWidget->RenderArrow();
   this->renderWidget->RenderRoadMeshes(this->model->Meshes());
   this->renderWidget->RenderLabels(this->model->Labels());
 
@@ -93,6 +94,10 @@ void MaliputViewerWidget::OnVisualClicked(ignition::rendering::RayQueryResult ra
       const std::string& lane_id = lane->id().string();
       ignmsg << "Clicked lane ID: " << lane_id << "\n";
       OnRulesForLaneRequested(QString(lane_id.c_str()));
+      this->renderWidget->PutArrowAt(rayResult.distance, rayResult.point);
+      this->renderWidget->SetArrowVisibility(true);
+    } else {
+      this->renderWidget->SetArrowVisibility(false);
     }
   }
 }

delphyne-gui/visualizer/maliput_viewer_widget.hh

@@ -75,6 +75,8 @@ class MaliputViewerWidget : public ignition::gui::Plugin {
   /// \brief Builds the widgets of the GUI.
   void BuildGUI();
 
+  void RenderArrow();
+
   /// \brief Widget to hold and modify the visualization status of each layer.
   LayerSelectionWidget* layerSelectionWidget{nullptr};
 

delphyne-gui/visualizer/render_maliput_widget.cc

@@ -28,6 +28,10 @@
 
 #include <maliput-utilities/generate_obj.h>
 
+#include <memory>
+
+#include "arrow_mesh.hh"
+
 using namespace delphyne;
 using namespace gui;
 
@@ -57,6 +61,7 @@ RenderMaliputWidget::~RenderMaliputWidget() {
     // For right now, disable this, but we should debug this and re-enable this
     // cleanup.
     // this->engine->Fini();
+    this->arrow.reset();
     this->orbitViewControl.reset();
     this->camera->RemoveChildren();
     this->camera.reset();
@@ -417,6 +422,23 @@ void RenderMaliputWidget::RenderLabels(const std::map<MaliputLabelType, std::vec
   }
 }
 
+/////////////////////////////////////////////////
+void RenderMaliputWidget::RenderArrow() {
+  if (arrow == nullptr) {
+    arrow = std::make_unique<ArrowMesh>(this->scene, 0.5);
+  }
+}
+
+void RenderMaliputWidget::PutArrowAt(double _distance, const ignition::math::Vector3d& _worldPosition) {
+  DELPHYNE_DEMAND(this->arrow != nullptr);
+  this->arrow->SelectAt(_distance, _worldPosition);
+}
+
+void RenderMaliputWidget::SetArrowVisibility(bool _visible) {
+  DELPHYNE_DEMAND(arrow != nullptr);
+  arrow->SetVisibility(_visible);
+}
+
 /////////////////////////////////////////////////
 void RenderMaliputWidget::Clear() {
   // Clears the text labels.
@@ -460,6 +482,9 @@ void RenderMaliputWidget::paintEvent(QPaintEvent* _e) {
   if (this->renderWindow && this->camera) {
     this->camera->Update();
   }
+  if (arrow) {
+    arrow->Update();
+  }
 
   _e->accept();
 }
@@ -500,9 +525,10 @@ void RenderMaliputWidget::mousePressEvent(QMouseEvent* _e) {
     const ignition::rendering::RayQueryResult& rayResult = this->orbitViewControl->GetQueryResult();
     if (rayResult.distance > 0 && this->camera->Scene()->VisualById(rayResult.objectId) != nullptr) {
       emit VisualClicked(rayResult);
+    } else {
+      SetArrowVisibility(false);
     }
   }
-
   this->UpdateViewport();
 }
 
@@ -535,6 +561,11 @@ void RenderMaliputWidget::wheelEvent(QWheelEvent* _e) {
   }
 
   this->orbitViewControl->OnMouseWheel(_e);
+  const ignition::rendering::RayQueryResult& rayResult = this->orbitViewControl->GetQueryResult();
+  if (rayResult.distance > 0 && this->camera->Scene()->VisualById(rayResult.objectId) != nullptr) {
+    const double distance = this->camera->WorldPosition().Distance(this->orbitViewControl->GetQueryResult().point);
+    this->PutArrowAt(distance, this->orbitViewControl->GetQueryResult().point);
+  }
 
   this->UpdateViewport();
 }

delphyne-gui/visualizer/render_maliput_widget.hh

@@ -16,6 +16,7 @@
 
 #include <QtWidgets/QWidget>
 
+#include "arrow_mesh.hh"
 #include "maliput_viewer_model.hh"
 #include "orbit_view_control.hh"
 
@@ -71,6 +72,18 @@ class RenderMaliputWidget : public QWidget {
   // this method to properly set the transparency.
   void RenderLabels(const std::map<MaliputLabelType, std::vector<MaliputLabel>>& _labels);
 
+  /// \brief Create and render an arrow that will be positioned slightly above the selected road.
+  void RenderArrow();
+
+  /// \brief Move the arrow based on the distance travelled by the camera's ray distance.
+  /// \param[in] _distance Distance travelled from the camera's world position to the clicked object.
+  /// \param[in] _worldPosition Position where the camera's ray hit.
+  void PutArrowAt(double _distance, const ignition::math::Vector3d& _worldPosition);
+
+  /// \brief Change visibility of the arrow mesh.
+  /// \param[in] _visible Visibility of the arrow.
+  void SetArrowVisibility(bool _visible);
+
   /// \brief Clears all the references to text labels, meshes and the scene.
   void Clear();
 
@@ -189,6 +202,9 @@ class RenderMaliputWidget : public QWidget {
   /// \brief Controls the view of the scene.
   std::unique_ptr<OrbitViewControl> orbitViewControl;
 
+  /// \brief Arrow that points the location clicked in the visualizer.
+  std::unique_ptr<ArrowMesh> arrow;
+
   /// \brief A pointer to the rendering engine
   ignition::rendering::RenderEngine* engine;