diff --git a/delphyne-gui/visualizer/CMakeLists.txt b/delphyne-gui/visualizer/CMakeLists.txt index 95bf1965..5701c8bd 100644 --- a/delphyne-gui/visualizer/CMakeLists.txt +++ b/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} diff --git a/delphyne-gui/visualizer/arrow_mesh.cc b/delphyne-gui/visualizer/arrow_mesh.cc new file mode 100644 index 00000000..6a5ce9dd --- /dev/null +++ b/delphyne-gui/visualizer/arrow_mesh.cc @@ -0,0 +1,58 @@ +// Copyright 2018 Toyota Research Institute + +#include "arrow_mesh.hh" +#include +#include +#include +#include +#include + +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 diff --git a/delphyne-gui/visualizer/arrow_mesh.hh b/delphyne-gui/visualizer/arrow_mesh.hh new file mode 100644 index 00000000..40465209 --- /dev/null +++ b/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 +#include + +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 diff --git a/delphyne-gui/visualizer/maliput_viewer_widget.cc b/delphyne-gui/visualizer/maliput_viewer_widget.cc index 681c0687..11e42fc4 100644 --- a/delphyne-gui/visualizer/maliput_viewer_widget.cc +++ b/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); } } } diff --git a/delphyne-gui/visualizer/maliput_viewer_widget.hh b/delphyne-gui/visualizer/maliput_viewer_widget.hh index 2bf96367..cf384f02 100644 --- a/delphyne-gui/visualizer/maliput_viewer_widget.hh +++ b/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}; diff --git a/delphyne-gui/visualizer/render_maliput_widget.cc b/delphyne-gui/visualizer/render_maliput_widget.cc index 43cb1315..225f928c 100644 --- a/delphyne-gui/visualizer/render_maliput_widget.cc +++ b/delphyne-gui/visualizer/render_maliput_widget.cc @@ -28,6 +28,10 @@ #include +#include + +#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(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(); } diff --git a/delphyne-gui/visualizer/render_maliput_widget.hh b/delphyne-gui/visualizer/render_maliput_widget.hh index 9f588d54..82c94d66 100644 --- a/delphyne-gui/visualizer/render_maliput_widget.hh +++ b/delphyne-gui/visualizer/render_maliput_widget.hh @@ -16,6 +16,7 @@ #include +#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>& _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; + /// \brief Arrow that points the location clicked in the visualizer. + std::unique_ptr arrow; + /// \brief A pointer to the rendering engine ignition::rendering::RenderEngine* engine;