From 1bd709ce162af99ee86bc8cf671496676b2bd2d8 Mon Sep 17 00:00:00 2001 From: Brian Date: Wed, 14 Aug 2019 17:34:59 -0300 Subject: [PATCH 1/3] Create arrow class and move it to selected lane --- delphyne-gui/visualizer/CMakeLists.txt | 1 + delphyne-gui/visualizer/arrow_mesh.cc | 85 +++++++++++++++++++ delphyne-gui/visualizer/arrow_mesh.hh | 71 ++++++++++++++++ .../visualizer/maliput_viewer_widget.cc | 5 ++ .../visualizer/maliput_viewer_widget.hh | 2 + .../visualizer/render_maliput_widget.cc | 33 ++++++- .../visualizer/render_maliput_widget.hh | 16 ++++ 7 files changed, 212 insertions(+), 1 deletion(-) create mode 100644 delphyne-gui/visualizer/arrow_mesh.cc create mode 100644 delphyne-gui/visualizer/arrow_mesh.hh 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..ddd0c931 --- /dev/null +++ b/delphyne-gui/visualizer/arrow_mesh.cc @@ -0,0 +1,85 @@ +// 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); + this->CreateCylinder(_scene); + 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); + this->cylinder->SetWorldPosition(_worldPosition.X(), _worldPosition.Y(), + _worldPosition.Z() + std::abs(minCylinderBoundingBox.Z())); + currentDirection = -1; + currentTick = 0; +} + +void ArrowMesh::SetVisibility(bool _visible) { + this->arrow->SetVisible(_visible); + this->cylinder->SetVisible(_visible); +} + +void ArrowMesh::Update() { + ignition::math::Vector3d worldPosition = this->arrow->WorldPosition(); + this->arrow->SetWorldPosition(worldPosition.X(), worldPosition.Y(), worldPosition.Z() + currentDirection * step); + ++currentTick; + if (currentTick == totalTicks) { + currentDirection = currentDirection * -1; + currentTick = 0; + } +} + +void ArrowMesh::CreateCylinder(ignition::rendering::ScenePtr& _scene) { + 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->cylinder = _scene->CreateVisual(); + this->cylinder->AddGeometry(_scene->CreateCylinder()); + this->cylinder->SetMaterial(material); + this->cylinder->SetVisible(false); + this->cylinder->SetWorldPosition(0., 0., 0.); + // This size is enough for visualization. + this->cylinder->SetWorldScale(0.5, 0.5, 0.2); + _scene->RootVisual()->AddChild(this->cylinder); + ignition::common::MeshManager* meshManager = ignition::common::MeshManager::Instance(); + const ignition::common::Mesh* unitCylinderMesh = meshManager->MeshByName("unit_cylinder"); + minCylinderBoundingBox = unitCylinderMesh->Min(); + minCylinderBoundingBox.X() = minCylinderBoundingBox.X() * 0.5; + minCylinderBoundingBox.Y() = minCylinderBoundingBox.Y() * 0.5; + minCylinderBoundingBox.Z() = minCylinderBoundingBox.Z() * 0.2; +} + +} // 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..7db19884 --- /dev/null +++ b/delphyne-gui/visualizer/arrow_mesh.hh @@ -0,0 +1,71 @@ +// 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 Creates a red cylinder that will be below the arrow to emphasize the click position. + void CreateCylinder(ignition::rendering::ScenePtr& _scene); + + /// \brief Bounding box of the cone in construction time. + ignition::math::Vector3d minArrowBoundingBox; + + /// \brief Bounding box of the cylinder in construction time + ignition::math::Vector3d minCylinderBoundingBox; + + /// \brief Visual pointer of the created cone. + ignition::rendering::VisualPtr arrow; + + /// \brief Small cylinder that will be below the arrow to emphasize the click position. + ignition::rendering::VisualPtr cylinder; + + /// \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; From ccbfdd18c9377934663624f0ea3190a00a7e1782 Mon Sep 17 00:00:00 2001 From: Brian Date: Tue, 27 Aug 2019 11:58:20 -0300 Subject: [PATCH 2/3] Remove cylinder --- delphyne-gui/visualizer/arrow_mesh.cc | 27 +-------------------------- delphyne-gui/visualizer/arrow_mesh.hh | 8 -------- 2 files changed, 1 insertion(+), 34 deletions(-) diff --git a/delphyne-gui/visualizer/arrow_mesh.cc b/delphyne-gui/visualizer/arrow_mesh.cc index ddd0c931..11bc37d5 100644 --- a/delphyne-gui/visualizer/arrow_mesh.cc +++ b/delphyne-gui/visualizer/arrow_mesh.cc @@ -28,7 +28,6 @@ ArrowMesh::ArrowMesh(ignition::rendering::ScenePtr& _scene, double _zOffset, dou this->arrow->SetWorldPosition(0., 0., 0.); this->arrow->SetWorldRotation(0, IGN_PI, 0.); _scene->RootVisual()->AddChild(this->arrow); - this->CreateCylinder(_scene); ignition::common::MeshManager* meshManager = ignition::common::MeshManager::Instance(); const ignition::common::Mesh* unitConeMesh = meshManager->MeshByName("unit_cone"); minArrowBoundingBox = unitConeMesh->Min(); @@ -40,46 +39,22 @@ void ArrowMesh::SelectAt(double _distanceFromCamera, const ignition::math::Vecto this->arrow->SetWorldScale(scaleIncrement, scaleIncrement, scaleIncrement); this->arrow->SetWorldPosition(_worldPosition.X(), _worldPosition.Y(), _worldPosition.Z() + std::abs(newMinArrowBBZAxis) + zOffset); - this->cylinder->SetWorldPosition(_worldPosition.X(), _worldPosition.Y(), - _worldPosition.Z() + std::abs(minCylinderBoundingBox.Z())); currentDirection = -1; currentTick = 0; } void ArrowMesh::SetVisibility(bool _visible) { this->arrow->SetVisible(_visible); - this->cylinder->SetVisible(_visible); } void ArrowMesh::Update() { ignition::math::Vector3d worldPosition = this->arrow->WorldPosition(); this->arrow->SetWorldPosition(worldPosition.X(), worldPosition.Y(), worldPosition.Z() + currentDirection * step); - ++currentTick; - if (currentTick == totalTicks) { + if (currentTick++ == totalTicks) { currentDirection = currentDirection * -1; currentTick = 0; } } -void ArrowMesh::CreateCylinder(ignition::rendering::ScenePtr& _scene) { - 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->cylinder = _scene->CreateVisual(); - this->cylinder->AddGeometry(_scene->CreateCylinder()); - this->cylinder->SetMaterial(material); - this->cylinder->SetVisible(false); - this->cylinder->SetWorldPosition(0., 0., 0.); - // This size is enough for visualization. - this->cylinder->SetWorldScale(0.5, 0.5, 0.2); - _scene->RootVisual()->AddChild(this->cylinder); - ignition::common::MeshManager* meshManager = ignition::common::MeshManager::Instance(); - const ignition::common::Mesh* unitCylinderMesh = meshManager->MeshByName("unit_cylinder"); - minCylinderBoundingBox = unitCylinderMesh->Min(); - minCylinderBoundingBox.X() = minCylinderBoundingBox.X() * 0.5; - minCylinderBoundingBox.Y() = minCylinderBoundingBox.Y() * 0.5; - minCylinderBoundingBox.Z() = minCylinderBoundingBox.Z() * 0.2; -} - } // namespace gui } // namespace delphyne diff --git a/delphyne-gui/visualizer/arrow_mesh.hh b/delphyne-gui/visualizer/arrow_mesh.hh index 7db19884..05882dce 100644 --- a/delphyne-gui/visualizer/arrow_mesh.hh +++ b/delphyne-gui/visualizer/arrow_mesh.hh @@ -35,21 +35,13 @@ class ArrowMesh { void Update(); private: - /// \brief Creates a red cylinder that will be below the arrow to emphasize the click position. - void CreateCylinder(ignition::rendering::ScenePtr& _scene); /// \brief Bounding box of the cone in construction time. ignition::math::Vector3d minArrowBoundingBox; - /// \brief Bounding box of the cylinder in construction time - ignition::math::Vector3d minCylinderBoundingBox; - /// \brief Visual pointer of the created cone. ignition::rendering::VisualPtr arrow; - /// \brief Small cylinder that will be below the arrow to emphasize the click position. - ignition::rendering::VisualPtr cylinder; - /// \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. From 98083d86d912eef05f285a3766d36c6e73967c8e Mon Sep 17 00:00:00 2001 From: Brian Date: Wed, 28 Aug 2019 14:49:15 -0300 Subject: [PATCH 3/3] Run clang format --- delphyne-gui/visualizer/arrow_mesh.cc | 4 +--- delphyne-gui/visualizer/arrow_mesh.hh | 1 - 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/delphyne-gui/visualizer/arrow_mesh.cc b/delphyne-gui/visualizer/arrow_mesh.cc index 11bc37d5..6a5ce9dd 100644 --- a/delphyne-gui/visualizer/arrow_mesh.cc +++ b/delphyne-gui/visualizer/arrow_mesh.cc @@ -43,9 +43,7 @@ void ArrowMesh::SelectAt(double _distanceFromCamera, const ignition::math::Vecto currentTick = 0; } -void ArrowMesh::SetVisibility(bool _visible) { - this->arrow->SetVisible(_visible); -} +void ArrowMesh::SetVisibility(bool _visible) { this->arrow->SetVisible(_visible); } void ArrowMesh::Update() { ignition::math::Vector3d worldPosition = this->arrow->WorldPosition(); diff --git a/delphyne-gui/visualizer/arrow_mesh.hh b/delphyne-gui/visualizer/arrow_mesh.hh index 05882dce..40465209 100644 --- a/delphyne-gui/visualizer/arrow_mesh.hh +++ b/delphyne-gui/visualizer/arrow_mesh.hh @@ -35,7 +35,6 @@ class ArrowMesh { void Update(); private: - /// \brief Bounding box of the cone in construction time. ignition::math::Vector3d minArrowBoundingBox;