From c0cf397d412cc91a69a1cb6d3a457810e8e7933d Mon Sep 17 00:00:00 2001 From: Franco Cipollone <53065142+francocipollone@users.noreply.github.com> Date: Wed, 4 Aug 2021 09:03:15 -0300 Subject: [PATCH] Deprecates ign_gui0-based visualizer. (#423) --- .github/dependencies.repos | 4 - delphyne_gui/CMakeLists.txt | 5 +- delphyne_gui/package.xml | 2 - delphyne_gui/tools/maliput_viewer0.sh | 36 - delphyne_gui/visualizer/CMakeLists.txt | 175 +---- .../visualizer/display_plugins/CMakeLists.txt | 62 -- .../display_plugins/agent_info_display0.cc | 256 ------- .../display_plugins/agent_info_display0.hh | 69 -- .../display_plugins/origin_display0.cc | 79 -- .../display_plugins/origin_display0.hh | 34 - delphyne_gui/visualizer/icons/pause.svg | 85 --- delphyne_gui/visualizer/icons/play.svg | 76 -- delphyne_gui/visualizer/icons/rewind.svg | 88 --- delphyne_gui/visualizer/icons/step.svg | 83 -- .../visualizer/layer_selection_widget.cc | 269 ------- .../visualizer/layer_selection_widget.hh | 149 ---- .../visualizer/layout_for_playback.config | 138 ---- .../visualizer/layout_maliput_viewer.config | 20 - .../visualizer/layout_with_render_only.config | 27 - .../visualizer/layout_with_teleop.config | 178 ----- .../visualizer/maliput_viewer_widget.cc | 332 -------- .../visualizer/maliput_viewer_widget.hh | 199 ----- delphyne_gui/visualizer/orbit_view_control.cc | 149 ---- delphyne_gui/visualizer/orbit_view_control.hh | 115 --- delphyne_gui/visualizer/playback.qrc | 8 - delphyne_gui/visualizer/playback_widget.cc | 247 ------ delphyne_gui/visualizer/playback_widget.hh | 153 ---- .../visualizer/render_maliput_widget.cc | 624 --------------- .../visualizer/render_maliput_widget.hh | 272 ------- delphyne_gui/visualizer/render_widget.cc | 711 ------------------ delphyne_gui/visualizer/render_widget.hh | 254 ------- .../visualizer/rules_visualizer_widget.cc | 122 --- .../visualizer/rules_visualizer_widget.hh | 86 --- delphyne_gui/visualizer/teleop_widget.cc | 253 ------- delphyne_gui/visualizer/teleop_widget.hh | 117 --- delphyne_gui/visualizer/visualizer0.cc | 169 ----- 36 files changed, 2 insertions(+), 5644 deletions(-) delete mode 100755 delphyne_gui/tools/maliput_viewer0.sh delete mode 100644 delphyne_gui/visualizer/display_plugins/agent_info_display0.cc delete mode 100644 delphyne_gui/visualizer/display_plugins/agent_info_display0.hh delete mode 100644 delphyne_gui/visualizer/display_plugins/origin_display0.cc delete mode 100644 delphyne_gui/visualizer/display_plugins/origin_display0.hh delete mode 100644 delphyne_gui/visualizer/icons/pause.svg delete mode 100644 delphyne_gui/visualizer/icons/play.svg delete mode 100644 delphyne_gui/visualizer/icons/rewind.svg delete mode 100644 delphyne_gui/visualizer/icons/step.svg delete mode 100644 delphyne_gui/visualizer/layer_selection_widget.cc delete mode 100644 delphyne_gui/visualizer/layer_selection_widget.hh delete mode 100644 delphyne_gui/visualizer/layout_for_playback.config delete mode 100644 delphyne_gui/visualizer/layout_maliput_viewer.config delete mode 100644 delphyne_gui/visualizer/layout_with_render_only.config delete mode 100644 delphyne_gui/visualizer/layout_with_teleop.config delete mode 100644 delphyne_gui/visualizer/maliput_viewer_widget.cc delete mode 100644 delphyne_gui/visualizer/maliput_viewer_widget.hh delete mode 100644 delphyne_gui/visualizer/orbit_view_control.cc delete mode 100644 delphyne_gui/visualizer/orbit_view_control.hh delete mode 100644 delphyne_gui/visualizer/playback.qrc delete mode 100644 delphyne_gui/visualizer/playback_widget.cc delete mode 100644 delphyne_gui/visualizer/playback_widget.hh delete mode 100644 delphyne_gui/visualizer/render_maliput_widget.cc delete mode 100644 delphyne_gui/visualizer/render_maliput_widget.hh delete mode 100644 delphyne_gui/visualizer/render_widget.cc delete mode 100644 delphyne_gui/visualizer/render_widget.hh delete mode 100644 delphyne_gui/visualizer/rules_visualizer_widget.cc delete mode 100644 delphyne_gui/visualizer/rules_visualizer_widget.hh delete mode 100644 delphyne_gui/visualizer/teleop_widget.cc delete mode 100644 delphyne_gui/visualizer/teleop_widget.hh delete mode 100644 delphyne_gui/visualizer/visualizer0.cc diff --git a/.github/dependencies.repos b/.github/dependencies.repos index 5f264e2b..632083d3 100644 --- a/.github/dependencies.repos +++ b/.github/dependencies.repos @@ -3,10 +3,6 @@ repositories: type: git url: https://github.com/ToyotaResearchInstitute/ament_cmake_doxygen version: main - ign-gui0: - type: git - url: https://github.com/scpeters/ign-gui - version: gui0_citadel pybind11: type: git url: https://github.com/RobotLocomotion/pybind11.git diff --git a/delphyne_gui/CMakeLists.txt b/delphyne_gui/CMakeLists.txt index 42e85d13..ad74e3af 100644 --- a/delphyne_gui/CMakeLists.txt +++ b/delphyne_gui/CMakeLists.txt @@ -51,7 +51,6 @@ find_package(ament_cmake REQUIRED) find_package(ignition-common3 REQUIRED) find_package(ignition-math6 REQUIRED) find_package(ignition-msgs5 REQUIRED) -find_package(ignition-gui0 0.1 REQUIRED) find_package(ignition-gui3 REQUIRED) find_package(ignition-rendering3 REQUIRED) find_package(ignition-transport8 REQUIRED) @@ -110,9 +109,7 @@ add_subdirectory(python) ament_environment_hooks(setup.sh.in) ################################################# -# Installs maliput_viewer0.sh and maliput_viewer2.sh as a shortcut to run both -# visualizers. -install (PROGRAMS tools/maliput_viewer0.sh DESTINATION bin) +# Installs maliput_viewer2.sh as a shortcut to run the visualizer. install (PROGRAMS tools/maliput_viewer2.sh DESTINATION bin) ament_package() diff --git a/delphyne_gui/package.xml b/delphyne_gui/package.xml index 93a4b562..d897c588 100644 --- a/delphyne_gui/package.xml +++ b/delphyne_gui/package.xml @@ -17,7 +17,6 @@ ignition-msgs5 ignition-transport8 ignition-rendering3 - ignition-gui0 ignition-gui3 delphyne @@ -28,7 +27,6 @@ ignition-msgs5 ignition-transport8 ignition-rendering3 - ignition-gui0 ignition-gui3 delphyne diff --git a/delphyne_gui/tools/maliput_viewer0.sh b/delphyne_gui/tools/maliput_viewer0.sh deleted file mode 100755 index eabc3d58..00000000 --- a/delphyne_gui/tools/maliput_viewer0.sh +++ /dev/null @@ -1,36 +0,0 @@ -#!/bin/bash - -# Copyright 2018 Open Source Robotics Foundation -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# 3. Neither the name of the copyright holder nor the names of its contributors -# may be used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -# Shortcut to run `visualizer0` with the layout for the MaliputViewer. -# CLI arguments will be forwarded to `visualizer`. -# It is assumed that the environment is already configured. -visualizer0 \ - --layout=${DELPHYNE_GUI_RESOURCE_ROOT}/layouts/layout_maliput_viewer.config \ - "$@" diff --git a/delphyne_gui/visualizer/CMakeLists.txt b/delphyne_gui/visualizer/CMakeLists.txt index fdad9f45..78a3b1a0 100644 --- a/delphyne_gui/visualizer/CMakeLists.txt +++ b/delphyne_gui/visualizer/CMakeLists.txt @@ -25,176 +25,7 @@ install( ARCHIVE DESTINATION lib ) -#------------------------------------------------------------------------------- -# MaliputViewerWidget GUI plugin. -QT5_WRAP_CPP(MaliputViewerWidget_MOC maliput_viewer_widget.hh) -QT5_WRAP_CPP(LayerSelectionWidget_MOC layer_selection_widget.hh) -QT5_WRAP_CPP(RenderMaliputWidget_MOC render_maliput_widget.hh) -QT5_WRAP_CPP(RulesVisualizerWidget_MOC rules_visualizer_widget.hh) - -add_library(maliput_viewer_widget - ${CMAKE_CURRENT_SOURCE_DIR}/layer_selection_widget.cc - ${CMAKE_CURRENT_SOURCE_DIR}/maliput_viewer_widget.cc - ${CMAKE_CURRENT_SOURCE_DIR}/render_maliput_widget.cc - ${CMAKE_CURRENT_SOURCE_DIR}/rules_visualizer_widget.cc - orbit_view_control.cc - render_maliput_widget.cc - ${MaliputViewerWidget_MOC} - ${LayerSelectionWidget_MOC} - ${RenderMaliputWidget_MOC} - ${RulesVisualizerWidget_MOC} -) -add_library(delphyne_gui::maliput_viewer_widget ALIAS maliput_viewer_widget) -set_target_properties(maliput_viewer_widget - PROPERTIES - OUTPUT_NAME delphyne_gui_maliput_viewer_widget -) - -target_link_libraries(maliput_viewer_widget - ${drake_LIBRARIES} - delphyne_gui::arrow_mesh - delphyne_gui::global_attributes - delphyne_gui::maliput_viewer_model - delphyne_gui::selector - delphyne_gui::traffic_light_manager - ignition-common3::ignition-common3 - ignition-gui0::ignition-gui0 - ignition-msgs5::ignition-msgs5 - ignition-rendering3::ignition-rendering3 - ${Qt5Core_LIBRARIES} - ${Qt5Widgets_LIBRARIES} -) - -install( - TARGETS maliput_viewer_widget - EXPORT ${PROJECT_NAME}-targets - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib -) - -# RenderWidget GUI plugin. -QT5_WRAP_CPP(RenderWidget_MOC render_widget.hh) - -add_library(render_widget - ${CMAKE_CURRENT_SOURCE_DIR}/render_widget.cc - orbit_view_control.cc - ${RenderWidget_MOC} -) -add_library(delphyne_gui::render_widget ALIAS render_widget) -set_target_properties(render_widget - PROPERTIES - OUTPUT_NAME delphyne_gui_render_widget -) - -target_link_libraries(render_widget - ignition-common3::ignition-common3 - ignition-gui0::ignition-gui0 - ignition-msgs5::ignition-msgs5 - ignition-rendering3::ignition-rendering3 - ignition-transport8::ignition-transport8 - ${Qt5Core_LIBRARIES} - ${Qt5Widgets_LIBRARIES} - delphyne::protobuf_messages - delphyne::utility -) - -install( - TARGETS render_widget - EXPORT ${PROJECT_NAME}-targets - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib -) - -# TeleopWidget GUI plugin. -QT5_WRAP_CPP(TeleopWidget_MOC teleop_widget.hh) - -add_library(teleop_widget - ${CMAKE_CURRENT_SOURCE_DIR}/teleop_widget.cc - ${TeleopWidget_MOC} -) -add_library(delphyne_gui::teleop_widget ALIAS teleop_widget) -set_target_properties(teleop_widget - PROPERTIES - OUTPUT_NAME delphyne_gui_teleop_widget -) - -target_link_libraries(teleop_widget - ignition-common3::ignition-common3 - ignition-gui0::ignition-gui0 - ignition-msgs5::ignition-msgs5 - ignition-transport8::ignition-transport8 - ${Qt5Core_LIBRARIES} - ${Qt5Widgets_LIBRARIES} - delphyne::protobuf_messages -) - -install( - TARGETS teleop_widget - EXPORT ${PROJECT_NAME}-targets - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib -) - -# PlaybackWidget GUI plugin. -QT5_WRAP_CPP(PlaybackWidget_MOC playback_widget.hh) -QT5_ADD_RESOURCES(PlaybackWidget_resources_RCC playback.qrc) - -add_library(playback_widget - ${CMAKE_CURRENT_SOURCE_DIR}/playback_widget.cc - ${PlaybackWidget_MOC} - ${PlaybackWidget_resources_RCC} -) -add_library(delphyne_gui::playback_widget ALIAS playback_widget) -set_target_properties(playback_widget - PROPERTIES - OUTPUT_NAME delphyne_gui_playback_widget -) - -target_link_libraries(playback_widget - ignition-common3::ignition-common3 - ignition-gui0::ignition-gui0 - ignition-msgs5::ignition-msgs5 - ignition-transport8::ignition-transport8 - ${Qt5Core_LIBRARIES} - ${Qt5Widgets_LIBRARIES} - delphyne::protobuf_messages - delphyne::public_headers -) - -install( - TARGETS playback_widget - EXPORT ${PROJECT_NAME}-targets - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib -) - -#------------------------------------------------------------------------------- - -# Visualizer with ign-gui0 -add_executable(visualizer0 - visualizer0.cc -) - -target_link_libraries(visualizer0 - ignition-common3::ignition-common3 - ignition-gui0::ignition-gui0 - ${Qt5Core_LIBRARIES} - ${Qt5Widgets_LIBRARIES} - delphyne::utility - global_attributes -) - -install( - TARGETS visualizer0 - EXPORT ${PROJECT_NAME}-targets - DESTINATION bin -) - -# Visualizer with ign-gui3 +# Visualizer add_executable(visualizer visualizer2.cc ) @@ -217,10 +48,6 @@ install( # Install .config files. install( FILES - layout_with_render_only.config - layout_with_teleop.config - layout_maliput_viewer.config - layout_for_playback.config layout2_for_playback.config layout2_maliput_viewer.config layout2_with_teleop.config diff --git a/delphyne_gui/visualizer/display_plugins/CMakeLists.txt b/delphyne_gui/visualizer/display_plugins/CMakeLists.txt index cf0c8aa4..48a72abb 100644 --- a/delphyne_gui/visualizer/display_plugins/CMakeLists.txt +++ b/delphyne_gui/visualizer/display_plugins/CMakeLists.txt @@ -3,68 +3,6 @@ include_directories( ${CMAKE_SOURCE_DIR} ) -# Origin display plugin. -QT5_WRAP_CPP(OriginDisplay0_MOC origin_display0.hh) - -add_library(origin_display0 - ${CMAKE_CURRENT_SOURCE_DIR}/origin_display0.cc - ${OriginDisplay0_MOC} -) -add_library(delphyne_gui::origin_display0 ALIAS origin_display0) -set_target_properties(origin_display0 - PROPERTIES - OUTPUT_NAME delphyne_gui_origin_display0 -) - -target_link_libraries(origin_display0 - ignition-common3::ignition-common3 - ignition-gui0::ignition-gui0 - ignition-msgs5::ignition-msgs5 - ignition-transport8::ignition-transport8 - ${Qt5Core_LIBRARIES} - ${Qt5Widgets_LIBRARIES} - delphyne::protobuf_messages -) - -install( - TARGETS origin_display0 - EXPORT ${PROJECT_NAME}-targets - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib -) - -# Agent Info display plugin. -QT5_WRAP_CPP(AgentInfoDisplay0_MOC agent_info_display0.hh) - -add_library(agent_info_display0 - ${CMAKE_CURRENT_SOURCE_DIR}/agent_info_display0.cc - ${AgentInfoDisplay0_MOC} -) -add_library(delphyne_gui::agent_info_display0 ALIAS agent_info_display0) -set_target_properties(agent_info_display0 - PROPERTIES - OUTPUT_NAME delphyne_gui_agent_info_display0 -) - -target_link_libraries(agent_info_display0 - ignition-common3::ignition-common3 - ignition-gui0::ignition-gui0 - ignition-msgs5::ignition-msgs5 - ignition-transport8::ignition-transport8 - ${Qt5Core_LIBRARIES} - ${Qt5Widgets_LIBRARIES} - delphyne::protobuf_messages -) - -install( - TARGETS agent_info_display0 - EXPORT ${PROJECT_NAME}-targets - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib -) - #------------------------------------------------------------------------------- # AgentInfo display (ign-gui 3) QT5_WRAP_CPP(AgentInfoDisplay_MOC agent_info_display.hh) diff --git a/delphyne_gui/visualizer/display_plugins/agent_info_display0.cc b/delphyne_gui/visualizer/display_plugins/agent_info_display0.cc deleted file mode 100644 index 5e077d9c..00000000 --- a/delphyne_gui/visualizer/display_plugins/agent_info_display0.cc +++ /dev/null @@ -1,256 +0,0 @@ -// Copyright 2018 Toyota Research Institute - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "agent_info_display0.hh" - -namespace delphyne { -namespace gui { -namespace display_plugins { -struct AgentInfoText { - /// \brief The text display - ignition::rendering::TextPtr text; - ignition::rendering::VisualPtr textVis; - bool visible{true}; -}; - -class AgentInfoDisplayPrivate { - /// \brief Message holding latest agent states - public: - ignition::msgs::AgentState_V msg; - - /// \brief Mutex to protect msg - public: - std::recursive_mutex mutex; - - public: - QStackedLayout* stackedLayout; - - public: - QWidget* widget; - - public: - QSignalMapper* signalMapper; - - /// \brief A transport node. - public: - ignition::transport::Node node; - - public: - std::map> agentInfoText; -}; -} // namespace display_plugins -} // namespace gui -} // namespace delphyne - -static constexpr double charHeight = 0.3; - -using namespace delphyne; -using namespace gui; -using namespace display_plugins; - -///////////////////////////////////////////////// -AgentInfoDisplay::AgentInfoDisplay() : DisplayPlugin(), dataPtr(new AgentInfoDisplayPrivate) { - this->title = "Agent Info"; - this->dataPtr->node.Subscribe("agents/state", &AgentInfoDisplay::OnAgentState, this); - - this->dataPtr->signalMapper = new QSignalMapper(this); - connect(this->dataPtr->signalMapper, SIGNAL(mapped(QString)), this, SLOT(ToggleText(QString))); -} - -///////////////////////////////////////////////// -AgentInfoDisplay::~AgentInfoDisplay() {} - -std::string AgentInfoDisplay::NameFromAgent(const ignition::msgs::AgentState& agent) { - // The names that we get from the agents are of the form: - // - // "/agent/0/state" - // - // To reduce screen real estate, remove the "/agent/" from the start and - // "/state" from the rear. - return agent.name().substr(7, agent.name().length() - 7 - 6); -} - -///////////////////////////////////////////////// -void AgentInfoDisplay::ToggleText(const QString& _agentName) { - std::shared_ptr agentInfoText = this->dataPtr->agentInfoText[_agentName.toStdString()]; - - agentInfoText->visible = !agentInfoText->visible; - - if (agentInfoText->visible) { - agentInfoText->text->SetCharHeight(charHeight); - } else { - agentInfoText->text->SetCharHeight(0.0); - } -} - -///////////////////////////////////////////////// -QWidget* AgentInfoDisplay::CreateCustomProperties() const { - // This is more complicated than one might hope, mostly due to types. - // We really want a "stacked" layout so that we can dynamically add in a new - // layout once we get data from the ignition subscription. To acheive that, - // and keep the objects all attached to the window, we have to: - // - // 1. Create a "top-level" widget; we will hold a reference to this and return - // the pointer to it from this method. - // 2. Create a QStackedLayout that is the layout for the top-level widget. - // A QStackedLayout allows switching between layouts in a "stack", so that - // once we get the first message, we can overlay that new layout on the - // original, empty layout. - // - // Now when we want to attach something new to the stacked layout later, we - // have to: - // 1. Create a layout that contains those widgets. - // 2. Create the widgets that we actually want to see in the end. - // 3. Create a "dummy" top-level widget, and attach the above layout to - // that widget. - // 4. Attach the top-level widget to the stackedLayout. - // 5. Update the stackedLayout to point to the newest widget. - - this->dataPtr->widget = new QWidget(); - this->dataPtr->stackedLayout = new QStackedLayout(); - this->dataPtr->widget->setLayout(this->dataPtr->stackedLayout); - - return this->dataPtr->widget; -} - -///////////////////////////////////////////////// -std::shared_ptr AgentInfoDisplay::CreateAgentText( - const std::string& _agentName, QVBoxLayout* _layout, std::shared_ptr _scenePtr) { - auto visibleCheck = new QCheckBox(QString::fromStdString(_agentName), this); - visibleCheck->setChecked(true); - _layout->addWidget(visibleCheck); - - this->dataPtr->signalMapper->setMapping(visibleCheck, QString::fromStdString(_agentName)); - connect(visibleCheck, SIGNAL(clicked()), this->dataPtr->signalMapper, SLOT(map())); - - // Now that we've created the widgets, create the hovering text - auto agentInfoText = std::make_shared(); - - agentInfoText->visible = true; - - agentInfoText->text = _scenePtr->CreateText(); - agentInfoText->text->SetShowOnTop(true); - agentInfoText->text->SetCharHeight(charHeight); - - agentInfoText->textVis = _scenePtr->CreateVisual(); - agentInfoText->textVis->SetLocalScale(1.0, 1.0, 1.0); - agentInfoText->textVis->AddGeometry(agentInfoText->text); - - this->Visual()->AddChild(agentInfoText->textVis); - - this->dataPtr->agentInfoText[_agentName] = agentInfoText; - - return agentInfoText; -} - -///////////////////////////////////////////////// -void AgentInfoDisplay::UpdateAgentLabel(const ignition::msgs::AgentState& _agent, const std::string& _agentName, - std::shared_ptr _agentInfoText) { - double x = 0.0; - double y = 0.0; - double z = 0.0; - double roll = 0.0; - double pitch = 0.0; - double yaw = 0.0; - double vx = 0.0; - double vy = 0.0; - double vz = 0.0; - - if (_agent.has_position()) { - x = _agent.position().x(); - y = _agent.position().y(); - z = _agent.position().z(); - } - if (_agent.has_orientation()) { - roll = _agent.orientation().roll(); - pitch = _agent.orientation().pitch(); - yaw = _agent.orientation().yaw(); - } - if (_agent.has_linear_velocity()) { - vx = _agent.linear_velocity().x(); - vy = _agent.linear_velocity().y(); - vz = _agent.linear_velocity().z(); - } - - std::stringstream ss; - ss << _agentName << ":\n pos:(x:" << std::setprecision(2) << x << ",y:" << y << ",z:" << z << ",yaw:" << yaw << ")" - << "\n vel:(x:" << vx << ",y:" << vy << ",z:" << vz << ")"; - _agentInfoText->text->SetTextString(ss.str()); - _agentInfoText->textVis->SetLocalPose(ignition::math::Pose3d(x, y, z + 2.6, roll, pitch, yaw)); -} - -///////////////////////////////////////////////// -QVBoxLayout* AgentInfoDisplay::CreateLayout() { - // Step 1 from above - QVBoxLayout* layout = new QVBoxLayout(); - layout->setContentsMargins(0, 0, 0, 0); - layout->setSpacing(0); - - // Step 3 from above - auto widget = new QWidget(); - widget->setLayout(layout); - - // Step 4 from above - this->dataPtr->stackedLayout->addWidget(widget); - - // Step 5 from above - this->dataPtr->stackedLayout->setCurrentIndex(this->dataPtr->stackedLayout->count() - 1); - - return layout; -} - -///////////////////////////////////////////////// -void AgentInfoDisplay::ProcessMsg() { - std::lock_guard lock(this->dataPtr->mutex); - - std::shared_ptr scenePtr = this->Scene().lock(); - if (!scenePtr) { - ignerr << "Scene invalid. Agent Info display not initialized." << std::endl; - return; - } - - QVBoxLayout* layout{nullptr}; - - if (this->dataPtr->stackedLayout->count() == 0) { - // This is the first message; create the widgets and the hovering text that we'll use - layout = CreateLayout(); - // We delay doing step 2 until the loop below so we can just iterate over - // the agents once. - } - - for (int i = 0; i < this->dataPtr->msg.states_size(); ++i) { - ignition::msgs::AgentState agent = this->dataPtr->msg.states(i); - std::shared_ptr agentInfoText; - std::string agentName = NameFromAgent(agent); - - // Step 2 from above if necessary - if (layout != nullptr) { - agentInfoText = CreateAgentText(agentName, layout, scenePtr); - } else { - agentInfoText = this->dataPtr->agentInfoText[agentName]; - } - - UpdateAgentLabel(agent, agentName, agentInfoText); - } -} - -///////////////////////////////////////////////// -void AgentInfoDisplay::OnAgentState(const ignition::msgs::AgentState_V& _msg) { - std::lock_guard lock(this->dataPtr->mutex); - - this->dataPtr->msg.CopyFrom(_msg); - QMetaObject::invokeMethod(this, "ProcessMsg"); -} - -// Register this plugin -IGN_COMMON_REGISTER_SINGLE_PLUGIN(delphyne::gui::display_plugins::AgentInfoDisplay, ignition::gui::DisplayPlugin) diff --git a/delphyne_gui/visualizer/display_plugins/agent_info_display0.hh b/delphyne_gui/visualizer/display_plugins/agent_info_display0.hh deleted file mode 100644 index bc1b756b..00000000 --- a/delphyne_gui/visualizer/display_plugins/agent_info_display0.hh +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright 2018 Toyota Research Institute - -#ifndef DELPHYNE_GUI_DISPLAYS_AGENTINFODISPLAY0_HH -#define DELPHYNE_GUI_DISPLAYS_AGENTINFODISPLAY0_HH - -#include -#include - -#include -#include -#include -#include - -namespace delphyne { -namespace gui { -namespace display_plugins { - -struct AgentInfoText; -class AgentInfoDisplayPrivate; - -/// \brief Display the origin on an Ignition Rendering scene. -class AgentInfoDisplay : public ignition::gui::DisplayPlugin { - Q_OBJECT - - /// \brief Constructor - public: - AgentInfoDisplay(); - - /// \brief Destructor - public: - virtual ~AgentInfoDisplay(); - - private slots: - void ProcessMsg(); - - private slots: - void ToggleText(const QString& _agentName); - - // Documentation inherited - public: - QWidget* CreateCustomProperties() const override; - - private: - void OnAgentState(const ignition::msgs::AgentState_V& _msg); - - private: - std::string NameFromAgent(const ignition::msgs::AgentState& agent); - - private: - std::shared_ptr CreateAgentText(const std::string& _agentName, QVBoxLayout* _layout, - std::shared_ptr _scenePtr); - - private: - void UpdateAgentLabel(const ignition::msgs::AgentState& _agent, const std::string& _agentName, - std::shared_ptr _agentInfoText); - - private: - QVBoxLayout* CreateLayout(); - - /// \internal - /// \brief Pointer to private data. - private: - std::unique_ptr dataPtr; -}; -} // namespace display_plugins -} // namespace gui -} // namespace delphyne - -#endif diff --git a/delphyne_gui/visualizer/display_plugins/origin_display0.cc b/delphyne_gui/visualizer/display_plugins/origin_display0.cc deleted file mode 100644 index beff9d34..00000000 --- a/delphyne_gui/visualizer/display_plugins/origin_display0.cc +++ /dev/null @@ -1,79 +0,0 @@ -// Copyright 2018 Toyota Research Institute - -#include -#include -#include - -#include "origin_display0.hh" - -using namespace delphyne; -using namespace gui; -using namespace display_plugins; - -///////////////////////////////////////////////// -OriginDisplay::OriginDisplay() : DisplayPlugin() { this->title = "Origin"; } - -///////////////////////////////////////////////// -OriginDisplay::~OriginDisplay() {} - -///////////////////////////////////////////////// -void OriginDisplay::Initialize(const tinyxml2::XMLElement* /*_pluginElem*/) { - const double kAxisRadius = 0.02; - const double kAxisLength = 10000.0; - const double kAxisHalfLength = kAxisLength / 2.0; - if (auto scenePtr = this->Scene().lock()) { - // Create the visual axes. - std::array axes; - for (auto& axis : axes) { - axis = scenePtr->CreateVisual(); - if (!axis) { - ignerr << "Failed to create axis visual" << std::endl; - return; - } - axis->SetLocalScale(kAxisRadius, kAxisRadius, kAxisLength); - axis->AddGeometry(scenePtr->CreateCylinder()); - } - // X axis - double xPosition = kAxisHalfLength; - double yPosition = 0.0; - double zPosition = 0.0; - double roll = 0.0; - double pitch = IGN_PI_2; - double yaw = 0.0; - const ignition::math::Pose3d kAxisPoseX(xPosition, yPosition, zPosition, roll, pitch, yaw); - axes[0]->SetLocalPose(kAxisPoseX); - axes[0]->SetMaterial("Default/TransRed"); - - // Y axis - xPosition = 0.0; - yPosition = kAxisHalfLength; - zPosition = 0.0; - roll = IGN_PI_2; - pitch = 0.0; - yaw = 0.0; - const ignition::math::Pose3d kAxisPoseY(xPosition, yPosition, zPosition, roll, pitch, yaw); - axes[1]->SetLocalPose(kAxisPoseY); - axes[1]->SetMaterial("Default/TransGreen"); - - // Z axis - xPosition = 0.0; - yPosition = 0.0; - zPosition = kAxisHalfLength; - roll = 0.0; - pitch = 0.0; - yaw = 0.0; - const ignition::math::Pose3d kAxisPoseZ(xPosition, yPosition, zPosition, roll, pitch, yaw); - axes[2]->SetLocalPose(kAxisPoseZ); - axes[2]->SetMaterial("Default/TransBlue"); - - for (auto& axis : axes) { - this->Visual()->AddChild(axis); - } - } else { - ignerr << "Scene invalid. Origin display not initialized." << std::endl; - return; - } -} - -// Register this plugin -IGN_COMMON_REGISTER_SINGLE_PLUGIN(delphyne::gui::display_plugins::OriginDisplay, ignition::gui::DisplayPlugin) diff --git a/delphyne_gui/visualizer/display_plugins/origin_display0.hh b/delphyne_gui/visualizer/display_plugins/origin_display0.hh deleted file mode 100644 index b4ed5b64..00000000 --- a/delphyne_gui/visualizer/display_plugins/origin_display0.hh +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2018 Toyota Research Institute - -#ifndef DELPHYNE_GUI_DISPLAYS_ORIGINDISPLAY_HH -#define DELPHYNE_GUI_DISPLAYS_ORIGINDISPLAY_HH - -#include - -#include -#include - -namespace delphyne { -namespace gui { -namespace display_plugins { -/// \brief Display the origin on an Ignition Rendering scene. -class OriginDisplay : public ignition::gui::DisplayPlugin { - Q_OBJECT - - /// \brief Constructor - public: - OriginDisplay(); - - /// \brief Destructor - public: - virtual ~OriginDisplay(); - - // Documentation inherited - public: - virtual void Initialize(const tinyxml2::XMLElement* _pluginElem) override; -}; -} // namespace display_plugins -} // namespace gui -} // namespace delphyne - -#endif diff --git a/delphyne_gui/visualizer/icons/pause.svg b/delphyne_gui/visualizer/icons/pause.svg deleted file mode 100644 index aef1c59f..00000000 --- a/delphyne_gui/visualizer/icons/pause.svg +++ /dev/null @@ -1,85 +0,0 @@ - - - - - - - - - - - - image/svg+xml - - - - - - - - - - diff --git a/delphyne_gui/visualizer/icons/play.svg b/delphyne_gui/visualizer/icons/play.svg deleted file mode 100644 index 66dd6c76..00000000 --- a/delphyne_gui/visualizer/icons/play.svg +++ /dev/null @@ -1,76 +0,0 @@ - - - - - - - - - - - - image/svg+xml - - - - - - - - - diff --git a/delphyne_gui/visualizer/icons/rewind.svg b/delphyne_gui/visualizer/icons/rewind.svg deleted file mode 100644 index c44a177a..00000000 --- a/delphyne_gui/visualizer/icons/rewind.svg +++ /dev/null @@ -1,88 +0,0 @@ - - - - - - - - - - - - image/svg+xml - - - - - - - - - - - diff --git a/delphyne_gui/visualizer/icons/step.svg b/delphyne_gui/visualizer/icons/step.svg deleted file mode 100644 index 87f643b1..00000000 --- a/delphyne_gui/visualizer/icons/step.svg +++ /dev/null @@ -1,83 +0,0 @@ - - - - - - - - - - - - image/svg+xml - - - - - - - - - - diff --git a/delphyne_gui/visualizer/layer_selection_widget.cc b/delphyne_gui/visualizer/layer_selection_widget.cc deleted file mode 100644 index e200670c..00000000 --- a/delphyne_gui/visualizer/layer_selection_widget.cc +++ /dev/null @@ -1,269 +0,0 @@ -// Copyright 2018 Toyota Research Institute - -#include "layer_selection_widget.hh" -#include - -#include -#include -#include -#include -#include -#include - -using namespace delphyne; -using namespace gui; - -/////////////////////////////////////////////////////// -LayerSelectionWidget::LayerSelectionWidget(QWidget* parent) : QWidget(parent) { - // Build the widget. - this->Build(); - // Connects all the check box events. - QObject::connect(this->asphaltCheckBox, SIGNAL(stateChanged(int)), this, SLOT(onAsphaltValueChanged(int))); - QObject::connect(this->laneCheckBox, SIGNAL(stateChanged(int)), this, SLOT(onLaneValueChanged(int))); - QObject::connect(this->markerCheckBox, SIGNAL(stateChanged(int)), this, SLOT(onMarkerValueChanged(int))); - QObject::connect(this->hboundCheckBox, SIGNAL(stateChanged(int)), this, SLOT(onHBoundsValueChanged(int))); - QObject::connect(this->branchPointCheckBox, SIGNAL(stateChanged(int)), this, SLOT(onBranchPointValueChanged(int))); - QObject::connect(this->grayedAsphaltCheckBox, SIGNAL(stateChanged(int)), this, - SLOT(onGrayedAsphaltValueChanged(int))); - QObject::connect(this->grayedLaneCheckBox, SIGNAL(stateChanged(int)), this, SLOT(onGrayedLaneValueChanged(int))); - QObject::connect(this->grayedMarkerCheckBox, SIGNAL(stateChanged(int)), this, SLOT(onGrayedMarkerValueChanged(int))); -} - -/////////////////////////////////////////////////////// -LayerSelectionWidget::~LayerSelectionWidget() {} - -/////////////////////////////////////////////////////// -void LayerSelectionWidget::Build() { - this->asphaltCheckBox = new QCheckBox("Asphalt", this); - this->laneCheckBox = new QCheckBox("Lane", this); - this->markerCheckBox = new QCheckBox("Marker", this); - this->hboundCheckBox = new QCheckBox("HBounds", this); - this->branchPointCheckBox = new QCheckBox("Branch point", this); - this->grayedAsphaltCheckBox = new QCheckBox("Grayed asphalt", this); - this->grayedLaneCheckBox = new QCheckBox("Grayed lane", this); - this->grayedMarkerCheckBox = new QCheckBox("Grayed marker", this); - - // Sets all the check boxes as activated. - this->asphaltCheckBox->setChecked(true); - this->laneCheckBox->setChecked(true); - this->markerCheckBox->setChecked(true); - this->hboundCheckBox->setChecked(true); - this->branchPointCheckBox->setChecked(true); - // TODO(agalbachicar): Once grayed layers are created, we can allow to toggle - // them. In the meantime, we have just wired them and disabled. - this->grayedAsphaltCheckBox->setCheckable(false); - this->grayedLaneCheckBox->setCheckable(false); - this->grayedMarkerCheckBox->setCheckable(false); - - auto* layout = new QVBoxLayout(this); - layout->addWidget(asphaltCheckBox); - layout->addWidget(laneCheckBox); - layout->addWidget(markerCheckBox); - layout->addWidget(hboundCheckBox); - layout->addWidget(branchPointCheckBox); - layout->addWidget(grayedAsphaltCheckBox); - layout->addWidget(grayedLaneCheckBox); - layout->addWidget(grayedMarkerCheckBox); - auto* groupBox = new QGroupBox("Layers", this); - groupBox->setLayout(layout); - - auto* widgetLayout = new QVBoxLayout(this); - widgetLayout->addWidget(groupBox); - widgetLayout->addStretch(); - this->setLayout(widgetLayout); -} - -/////////////////////////////////////////////////////// -void LayerSelectionWidget::onAsphaltValueChanged(int) { - emit valueChanged(kAsphalt, this->asphaltCheckBox->isChecked()); -} - -/////////////////////////////////////////////////////// -void LayerSelectionWidget::onLaneValueChanged(int) { emit valueChanged(kLaneAll, this->laneCheckBox->isChecked()); } - -/////////////////////////////////////////////////////// -void LayerSelectionWidget::onMarkerValueChanged(int) { - emit valueChanged(kMarkerAll, this->markerCheckBox->isChecked()); -} - -/////////////////////////////////////////////////////// -void LayerSelectionWidget::onHBoundsValueChanged(int) { - emit valueChanged(kHBounds, this->hboundCheckBox->isChecked()); -} - -/////////////////////////////////////////////////////// -void LayerSelectionWidget::onBranchPointValueChanged(int) { - emit valueChanged(kBranchPointAll, this->branchPointCheckBox->isChecked()); -} - -/////////////////////////////////////////////////////// -void LayerSelectionWidget::onGrayedAsphaltValueChanged(int) { - emit valueChanged(kGrayedAsphalt, this->grayedAsphaltCheckBox->isChecked()); -} - -/////////////////////////////////////////////////////// -void LayerSelectionWidget::onGrayedLaneValueChanged(int) { - emit valueChanged(kGrayedLaneAll, this->grayedLaneCheckBox->isChecked()); -} - -/////////////////////////////////////////////////////// -void LayerSelectionWidget::onGrayedMarkerValueChanged(int) { - emit valueChanged(kGrayedMarkerAll, this->grayedMarkerCheckBox->isChecked()); -} - -/////////////////////////////////////////////////////// -LabelSelectionWidget::LabelSelectionWidget(QWidget* parent) : QWidget(parent) { - // Build the widget. - this->Build(); - // Connects all the check box events. - QObject::connect(this->laneCheckBox, SIGNAL(stateChanged(int)), this, SLOT(onLaneValueChanged(int))); - QObject::connect(this->branchPointCheckBox, SIGNAL(stateChanged(int)), this, SLOT(onBranchPointValueChanged(int))); -} - -/////////////////////////////////////////////////////// -LabelSelectionWidget::~LabelSelectionWidget() {} - -/////////////////////////////////////////////////////// -void LabelSelectionWidget::onLaneValueChanged(int) { - emit valueChanged(kLaneTextLabel, this->laneCheckBox->isChecked()); -} - -/////////////////////////////////////////////////////// -void LabelSelectionWidget::onBranchPointValueChanged(int) { - emit valueChanged(kBranchPointTextLabel, this->branchPointCheckBox->isChecked()); -} - -/////////////////////////////////////////////////////// -void LabelSelectionWidget::Build() { - this->laneCheckBox = new QCheckBox("Lane", this); - this->branchPointCheckBox = new QCheckBox("BranchPoint", this); - - // Sets all the check boxes as activated. - this->laneCheckBox->setChecked(true); - this->branchPointCheckBox->setChecked(true); - - auto* layout = new QVBoxLayout(this); - layout->addWidget(laneCheckBox); - layout->addWidget(branchPointCheckBox); - - auto* groupBox = new QGroupBox("Labels", this); - groupBox->setLayout(layout); - - auto* widgetLayout = new QVBoxLayout(this); - widgetLayout->addWidget(groupBox); - widgetLayout->addStretch(); - this->setLayout(widgetLayout); -} - -/////////////////////////////////////////////////////// -MaliputFileSelectionWidget::MaliputFileSelectionWidget(QWidget* parent) : QWidget(parent) { - if (!QDir::homePath().isEmpty()) { - this->fileDialogOpenPath = QDir::toNativeSeparators(QDir::homePath()).toStdString(); - } else { - this->fileDialogOpenPath = "/"; - } - // Build the widget. - this->Build(); - // Connects all the check box events. - QObject::connect(this->loadButton, SIGNAL(released()), this, SLOT(onLoadButtonPressed())); - QObject::connect(this->roadRulebookButton, SIGNAL(released()), this, SLOT(onRoadRulebookButtonPressed())); - QObject::connect(this->trafficLightRulesButton, SIGNAL(released()), this, SLOT(onTrafficLightrulesButtonPressed())); - QObject::connect(this->phaseRingButton, SIGNAL(released()), this, SLOT(onPhaseRingButtonPressed())); -} - -/////////////////////////////////////////////////////// -MaliputFileSelectionWidget::~MaliputFileSelectionWidget() {} - -/////////////////////////////////////////////////////// -void MaliputFileSelectionWidget::SetFileNameLabel(const std::string& fileName) { - this->fileNameLabel->setText(QString::fromStdString(fileName)); -} - -void MaliputFileSelectionWidget::ClearLineEdits(bool keepOldTextsAsPlaceholders) { - if (keepOldTextsAsPlaceholders) { - QFileInfo ruleFile(this->roadRulebookLineEdit->text()); - this->roadRulebookLineEdit->setPlaceholderText(ruleFile.baseName()); - - ruleFile.setFile(this->trafficLightRulesLineEdit->text()); - this->trafficLightRulesLineEdit->setPlaceholderText(ruleFile.baseName()); - - ruleFile.setFile(this->phaseRingLineEdit->text()); - this->phaseRingLineEdit->setPlaceholderText(ruleFile.baseName()); - } else { - this->roadRulebookLineEdit->setPlaceholderText(QString()); - this->trafficLightRulesLineEdit->setPlaceholderText(QString()); - this->phaseRingLineEdit->setPlaceholderText(QString()); - } - this->roadRulebookLineEdit->clear(); - this->trafficLightRulesLineEdit->clear(); - this->phaseRingLineEdit->clear(); -} - -/////////////////////////////////////////////////////// -void MaliputFileSelectionWidget::onLoadButtonPressed() { - QString fileName = QFileDialog::getOpenFileName(this, tr("Open Maliput XODR or YAML"), - QString::fromStdString(this->fileDialogOpenPath), - tr("XODR Files (*.xodr);;YAML files (*.yaml)")); - if (!fileName.isEmpty()) { - this->fileDialogOpenPath = fileName.toStdString(); - } - emit maliputFileChanged(fileName.toStdString(), this->roadRulebookLineEdit->text().toStdString(), - this->trafficLightRulesLineEdit->text().toStdString(), - this->phaseRingLineEdit->text().toStdString()); -} - -void MaliputFileSelectionWidget::onRoadRulebookButtonPressed() { - QString fileName = QFileDialog::getOpenFileName( - this, tr("Open Road Rulebook YAML"), QString::fromStdString(this->fileDialogOpenPath), tr("YAML files (*.yaml)")); - this->roadRulebookLineEdit->setText(fileName); -} - -void MaliputFileSelectionWidget::onTrafficLightrulesButtonPressed() { - QString fileName = - QFileDialog::getOpenFileName(this, tr("Open Traffic Lights Rulebook YAML"), - QString::fromStdString(this->fileDialogOpenPath), tr("YAML files (*.yaml)")); - this->trafficLightRulesLineEdit->setText(fileName); -} - -void MaliputFileSelectionWidget::onPhaseRingButtonPressed() { - QString fileName = - QFileDialog::getOpenFileName(this, tr("Open Traffic Lights Rulebook YAML"), - QString::fromStdString(this->fileDialogOpenPath), tr("YAML files (*.yaml)")); - this->phaseRingLineEdit->setText(fileName); -} - -/////////////////////////////////////////////////////// -void MaliputFileSelectionWidget::Build() { - this->loadButton = new QPushButton("Load", this); - this->fileNameLabel = new QLabel("", this); - this->roadRulebookLineEdit = new QLineEdit(this); - this->trafficLightRulesLineEdit = new QLineEdit(this); - this->phaseRingLineEdit = new QLineEdit(this); - this->roadRulebookButton = new QPushButton("Select road rulebook", this); - this->trafficLightRulesButton = new QPushButton("Select traffic lights book", this); - this->phaseRingButton = new QPushButton("Select phase ring book", this); - - QWidget* roadRulebookWidgetContainer = new QWidget(this); - QHBoxLayout* roadRulebookLayout = new QHBoxLayout(roadRulebookWidgetContainer); - roadRulebookLayout->addWidget(roadRulebookLineEdit); - roadRulebookLayout->addWidget(roadRulebookButton); - - QWidget* trafficLightRulebookWidgetContainer = new QWidget(this); - QHBoxLayout* trafficLightRulesLayout = new QHBoxLayout(trafficLightRulebookWidgetContainer); - trafficLightRulesLayout->addWidget(trafficLightRulesLineEdit); - trafficLightRulesLayout->addWidget(trafficLightRulesButton); - - QWidget* phaseRingWidgetContainer = new QWidget(this); - QHBoxLayout* phaseRingLayout = new QHBoxLayout(phaseRingWidgetContainer); - phaseRingLayout->addWidget(phaseRingLineEdit); - phaseRingLayout->addWidget(phaseRingButton); - - auto* layout = new QVBoxLayout(this); - layout->addWidget(fileNameLabel); - layout->addWidget(loadButton); - layout->addWidget(roadRulebookWidgetContainer); - layout->addWidget(trafficLightRulebookWidgetContainer); - layout->addWidget(phaseRingWidgetContainer); - - this->setLayout(layout); -} diff --git a/delphyne_gui/visualizer/layer_selection_widget.hh b/delphyne_gui/visualizer/layer_selection_widget.hh deleted file mode 100644 index 772bf986..00000000 --- a/delphyne_gui/visualizer/layer_selection_widget.hh +++ /dev/null @@ -1,149 +0,0 @@ -// Copyright 2018 Toyota Research Institute - -#ifndef DELPHYNE_GUI_LAYERSELECTIONWIDGET_HH -#define DELPHYNE_GUI_LAYERSELECTIONWIDGET_HH - -#include - -#include -#include -#include -#include -#include - -namespace delphyne { -namespace gui { - -/// \brief Widget with checkboxes to enable / disable mesh visualization. -class LayerSelectionWidget : public QWidget { - Q_OBJECT - - public: - /// \brief Default constructor. - explicit LayerSelectionWidget(QWidget* parent = 0); - - /// \brief Default Destructor. - virtual ~LayerSelectionWidget(); - - public slots: - void onAsphaltValueChanged(int state); - void onLaneValueChanged(int state); - void onMarkerValueChanged(int state); - void onHBoundsValueChanged(int state); - void onBranchPointValueChanged(int state); - void onGrayedAsphaltValueChanged(int state); - void onGrayedLaneValueChanged(int state); - void onGrayedMarkerValueChanged(int state); - - signals: - void valueChanged(const std::string& key, bool newValue); - - private: - /// \brief Builds the GUI with all the check boxes for mesh toggling. - void Build(); - - QCheckBox* asphaltCheckBox{nullptr}; - QCheckBox* laneCheckBox{nullptr}; - QCheckBox* markerCheckBox{nullptr}; - QCheckBox* hboundCheckBox{nullptr}; - QCheckBox* branchPointCheckBox{nullptr}; - QCheckBox* grayedAsphaltCheckBox{nullptr}; - QCheckBox* grayedLaneCheckBox{nullptr}; - QCheckBox* grayedMarkerCheckBox{nullptr}; - - // TODO(anyone): The below string constants are used as keys for the mesh map - // that contains all of the meshes of the loaded road geometry. These keys are - // currently in use in three separate files: `maliput_viewer_widget.cc`, - // `layer_selection_widget.cc`, and `generate_obj.cc` within maliput. These keys - // should be consolidated into a singular location to prevent any accidental - // disruption of functionality going forward. - - const std::string kAsphalt{"asphalt"}; - const std::string kLaneAll{"lane_all"}; - const std::string kMarkerAll{"marker_all"}; - const std::string kHBounds{"h_bounds"}; - const std::string kBranchPointAll{"branch_point_all"}; - const std::string kGrayedAsphalt{"grayed_asphalt"}; - const std::string kGrayedLaneAll{"grayed_lane_all"}; - const std::string kGrayedMarkerAll{"grayed_marker_all"}; -}; - -/// \brief Widget with checkboxes to enable / disable label visualization. -class LabelSelectionWidget : public QWidget { - Q_OBJECT - - public: - /// \brief Default constructor. - explicit LabelSelectionWidget(QWidget* parent = 0); - - /// \brief Default Destructor. - virtual ~LabelSelectionWidget(); - - public slots: - void onLaneValueChanged(int state); - void onBranchPointValueChanged(int state); - - signals: - void valueChanged(const std::string& key, bool newValue); - - private: - /// \brief Builds the GUI with all the check boxes for label toggling. - void Build(); - - QCheckBox* branchPointCheckBox{nullptr}; - QCheckBox* laneCheckBox{nullptr}; - - const std::string kLaneTextLabel{"lane_text_label"}; - const std::string kBranchPointTextLabel{"branch_point_text_label"}; -}; - -/// \brief Controls the button and the file dialog to load a yaml file. -class MaliputFileSelectionWidget : public QWidget { - Q_OBJECT - - public: - /// \brief Constructor. - explicit MaliputFileSelectionWidget(QWidget* parent = 0); - - /// \brief Destructor. - virtual ~MaliputFileSelectionWidget(); - - /// Sets @p fileName into the label to display the loaded file. - void SetFileNameLabel(const std::string& fileName); - - /// \brief Clear rules line edits. - /// \param[in] keepOldTextsAsPlaceholders Use the last file opened as a placeholder to remember the user which file - /// he used. - void ClearLineEdits(bool keepOldTextsAsPlaceholders = true); - - public slots: - void onLoadButtonPressed(); - - signals: - void maliputFileChanged(const std::string& filePath, const std::string& roadRulebookFilePath, - const std::string& trafficLightRulesFilePath, const std::string& phaseRingFilePath); - - private slots: - void onRoadRulebookButtonPressed(); - void onTrafficLightrulesButtonPressed(); - void onPhaseRingButtonPressed(); - - private: - /// \brief Builds the GUI with a button to load a file dialog and a label to - /// display the name of the loaded file. - void Build(); - - QPushButton* loadButton{nullptr}; - QLabel* fileNameLabel{nullptr}; - QPushButton* roadRulebookButton{nullptr}; - QPushButton* trafficLightRulesButton{nullptr}; - QPushButton* phaseRingButton{nullptr}; - QLineEdit* roadRulebookLineEdit{nullptr}; - QLineEdit* trafficLightRulesLineEdit{nullptr}; - QLineEdit* phaseRingLineEdit{nullptr}; - std::string fileDialogOpenPath{}; -}; - -} // namespace gui -} // namespace delphyne -#endif diff --git a/delphyne_gui/visualizer/layout_for_playback.config b/delphyne_gui/visualizer/layout_for_playback.config deleted file mode 100644 index d5ce0ff2..00000000 --- a/delphyne_gui/visualizer/layout_for_playback.config +++ /dev/null @@ -1,138 +0,0 @@ - - - - AAAA/wAAAAD9AAAAAQAAAAIAAAZZAAACw/wBAAAAAvwAAAAAAAAERgAAAtoA/////AIAAAAE+wAAABgAUgBlAG4AZABlAHIAVwBpAGQAZwBlAHQBAAAAFwAAAbH/////AP///vsAAAAcAFAAbABhAHkAYgBhAGMAawBXAGkAZABnAGUAdAEAAAHPAAAARQAAAEUA////+wAAABgAVABvAHAAaQBjAHMAIABzAHQAYQB0AHMBAAACGwAAAL8AAACgAP////sAAAAUAFQAaQBtAGUAIABwAGEAbgBlAGwBAAAClAAAAEYAAAAAAAAAAPwAAARNAAACDAAAAV4A/////AIAAAAG+wAAABgAVABvAHAAaQBjACAAdgBpAGUAdwBlAHIBAAAAFwAAAWIAAACgAP////sAAAAUAFMAYwBlAG4AZQAgAHQAcgBlAGUBAAABgAAAAVoAAAB/AP////sAAAAIAFQAaQBtAGUBAAABugAAAHkAAAAAAAAAAPsAAAAYAFQAaQBtAGUAIABjAG8AbgB0AHIAbwBsAQAAAdkAAABeAAAAAAAAAAD7AAAAHgBUAG8AcABpAGMAIABpAG4AdABlAHIAZgBhAGMAZQEAAAHOAAAAewAAAAAAAAAA/AAAAjkAAAChAAAAAAD////8AQAAAAL7AAAAGABUAGUAbABlAG8AcABXAGkAZABnAGUAdAEAAAOjAAABjQAAAAAAAAAA+wAAABgAVABlAGwAZQBvAHAAVwBpAGQAZwBlAHQAAAAERAAAAOwAAAAAAAAAAAAABlkAAAAAAAAABAAAAAQAAAAIAAAACPwAAAAA - 1328 - 730 - - /* ---------------------------- */ - /* VariablePill */ - /* ---------------------------- */ - - /* General */ - ignition--gui--VariablePill - { - border-radius: 10px; - margin: 0px; - } - - ignition--gui--VariablePill > QLabel - { - border-radius: 10px; - color: #ffffff; - padding-left: 8px; - padding-right: 8px; - padding-top: 2px; - padding-bottom: 2px; - } - - /* Not selected pill */ - ignition--gui--VariablePill[multiPillParent=false][selectedPill=false], - ignition--gui--VariablePill[multiPillParent=true][selectedPill=false] > QLabel - { - border: 1.5px solid #2196f3; - } - - /* Selected pill */ - ignition--gui--VariablePill[multiPillParent=false][selectedPill=true], - ignition--gui--VariablePill[multiPillParent=true][selectedPill=true] > QLabel - { - border: 1.5px solid #1565c0; - } - - /* Not child */ - ignition--gui--VariablePill[multiPillChild=false], - ignition--gui--VariablePill[multiPillParent=false][multiPillChild=false] > QLabel - { - border-radius: 10px; - border: 0.5px solid #2196f3; - background-color: #2196f3; - } - - /* Child */ - ignition--gui--VariablePill[multiPillChild=true], - ignition--gui--VariablePill[multiPillParent=true][multiPillChild=false] > QLabel - { - background-color: #64b5f6; - } - - /* ---------------------------- */ - /* Buttons */ - /* ---------------------------- */ - QPushButton#collapsibleButton - { - padding: 8px; - } - - - - - Plot - Responder - TopicInterface - TopicsStats - TopicViewer - - - position - - - false - - - Scene tree - /scene - ignition.msgs.Scene - ambient - background - fog - grid - header - joint - light - model::deleted - model::header - model::id - model::is_static - model::joint - model::joint::bounce - model::joint::cfm - model::joint::child_id - model::joint::fudge_factor - model::joint::gearbox - model::joint::header - model::joint::id - model::joint::limit_cfm - model::joint::limit_erp - model::joint::parent_id - model::joint::screw - model::joint::sensor - model::joint::suspension_cfm - model::joint::suspension_erp - model::link::battery - model::link::canonical - model::link::collision - model::link::density - model::link::enabled - model::link::gravity - model::link::header - model::link::id - model::link::inertial - model::link::kinematic - model::link::name - model::link::projector - model::link::self_collide - model::link::sensor - model::link::visual - model::model - model::scale - model::self_collide - model::visual - name - origin_visual - shadows - sky - - - - diff --git a/delphyne_gui/visualizer/layout_maliput_viewer.config b/delphyne_gui/visualizer/layout_maliput_viewer.config deleted file mode 100644 index 6ae80d82..00000000 --- a/delphyne_gui/visualizer/layout_maliput_viewer.config +++ /dev/null @@ -1,20 +0,0 @@ - - - - AAAA/wAAAAD9AAAAAQAAAAIAAAUwAAAC2vwBAAAAAvwAAAAAAAADTAAAAtoA/////AIAAAAC+wAAABgAUgBlAG4AZABlAHIAVwBpAGQAZwBlAHQBAAAAAAAAAhkAAAB9AP////sAAAAYAFQAbwBwAGkAYwBzACAAcwB0AGEAdABzAQAAAh8AAAC7AAAAmgD////8AAADUgAAAd4AAAHaAP////wCAAAAAvsAAAAYAFQAbwBwAGkAYwAgAHYAaQBlAHcAZQByAQAAAAAAAAJJAAAAmgD////8AAACTwAAAIsAAACLAP////wBAAAAAvsAAAAYAFQAZQBsAGUAbwBwAFcAaQBkAGcAZQB0AQAAA1IAAADsAAAA6gD////7AAAAGABUAGUAbABlAG8AcABXAGkAZABnAGUAdAEAAAREAAAA7AAAAOoA////AAAFMAAAAAAAAAAEAAAABAAAAAgAAAAI/AAAAAA= - 278 - 172 - 1328 - 730 - position - - /* We need an empty stylesheet so that it loads the system's default style */ - - - - - - - - false - diff --git a/delphyne_gui/visualizer/layout_with_render_only.config b/delphyne_gui/visualizer/layout_with_render_only.config deleted file mode 100644 index ecdcfdc4..00000000 --- a/delphyne_gui/visualizer/layout_with_render_only.config +++ /dev/null @@ -1,27 +0,0 @@ - - - - AAAA/wAAAAD9AAAAAQAAAAIAAAUwAAACwfwBAAAAAvwAAAAAAAADvwAAAtoA/////AIAAAAD+wAAABgAUgBlAG4AZABlAHIAVwBpAGQAZwBlAHQBAAAAGQAAAeX/////AP///vsAAAAYAFQAbwBwAGkAYwBzACAAcwB0AGEAdABzAQAAAgQAAADWAAAAmgD////7AAAAFABUAGkAbQBlACAAcABhAG4AZQBsAQAAApQAAABGAAAAAAAAAAD8AAADxQAAAWsAAAFrAAABa/wCAAAABfsAAAAYAFQAbwBwAGkAYwAgAHYAaQBlAHcAZQByAQAAABkAAAEuAAAAmgD////7AAAAFABTAGMAZQBuAGUAIAB0AHIAZQBlAQAAAU0AAAEpAAAAeQD////7AAAAGABUAGkAbQBlACAAYwBvAG4AdAByAG8AbAEAAAJ8AAAAXgAAAFwAAABc+wAAAB4AVABvAHAAaQBjACAAaQBuAHQAZQByAGYAYQBjAGUBAAABzgAAAHsAAAAAAAAAAPwAAAJPAAAAiwAAAAAA/////AEAAAAC+wAAABgAVABlAGwAZQBvAHAAVwBpAGQAZwBlAHQBAAAC4AAAAlAAAAAAAAAAAPsAAAAYAFQAZQBsAGUAbwBwAFcAaQBkAGcAZQB0AAAABEQAAADsAAAAAAAAAAAAAAUwAAAAAAAAAAQAAAAEAAAACAAAAAj8AAAAAA== - 278 - 172 - 1328 - 730 - position - - /* We need an empty stylesheet so that it loads the system's default style */ - - - - Requester - Responder - delphyne_gui_teleop_widget - TopicsStats - TopicViewer - - - - - - false - false - diff --git a/delphyne_gui/visualizer/layout_with_teleop.config b/delphyne_gui/visualizer/layout_with_teleop.config deleted file mode 100644 index 6678ad8c..00000000 --- a/delphyne_gui/visualizer/layout_with_teleop.config +++ /dev/null @@ -1,178 +0,0 @@ - - - - 278 - 172 - AAAA/wAAAAD9AAAAAQAAAAIAAAUwAAACxvwBAAAAAvwAAAAAAAADXwAAAtoA/////AIAAAAD+wAAABgAUgBlAG4AZABlAHIAVwBpAGQAZwBlAHQBAAAAFAAAAej/////AP///vsAAAAYAFQAbwBwAGkAYwBzACAAcwB0AGEAdABzAQAAAgIAAADYAAAAlgD////7AAAAFABUAGkAbQBlACAAcABhAG4AZQBsAQAAApQAAABGAAAAAAAAAAD8AAADZQAAAcsAAAFeAP////wCAAAAB/sAAAAYAFQAbwBwAGkAYwAgAHYAaQBlAHcAZQByAQAAABQAAACpAAAAlgD////7AAAAFABTAGMAZQBuAGUAIAB0AHIAZQBlAQAAAMMAAACnAAAAdgD////7AAAAEABEAGkAcwBwAGwAYQB5AHMBAAABcAAAAHYAAAB2AP////sAAAAIAFQAaQBtAGUBAAAB7AAAAGYAAABmAP////sAAAAYAFQAaQBtAGUAIABjAG8AbgB0AHIAbwBsAQAAAdkAAABeAAAAAAAAAAD7AAAAHgBUAG8AcABpAGMAIABpAG4AdABlAHIAZgBhAGMAZQEAAAHOAAAAewAAAAAAAAAA/AAAAlgAAACCAAAAggD////8AQAAAAL7AAAAGABUAGUAbABlAG8AcABXAGkAZABnAGUAdAEAAANlAAABywAAANgA////+wAAABgAVABlAGwAZQBvAHAAVwBpAGQAZwBlAHQAAAAERAAAAOwAAAAAAAAAAAAABTAAAAAAAAAABAAAAAQAAAAIAAAACPwAAAAA - 1328 - 730 - position - - /* ---------------------------- */ - /* VariablePill */ - /* ---------------------------- */ - - /* General */ - ignition--gui--VariablePill - { - border-radius: 10px; - margin: 0px; - } - - ignition--gui--VariablePill > QLabel - { - border-radius: 10px; - color: #ffffff; - padding-left: 8px; - padding-right: 8px; - padding-top: 2px; - padding-bottom: 2px; - } - - /* Not selected pill */ - ignition--gui--VariablePill[multiPillParent=false][selectedPill=false], - ignition--gui--VariablePill[multiPillParent=true][selectedPill=false] > QLabel - { - border: 1.5px solid #2196f3; - } - - /* Selected pill */ - ignition--gui--VariablePill[multiPillParent=false][selectedPill=true], - ignition--gui--VariablePill[multiPillParent=true][selectedPill=true] > QLabel - { - border: 1.5px solid #1565c0; - } - - /* Not child */ - ignition--gui--VariablePill[multiPillChild=false], - ignition--gui--VariablePill[multiPillParent=false][multiPillChild=false] > QLabel - { - border-radius: 10px; - border: 0.5px solid #2196f3; - background-color: #2196f3; - } - - /* Child */ - ignition--gui--VariablePill[multiPillChild=true], - ignition--gui--VariablePill[multiPillParent=true][multiPillChild=false] > QLabel - { - background-color: #64b5f6; - } - - /* ---------------------------- */ - /* Buttons */ - /* ---------------------------- */ - QPushButton#collapsibleButton - { - padding: 8px; - } - - - - - Plot - Requester - Responder - delphyne_gui_teleop_widget - TimePanel - TopicInterface - TopicsStats - TopicViewer - - - - - false - - - - Time - - true - false - /world_control - - - - true - /world_stats - - - - - Scene tree - /scene - ignition.msgs.Scene - ambient - background - fog - grid - header - joint - light - model::deleted - model::header - model::id - model::is_static - model::joint - model::joint::bounce - model::joint::cfm - model::joint::child_id - model::joint::fudge_factor - model::joint::gearbox - model::joint::header - model::joint::id - model::joint::limit_cfm - model::joint::limit_erp - model::joint::parent_id - model::joint::screw - model::joint::sensor - model::joint::suspension_cfm - model::joint::suspension_erp - model::link::battery - model::link::canonical - model::link::collision - model::link::density - model::link::enabled - model::link::gravity - model::link::header - model::link::id - model::link::inertial - model::link::kinematic - model::link::name - model::link::projector - model::link::self_collide - model::link::sensor - model::link::visual - model::model - model::scale - model::self_collide - model::visual - name - origin_visual - shadows - sky - - - - - - - - 0 - - - - - - - 250 - 0 - 1.000000 - 0 0 0 0 -0 0 - 0.7 0.7 0.7 0.3 - - - - - diff --git a/delphyne_gui/visualizer/maliput_viewer_widget.cc b/delphyne_gui/visualizer/maliput_viewer_widget.cc deleted file mode 100644 index 3af3b50a..00000000 --- a/delphyne_gui/visualizer/maliput_viewer_widget.cc +++ /dev/null @@ -1,332 +0,0 @@ -// Copyright 2017 Toyota Research Institute - -#include "maliput_viewer_widget.hh" - -#include -#include -#include - -#include "global_attributes.hh" -#include "rules_visualizer_widget.hh" - -using namespace delphyne; -using namespace gui; - -///////////////////////////////////////////////// -MaliputViewerWidget::MaliputViewerWidget(QWidget*) : Plugin() { - // Loads the GUI. - this->BuildGUI(); - // Loads the maliput file path if any and parses it. - this->model = std::make_unique(); - if (GlobalAttributes::HasArgument("xodr_file")) { - if (GlobalAttributes::HasArgument("malidrive_backend")) { - this->model->SetOpenDriveBackend(GlobalAttributes::GetArgument("malidrive_backend")); - } - this->model->Load(GlobalAttributes::GetArgument("xodr_file")); - this->VisualizeFileName(GlobalAttributes::GetArgument("xodr_file")); - } else if (GlobalAttributes::HasArgument("yaml_file")) { - this->model->Load(GlobalAttributes::GetArgument("yaml_file")); - this->VisualizeFileName(GlobalAttributes::GetArgument("yaml_file")); - } - - for (const std::string& key : {kLane, kMarker, kBranchPoint, kBranchPointLabels, kLaneLabels}) { - meshDefaults[key] = true; - } - - QObject::connect(this->layerSelectionWidget, SIGNAL(valueChanged(const std::string&, bool)), this, - SLOT(OnLayerMeshChanged(const std::string&, bool))); - - QObject::connect(this->labelSelectionWidget, SIGNAL(valueChanged(const std::string&, bool)), this, - SLOT(OnTextLabelChanged(const std::string&, bool))); - - QObject::connect( - this->maliputFileSelectionWidget, - SIGNAL(maliputFileChanged(const std::string&, const std::string&, const std::string&, const std::string&)), this, - SLOT(OnNewRoadNetwork(const std::string&, const std::string&, const std::string&, const std::string&))); -} - -///////////////////////////////////////////////// -void MaliputViewerWidget::UpdateMeshDefaults(const std::string& key, bool newValue) { - // Store the value the mesh should return to if not selected - if (FoundKeyword(key, kMarker)) { - this->meshDefaults[kMarker] = newValue; - } else if (FoundKeyword(key, kLaneTextLabel)) { - this->meshDefaults[kLaneLabels] = newValue; - } else if (FoundKeyword(key, kBranchPointTextLabel)) { - this->meshDefaults[kBranchPointLabels] = newValue; - } else if (FoundKeyword(key, kLane)) { - this->meshDefaults[kLane] = newValue; - } else if (FoundKeyword(key, kBranchPoint)) { - this->meshDefaults[kBranchPoint] = newValue; - } -} - -///////////////////////////////////////////////// -bool MaliputViewerWidget::FoundKeyword(const std::string& key, const std::string& keyword) { - return key.find(keyword) != std::string::npos; -} - -///////////////////////////////////////////////// -std::string MaliputViewerWidget::GetID(const std::string& keyword) { - const std::size_t firstNum = keyword.find_first_of("0123456789"); - return keyword.substr(firstNum, keyword.length() - firstNum + 1); -} - -///////////////////////////////////////////////// -void MaliputViewerWidget::OnLayerMeshChanged(const std::string& key, bool newValue) { - // If the keyword "all" is found, enable all of the parsed type - const std::size_t all_keyword = key.find(kAll); - if (FoundKeyword(key, kAll)) { - const std::string keyword = key.substr(0, all_keyword); - this->UpdateMeshDefaults(key, newValue); - for (auto const& it : this->model->Meshes()) { - if (FoundKeyword(it.first, keyword)) { - const std::string id = GetID(it.first); - // If the region is not selected, update with the default setting - if (!this->renderWidget->IsSelected(id)) { - // Updates the model. - this->model->SetLayerState(it.first, newValue); - } - } - } - } else { - // Updates the model. - this->model->SetLayerState(key, newValue); - // If the asphalt is turned off, deselect all lanes - if (FoundKeyword(key, kAsphalt) && !newValue) { - this->renderWidget->DeselectAll(); - this->renderWidget->SetArrowVisibility(false); - } - } - - // Replicates into the GUI. - this->renderWidget->RenderRoadMeshes(this->model->Meshes()); -} - -///////////////////////////////////////////////// -void MaliputViewerWidget::OnTextLabelChanged(const std::string& key, bool newValue) { - // Updates the model. - if (key == kLaneTextLabel || key == kBranchPointTextLabel) { - const int keyword_index = key.find(kText); - const std::string keyword = key.substr(0, keyword_index); - this->UpdateMeshDefaults(key, newValue); - for (auto const& it : this->model->Labels()) { - if (FoundKeyword(it.first, keyword)) { - // Updates the model. - if (!this->renderWidget->IsSelected(it.second.text)) { - this->model->SetTextLabelState(it.first, newValue); - } - } - } - } else { - this->model->SetTextLabelState(key, newValue); - } - - // Replicates into the GUI. - this->renderWidget->RenderLabels(this->model->Labels()); -} - -///////////////////////////////////////////////// -std::string MaliputViewerWidget::MarkerMeshMapKey(const std::string& id) { return kMarker + "_" + id; } - -///////////////////////////////////////////////// -std::string MaliputViewerWidget::BranchPointMeshMapKey(const std::string& id) { return kBranchPoint + "_" + id; } - -///////////////////////////////////////////////// -std::string MaliputViewerWidget::LaneMeshMapKey(const std::string& id) { return kLane + "_" + id; } - -///////////////////////////////////////////////// -void MaliputViewerWidget::OnNewRoadNetwork(const std::string& filePath, const std::string& roadRulebookFilePath, - const std::string& trafficLightRulesFilePath, - const std::string& phaseRingFilePath) { - if (filePath.empty()) { - return; - } - - // Clears the GUI meshes and then populates with the meshes. - this->renderWidget->Clear(); - - // Loads the new file. - this->model = std::make_unique(); - if (GlobalAttributes::HasArgument("malidrive_backend")) { - this->model->SetOpenDriveBackend(GlobalAttributes::GetArgument("malidrive_backend")); - } - this->model->Load(filePath, roadRulebookFilePath, trafficLightRulesFilePath, phaseRingFilePath); - this->rulesVisualizerWidget->ClearText(); - this->rulesVisualizerWidget->ClearLaneList(); - this->rulesVisualizerWidget->ConstructPhaseRingTree(this->model->GetPhaseRings()); - const auto lane_ids = this->model->GetAllLaneIds>(); - for (const QString& lane_id : lane_ids) { - this->rulesVisualizerWidget->AddLaneId(lane_id); - } - - this->renderWidget->RenderTrafficLights(this->model->GetTrafficLights()); - this->renderWidget->RenderArrow(); - this->renderWidget->RenderRoadMeshes(this->model->Meshes()); - this->renderWidget->RenderLabels(this->model->Labels()); - - this->VisualizeFileName(filePath); - this->maliputFileSelectionWidget->ClearLineEdits(true); -} - -void MaliputViewerWidget::UpdateSelectedWithDefault() { - const std::vector selectedLanes = this->renderWidget->GetSelectedLanes(); - const std::vector selectedBranchPoints = this->renderWidget->GetSelectedBranchPoints(); - for (const auto& id : selectedLanes) { - this->model->SetLayerState(LaneMeshMapKey(id), meshDefaults[kLane]); - this->model->SetLayerState(MarkerMeshMapKey(id), meshDefaults[kMarker]); - this->model->SetTextLabelState(LaneMeshMapKey(id), meshDefaults[kLaneLabels]); - } - for (const auto& id : selectedBranchPoints) { - this->model->SetLayerState(BranchPointMeshMapKey(id), meshDefaults[kBranchPoint]); - this->model->SetTextLabelState(BranchPointMeshMapKey(id), meshDefaults[kBranchPointLabels]); - } -} - -///////////////////////////////////////////////// -void MaliputViewerWidget::VisualizeFileName(const std::string& filePath) { - const auto filePathDividerPosition = filePath.rfind("/"); - if (filePathDividerPosition != std::string::npos) { - this->maliputFileSelectionWidget->SetFileNameLabel(filePath.substr(filePathDividerPosition + 1)); - } -} - -///////////////////////////////////////////////// -void MaliputViewerWidget::OnSetAllSelectedRegionsToDefault() { - UpdateSelectedWithDefault(); - - // Replicates into the GUI. - this->renderWidget->RenderRoadMeshes(this->model->Meshes()); - this->renderWidget->RenderLabels(this->model->Labels()); -} - -void MaliputViewerWidget::UpdateLane(const std::string& id) { - const bool isLaneVisualized = this->renderWidget->IsSelected(id) || meshDefaults[kLane]; - const bool isMarkerVisualized = this->renderWidget->IsSelected(id) || meshDefaults[kMarker]; - const bool isLaneLabelVisualized = this->renderWidget->IsSelected(id) || meshDefaults[kLaneLabels]; - - const std::string laneKey = LaneMeshMapKey(id); - const std::string markerKey = MarkerMeshMapKey(id); - - this->OnLayerMeshChanged(laneKey, isLaneVisualized); - this->OnLayerMeshChanged(markerKey, isMarkerVisualized); - this->OnTextLabelChanged(laneKey, isLaneLabelVisualized); -} - -///////////////////////////////////////////////// -void MaliputViewerWidget::UpdateBranchPoint(const std::string& id) { - const bool isBPMeshVisualized = this->renderWidget->IsSelected(id) || meshDefaults[kBranchPoint]; - const bool isBPLabelVisualized = this->renderWidget->IsSelected(id) || meshDefaults[kBranchPointLabels]; - const std::string key = BranchPointMeshMapKey(id); - - this->OnLayerMeshChanged(key, isBPMeshVisualized); - this->OnTextLabelChanged(key, isBPLabelVisualized); -} - -///////////////////////////////////////////////// -void MaliputViewerWidget::OnVisualClicked(ignition::rendering::RayQueryResult rayResult) { - if (this->model) { - const maliput::api::Lane* lane = this->model->GetLaneFromWorldPosition(rayResult.point); - if (lane) { - const std::string& lane_id = lane->id().string(); - const std::string start_bp_id = lane->GetBranchPoint(maliput::api::LaneEnd::kStart)->id().string(); - const std::string end_bp_id = lane->GetBranchPoint(maliput::api::LaneEnd::kFinish)->id().string(); - - ignmsg << "Clicked lane ID: " << lane_id << "\n"; - this->renderWidget->SelectLane(lane); - - // Update visualization to default if it is deselected - UpdateLane(lane_id); - UpdateBranchPoint(start_bp_id); - UpdateBranchPoint(end_bp_id); - - PhaseRingPhaseIds phaseRingIdAndPhaseIdSelected = this->rulesVisualizerWidget->GetSelectedPhaseRingAndPhaseId(); - emit this->rulesVisualizerWidget->ReceiveRules( - QString(lane_id.c_str()), - this->model->GetRulesOfLane(phaseRingIdAndPhaseIdSelected.phase_ring_id.toStdString(), - phaseRingIdAndPhaseIdSelected.phase_id.toStdString(), lane_id)); - this->renderWidget->PutArrowAt(rayResult.distance, rayResult.point); - this->renderWidget->SetArrowVisibility(true); - } else { - UpdateSelectedWithDefault(); - this->renderWidget->SetArrowVisibility(false); - this->renderWidget->DeselectAll(); - } - } -} - -///////////////////////////////////////////////// -QPaintEngine* MaliputViewerWidget::paintEngine() const { return nullptr; } - -///////////////////////////////////////////////// -void MaliputViewerWidget::BuildGUI() { - this->setFocusPolicy(Qt::StrongFocus); - this->setMouseTracking(true); - this->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - - this->title = "MaliputViewerWidget"; - - this->setMinimumHeight(100); - - this->maliputFileSelectionWidget = new MaliputFileSelectionWidget(this); - this->layerSelectionWidget = new LayerSelectionWidget(this); - this->labelSelectionWidget = new LabelSelectionWidget(this); - this->renderWidget = new RenderMaliputWidget(this); - this->rulesVisualizerWidget = new RulesVisualizerWidget(this); - - QObject::connect(this->renderWidget, SIGNAL(VisualClicked(ignition::rendering::RayQueryResult)), this, - SLOT(OnVisualClicked(ignition::rendering::RayQueryResult))); - - QObject::connect(this->renderWidget, SIGNAL(SetAllSelectedRegionsToDefault()), this, - SLOT(OnSetAllSelectedRegionsToDefault())); - - QObject::connect(this->rulesVisualizerWidget, SIGNAL(RequestRules()), this, SLOT(OnRulesForLaneRequested())); - - auto verticalLayout = new QVBoxLayout(this); - verticalLayout->addWidget(this->maliputFileSelectionWidget); - verticalLayout->addWidget(this->layerSelectionWidget); - verticalLayout->addWidget(this->labelSelectionWidget); - verticalLayout->addWidget(this->rulesVisualizerWidget); - auto controlGroup = new QGroupBox("Control panel", this); - controlGroup->setLayout(verticalLayout); - controlGroup->setSizePolicy(QSizePolicy::Policy::Fixed, QSizePolicy::Policy::Fixed); - - auto horizontalLayout = new QHBoxLayout(this); - horizontalLayout->addWidget(this->renderWidget); - horizontalLayout->addWidget(controlGroup); - - this->setLayout(horizontalLayout); -} - -///////////////////////////////////////////////// -void MaliputViewerWidget::paintEvent(QPaintEvent* _e) { - QWidget::paintEvent(_e); - - // TODO(agalbachicar): Properly sync the first paint event, so road meshes can - // be queried from the model and set to the view. - if (!first_run_) { - first_run_ = true; - this->renderWidget->RenderRoadMeshes(this->model->Meshes()); - this->renderWidget->RenderLabels(this->model->Labels()); - } - - this->renderWidget->paintEvent(_e); -} - -void MaliputViewerWidget::OnRulesForLaneRequested() { - PhaseRingPhaseIds phaseRingPhaseIds = this->rulesVisualizerWidget->GetSelectedPhaseRingAndPhaseId(); - const std::string phase_ring_id = phaseRingPhaseIds.phase_ring_id.toStdString(); - const std::string phase_id = phaseRingPhaseIds.phase_id.toStdString(); - this->renderWidget->SetStateOfTrafficLights(this->model->GetBulbStates(phase_ring_id, phase_id)); - - const std::string lane_id = this->rulesVisualizerWidget->GetSelectedLaneId().toStdString(); - if (!lane_id.empty()) { - this->renderWidget->SelectLane(this->model->GetLaneFromId(lane_id)); - emit this->rulesVisualizerWidget->ReceiveRules( - this->rulesVisualizerWidget->GetSelectedLaneId(), - this->model->GetRulesOfLane(phase_ring_id, phase_id, lane_id)); - } -} - -///////////////////////////////////////////////// -IGN_COMMON_REGISTER_SINGLE_PLUGIN(delphyne::gui::MaliputViewerWidget, ignition::gui::Plugin) diff --git a/delphyne_gui/visualizer/maliput_viewer_widget.hh b/delphyne_gui/visualizer/maliput_viewer_widget.hh deleted file mode 100644 index 13138cfd..00000000 --- a/delphyne_gui/visualizer/maliput_viewer_widget.hh +++ /dev/null @@ -1,199 +0,0 @@ -// Copyright 2017 Toyota Research Institute - -#ifndef DELPHYNE_GUI_MALIPUTVIEWERWIDGET_HH -#define DELPHYNE_GUI_MALIPUTVIEWERWIDGET_HH - -#include -#include - -#include - -#include -#include - -#include "layer_selection_widget.hh" -#include "maliput_viewer_plugin/maliput_viewer_model.hh" -#include "render_maliput_widget.hh" -#include "rules_visualizer_widget.hh" - -namespace delphyne { -namespace gui { - -/// \brief Plugin class that acts as a controller for the application. -/// -/// Holds different view widgets and connects the model with the events and -/// changes in the GUI. -class MaliputViewerWidget : public ignition::gui::Plugin { - Q_OBJECT - - public: - /// \brief Default constructor. - explicit MaliputViewerWidget(QWidget* parent = 0); - - /// \brief Default Destructor. - virtual ~MaliputViewerWidget() = default; - - protected: - /// \brief Override paintEngine to stop Qt From trying to draw on top of - /// render window. - /// \return NULL. - virtual QPaintEngine* paintEngine() const; - - protected slots: - /// \brief Updates the model based on the @p key mesh name and @p newValue and - /// the mesh on the GUI. - /// \param[in] key Name of the mesh. - /// \param[in] newValue New mesh visualization status. - void OnLayerMeshChanged(const std::string& key, bool newValue); - - /// \brief Updates the model based on the @p key text label group name and - /// @p newValue and the label group on the GUI. - /// \param[in] key Name of the label group. - /// \param[in] newValue New label group visualization status. - void OnTextLabelChanged(const std::string& key, bool newValue); - - /// \brief Clears the visualizer, loads the new set of meshes and text labels. - /// \param filePath The path to the YAML file to load and visualize. - void OnNewRoadNetwork(const std::string& filePath, const std::string& roadRulebookFilePath, - const std::string& trafficLightRulesFilePath, const std::string& phaseRingFilePath); - - /// \brief Prints the ID of the lane if any was selected. - /// \param[in] rayResult Ray that contains the point where the click hit. - void OnVisualClicked(ignition::rendering::RayQueryResult rayResult); - - /// \brief Sets all selected regions back to the default values indicated by the GUI checkboxes - /// and updates the visualizer - void OnSetAllSelectedRegionsToDefault(); - - /// \brief Emits rulesVisualizerWidget's ReceiveRules signal with all the rules related - /// to the selected lane, phase ring and phase if any. - void OnRulesForLaneRequested(); - - protected: - /// \brief Overridden method to receive Qt paint event. - /// \param[in] _e The event that happened. - virtual void paintEvent(QPaintEvent* _e); - - private: - /// \brief Loads in the GUI the file name that @p filePath refers to. - /// \param filePath The file path to the yaml file. - void VisualizeFileName(const std::string& filePath); - - /// \brief Builds the widgets of the GUI. - void BuildGUI(); - - /// \brief Renders the arrow that will be spawned above a clicked lane. - void RenderArrow(); - - /// \brief Updates all stored defaults of the meshes. - /// \param[in] key The key indicating which default to update. - /// \param[in] newValue The new value to set the default value. - void UpdateMeshDefaults(const std::string& key, bool newValue); - - /// \brief Updates a lane so that if it is selected then all meshes are on, but if it is - /// not selected, all meshes are set to the default values. - /// \param[in] id The id of the lane. - void UpdateLane(const std::string& id); - - /// \brief Updates a branch point so that if it is selected then all meshes are on, but if it is - /// not selected, all meshes are set to the default values. - /// \param[in] id The id of the branch point. - void UpdateBranchPoint(const std::string& id); - - /// \brief Updates all of the selected regions to the default mesh values. - void UpdateSelectedWithDefault(); - - /// \brief Returns the corresponding map key for the marker. The string is - /// calculated as being `kMarker` + "_" + id. - /// \param[in] id The id of the marker. - std::string MarkerMeshMapKey(const std::string& id); - - /// \brief Returns the corresponding map key for the branch point. The string is - /// calculated as being `kBranchPoint` + "_" + id. - /// \param[in] id The id of the branch point. - std::string BranchPointMeshMapKey(const std::string& id); - - /// \brief Returns the corresponding map key for the lane. The string is - /// calculated as being `kLane` + "_" + id. - /// \param[in] id The id of the lane. - std::string LaneMeshMapKey(const std::string& id); - - bool FoundKeyword(const std::string& key, const std::string& keyword); - - /// \brief Obtains the id of a branchpoint or lane from the string used as it's - /// key value in the mesh map. - /// \param[in] key The key that the lane or branchpoint is associated with in the - /// mesh map. - /// \returns The lane or branchpoint id. - std::string GetID(const std::string& key); - - /// \brief Widget to hold and modify the visualization status of each layer. - LayerSelectionWidget* layerSelectionWidget{nullptr}; - - /// \brief Widget to hold and modify the visualization status of lane and - /// branch point text labels. - LabelSelectionWidget* labelSelectionWidget{nullptr}; - - /// \brief Widget to load yaml files. - MaliputFileSelectionWidget* maliputFileSelectionWidget{nullptr}; - - /// \brief World render widget. - RenderMaliputWidget* renderWidget{nullptr}; - - /// \brief Rules visualizer widget. - RulesVisualizerWidget* rulesVisualizerWidget{nullptr}; - - /// \brief Model that holds the meshes and the visualization status. - std::unique_ptr model{}; - - // \brief A map that contains the default of the checkbox meshes. - std::map meshDefaults; - - /// \brief Flag used to record the first paint event and sync on the road - /// meshes when required. - bool first_run_{false}; - - // TODO(anyone): The below string constants are used as keys for the mesh map - // that contains all of the meshes of the loaded road geometry. These keys are - // currently in use in three separate files: `maliput_viewer_widget.cc`, - // `layer_selection_widget.cc`, and `generate_obj.cc` within maliput. These keys - // should be consolidated into a singular location to prevent any accidental - // disruption of functionality going forward. - - /// \brief Key used to detect an asphalt checkbox event. - const std::string kAsphalt{"asphalt"}; - - /// \brief Keyword used by the checkboxes to indicate the new default for all - /// of the provided mesh. - const std::string kAll{"all"}; - - /// \brief Key for the marker mesh in the default map. - const std::string kMarker{"marker"}; - - /// \brief Key for the lane mesh in the default map. - const std::string kLane{"lane"}; - - /// \brief Used to detect the below label keys upon a checkbox event as text is the - /// unique identifier for enabling or disabling all labels. - const std::string kText{"text"}; - - /// \brief Key sent by a lane label checkbox click event. - const std::string kLaneTextLabel{"lane_" + kText + "_label"}; - - /// \brief Key sent by a branch point label checkbox click event. - const std::string kBranchPointTextLabel{"branch_point_" + kText + "_label"}; - - /// \brief Key for the branch point mesh in the default map. - const std::string kBranchPoint{"branch_point"}; - - /// \brief Key for the branch point label mesh in the default map. - const std::string kBranchPointLabels{"branch_point_labels"}; - - /// \brief Key for the lane label mesh in the default map. - const std::string kLaneLabels{"lane_labels"}; -}; - -} // namespace gui -} // namespace delphyne - -#endif diff --git a/delphyne_gui/visualizer/orbit_view_control.cc b/delphyne_gui/visualizer/orbit_view_control.cc deleted file mode 100644 index d93f5190..00000000 --- a/delphyne_gui/visualizer/orbit_view_control.cc +++ /dev/null @@ -1,149 +0,0 @@ -// Copyright 2017 Toyota Research Institute - -#include - -#include -#include -#include - -#include "orbit_view_control.hh" - -using namespace delphyne; -using namespace gui; - -///////////////////////////////////////////////// -OrbitViewControl::OrbitViewControl(const ignition::rendering::CameraPtr& _cam) : camera(_cam) { - this->rayQuery = this->camera->Scene()->CreateRayQuery(); - if (!this->rayQuery) { - ignerr << "Failed to create Ray Query" << std::endl; - return; - } -} - -///////////////////////////////////////////////// -OrbitViewControl::~OrbitViewControl() {} - -///////////////////////////////////////////////// -void OrbitViewControl::OnMousePress(const QMouseEvent* _e) { this->OnMouseButtonAction(_e, ButtonState_t::PRESSED); } - -///////////////////////////////////////////////// -void OrbitViewControl::OnMouseRelease(const QMouseEvent* _e) { this->OnMouseButtonAction(_e, ButtonState_t::RELEASED); } - -///////////////////////////////////////////////// -void OrbitViewControl::OnMouseMove(const QMouseEvent* _e) { - if (!this->camera) { - return; - } - - { - std::lock_guard lock(this->mouseMutex); - int deltaX = _e->x() - this->mouse.motionX; - int deltaY = _e->y() - this->mouse.motionY; - this->mouse.motionX = _e->x(); - this->mouse.motionY = _e->y(); - - if (this->mouse.motionDirty) { - this->mouse.dragX += deltaX; - this->mouse.dragY += deltaY; - } else { - this->mouse.dragX = deltaX; - this->mouse.dragY = deltaY; - } - this->mouse.motionDirty = true; - } - - this->Update(); -} - -///////////////////////////////////////////////// -void OrbitViewControl::OnMouseWheel(const QWheelEvent* _e) { - if (!this->camera) { - return; - } - - std::lock_guard lock(this->mouseMutex); - double scroll = (_e->angleDelta().y() > 0) ? -1.0 : 1.0; - double distance = this->camera->WorldPosition().Distance(this->target.point); - int factor = 1; - double amount = -(scroll * factor) * (distance / 5.0); - - this->viewControl.SetCamera(this->camera); - this->viewControl.SetTarget(this->target.point); - this->viewControl.Zoom(amount); -} - -////////////////////////////////////////////////// -const ignition::rendering::RayQueryResult& OrbitViewControl::GetQueryResult() const { return this->target; } - -////////////////////////////////////////////////// -void OrbitViewControl::OnMouseButtonAction(const QMouseEvent* _e, const ButtonState_t& _state) { - if (!this->camera) { - return; - } - - // Ignore unknown mouse buttons. - if (_e->button() != Qt::LeftButton && _e->button() != Qt::RightButton && _e->button() != Qt::MidButton) { - return; - } - - { - std::lock_guard lock(this->mouseMutex); - this->mouse.button = _e->button(); - this->mouse.state = _state; - this->mouse.x = _e->x(); - this->mouse.y = _e->y(); - this->mouse.motionX = _e->x(); - this->mouse.motionY = _e->y(); - this->mouse.buttonDirty = true; - } - - this->Update(); -} - -////////////////////////////////////////////////// -void OrbitViewControl::Update() { - if (!this->camera || !this->rayQuery) { - return; - } - - std::lock_guard lock(this->mouseMutex); - if (this->mouse.buttonDirty) { - this->mouse.buttonDirty = false; - double nx = 2.0 * this->mouse.x / static_cast(this->camera->ImageWidth()) - 1.0; - double ny = 1.0 - 2.0 * this->mouse.y / static_cast(this->camera->ImageHeight()); - this->rayQuery->SetFromCamera(this->camera, ignition::math::Vector2d(nx, ny)); - this->target = this->rayQuery->ClosestPoint(); - if (!this->target) { - this->target.point = this->rayQuery->Origin() + this->rayQuery->Direction() * 10; - return; - } - } - - if (this->mouse.motionDirty) { - this->mouse.motionDirty = false; - auto drag = ignition::math::Vector2d(this->mouse.dragX, this->mouse.dragY); - - // Left mouse button pan. - if (this->mouse.button == Qt::LeftButton && this->mouse.state == ButtonState_t::PRESSED) { - this->viewControl.SetCamera(this->camera); - this->viewControl.SetTarget(this->target.point); - this->viewControl.Pan(drag); - } else if (this->mouse.button == Qt::MidButton && this->mouse.state == ButtonState_t::PRESSED) { - this->viewControl.SetCamera(this->camera); - this->viewControl.SetTarget(this->target.point); - this->viewControl.Orbit(drag); - } - // Right mouse button zoom. - else if (this->mouse.button == Qt::RightButton && this->mouse.state == ButtonState_t::PRESSED) { - double hfov = this->camera->HFOV().Radian(); - double vfov = 2.0f * atan(tan(hfov / 2.0f) / this->camera->AspectRatio()); - double distance = this->camera->WorldPosition().Distance(this->target.point); - double amount = - ((-this->mouse.dragY / static_cast(this->camera->ImageHeight())) * distance * tan(vfov / 2.0) * 6.0); - - this->viewControl.SetCamera(this->camera); - this->viewControl.SetTarget(this->target.point); - this->viewControl.Zoom(amount); - } - } -} diff --git a/delphyne_gui/visualizer/orbit_view_control.hh b/delphyne_gui/visualizer/orbit_view_control.hh deleted file mode 100644 index ac1f2854..00000000 --- a/delphyne_gui/visualizer/orbit_view_control.hh +++ /dev/null @@ -1,115 +0,0 @@ -// Copyright 2017 Toyota Research Institute - -#ifndef DELPHYNE_GUI_ORBITVIEWCONTROL_HH -#define DELPHYNE_GUI_ORBITVIEWCONTROL_HH - -#include - -#include -#include -#include -#include - -namespace delphyne { -namespace gui { - -/// \def ButtonState_t Possible button states. -enum class ButtonState_t { - /// \brief Button has been pressed. - PRESSED, - /// \brief Button has been released. - RELEASED, -}; - -/// \brief Stores the mouse state. -struct MouseButton { - /// \brief The active button. - Qt::MouseButton button = Qt::NoButton; - /// \brief Pressed or released. - ButtonState_t state = ButtonState_t::RELEASED; - /// \brief X position of the cursor relative to the widget that received the - /// event after a press or release. - int x = 0; - /// \brief Y position of the cursor relative to the widget that received the - /// event after a press or release. - int y = 0; - /// \brief X position of the cursor relative to the widget that received the - /// event after a move. - int motionX = 0; - /// \brief Y position of the cursor relative to the widget that received the - /// event after a move. - int motionY = 0; - /// \brief Number of pixels in X position during dragging. - int dragX = 0; - /// \brief Number of pixels in Y position during dragging. - int dragY = 0; - /// \bried When true, a button action needs to be processed. - bool buttonDirty = false; - /// \brief When true, a move action needs to be processed. - bool motionDirty = false; -}; - -/// \class OrbitViewControl -/// \brief Modifies the camera view according to the mouse events received. -/// Note that the mouse events need to be captured outside of this class. -class OrbitViewControl { - public: - /// \brief Default constructor. - /// \param[in] _cam The camera to be controlled by this class. - explicit OrbitViewControl(const ignition::rendering::CameraPtr& _cam); - - /// \brief Default Destructor. - virtual ~OrbitViewControl(); - - /// \brief Callback executed after a mouse press event. - /// \param[in] _e The Qt mouse event. - void OnMousePress(const QMouseEvent* _e); - - /// \brief Callback executed after a mouse release event. - /// \param[in] _e The Qt mouse event. - void OnMouseRelease(const QMouseEvent* _e); - - /// \brief Callback executed after a mouse move event. - /// \param[in] _e The Qt mouse event. - void OnMouseMove(const QMouseEvent* _e); - - /// \brief Callback executed after a mouse wheel event. - /// \param[in] _e The Qt mouse event. - void OnMouseWheel(const QWheelEvent* _e); - - /// \brief Getter for the ray cast result obtained when the user - /// clicks any part of the screen. - const ignition::rendering::RayQueryResult& GetQueryResult() const; - - private: - /// \brief Update the camera view. - void Update(); - - /// \brief Callback executed after a mouse click event. - /// \param[in] _e The Qt mouse event. - /// \param[in] _state The state of the button. - void OnMouseButtonAction(const QMouseEvent* _e, const ButtonState_t& _state); - - /// \brief Allows to pan, rotate and zoom the camera view. - ignition::rendering::OrbitViewController viewControl; - - /// \brief The camera to control. - ignition::rendering::CameraPtr camera; - - /// \brief Let us cast some rays. - ignition::rendering::RayQueryPtr rayQuery; - - /// \brief Stores the result of a ray casting. - ignition::rendering::RayQueryResult target; - - /// \brief Stores the mouse state. - struct MouseButton mouse; - - /// \brief Protect the MouseButton struct from being simultaneously modified - /// and read. - std::mutex mouseMutex; -}; -} // namespace gui -} // namespace delphyne - -#endif diff --git a/delphyne_gui/visualizer/playback.qrc b/delphyne_gui/visualizer/playback.qrc deleted file mode 100644 index 3daa832c..00000000 --- a/delphyne_gui/visualizer/playback.qrc +++ /dev/null @@ -1,8 +0,0 @@ - - - icons/rewind.svg - icons/play.svg - icons/pause.svg - icons/step.svg - - diff --git a/delphyne_gui/visualizer/playback_widget.cc b/delphyne_gui/visualizer/playback_widget.cc deleted file mode 100644 index 5860968e..00000000 --- a/delphyne_gui/visualizer/playback_widget.cc +++ /dev/null @@ -1,247 +0,0 @@ -// Copyright 2018 Toyota Research Institute - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -#include "playback_widget.hh" - -Q_DECLARE_METATYPE(ignition::msgs::PlaybackStatus) - -namespace delphyne { -namespace gui { - -namespace { - -std::chrono::nanoseconds TimeToChrono(const ignition::msgs::Time& src) { - return (std::chrono::seconds(src.sec()) + std::chrono::nanoseconds(src.nsec())); -} - -void ChronoToDuration(const std::chrono::nanoseconds& src, ignition::msgs::Duration* dst) { - DELPHYNE_DEMAND(dst != nullptr); - std::chrono::seconds src_in_seconds = std::chrono::duration_cast(src); - dst->set_sec(std::floor(src_in_seconds.count())); - dst->set_nsec((src - src_in_seconds).count()); -} - -} // namespace - -///////////////////////////////////////////////// -PlaybackWidget::PlaybackWidget(QWidget*) : Plugin() { qRegisterMetaType(); } - -///////////////////////////////////////////////// -PlaybackWidget::~PlaybackWidget() {} - -///////////////////////////////////////////////// -void PlaybackWidget::LoadConfig(const tinyxml2::XMLElement*) { - this->title = "PlaybackWidget"; - this->setEnabled(false); - - // Creates all child widgets. - rewind_button_ = new QPushButton(QIcon(":/icons/rewind.svg"), ""); - rewind_button_->setToolTip("Rewind"); - this->connect(rewind_button_, SIGNAL(clicked()), this, SLOT(OnRewindButtonPush())); - - pause_button_ = new QPushButton(QIcon(":/icons/pause.svg"), ""); - pause_button_->setToolTip("Pause"); - pause_button_->setCheckable(true); - pause_button_->setAutoExclusive(true); - this->connect(pause_button_, SIGNAL(clicked()), this, SLOT(OnPauseButtonPush())); - - play_button_ = new QPushButton(QIcon(":/icons/play.svg"), ""); - play_button_->setToolTip("Play"); - play_button_->setCheckable(true); - play_button_->setAutoExclusive(true); - this->connect(play_button_, SIGNAL(clicked()), this, SLOT(OnPlayButtonPush())); - - step_button_ = new QPushButton(QIcon(":/icons/step.svg"), ""); - step_button_->setToolTip("Step forward"); - this->connect(step_button_, SIGNAL(clicked()), this, SLOT(OnStepButtonPush())); - - time_step_spinbox_ = new QSpinBox(); - // Max value gets overwritten as soon as the time range is known. - time_step_spinbox_->setRange(10, INT32_MAX); - time_step_spinbox_->setSuffix(" ms"); - time_step_spinbox_->setSingleStep(1); - time_step_spinbox_->setToolTip("Time step"); - - time_label_ = new QLabel; - - timeline_slider_ = new QSlider; - timeline_slider_->setMinimum(0); - timeline_slider_->setTracking(true); - timeline_slider_->setOrientation(Qt::Horizontal); - this->connect(timeline_slider_, SIGNAL(sliderPressed()), this, SLOT(OnTimelinePress())); - this->connect(timeline_slider_, SIGNAL(sliderMoved(int)), this, SLOT(OnTimelineMove(int))); - this->connect(timeline_slider_, SIGNAL(sliderReleased()), this, SLOT(OnTimelineRelease())); - - duration_label_ = new QLabel; - - // Creates the layout to hold children widgets. - auto layout = new QHBoxLayout; - layout->addWidget(rewind_button_); - layout->addWidget(pause_button_); - layout->addWidget(play_button_); - layout->addWidget(step_button_); - layout->addWidget(time_step_spinbox_); - layout->addWidget(time_label_); - layout->addWidget(timeline_slider_); - layout->addWidget(duration_label_); - - // Uses the layout. - this->setLayout(layout); - - // Connects update slot with status subscription through an - // auxiliary signal. - this->connect(this, SIGNAL(OnStatusChange(const ignition::msgs::PlaybackStatus&)), this, - SLOT(Update(const ignition::msgs::PlaybackStatus&))); - - if (!node_.Subscribe(kStatusTopicName, &PlaybackWidget::OnStatusMessage, this)) { - ignerr << "Error subscribing to topic " - << "[" << kStatusTopicName << "]" << std::endl; - } - - // Spins up a timer to monitor backend responsiveness. - this->startTimer((kStatusUpdateMaxDelay / 10).count()); -} - -///////////////////////////////////////////////// -void PlaybackWidget::OnRewindButtonPush() { - ignition::msgs::Duration msg; - msg.set_sec(0); - msg.set_nsec(0); - this->RequestSeek(msg); -} - -///////////////////////////////////////////////// -void PlaybackWidget::OnPauseButtonPush() { this->RequestPause(); } - -///////////////////////////////////////////////// -void PlaybackWidget::OnPlayButtonPush() { this->RequestResume(); } - -///////////////////////////////////////////////// -void PlaybackWidget::OnStepButtonPush() { - const std::chrono::milliseconds time_step(time_step_spinbox_->value()); - ignition::msgs::Duration time_step_msg; - ChronoToDuration(time_step, &time_step_msg); - this->RequestStep(time_step_msg); -} - -///////////////////////////////////////////////// -void PlaybackWidget::OnTimelinePress() { timeline_interaction_ = true; } - -///////////////////////////////////////////////// -void PlaybackWidget::OnTimelineMove(int slider_location) { - ignition::msgs::Duration msg; - ChronoToDuration(slider_location * timeline_scale_, &msg); - this->RequestSeek(msg); -} - -///////////////////////////////////////////////// -void PlaybackWidget::OnTimelineRelease() { timeline_interaction_ = false; } - -///////////////////////////////////////////////// -void PlaybackWidget::timerEvent(QTimerEvent*) { - const std::chrono::nanoseconds current_time = std::chrono::steady_clock::now().time_since_epoch(); - // Disables the entire widget if playback status updates have stopped coming. - this->setEnabled(current_time - last_update_time_ < kStatusUpdateMaxDelay); -} - -///////////////////////////////////////////////// -void PlaybackWidget::OnStatusMessage(const ignition::msgs::PlaybackStatus& msg) { emit this->OnStatusChange(msg); } - -///////////////////////////////////////////////// -void PlaybackWidget::Update(const ignition::msgs::PlaybackStatus& status) { - // Enables step button if playback is paused. - step_button_->setEnabled(status.paused()); - // Sets play and pause buttons accordingly. - pause_button_->setChecked(status.paused()); - play_button_->setChecked(!status.paused()); - // Computes playback time stats. - const std::chrono::nanoseconds start_time = TimeToChrono(status.start_time()); - const std::chrono::nanoseconds end_time = TimeToChrono(status.end_time()); - const std::chrono::nanoseconds time_range = end_time - start_time; - const std::chrono::nanoseconds current_time = TimeToChrono(status.current_time()); - const std::chrono::nanoseconds elapsed_time = current_time - start_time; - const std::chrono::milliseconds time_step(time_step_spinbox_->value()); - if (!timeline_interaction_) { - // Sets timeline slider range and value. - timeline_slider_->setMaximum(time_range / time_step); - timeline_slider_->setValue(elapsed_time / time_step); - // Sets timeline scale (or in other words, its tick duration). - const int slider_range = timeline_slider_->maximum() - timeline_slider_->minimum(); - timeline_scale_ = time_range / slider_range; - } - using seconds = std::chrono::duration; - constexpr int kFieldWidth{0}; // Default. - constexpr char kFieldFormat{'f'}; // No scientific notation. - constexpr int kFieldPrecision{3}; // Up to three (3) places after - // the decimal dot. - // Uses time label to track playback time, in seconds - // since the Epoch.. - time_label_->setText( - QString("Time: %1") - .arg(std::chrono::duration_cast(current_time).count(), kFieldWidth, kFieldFormat, kFieldPrecision)); - // Uses duration label to track playback duration, in seconds. - duration_label_->setText( - QString("%1 s / %2 s") - .arg(std::chrono::duration_cast(elapsed_time).count(), kFieldWidth, kFieldFormat, kFieldPrecision) - .arg(std::chrono::duration_cast(time_range).count(), kFieldWidth, kFieldFormat, kFieldPrecision)); - // Updates maximum value in time step spinbox with the actual lenght of the - // playback. This runs only once since the widget gets enabled afterwards. - if (!this->isEnabled()) { - time_step_spinbox_->setMaximum(std::chrono::duration_cast(time_range).count()); - } - - // Enables the entire widget. - this->setEnabled(true); - // Tracks time of update. - last_update_time_ = std::chrono::steady_clock::now().time_since_epoch(); -} - -///////////////////////////////////////////////// -void PlaybackWidget::RequestPause() { - const ignition::msgs::Empty msg; - if (!node_.Request(kPauseServiceName, msg)) { - ignerr << "Failed to request a playback pause." << std::endl; - } -} - -///////////////////////////////////////////////// -void PlaybackWidget::RequestResume() { - const ignition::msgs::Empty msg; - if (!node_.Request(kResumeServiceName, msg)) { - ignerr << "Failed to request a playback resume." << std::endl; - } -} - -///////////////////////////////////////////////// -void PlaybackWidget::RequestStep(const ignition::msgs::Duration& step_size) { - if (!node_.Request(kStepServiceName, step_size)) { - ignerr << "Failed to request a playback step." << std::endl; - } -} - -///////////////////////////////////////////////// -void PlaybackWidget::RequestSeek(const ignition::msgs::Duration& seek_offset) { - if (!node_.Request(kSeekServiceName, seek_offset)) { - ignerr << "Failed to request a playback seek." << std::endl; - } -} - -} // namespace gui -} // namespace delphyne - -IGN_COMMON_REGISTER_SINGLE_PLUGIN(delphyne::gui::PlaybackWidget, ignition::gui::Plugin) diff --git a/delphyne_gui/visualizer/playback_widget.hh b/delphyne_gui/visualizer/playback_widget.hh deleted file mode 100644 index a138b2aa..00000000 --- a/delphyne_gui/visualizer/playback_widget.hh +++ /dev/null @@ -1,153 +0,0 @@ -// Copyright 2018 Toyota Research Institute - -#ifndef DELPHYNE_GUI_PLAYBACKWIDGET_HH -#define DELPHYNE_GUI_PLAYBACKWIDGET_HH - -#include -#include -#include -#include -#include -#include - -#include - -#include - -// Forward declarations. -namespace tinyxml2 { -class XMLElement; -} -namespace ignition { -namespace msgs { -class Duration; -class PlaybackStatus; -} // namespace msgs -} // namespace ignition - -namespace delphyne { -namespace gui { - -/// This is a class that implements a simple ign-gui widget for -/// rendering a scene, using the ign-rendering functionality. -class PlaybackWidget : public ignition::gui::Plugin { - Q_OBJECT - - public: - /// Default constructor. - explicit PlaybackWidget(QWidget* parent = 0); - - /// Default destructor. - virtual ~PlaybackWidget(); - - /// @brief Overridden method to load user configuration. - /// @param[in] _pluginElem The data containing the configuration. - virtual void LoadConfig(const tinyxml2::XMLElement* _pluginElem) override; - - public slots: - /// Updates widget using the given playback @p status message. - void Update(const ignition::msgs::PlaybackStatus& status); - - signals: - /// Notify that playback status has changed. - /// @param[in] status New playback status message. - void OnStatusChange(const ignition::msgs::PlaybackStatus& status); - - protected: - // Documentation inherited. - void timerEvent(QTimerEvent* event) override; - - private slots: - // A slot to react to a rewind button push. - void OnRewindButtonPush(); - - // A slot to react to a pause button push. - void OnPauseButtonPush(); - - // A slot to react to a play button push. - void OnPlayButtonPush(); - - // A slot to react to a step button push. - void OnStepButtonPush(); - - // A slot to detect the start of a timeline interaction. - void OnTimelinePress(); - - // A slot to react to timeline interactions (i.e. the user moving - // the timeline slider). - // @param[in] location Timeline slider location (i.e. its value). - void OnTimelineMove(int location); - - // A slot to detect the end of a timeline interaction. - void OnTimelineRelease(); - - private: - // Playback status topic subscription callback. - // @param[in] status The playback status message received. - void OnStatusMessage(const ignition::msgs::PlaybackStatus& status); - - // Issues a playback pause request. - void RequestPause(); - - // Issues a playback resume request. - void RequestResume(); - - // Issues a playback step request, using the given @p step_size. - void RequestStep(const ignition::msgs::Duration& step_size); - - // Issues a playback seek request, to the given @p seek_offset - // from playback start. - void RequestSeek(const ignition::msgs::Duration& seek_offset); - - // A button to rewind a playback (i.e. back to the start). - QPushButton* rewind_button_{nullptr}; - - // A button to pause a playback. - QPushButton* pause_button_{nullptr}; - - // A button to resume a playback. - QPushButton* play_button_{nullptr}; - - // A button to step a playback. - QPushButton* step_button_{nullptr}; - - // A spinbox to adjust time step sizes. - QSpinBox* time_step_spinbox_{nullptr}; - - // A label to track playback time. - QLabel* time_label_{nullptr}; - - // A slider to act as interactive playback timeline. - QSlider* timeline_slider_{nullptr}; - - // Timeline scale i.e. tick duration. - std::chrono::nanoseconds timeline_scale_{0}; - - // Whether the user is interacting with the - // timeline or not. - bool timeline_interaction_{false}; - - // A label to track playback duration. - QLabel* duration_label_{nullptr}; - - // Last playback status update time. - std::chrono::nanoseconds last_update_time_{0}; - - // Maximum playback status delay to withstand before - // assuming no playback is taking place. - static constexpr std::chrono::milliseconds kStatusUpdateMaxDelay{200}; - - // Playback's ignition transport services and topics. - static constexpr const char* const kStatusTopicName{"/replayer/status"}; - static constexpr const char* const kPauseServiceName{"/replayer/pause"}; - static constexpr const char* const kResumeServiceName{"/replayer/resume"}; - static constexpr const char* const kStepServiceName{"/replayer/step"}; - static constexpr const char* const kSeekServiceName{"/replayer/seek"}; - - // An ignition transport node. - ignition::transport::Node node_; -}; -} // namespace gui -} // namespace delphyne - -#endif diff --git a/delphyne_gui/visualizer/render_maliput_widget.cc b/delphyne_gui/visualizer/render_maliput_widget.cc deleted file mode 100644 index be8e1c87..00000000 --- a/delphyne_gui/visualizer/render_maliput_widget.cc +++ /dev/null @@ -1,624 +0,0 @@ -// Copyright 2018 Toyota Research Institute - -#include "render_maliput_widget.hh" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include "maliput_viewer_plugin/arrow_mesh.hh" - -using namespace delphyne; -using namespace gui; - -///////////////////////////////////////////////// -RenderMaliputWidget::RenderMaliputWidget(QWidget*) : engine(nullptr) { - this->setAttribute(Qt::WA_OpaquePaintEvent, true); - this->setAttribute(Qt::WA_PaintOnScreen, true); - this->setAttribute(Qt::WA_NoSystemBackground, true); - - this->setFocusPolicy(Qt::StrongFocus); - this->setMouseTracking(true); - this->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - - // The below block means that every time the updateTime expires, we do an - // update on the widget. Later on, we call the start() method to start this - // time at a fixed frequency. Note that we do not start this timer until the - // first time that showEvent() is called. - this->updateTimer = new QTimer(this); - this->trafficLightsTickTimer = new QTimer(this); - QObject::connect(this->updateTimer, SIGNAL(timeout()), this, SLOT(update())); - QObject::connect(this->trafficLightsTickTimer, SIGNAL(timeout()), this, SLOT(TickTrafficLights())); -} - -///////////////////////////////////////////////// -RenderMaliputWidget::~RenderMaliputWidget() { - if (this->engine != nullptr) { - // TODO(clalancette): We need to call this->engine->Fini() to clean up - // some engine resources, but this sometimes causes a hang on quit. - // 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(); - this->scene->DestroyNodes(); - this->scene->DestroySensors(); - this->scene.reset(); - } -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::RenderGrid(const unsigned int _cellCount, const double _cellLength, - const unsigned int _verticalCellCount, - const ignition::rendering::MaterialPtr& _material, - const ignition::math::Pose3d& _pose) { - auto gridGeom = this->scene->CreateGrid(); - if (!gridGeom) { - ignerr << "Unable to create grid geometry" << std::endl; - return; - } - gridGeom->SetCellCount(_cellCount); - gridGeom->SetCellLength(_cellLength); - gridGeom->SetVerticalCellCount(_verticalCellCount); - - auto grid = this->scene->CreateVisual(); - if (!grid) { - ignerr << "Unable to create grid visual" << std::endl; - return; - } - - grid->AddGeometry(gridGeom); - grid->SetLocalPose(_pose); - grid->SetMaterial(_material); - this->scene->RootVisual()->AddChild(grid); -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::RenderGroundPlaneGrid() { - auto gridMaterial = this->scene->CreateMaterial(); - if (gridMaterial) { - // Light blue color. - const double lightRed = 0.404; - const double lightGreen = 0.623; - const double lightBlue = 0.8; - gridMaterial->SetAmbient(lightRed, lightGreen, lightBlue); - gridMaterial->SetDiffuse(lightRed, lightGreen, lightBlue); - gridMaterial->SetSpecular(lightRed, lightGreen, lightBlue); - - const unsigned int kCellCount = 50u; - const double kCellLength = 1; - const unsigned int kVerticalCellCount = 0u; - - this->RenderGrid(kCellCount, kCellLength, kVerticalCellCount, gridMaterial, ignition::math::Pose3d::Zero); - } else { - ignerr << "Failed to create material for the grid" << std::endl; - } -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::RenderOrigin() { - const double kAxisRadius = 0.02; - const double kAxisLength = 10000; - const double kAxisHalfLength = kAxisLength / 2.0; - - // Create the visual axes. - std::array axes; - for (auto& axis : axes) { - axis = this->scene->CreateVisual(); - if (!axis) { - ignerr << "Failed to create axis visual" << std::endl; - return; - } - axis->SetLocalScale(kAxisRadius, kAxisRadius, kAxisLength); - axis->AddGeometry(scene->CreateCylinder()); - } - - const ignition::math::Pose3d kAxisPoseX(kAxisHalfLength, 0, 0, 0, IGN_PI_2, 0); - axes[0]->SetLocalPose(kAxisPoseX); - axes[0]->SetMaterial("Default/TransRed"); - const ignition::math::Pose3d kAxisPoseY(0, kAxisHalfLength, 0, IGN_PI_2, 0, 0); - axes[1]->SetLocalPose(kAxisPoseY); - axes[1]->SetMaterial("Default/TransGreen"); - const ignition::math::Pose3d kAxisPoseZ(0, 0, kAxisHalfLength, 0, 0, 0); - axes[2]->SetLocalPose(kAxisPoseZ); - axes[2]->SetMaterial("Default/TransBlue"); - - for (auto& axis : axes) { - this->scene->RootVisual()->AddChild(axis); - } -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::CreateRenderWindow() { - std::string engineName = "ogre"; - ignition::rendering::RenderEngineManager* manager = ignition::rendering::RenderEngineManager::Instance(); - this->engine = manager->Engine(engineName); - if (!this->engine) { - ignerr << "Engine '" << engineName << "' is not supported" << std::endl; - return; - } - - // Try to reutilize the scene if exists - this->scene = engine->SceneByName("scene"); - if (!this->scene) { - // Create a scene - this->scene = engine->CreateScene("scene"); - if (!this->scene) { - ignerr << "Failed to create scene" << std::endl; - return; - } - } - - // Lights. - const double lightRed = 0.88; - const double lightGreen = 0.88; - const double lightBlue = 0.95; - this->scene->SetAmbientLight(lightRed, lightGreen, lightBlue); - ignition::rendering::VisualPtr root = this->scene->RootVisual(); - if (!root) { - ignerr << "Failed to find the root visual" << std::endl; - return; - } - auto directionalLight = this->scene->CreateDirectionalLight(); - if (!directionalLight) { - ignerr << "Failed to create a directional light" << std::endl; - return; - } - directionalLight->SetDirection(-0.5, -0.5, -1); - directionalLight->SetDiffuseColor(lightRed, lightGreen, lightBlue); - directionalLight->SetSpecularColor(lightRed, lightGreen, lightBlue); - root->AddChild(directionalLight); - - // create user camera - this->camera = this->scene->CreateCamera("user_camera"); - if (!this->camera) { - ignerr << "Failed to create camera" << std::endl; - return; - } - // Rotate 135 deg on the Z axis - auto rotation = this->userSettings.userCameraPose.Rot().Euler(); - this->camera->SetLocalRotation(rotation.X(), rotation.Y(), rotation.Z()); - // Step away from the center of the scene - auto position = this->userSettings.userCameraPose.Pos(); - this->camera->SetLocalPosition(position.X(), position.Y(), position.Z()); - this->camera->SetAspectRatio(static_cast(this->width()) / this->height()); - this->camera->SetHFOV(IGN_DTOR(60)); - root->AddChild(this->camera); - - this->scene->SetBackgroundColor(lightRed, lightGreen, lightBlue); - - // create render window - std::string winHandle = std::to_string(static_cast(this->winId())); - this->renderWindow = this->camera->CreateRenderWindow(); - if (!this->renderWindow) { - ignerr << "Failed to create camera render window" << std::endl; - return; - } - this->renderWindow->SetHandle(winHandle); - this->renderWindow->SetWidth(this->width()); - this->renderWindow->SetHeight(this->height()); - - // render once to create the window. - this->camera->Update(); - - // Render the grid over the ground plane. - this->RenderGroundPlaneGrid(); - - // Render the origin reference frame. - this->RenderOrigin(); - - this->CreateRoadRootVisual(); - - this->orbitViewControl.reset(new OrbitViewControl(this->camera)); - - this->selector = std::make_unique(this->scene, kSelectorScaleX, kSelectorScaleY, kSelectorScaleZ, - kSelectorPoolSize, kNumLanes, kSelectorMinTolerance); -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::CreateRoadRootVisual() { - this->rootVisual = this->scene->CreateVisual(); - // The visual's root pose is initialized to zero because maliput's roads don't - // have a center. - const ignition::math::Pose3d origin(0, 0, 0, 1, 0, 0, 0); - this->rootVisual->SetLocalPose(origin); - this->scene->RootVisual()->AddChild(this->rootVisual); -} - -///////////////////////////////////////////////// -bool RenderMaliputWidget::FillMaterial(const maliput::utility::Material* _maliputMaterial, - ignition::rendering::MaterialPtr& _ignitionMaterial) const { - if (!_maliputMaterial) { - return false; - } - - // clang-format off - _ignitionMaterial->SetDiffuse(_maliputMaterial->diffuse.x(), - _maliputMaterial->diffuse.y(), - _maliputMaterial->diffuse.z()); - _ignitionMaterial->SetAmbient(_maliputMaterial->ambient.x(), - _maliputMaterial->ambient.y(), - _maliputMaterial->ambient.z()); - _ignitionMaterial->SetSpecular(_maliputMaterial->specular.x(), - _maliputMaterial->specular.y(), - _maliputMaterial->specular.z()); - // clang-format on - _ignitionMaterial->SetShininess(_maliputMaterial->shininess); - _ignitionMaterial->SetTransparency(_maliputMaterial->transparency); - - return true; -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::CreateLaneLabelMaterial(ignition::rendering::MaterialPtr& _material) const { - _material->SetDiffuse(0.8, 0.8, 0.0); - _material->SetAmbient(1.0, 1.0, 0.0); - _material->SetSpecular(1.0, 1.0, 0.5); - _material->SetShininess(10.); - _material->SetTransparency(0.5); -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::CreateBranchPointLabelMaterial(ignition::rendering::MaterialPtr& _material) const { - _material->SetDiffuse(0.0, 0.7, 0.0); - _material->SetAmbient(1.0, 1.0, 0.0); - _material->SetSpecular(1.0, 1.0, 0.5); - _material->SetShininess(10.); - _material->SetTransparency(0.5); -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::RenderRoadMeshes(const std::map>& _maliputMeshes) { - for (const auto& it : _maliputMeshes) { - /* TODO: Enable once ignition-rendering3 supports culling - material->SetCulling(ignition::rendering::CullMode::CM_NONE);*/ - - // Checks if the mesh to be rendered already exists or not. - const auto meshExists = this->meshes.find(it.first); - // If the mesh is disabled, there is no mesh for it so it must be set to - // transparent. - if (!it.second->enabled) { - // If the mesh already exists, set visibility to false - if (meshExists != this->meshes.end()) { - this->meshes[it.first]->SetVisible(false); - } - } else { - // If the mesh doesn't exist, it creates new one. Otherwise, just gathers - // the pointer to set the correct material. - ignition::rendering::VisualPtr visual; - if (meshExists == this->meshes.end()) { - // Creates a material for the visual. - ignition::rendering::MaterialPtr material = this->scene->CreateMaterial(); - if (!material) { - ignerr << "Failed to create material.\n"; - continue; - } - visual = this->scene->CreateVisual(); - if (!visual) { - ignerr << "Failed to create visual.\n"; - continue; - } - // Adds the visual to the map for later reference. - this->meshes[it.first] = visual; - // Sets the pose of the mesh. - visual->SetLocalPose(ignition::math::Pose3d(0, 0, 0, 1, 0, 0, 0)); - // Loads the mesh into the visual. - ignition::rendering::MeshDescriptor descriptor(it.second->mesh.get()); - descriptor.Load(); - ignition::rendering::MeshPtr meshGeom = this->scene->CreateMesh(descriptor); - visual->AddGeometry(meshGeom); - // Adds the mesh to the parent root visual. - this->rootVisual->AddChild(visual); - - // Applies the correct material to the mesh. - if (!this->FillMaterial(it.second->material.get(), material)) { - ignerr << "Failed to fill " << it.first << " material information.\n"; - continue; - } - - visual->SetMaterial(material); - } else { - visual = this->meshes[it.first]; - } - this->meshes[it.first]->SetVisible(it.second->visible); - } - } -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::RenderLabels(const std::map& _labels) { - for (const auto& it : _labels) { - // Checks if the text labels to be rendered already exists or not. - const auto labelExists = this->textLabels.find(it.second.text); - // If the text label is disabled, there is no visual for it so it must be - // set to a non-visible state - if (!it.second.enabled) { - // If the text label already exists, set visibility to false - if (labelExists != this->textLabels.end()) { - this->textLabels[it.second.text]->SetVisible(false); - } - } else { - // If the text label doesn't exist, it creates new one. Otherwise, - // it just gathers the pointer to set the correct material. - ignition::rendering::VisualPtr visual; - if (labelExists == this->textLabels.end()) { - // Creates a material for the visual. - ignition::rendering::MaterialPtr material = this->scene->CreateMaterial(); - if (!material) { - ignerr << "Failed to create material.\n"; - continue; - } - visual = this->scene->CreateVisual(); - if (!visual) { - ignerr << "Failed to create visual.\n"; - continue; - } - // Adds the visual to the map for later reference. - this->textLabels[it.second.text] = visual; - visual->SetLocalPose(ignition::math::Pose3d(it.second.position, ignition::math::Quaterniond())); - // Creates the text geometry. - ignition::rendering::TextPtr textGeometry = this->scene->CreateText(); - textGeometry->SetFontName("Liberation Sans"); - textGeometry->SetTextString(it.second.text); - textGeometry->SetShowOnTop(true); - textGeometry->SetTextAlignment(ignition::rendering::TextHorizontalAlign::CENTER, - ignition::rendering::TextVerticalAlign::CENTER); - visual->AddGeometry(textGeometry); - // Adds the mesh to the parent root visual. - this->rootVisual->AddChild(visual); - // Assigns a material for the visual. - if (it.second.labelType == MaliputLabelType::kLane) { - CreateLaneLabelMaterial(material); - } else if (it.second.labelType == MaliputLabelType::kBranchPoint) { - CreateBranchPointLabelMaterial(material); - } else { - ignerr << "Unsupported label type for: " << it.second.text << std::endl; - } - // Applies the correct material to the mesh. - visual->SetMaterial(material); - } else { - visual = this->textLabels[it.second.text]; - } - this->textLabels[it.second.text]->SetVisible(it.second.visible); - } - } -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::RenderArrow() { - if (arrow == nullptr) { - arrow = std::make_unique(this->scene, 0.5); - } -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::RenderTrafficLights( - const std::vector& _trafficLights) { - this->trafficLightManager->CreateTrafficLights(_trafficLights); - // TODO: Consider using maliput::api::rules::PhaseProvider::Result::Next::duration_until for the blinking duration. - this->trafficLightsTickTimer->start(this->kTrafficLightsTickPeriod); -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::SetStateOfTrafficLights(const maliput::api::rules::BulbStates& _bulbStates) { - this->trafficLightManager->SetBulbStates(_bulbStates); -} - -///////////////////////////////////////////////// -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) { - if (this->arrow) { - this->arrow->SetVisibility(_visible); - } -} - -///////////////////////////////////////////////// -std::vector RenderMaliputWidget::GetSelectedBranchPoints() { - return this->selector ? this->selector->GetSelectedBranchPoints() : std::vector{}; -} - -///////////////////////////////////////////////// -std::vector RenderMaliputWidget::GetSelectedLanes() { - return this->selector ? this->selector->GetSelectedLanes() : std::vector{}; -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::DeselectAll() { - if (this->selector) { - emit SetAllSelectedRegionsToDefault(); - this->selector->DeselectAll(); - } -} - -///////////////////////////////////////////////// -bool RenderMaliputWidget::IsSelected(const std::string& _laneId) { - return this->selector ? this->selector->IsSelected(_laneId) : false; -} - -///////////////////////////////////////////////// -bool RenderMaliputWidget::IsSelected(const maliput::api::Lane* _lane) { return this->IsSelected(_lane->id().string()); } - -///////////////////////////////////////////////// -void RenderMaliputWidget::Clear() { - // Clears the text labels. - for (auto it : textLabels) { - this->rootVisual->RemoveChild(it.second); - } - // Clears the meshes. - for (auto it : meshes) { - this->rootVisual->RemoveChild(it.second); - } - DeselectAll(); - if (this->arrow) { - SetArrowVisibility(false); - } - this->trafficLightsTickTimer->stop(); - this->trafficLightManager->Clear(); - textLabels.clear(); - meshes.clear(); -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::SelectLane(const maliput::api::Lane* _lane) { this->selector->SelectLane(_lane); } - -///////////////////////////////////////////////// -void RenderMaliputWidget::showEvent(QShowEvent* _e) { - QApplication::flush(); - - if (!this->renderWindow) { - this->CreateRenderWindow(); - this->trafficLightManager = std::make_unique(this->scene); - this->updateTimer->start(this->kUpdateTimeFrequency); - } - - QWidget::showEvent(_e); - - this->raise(); - this->setFocus(); -} - -///////////////////////////////////////////////// -QPaintEngine* RenderMaliputWidget::paintEngine() const { return nullptr; } - -///////////////////////////////////////////////// -// Replace inherited implementation with a do-nothing one, so that the -// context menu doesn't appear and we get back the zoom in/out using the -// right mouse button. -void RenderMaliputWidget::ShowContextMenu(const QPoint&) {} - -///////////////////////////////////////////////// -void RenderMaliputWidget::paintEvent(QPaintEvent* _e) { - if (this->renderWindow && this->camera) { - this->camera->Update(); - } - if (arrow) { - arrow->Update(); - } - - _e->accept(); -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::resizeEvent(QResizeEvent* _e) { - if (!this->renderWindow) { - return; - } - - this->renderWindow->OnResize(_e->size().width(), _e->size().height()); - this->camera->SetAspectRatio(static_cast(this->width()) / this->height()); - // This is a bit janky. We need to update ign-rendering so that the - // vertical FOV is auto updated when the aspect ratio is changed - this->camera->SetHFOV(IGN_DTOR(60)); -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::moveEvent(QMoveEvent* _e) { - QWidget::moveEvent(_e); - - if (!_e->isAccepted() || !this->renderWindow) { - return; - } - this->renderWindow->OnMove(); - - this->UpdateViewport(); -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::mousePressEvent(QMouseEvent* _e) { - if (!this->orbitViewControl) { - return; - } - - this->orbitViewControl->OnMousePress(_e); - if (_e->button() == Qt::LeftButton) { - 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); - DeselectAll(); - } - } - this->UpdateViewport(); -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::mouseReleaseEvent(QMouseEvent* _e) { - if (!this->orbitViewControl) { - return; - } - - this->orbitViewControl->OnMouseRelease(_e); - - this->UpdateViewport(); -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::mouseMoveEvent(QMouseEvent* _e) { - if (!this->orbitViewControl) { - return; - } - - this->orbitViewControl->OnMouseMove(_e); - - this->UpdateViewport(); -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::wheelEvent(QWheelEvent* _e) { - if (!this->orbitViewControl) { - return; - } - - 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(); -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::UpdateViewport() { - if (this->renderWindow && this->camera) { - this->camera->Update(); - } -} - -///////////////////////////////////////////////// -void RenderMaliputWidget::TickTrafficLights() { this->trafficLightManager->Tick(); } diff --git a/delphyne_gui/visualizer/render_maliput_widget.hh b/delphyne_gui/visualizer/render_maliput_widget.hh deleted file mode 100644 index 6c65fb0c..00000000 --- a/delphyne_gui/visualizer/render_maliput_widget.hh +++ /dev/null @@ -1,272 +0,0 @@ -// Copyright 2018 Toyota Research Institute - -#ifndef DELPHYNE_GUI_RENDERMALIPUTWIDGET_HH -#define DELPHYNE_GUI_RENDERMALIPUTWIDGET_HH - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "maliput_viewer_plugin/arrow_mesh.hh" -#include "maliput_viewer_plugin/maliput_viewer_model.hh" -#include "maliput_viewer_plugin/selector.hh" -#include "maliput_viewer_plugin/traffic_light_manager.hh" -#include "orbit_view_control.hh" - -namespace delphyne { -namespace gui { - -/// \brief Renders a group of meshes to allow the correct -class RenderMaliputWidget : public QWidget { - Q_OBJECT - - /// \brief All user options that can be configured. - class UserSettings { - public: - /// \brief Default user camera pose. - ignition::math::Pose3d userCameraPose = {4.0, 4.0, 1.0, 0.0, 0.0, -2.35619}; - }; - - public: - /// \brief Constructor. - explicit RenderMaliputWidget(QWidget* parent = 0); - - /// \brief Destructor. - virtual ~RenderMaliputWidget(); - - /// \brief Builds visuals for each mesh inside @p _maliputMeshes that is - /// enabled. - /// \details When meshes are disabled but were previously created, their - /// material are set to transparent. The same happens to those which - /// are both enabled and visibility flag is false. Otherwise, the mesh is - /// created (if necessary) with its appropriate material. - /// \param[in] _maliputMeshes A map of meshes to render. - void RenderRoadMeshes(const std::map>& _maliputMeshes); - - /// \brief Builds visuals for each label inside @p _labels that is enabled. - /// \details When labels are disabled but were previously created, their - /// materials are set to transparent. The same happens to those which are - /// both enabled and visibility flag is false. Otherwise, the text visual is - /// created (if necessary) and its appropriate material is assigned. - /// \param[in] _labels A map of labels to render. - void RenderLabels(const std::map& _labels); - - /// \brief Create and render an arrow that will be positioned slightly above the selected road. - void RenderArrow(); - - /// \brief Render all traffic lights for a given vector of them. - /// \param[in] _traffic_lights Vector containing a set of traffic lights for the current road network. - void RenderTrafficLights(const std::vector& _trafficLights); - - /// \brief Set the state of all the bulbs by the traffic light manager. - /// \param[in] _bulb_states Unordered map containing the new state for each bulb in the road network. - void SetStateOfTrafficLights(const maliput::api::rules::BulbStates& _bulbStates); - - /// \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 Deselects the currently selected region. - void DeselectAll(); - - /// \brief Gets the currently selected lanes as a string vector. - /// \returns A vector of the lane_id's which are selected. - std::vector GetSelectedLanes(); - - /// \brief Gets the currently selected branch points as a string vector. - /// \returns A vector of the branch_point_id's which are selected. - std::vector GetSelectedBranchPoints(); - - /// \brief Clears all the references to text labels, meshes and the scene. - void Clear(); - - /// \brief Selects `_lane`'s mesh when it is not selected, or deselects it when it is selected. - /// \param[in] _lane Lane to select or deselect. - void SelectLane(const maliput::api::Lane* _lane); - - /// \brief Finds if the passed in lane corresponding to the lane id is currently selected or not. - /// \param[in] _laneId Lane id of the lane to be evaluated. - /// \returns Boolean that determines whether the lane is selected or not. - bool IsSelected(const std::string& _laneId); - - /// \brief Finds if the passed in lane is currently selected or not. - /// \param[in] _lane Lane to be evaluated. - /// \returns Boolean that determines whether the lane is selected or not. - bool IsSelected(const maliput::api::Lane* _lane); - - /// \brief Overridden method to receive Qt paint event. - /// \param[in] _e The event that happened. - virtual void paintEvent(QPaintEvent* _e); - - signals: - /// \brief Signal that gets fired when a click happens on a visual (mesh) - void VisualClicked(ignition::rendering::RayQueryResult rayResult); - - /// \brief Signal that gets fired upon a call to deselect all selected regions. - void SetAllSelectedRegionsToDefault(); - - protected: - /// \brief Overridden method to receive Qt show event. - /// \param[in] _e The event that happened. - virtual void showEvent(QShowEvent* _e); - - /// \brief Overridden method to receive Qt resize event. - /// \param[in] _e The event that happened. - virtual void resizeEvent(QResizeEvent* _e); - - /// \brief Overridden method to receive Qt move event. - /// \param[in] _e The event that happened. - virtual void moveEvent(QMoveEvent* _e); - - /// \brief Overridden method to receive Qt mouse press event. - /// \param[in] _e The mouse event that happened. - virtual void mousePressEvent(QMouseEvent* _event); - - /// \brief Overridden method to receive Qt mouse release event. - /// \param[in] _e The mouse event that happened. - virtual void mouseReleaseEvent(QMouseEvent* _event); - - /// \brief Overridden method to receive Qt mouse move event. - /// \param[in] _e The mouse event that happened. - virtual void mouseMoveEvent(QMouseEvent* _event); - - /// \brief Overridden method to receive Qt mouse wheel event. - /// \param[in] _e The mouse event that happened. - virtual void wheelEvent(QWheelEvent* _event); - - /// \brief Override paintEngine to stop Qt From trying to draw on top of - /// render window. - /// \return NULL. - virtual QPaintEngine* paintEngine() const; - - // Documentation inherited - protected slots: - void ShowContextMenu(const QPoint& _pos); - - void TickTrafficLights(); - - private: - /// \brief Internal method to create the render window the first time - /// RenderWidget::showEvent is called. - void CreateRenderWindow(); - - /// \brief Render a squared grid. - /// \param[in] _cellCount the number of cells per side. - /// \param[in] _cellLength the size of each cell. - /// \param[in] _verticalCellCount the number of vertical layers. - /// \param[in] _material the material used to draw the lines. - /// \param[in] _pose the pose of the grid. - void RenderGrid(const unsigned int _cellCount, const double _cellLength, const unsigned int _verticalCellCount, - const ignition::rendering::MaterialPtr& _material, const ignition::math::Pose3d& _pose); - - /// \brief Render a 50x50 grid over the ground plane. - void RenderGroundPlaneGrid(); - - /// \brief Render the origin reference frame. - void RenderOrigin(); - - /// \brief Fills @p _ignitionMaterial with @p _maliputMaterial properties. - /// \param[in] _maliputMaterial Material properties. - /// \param[in] _ignitionMaterial A valid ignition::rendering::MaterialPtr. - /// \return True when @p _maliputMaterial is valid and @p _ignitionMaterial - /// can be filled. - bool FillMaterial(const maliput::utility::Material* _maliputMaterial, - ignition::rendering::MaterialPtr& _ignitionMaterial) const; - - /// \brief Fills a material for a lane label. - /// \param[in] _material Material to be filled. - void CreateLaneLabelMaterial(ignition::rendering::MaterialPtr& _material) const; - - /// \brief Fills a material for a branch point label. - /// \param[in] _material Material to be filled. - void CreateBranchPointLabelMaterial(ignition::rendering::MaterialPtr& _material) const; - - /// \brief Creates a bare visual and adds it as a child of the scene's root - /// visual. - void CreateRoadRootVisual(); - - /// \brief Refreshes the ignition rendering viewport. - /// TODO(basicNew): For some reason I can't understand, the required repaint - /// after a mouse event stopped being triggered. As a quick workaround I - /// created this event an explicitly call it on every mouse event, but we - /// should definitely research the underlying cause. - void UpdateViewport(); - - /// \brief The frequency at which we'll do an update on the widget. - const int kUpdateTimeFrequency = static_cast(std::round(1000.0 / 60.0)); - - /// \brief Pointer to timer to call update on a periodic basis. - QTimer* updateTimer = nullptr; - QTimer* trafficLightsTickTimer = nullptr; - - /// \brief Pointer to the renderWindow created by this class. - ignition::rendering::RenderWindowPtr renderWindow; - - /// \brief Pointer to the camera created by this class. - ignition::rendering::CameraPtr camera; - - /// \brief The scene. - ignition::rendering::ScenePtr scene; - - /// \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 Selector used for selecting clicked lanes in the visualizer. - std::unique_ptr selector; - - /// \brief Manager of traffic lights visualization. - std::unique_ptr trafficLightManager; - - /// \brief A pointer to the rendering engine - ignition::rendering::RenderEngine* engine; - - /// \brief Store all the user settings. - UserSettings userSettings; - - /// \brief Root visual where all the child mesh visuals are added. - ignition::rendering::VisualPtr rootVisual; - - /// \brief Map of mesh visual pointers. - std::map meshes; - - /// \brief Map of text labels visual pointers. - std::map textLabels; - - /// \brief Scale in the X axis for the cubes used in the selector. - static constexpr double kSelectorScaleX = 0.3; - /// \brief Scale in the Y axis for the cubes used in the selector. - static constexpr double kSelectorScaleY = 0.5; - /// \brief Scale in the Z axis for the cubes used in the selector. - static constexpr double kSelectorScaleZ = 0.1; - /// \brief Tolerance used for the selector. - static constexpr double kSelectorMinTolerance = 0.6; - /// \brief Max amount of cubes used for the selector. - static constexpr int kSelectorPoolSize = 50; - /// \brief Number of lanes to pre-initialize in selector cubes vector. - static constexpr int kNumLanes = 15; - /// \brief Every how much time should the lights blink in miliseconds. - static constexpr int kTrafficLightsTickPeriod = 500; -}; - -} // namespace gui -} // namespace delphyne - -#endif diff --git a/delphyne_gui/visualizer/render_widget.cc b/delphyne_gui/visualizer/render_widget.cc deleted file mode 100644 index 71f9ed17..00000000 --- a/delphyne_gui/visualizer/render_widget.cc +++ /dev/null @@ -1,711 +0,0 @@ -// Copyright 2017 Toyota Research Institute - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "render_widget.hh" - -Q_DECLARE_METATYPE(ignition::msgs::Model_V) -Q_DECLARE_METATYPE(ignition::msgs::Scene) - -using namespace delphyne; -using namespace gui; - -///////////////////////////////////////////////// -static void setVisualPose(const ignition::msgs::Pose& _pose, ignition::rendering::VisualPtr _shape) { - double x = 0.0; - double y = 0.0; - double z = 0.0; - double qw = 1.0; - double qx = 0.0; - double qy = 0.0; - double qz = 0.0; - - if (_pose.has_position()) { - x += _pose.position().x(); - y += _pose.position().y(); - z += _pose.position().z(); - } - if (_pose.has_orientation()) { - qw = _pose.orientation().w(); - qx = _pose.orientation().x(); - qy = _pose.orientation().y(); - qz = _pose.orientation().z(); - } - - ignition::math::Pose3d newpose(x, y, z, qw, qx, qy, qz); - - _shape->SetLocalPose(newpose); -} - -///////////////////////////////////////////////// -static void setPoseFromMessage(const ignition::msgs::Visual& _vis, ignition::rendering::VisualPtr _shape) { - if (_vis.has_pose()) { - setVisualPose(_vis.pose(), _shape); - } -} - -///////////////////////////////////////////////// -static void setPoseFromMessage(const ignition::msgs::Link& _link, ignition::rendering::VisualPtr _shape) { - if (_link.has_pose()) { - setVisualPose(_link.pose(), _shape); - } -} - -///////////////////////////////////////////////// -RenderWidget::RenderWidget(QWidget*) : Plugin(), initializedScene(false), engine(nullptr) { - qRegisterMetaType(); - qRegisterMetaType(); - - this->setAttribute(Qt::WA_OpaquePaintEvent, true); - this->setAttribute(Qt::WA_PaintOnScreen, true); - this->setAttribute(Qt::WA_NoSystemBackground, true); - - this->setFocusPolicy(Qt::StrongFocus); - this->setMouseTracking(true); - this->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - - this->title = "RenderWidget"; - - this->CreateRenderWindow(); - - // The below block means that every time the updateTime expires, we do an - // update on the widget. Later on, we call the start() method to start this - // time at a fixed frequency. Note that we do not start this timer until the - // first time that showEvent() is called. - this->updateTimer = new QTimer(this); - QObject::connect(this->updateTimer, SIGNAL(timeout()), this, SLOT(update())); - this->updateTimer->start(this->kUpdateTimeFrequency); - - QObject::connect(this, SIGNAL(NewInitialScene(const ignition::msgs::Scene&)), this, - SLOT(SetInitialScene(const ignition::msgs::Scene&))); - QObject::connect(this, SIGNAL(NewDraw(const ignition::msgs::Model_V&)), this, - SLOT(UpdateScene(const ignition::msgs::Model_V&))); - - // Request a scene - this->node.Request("/get_scene", &RenderWidget::OnSetScene, this); -} - -///////////////////////////////////////////////// -RenderWidget::~RenderWidget() { - if (this->engine != nullptr) { - // TODO(clalancette): We need to call this->engine->Fini() to clean up - // some engine resources, but this sometimes causes a hang on quit. - // For right now, disable this, but we should debug this and re-enable this - // cleanup. - // this->engine->Fini(); - this->orbitViewControl.reset(); - this->camera->RemoveChildren(); - this->camera.reset(); - this->mainDirectionalLight.reset(); - this->scene->DestroyNodes(); - this->scene->DestroySensors(); - this->scene.reset(); - } -} - -///////////////////////////////////////////////// -void RenderWidget::LoadConfig(const tinyxml2::XMLElement* _pluginElem) { - tinyxml2::XMLPrinter printer; - if (!_pluginElem->Accept(&printer)) { - ignwarn << "There was an error parsing the plugin element for [" << this->title << "]." << std::endl; - return; - } - - // Configure if we need to cast shadows. - bool castShadows = kCastShadowsByDefault; - - if (auto castShadowsElem = _pluginElem->FirstChildElement("cast_shadows")) { - castShadowsElem->QueryBoolText(&castShadows); - } - - this->mainDirectionalLight->SetCastShadows(castShadows); - - // Load the user camera options. - auto userCameraXML = _pluginElem->FirstChildElement("camera"); - while (userCameraXML) { - std::string cameraName; - if (userCameraXML->Attribute("name", "user_camera")) { - auto poseXML = userCameraXML->FirstChildElement("pose"); - if (poseXML) { - auto poseStr = poseXML->GetText(); - std::istringstream stream(poseStr); - stream >> this->userSettings.userCameraPose; - - if (this->camera) { - auto position = this->userSettings.userCameraPose.Pos(); - auto rotation = this->userSettings.userCameraPose.Rot().Euler(); - this->camera->SetLocalRotation(rotation.X(), rotation.Y(), rotation.Z()); - this->camera->SetLocalPosition(position.X(), position.Y(), position.Z()); - } - } else { - ignerr << "Unable to parse element within " << std::endl; - } - break; - } - - // Not a user camera, skip to the next camera. - userCameraXML = userCameraXML->NextSiblingElement("camera"); - } -} - -///////////////////////////////////////////////// -std::string RenderWidget::ConfigStr() const { - tinyxml2::XMLElement* pluginXML; - tinyxml2::XMLDocument xmlDoc; - - if (configStr.empty()) { - // If we have no defined plugin configuration, create the XML doc with the - // plugin element and initialize it with the basic properties. - pluginXML = xmlDoc.NewElement("plugin"); - pluginXML->SetAttribute("filename", "RenderWidget"); - xmlDoc.InsertFirstChild(pluginXML); - } else { - // In case we do have an existing config, parse it. - xmlDoc.Parse(configStr.c_str()); - pluginXML = xmlDoc.FirstChildElement("plugin"); - - // If there is a camera element, remove it as we will be overriding it below - auto cameraXML = pluginXML->FirstChildElement("camera"); - if (cameraXML) { - pluginXML->DeleteChild(cameraXML); - } - } - - // User camera options. - tinyxml2::XMLElement* userCameraXML = xmlDoc.NewElement("camera"); - userCameraXML->SetAttribute("name", "user_camera"); - pluginXML->InsertEndChild(userCameraXML); - tinyxml2::XMLElement* poseXML = xmlDoc.NewElement("pose"); - auto pos = this->camera->LocalPose().Pos(); - auto rot = this->camera->LocalPose().Rot().Euler(); - std::stringstream stream; - stream << pos.X() << " " << pos.Y() << " " << pos.Z() << " " << rot.X() << " " << rot.Y() << " " << rot.Z(); - poseXML->SetText(stream.str().c_str()); - userCameraXML->InsertEndChild(poseXML); - - tinyxml2::XMLPrinter printer; - xmlDoc.Print(&printer); - - return printer.CStr(); -} - -///////////////////////////////////////////////// -void RenderWidget::OnSetScene(const ignition::msgs::Scene& reply, const bool result) { - if (result) { - emit this->NewInitialScene(reply); - } -} - -///////////////////////////////////////////////// -void RenderWidget::OnUpdateScene(const ignition::msgs::Model_V& _msg) { emit this->NewDraw(_msg); } - -///////////////////////////////////////////////// -bool RenderWidget::CreateVisual(const ignition::msgs::Visual& _vis, ignition::rendering::VisualPtr& _visual, - ignition::rendering::MaterialPtr& _material) const { - _visual = this->scene->CreateVisual(); - if (!_visual) { - ignerr << "Failed to create visual" << std::endl; - return false; - } - - _material = this->scene->CreateMaterial(); - if (!_material) { - ignerr << "Failed to create material" << std::endl; - return false; - } - - if (_vis.has_material()) { - const auto& material = _vis.material(); - if (material.has_diffuse()) { - const auto& diffuse = material.diffuse(); - _material->SetDiffuse(diffuse.r(), diffuse.g(), diffuse.b()); - - const auto& ambient = material.ambient(); - _material->SetAmbient(ambient.r(), ambient.g(), ambient.b()); - - const auto& specular = material.specular(); - _material->SetSpecular(specular.r(), specular.g(), specular.b()); - } - } - - _material->SetTransparency(_vis.transparency()); - - _material->SetShininess(50); - _material->SetReflectivity(0); - - return true; -} - -///////////////////////////////////////////////// -ignition::rendering::VisualPtr RenderWidget::Render(const ignition::msgs::Visual& _vis, - const ignition::math::Vector3d& _scale, - const ignition::rendering::MaterialPtr& _material, - ignition::rendering::VisualPtr& _visual) { - setPoseFromMessage(_vis, _visual); - _visual->SetLocalScale(_scale.X(), _scale.Y(), _scale.Z()); - _visual->SetMaterial(_material); - return _visual; -} - -///////////////////////////////////////////////// -ignition::rendering::VisualPtr RenderWidget::RenderBox(const ignition::msgs::Visual& _vis, - ignition::rendering::VisualPtr& _visual, - ignition::rendering::MaterialPtr& _material) { - ignition::math::Vector3d scale = ignition::math::Vector3d::One; - auto geomBox = _vis.geometry().box(); - if (geomBox.has_size()) { - scale.X() = geomBox.size().x(); - scale.Y() = geomBox.size().y(); - scale.Z() = geomBox.size().z(); - } - - _visual->AddGeometry(this->scene->CreateBox()); - this->Render(_vis, scale, _material, _visual); - return _visual; -} - -///////////////////////////////////////////////// -ignition::rendering::VisualPtr RenderWidget::RenderSphere(const ignition::msgs::Visual& _vis, - ignition::rendering::VisualPtr& _visual, - ignition::rendering::MaterialPtr& _material) { - ignition::math::Vector3d scale = ignition::math::Vector3d::One; - auto geomSphere = _vis.geometry().sphere(); - scale.X() *= geomSphere.radius(); - scale.Y() *= geomSphere.radius(); - scale.Z() *= geomSphere.radius(); - - _visual->AddGeometry(this->scene->CreateSphere()); - this->Render(_vis, scale, _material, _visual); - return _visual; -} - -///////////////////////////////////////////////// -ignition::rendering::VisualPtr RenderWidget::RenderCylinder(const ignition::msgs::Visual& _vis, - ignition::rendering::VisualPtr& _visual, - ignition::rendering::MaterialPtr& _material) { - ignition::math::Vector3d scale = ignition::math::Vector3d::One; - auto geomCylinder = _vis.geometry().cylinder(); - scale.X() *= 2 * geomCylinder.radius(); - scale.Y() *= 2 * geomCylinder.radius(); - scale.Z() = geomCylinder.length(); - - _visual->AddGeometry(this->scene->CreateCylinder()); - this->Render(_vis, scale, _material, _visual); - return _visual; -} - -///////////////////////////////////////////////// -ignition::rendering::VisualPtr RenderWidget::RenderMesh(const ignition::msgs::Visual& _vis) { - // Sanity check: Make sure that the message contains all required fields. - if (!_vis.has_geometry()) { - ignerr << "Unable to find geometry in message" << std::endl; - } - - if (!_vis.geometry().has_mesh()) { - ignerr << "Unable to find mesh in message" << std::endl; - } - - if (_vis.geometry().mesh().filename().empty()) { - ignerr << "Unable to find filename in message" << std::endl; - } - - ignition::rendering::VisualPtr mesh = this->scene->CreateVisual(); - if (!mesh) { - ignerr << "Failed to create visual" << std::endl; - return nullptr; - } - - auto filename = _vis.geometry().mesh().filename(); - delphyne::utility::PackageManager* package_manager = delphyne::utility::PackageManager::Instance(); - const utility::Package& package_in_use = package_manager->package_in_use(); - ignition::rendering::MeshDescriptor descriptor; - ignition::common::URI mesh_uri = package_in_use.Resolve(filename); - if (!mesh_uri.Valid()) { - ignerr << "Unable to locate mesh [" << filename << "]" << std::endl; - return nullptr; - } - descriptor.meshName = "/" + mesh_uri.Path().Str(); - ignition::common::MeshManager* meshManager = ignition::common::MeshManager::Instance(); - descriptor.mesh = meshManager->Load(descriptor.meshName); - if (!descriptor.mesh) { - return nullptr; - } - - ignition::rendering::MeshPtr meshGeom = this->scene->CreateMesh(descriptor); - mesh->AddGeometry(meshGeom); - ignition::math::Vector3d center; - ignition::math::Vector3d minXYZ; - ignition::math::Vector3d maxXYZ; - descriptor.mesh->AABB(center, minXYZ, maxXYZ); - /* Position from messages have the world coordinates of the mesh. */ - minBBScene = ignition::math::Vector3d(std::min(minXYZ.X() + _vis.pose().position().x(), minBBScene.X()), - std::min(minXYZ.Y() + _vis.pose().position().y(), minBBScene.Y()), - std::min(minXYZ.Z() + _vis.pose().position().z(), minBBScene.Z())); - maxBBScene = ignition::math::Vector3d(std::max(maxXYZ.X() + _vis.pose().position().x(), maxBBScene.X()), - std::max(maxXYZ.Y() + _vis.pose().position().y(), maxBBScene.Y()), - std::max(maxXYZ.Z() + _vis.pose().position().z(), maxBBScene.Z())); - - setPoseFromMessage(_vis, mesh); - - return mesh; -} - -///////////////////////////////////////////////// -void RenderWidget::SetInitialScene(const ignition::msgs::Scene& _msg) { - if (this->initializedScene) { - return; - } - - for (int i = 0; i < _msg.model_size(); ++i) { - LoadModel(_msg.model(i)); - } - - // clang-format off - ignition::math::Vector3d center((minBBScene.X() + maxBBScene.X()) / 2.0, - (minBBScene.Y() + maxBBScene.Y()) / 2.0, - (minBBScene.Z() + maxBBScene.Z()) / 2.0); - // clang-format on - - const double sphereRadius = center.Distance(minBBScene); - const double fov = this->camera->HFOV().Radian(); - // Angle from the origin of the sphere. - constexpr double kInclinationAngle = IGN_PI / 180.0 * 60.0; - // Get camera distance required for a sphere circumscribing the scene - // bounding box to fit within the camera horizontal FOV. - const double distance = sphereRadius / sin(fov / 2.0); - const double azimuthAngle = atan((maxBBScene.X() - minBBScene.X()) / (maxBBScene.Y() - minBBScene.Y())); - this->camera->SetWorldPosition( - ignition::math::Vector3d(center.X() + distance * sin(kInclinationAngle) * cos(azimuthAngle), - center.Y() + distance * sin(kInclinationAngle) * sin(azimuthAngle), - center.Z() + distance * cos(kInclinationAngle))); - this->camera->SetWorldRotation(ignition::math::Matrix4d::LookAt(this->camera->WorldPosition(), center).Pose().Rot()); - - this->node.Subscribe("visualizer/scene_update", &RenderWidget::OnUpdateScene, this); -} - -///////////////////////////////////////////////// -ignition::rendering::VisualPtr RenderWidget::CreateLinkRootVisual(ignition::msgs::Link& _link, uint32_t _robotID) { - ignition::rendering::VisualPtr linkRootVisual = this->scene->CreateVisual(); - // The visual's root pose is initialized to zero because the load message - // from LCM doesn't include the link pose but only the poses of the inner - // visuals. - // This value will be updated on each new draw message received. - ignition::math::Pose3d newpose(0, 0, 0, 1, 0, 0, 0); - linkRootVisual->SetLocalPose(newpose); - this->scene->RootVisual()->AddChild(linkRootVisual); - - // Assign the visual created above as the root visual of the link - auto& links = this->allVisuals[_robotID]; - links.insert(std::make_pair(_link.name(), linkRootVisual)); - return linkRootVisual; -} - -///////////////////////////////////////////////// -void RenderWidget::LoadModel(const ignition::msgs::Model& _msg) { - for (int i = 0; i < _msg.link_size(); ++i) { - auto link = _msg.link(i); - - // Sanity check: Verify that the link contains the required name. - if (link.name().empty()) { - ignerr << "No name on link, skipping" << std::endl; - continue; - } - - // Sanity check: Verify that the visual doesn't exist already. - const auto& modelIt = this->allVisuals.find(_msg.id()); - if (modelIt != this->allVisuals.end()) { - if (modelIt->second.find(link.name()) != modelIt->second.end()) { - ignerr << "Duplicated link [" << link.name() << "] for model " << _msg.id() << ". Skipping" << std::endl; - continue; - } - } - - if (link.visual_size() == 0) { - continue; - } - - igndbg << "Rendering: [" << link.name() << "] (" << _msg.id() << ")" << std::endl; - - // Create a single root visual per link which will serve only - // for hierarchical purposes, so that it can become the parent - // of each of the "real" visuals of the link. - ignition::rendering::VisualPtr linkRootVisual = RenderWidget::CreateLinkRootVisual(link, _msg.id()); - - // Iterate through all the visuals of the link and add - // them as childs of the link's root visual - for (int j = 0; j < link.visual_size(); ++j) { - const auto& vis = link.visual(j); - if (!vis.has_geometry()) { - ignerr << "No geometry in link [" << link.name() << "][" << j << "]. Skipping" << std::endl; - continue; - } - - ignition::rendering::VisualPtr visual; - ignition::rendering::MaterialPtr material; - if (!this->CreateVisual(vis, visual, material)) { - continue; - } - - // A real visual that is going to become - // one of the link's root-visual childs - ignition::rendering::VisualPtr ignvis; - - if (vis.geometry().has_box()) { - ignvis = this->RenderBox(vis, visual, material); - } else if (vis.geometry().has_sphere()) { - ignvis = this->RenderSphere(vis, visual, material); - } else if (vis.geometry().has_cylinder()) { - ignvis = this->RenderCylinder(vis, visual, material); - } else if (vis.geometry().has_mesh()) { - ignvis = this->RenderMesh(vis); - } else { - ignerr << "Invalid shape for [" << link.name() << "]. Skipping" << std::endl; - continue; - } - - linkRootVisual->AddChild(ignvis); - - setPoseFromMessage(link, linkRootVisual); - } - } - - this->initializedScene = true; -} - -///////////////////////////////////////////////// -void RenderWidget::UpdateScene(const ignition::msgs::Model_V& _msg) { - for (int j = 0; j < _msg.models_size(); ++j) { - auto model = _msg.models(j); - - for (int i = 0; i < model.link_size(); ++i) { - auto link = model.link(i); - - // Sanity check: It's required to have a link name. - if (link.name().empty()) { - ignerr << "Skipping link without name" << std::endl; - continue; - } - - // Sanity check: Make sure that the model Id exists. - auto robotIt = this->allVisuals.find(model.id()); - if (robotIt == this->allVisuals.end()) { - ignerr << "Could not find model Id [" << model.id() << "]. Skipping" << std::endl; - continue; - } - - // Sanity check: Make sure that the link name exists. - auto visualsIt = robotIt->second.find(link.name()); - if (visualsIt == robotIt->second.end()) { - ignerr << "Could not find link name [" << link.name() << "]. Skipping" << std::endl; - continue; - } - // Update the pose of the root visual only; - // the relative poses of the children remain the same - auto& visual = visualsIt->second; - - setPoseFromMessage(link, visual); - } - } -} - -///////////////////////////////////////////////// -void RenderWidget::CreateRenderWindow() { - std::string engineName = "ogre"; - ignition::rendering::RenderEngineManager* manager = ignition::rendering::RenderEngineManager::Instance(); - this->engine = manager->Engine(engineName); - if (!this->engine) { - ignerr << "Engine '" << engineName << "' is not supported" << std::endl; - return; - } - - // Try to reutilize the scene if exists - this->scene = engine->SceneByName("scene"); - if (!this->scene) { - // Create a scene - this->scene = engine->CreateScene("scene"); - if (!this->scene) { - ignerr << "Failed to create scene" << std::endl; - return; - } - } - - // Lights. - this->scene->SetAmbientLight(0.9, 0.9, 0.9); - ignition::rendering::VisualPtr root = this->scene->RootVisual(); - if (!root) { - ignerr << "Failed to find the root visual" << std::endl; - return; - } - this->mainDirectionalLight = this->scene->CreateDirectionalLight(); - if (!this->mainDirectionalLight) { - ignerr << "Failed to create a directional light" << std::endl; - return; - } - this->mainDirectionalLight->SetDirection(-0.5, -0.5, -1); - this->mainDirectionalLight->SetDiffuseColor(0.9, 0.9, 0.9); - this->mainDirectionalLight->SetSpecularColor(0.9, 0.9, 0.9); - this->mainDirectionalLight->SetCastShadows(kCastShadowsByDefault); - root->AddChild(this->mainDirectionalLight); - - // create user camera - this->camera = this->scene->CreateCamera("user_camera"); - if (!this->camera) { - ignerr << "Failed to create camera" << std::endl; - return; - } - // Rotate 135 deg on the Z axis - auto rotation = this->userSettings.userCameraPose.Rot().Euler(); - this->camera->SetLocalRotation(rotation.X(), rotation.Y(), rotation.Z()); - // Step away from the center of the scene - auto position = this->userSettings.userCameraPose.Pos(); - this->camera->SetLocalPosition(position.X(), position.Y(), position.Z()); - this->camera->SetAspectRatio(static_cast(this->width()) / this->height()); - this->camera->SetHFOV(IGN_DTOR(60)); - root->AddChild(this->camera); - - // Set a gradient background color from white (top) to black (bottom) - std::array gradientBackgroundColor; - gradientBackgroundColor[0].Set(1.0, 1.0, 1.0); - gradientBackgroundColor[1].Set(0.0, 0.0, 0.0); - gradientBackgroundColor[2].Set(1.0, 1.0, 1.0); - gradientBackgroundColor[3].Set(0.0, 0.0, 0.0); - this->scene->SetGradientBackgroundColor(gradientBackgroundColor); - - // create render window - std::string winHandle = std::to_string(static_cast(this->winId())); - this->renderWindow = this->camera->CreateRenderWindow(); - if (!this->renderWindow) { - ignerr << "Failed to create camera render window" << std::endl; - return; - } - this->renderWindow->SetHandle(winHandle); - this->renderWindow->SetWidth(this->width()); - this->renderWindow->SetHeight(this->height()); - - // render once to create the window. - this->camera->Update(); - - this->orbitViewControl.reset(new OrbitViewControl(this->camera)); -} - -///////////////////////////////////////////////// -void RenderWidget::showEvent(QShowEvent* _e) { - QApplication::flush(); - - QWidget::showEvent(_e); - - this->raise(); - this->setFocus(); -} - -///////////////////////////////////////////////// -QPaintEngine* RenderWidget::paintEngine() const { return nullptr; } - -///////////////////////////////////////////////// -// Replace inherited implementation with a do-nothing one, so that the -// context menu doesn't appear and we get back the zoom in/out using the -// right mouse button. -void RenderWidget::ShowContextMenu(const QPoint&) {} - -///////////////////////////////////////////////// -void RenderWidget::paintEvent(QPaintEvent* _e) { - if (this->renderWindow && this->camera) { - this->camera->Update(); - } - - _e->accept(); -} - -///////////////////////////////////////////////// -void RenderWidget::resizeEvent(QResizeEvent* _e) { - if (!this->renderWindow) { - return; - } - this->renderWindow->OnResize(_e->size().width(), _e->size().height()); - this->camera->SetAspectRatio(static_cast(this->width()) / this->height()); - // This is a bit janky. We need to update ign-rendering so that the - // vertical FOV is auto updated when the aspect ratio is changed - this->camera->SetHFOV(IGN_DTOR(60)); -} - -///////////////////////////////////////////////// -void RenderWidget::moveEvent(QMoveEvent* _e) { - QWidget::moveEvent(_e); - - if (!_e->isAccepted() || !this->renderWindow) { - return; - } - this->renderWindow->OnMove(); -} - -///////////////////////////////////////////////// -void RenderWidget::mousePressEvent(QMouseEvent* _e) { - if (!this->orbitViewControl) { - return; - } - - this->orbitViewControl->OnMousePress(_e); -} - -///////////////////////////////////////////////// -void RenderWidget::mouseReleaseEvent(QMouseEvent* _e) { - if (!this->orbitViewControl) { - return; - } - - this->orbitViewControl->OnMouseRelease(_e); -} - -///////////////////////////////////////////////// -void RenderWidget::mouseMoveEvent(QMouseEvent* _e) { - if (!this->orbitViewControl) { - return; - } - - this->orbitViewControl->OnMouseMove(_e); -} - -///////////////////////////////////////////////// -void RenderWidget::wheelEvent(QWheelEvent* _e) { - if (!this->orbitViewControl) { - return; - } - - this->orbitViewControl->OnMouseWheel(_e); -} - -IGN_COMMON_REGISTER_SINGLE_PLUGIN(delphyne::gui::RenderWidget, ignition::gui::Plugin) diff --git a/delphyne_gui/visualizer/render_widget.hh b/delphyne_gui/visualizer/render_widget.hh deleted file mode 100644 index 8a6cde4f..00000000 --- a/delphyne_gui/visualizer/render_widget.hh +++ /dev/null @@ -1,254 +0,0 @@ -// Copyright 2017 Toyota Research Institute - -#ifndef DELPHYNE_GUI_RENDERWIDGET_HH -#define DELPHYNE_GUI_RENDERWIDGET_HH - -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -#include "orbit_view_control.hh" - -// Forward declarations. -namespace tinyxml2 { -class XMLElement; -} -namespace ignition { -namespace msgs { -class Model; -class Model_V; -class Scene; -class Visual; -} // namespace msgs -} // namespace ignition - -namespace delphyne { -namespace gui { - -/// \class RenderWidget -/// \brief This is a class that implements a simple ign-gui widget for -/// rendering a scene, using the ign-rendering functionality. -class RenderWidget : public ignition::gui::Plugin { - Q_OBJECT - - /// \def VisualPtr_V - /// \brief A vector of visual pointers. - using VisualPtr_V = std::vector; - - /// \brief All user options that can be configured. - class UserSettings { - /// \brief Default user camera pose. - public: - ignition::math::Pose3d userCameraPose = {4.0, 4.0, 1.0, 0.0, 0.0, -2.35619}; - }; - - public: - /// \brief Default constructor. - explicit RenderWidget(QWidget* parent = 0); - - /// \brief Default Destructor. - virtual ~RenderWidget(); - - /// \brief Overridden method to load user configuration. - /// \param[in] _pluginElem The data containing the configuration. - virtual void LoadConfig(const tinyxml2::XMLElement* _pluginElem); - - /// \brief Overridden method to get the configuration XML as a string. - /// \return Config element. - virtual std::string ConfigStr() const; - - /// \brief Callback to set the initial scene.. - /// \param[in] _msg The new scene. - public slots: - void SetInitialScene(const ignition::msgs::Scene& _msg); - - /// \brief Callback to update the scene. - /// \param[in] _msg Message containing an update. - public slots: - void UpdateScene(const ignition::msgs::Model_V& _msg); - - /// \brief Notify that there's a new scene. - /// \param[in] _msg The new scene. - signals: - void NewInitialScene(const ignition::msgs::Scene& _msg); - - /// \brief Notify that there's a new draw update. - /// \param[in] _msg Message contining the update. - signals: - void NewDraw(const ignition::msgs::Model_V& _msg); - - protected: - /// \brief Overridden method to receive Qt paint event. - /// \param[in] _e The event that happened. - virtual void paintEvent(QPaintEvent* _e); - - /// \brief Overridden method to receive Qt show event. - /// \param[in] _e The event that happened. - virtual void showEvent(QShowEvent* _e); - - /// \brief Overridden method to receive Qt resize event. - /// \param[in] _e The event that happened. - virtual void resizeEvent(QResizeEvent* _e); - - /// \brief Overridden method to receive Qt move event. - /// \param[in] _e The event that happened. - virtual void moveEvent(QMoveEvent* _e); - - /// \brief Overridden method to receive Qt mouse press event. - /// \param[in] _e The mouse event that happened. - virtual void mousePressEvent(QMouseEvent* _event); - - /// \brief Overridden method to receive Qt mouse release event. - /// \param[in] _e The mouse event that happened. - virtual void mouseReleaseEvent(QMouseEvent* _event); - - /// \brief Overridden method to receive Qt mouse move event. - /// \param[in] _e The mouse event that happened. - virtual void mouseMoveEvent(QMouseEvent* _event); - - /// \brief Overridden method to receive Qt mouse wheel event. - /// \param[in] _e The mouse event that happened. - virtual void wheelEvent(QWheelEvent* _event); - - /// \brief Override paintEngine to stop Qt From trying to draw on top of - /// render window. - /// \return NULL. - virtual QPaintEngine* paintEngine() const; - - // Documentation inherited - protected slots: - void ShowContextMenu(const QPoint& _pos); - - private: - /// \brief Set the initial scene if the result flag indicates that the service - /// call was successful - /// \param[in] reply The scene to be loaded - /// \param[in] result True if the scene request was successful, false otherwise - void OnSetScene(const ignition::msgs::Scene& reply, const bool result); - - /// \brief Internal method to create the render window the first time - /// RenderWidget::showEvent is called. - void CreateRenderWindow(); - - /// \brief Load an entire model in the scene - /// \param[in] _msg The new model. - void LoadModel(const ignition::msgs::Model& _msg); - - /// \brief Update an existing visual. - /// \param[in] _msg The pose of the new visual. - void OnUpdateScene(const ignition::msgs::Model_V& _msg); - - /// \brief Create a visual and material before rendering. - /// \param[in] _vis The input message containing the visual specs. - /// \param[out] _visual The new visual. - /// \param[out] _material The new material. - bool CreateVisual(const ignition::msgs::Visual& _vis, ignition::rendering::VisualPtr& _visual, - ignition::rendering::MaterialPtr& _material) const; - - /// \brief Creates a root visual for a given link - /// and adds it as a child of the scene - /// \param[in] _link The robot link - /// \param[in] _robotID The robot uuid - /// \return The root visual - ignition::rendering::VisualPtr CreateLinkRootVisual(ignition::msgs::Link& _link, const uint32_t _robotID); - - /// \brief Helper function used during the last phase of rendering. - /// \param[in] _vis The input message containing the visual specs. - /// \param[in] _scale The scale vector. - /// \param[in] _material The material. - /// \param[in, out] _visual The visual that is going to be rendered. - ignition::rendering::VisualPtr Render(const ignition::msgs::Visual& _vis, const ignition::math::Vector3d& _scale, - const ignition::rendering::MaterialPtr& _material, - ignition::rendering::VisualPtr& _visual); - - /// \brief Render a new box. - /// \param[in] _vis the visual containing the properties of the object. - /// \return A pointer to the new visual. - ignition::rendering::VisualPtr RenderBox(const ignition::msgs::Visual& _vis, ignition::rendering::VisualPtr& _visual, - ignition::rendering::MaterialPtr& _material); - - /// \brief Render a new sphere. - /// \param[in] _vis the visual containing the properties of the object. - /// \return A pointer to the new visual. - ignition::rendering::VisualPtr RenderSphere(const ignition::msgs::Visual& _vis, - ignition::rendering::VisualPtr& _visual, - ignition::rendering::MaterialPtr& _material); - - /// \brief Render a new cylinder. - /// \param[in] _vis the visual containing the properties of the object. - /// \return A pointer to the new visual. - ignition::rendering::VisualPtr RenderCylinder(const ignition::msgs::Visual& _vis, - ignition::rendering::VisualPtr& _visual, - ignition::rendering::MaterialPtr& _material); - - /// \brief Render a new mesh. - /// \param[in] _vis the visual containing the properties of the object. - /// \return A pointer to the new visual. - ignition::rendering::VisualPtr RenderMesh(const ignition::msgs::Visual& _vis); - - /// \brief The frequency at which we'll do an update on the widget. - const int kUpdateTimeFrequency = static_cast(std::round(1000.0 / 60.0)); - - /// \brief Pointer to timer to call update on a periodic basis. - QTimer* updateTimer = nullptr; - - /// \brief Pointer to the renderWindow created by this class. - ignition::rendering::RenderWindowPtr renderWindow; - - /// \brief Pointer to the camera created by this class. - ignition::rendering::CameraPtr camera; - - /// \brief Pointer to the main directional light created by this class. - ignition::rendering::DirectionalLightPtr mainDirectionalLight; - - /// \brief A transport node. - ignition::transport::Node node; - - /// \brief The scene. - ignition::rendering::ScenePtr scene; - - /// \brief Is the scene initialized?. - bool initializedScene; - - /// \brief The scene request message to be sent to the backend - ignition::msgs::SceneRequest sceneRequestMsg; - - /// \brief Controls the view of the scene. - std::unique_ptr orbitViewControl; - - /// \brief This the data structure that stores the pointers to all visuals. - /// The key is the model Id. - /// The value is another map, where the key is the link name, and the - /// value is a root visual of which all the link's visuals are childs - std::map> allVisuals; - - /// \brief A pointer to the rendering engine - ignition::rendering::RenderEngine* engine; - - /// \brief Store all the user settings. - UserSettings userSettings; - - /// \brief Min bounding box of the whole scene - ignition::math::Vector3d minBBScene; - - /// \brief Max bounding box of the whole scene - ignition::math::Vector3d maxBBScene; - - /// \brief Are we casting shadows by default? - bool kCastShadowsByDefault{true}; -}; -} // namespace gui -} // namespace delphyne - -#endif diff --git a/delphyne_gui/visualizer/rules_visualizer_widget.cc b/delphyne_gui/visualizer/rules_visualizer_widget.cc deleted file mode 100644 index 9b7f7b86..00000000 --- a/delphyne_gui/visualizer/rules_visualizer_widget.cc +++ /dev/null @@ -1,122 +0,0 @@ -// Copyright 2019 Toyota Research Institute - -#include "rules_visualizer_widget.hh" - -#include -#include -#include -#include -#include -#include -#include - -namespace delphyne { -namespace gui { - -RulesVisualizerWidget::RulesVisualizerWidget(QWidget* parent) : QWidget(parent) { - QVBoxLayout* layout = new QVBoxLayout(this); - this->rules_log_text_browser = new QTextBrowser(this); - this->rules_label = new QLabel("Rules", this); - this->lanes_label = new QLabel("Lanes", this); - this->lanes_list = new QListWidget(this); - this->phase_tree = new QTreeWidget(this); - this->phase_tree->setColumnCount(1); - this->phase_tree->setHeaderLabel("Phase ring"); - - QObject::connect(this->lanes_list, SIGNAL(itemClicked(QListWidgetItem*)), this, - SLOT(OnLaneItemClicked(QListWidgetItem*))); - - QObject::connect(this, SIGNAL(ReceiveRules(QString, QString)), this, SLOT(OnRulesReceived(QString, QString))); - QObject::connect(this->phase_tree, SIGNAL(itemClicked(QTreeWidgetItem*, int)), this, - SLOT(OnPhaseTreeItemClicked(QTreeWidgetItem*, int))); - - layout->addWidget(this->phase_tree); - layout->addWidget(this->lanes_label); - layout->addWidget(this->lanes_list); - layout->addWidget(this->rules_label); - layout->addWidget(this->rules_log_text_browser); - - this->setLayout(layout); -} - -void RulesVisualizerWidget::AddLaneId(const QString& lane_id) { - DELPHYNE_DEMAND(this->lanes_list != nullptr); - this->lanes_list->addItem(lane_id); -} - -void RulesVisualizerWidget::AddText(const QString& text) { - DELPHYNE_DEMAND(this->rules_log_text_browser != nullptr); - this->rules_log_text_browser->append(text + "\n"); -} - -void RulesVisualizerWidget::ClearLaneList() { - DELPHYNE_DEMAND(this->lanes_list != nullptr); - this->lanes_list->clear(); -} - -void RulesVisualizerWidget::ClearText() { - DELPHYNE_DEMAND(this->rules_log_text_browser != nullptr); - this->rules_log_text_browser->clear(); -} - -void RulesVisualizerWidget::ConstructPhaseRingTree( - std::unordered_map>&& phases_per_ring) { - DELPHYNE_DEMAND(this->phase_tree != nullptr); - this->phase_tree->clear(); - for (auto& phase_ring_n_phases : phases_per_ring) { - QTreeWidgetItem* phase_ring_item = new QTreeWidgetItem(this->phase_tree); - phase_ring_item->setText(0, QString::fromStdString(phase_ring_n_phases.first)); - phase_ring_item->setFlags(phase_ring_item->flags() & (~Qt::ItemIsSelectable)); - this->phase_tree->addTopLevelItem(phase_ring_item); - std::vector& phases = phase_ring_n_phases.second; - for (size_t i = 0; i < phases.size(); ++i) { - QTreeWidgetItem* phase_item = new QTreeWidgetItem(); - phase_item->setText(0, std::move(phases[i])); - phase_ring_item->addChild(phase_item); - } - } -} - -QString RulesVisualizerWidget::GetSelectedLaneId() const { - QListWidgetItem* selected_item = this->lanes_list->currentItem(); - if (selected_item) { - return selected_item->text(); - } - return QString(); -} - -PhaseRingPhaseIds RulesVisualizerWidget::GetSelectedPhaseRingAndPhaseId() const { - PhaseRingPhaseIds phase_ring_phase_ids; - QList selected_items = this->phase_tree->selectedItems(); - if (selected_items.size() > 0) { - // We asume that we only have 1 level of depth in the tree. Example: - // |_PhaseRing1 - // |__Phase1 - // |__Phase2 - // |_PhaseRing2 - // |__Phase1 - QTreeWidgetItem* parent = selected_items[0]->parent(); - phase_ring_phase_ids.phase_ring_id = parent->text(0); - phase_ring_phase_ids.phase_id = selected_items[0]->text(0); - } - return phase_ring_phase_ids; -} - -void RulesVisualizerWidget::OnLaneItemClicked(QListWidgetItem*) { emit RequestRules(); } - -void RulesVisualizerWidget::OnRulesReceived(QString lane_id, QString rules) { - QList items = this->lanes_list->findItems(lane_id, Qt::MatchExactly); - DELPHYNE_DEMAND(items.size() == 1); - items[0]->setSelected(true); - this->lanes_list->scrollToItem(items[0]); - this->ClearText(); - this->AddText(rules); - QTextCursor cursor = this->rules_log_text_browser->textCursor(); - cursor.setPosition(0); - this->rules_log_text_browser->setTextCursor(cursor); -} - -void RulesVisualizerWidget::OnPhaseTreeItemClicked(QTreeWidgetItem*, int) { emit RequestRules(); } - -} // namespace gui -} // namespace delphyne diff --git a/delphyne_gui/visualizer/rules_visualizer_widget.hh b/delphyne_gui/visualizer/rules_visualizer_widget.hh deleted file mode 100644 index 501db072..00000000 --- a/delphyne_gui/visualizer/rules_visualizer_widget.hh +++ /dev/null @@ -1,86 +0,0 @@ -// Copyright 2019 Toyota Research Institute - -#ifndef DELPHYNE_GUI_RULES_VISUALIZER_WIDGET_HH -#define DELPHYNE_GUI_RULES_VISUALIZER_WIDGET_HH - -#include - -#include -#include -#include -#include -#include - -namespace delphyne { -namespace gui { - -/// \brief Struct containing the phase ring id and a phase id that belongs to the phase ring. -/// An empty QString represents no item selection. -struct PhaseRingPhaseIds { - QString phase_ring_id; - QString phase_id; -}; - -/// \class RulesVisualizerWidget -/// \brief A class that implements a simple visualizer for loaded lanes -/// with their rules associated. -class RulesVisualizerWidget : public QWidget { - Q_OBJECT - - public: - /// \brief Default constructor. - explicit RulesVisualizerWidget(QWidget* parent = nullptr); - - /// \brief Default Destructor. - virtual ~RulesVisualizerWidget() = default; - - /// \brief Add lane id to ListWidget. - void AddLaneId(const QString& lane_id); - /// \brief Append text to TextBrowser adding a newline at the end. - void AddText(const QString& text); - /// \brief Clear the ListWidget. - void ClearLaneList(); - /// \brief Clear the text in TextBrowser. - void ClearText(); - /// \brief Construct the tree for the given phase_ring. - /// \param[in] phases Dictionary containing all the phases for all the possible phase rings of a road geometry. - /// \warning phases will be moved. - void ConstructPhaseRingTree(std::unordered_map>&& phases); - - /// \brief Get the select lane id from the list widget. - /// \returns QString containing the selected lane id - QString GetSelectedLaneId() const; - - /// \brief Get the selected phase ring id and phase id from the TreeWidget. - /// \returns PhaseRingPhaseIds containing selected phase ring id and phase id (if any) - PhaseRingPhaseIds GetSelectedPhaseRingAndPhaseId() const; - - signals: - - /// \brief Signal used to request rules for a given lane id - void RequestRules(); - /// \brief Signal connected internally to handle rules for a given lane id - void ReceiveRules(QString lane_id, QString rules); - - private slots: - - /// \brief Slot connected when the user clicks a lane item from the ListWidget. - /// Emits RequestRules signal - void OnLaneItemClicked(QListWidgetItem* item); - /// \brief Slot connected to ReceiveRules signal. Clears the text browser - /// and populates it with the rules for the requested lane id. - void OnRulesReceived(QString lane_id, QString rules); - - void OnPhaseTreeItemClicked(QTreeWidgetItem* tree_item, int column); - - private: - QLabel* lanes_label{nullptr}; - QLabel* rules_label{nullptr}; - QListWidget* lanes_list{nullptr}; - QTextBrowser* rules_log_text_browser{nullptr}; - QTreeWidget* phase_tree{nullptr}; -}; -} // namespace gui -} // namespace delphyne - -#endif // DELPHYNE_GUI_RULES_VISUALIZER_WIDGET_HH diff --git a/delphyne_gui/visualizer/teleop_widget.cc b/delphyne_gui/visualizer/teleop_widget.cc deleted file mode 100644 index d088ec63..00000000 --- a/delphyne_gui/visualizer/teleop_widget.cc +++ /dev/null @@ -1,253 +0,0 @@ -// Copyright 2017 Toyota Research Institute - -#include -#include -#include - -#include - -#include "teleop_widget.hh" - -#include -#include - -using namespace delphyne; -using namespace gui; - -Q_DECLARE_METATYPE(ignition::msgs::Boolean) - -///////////////////////////////////////////////// -TeleopWidget::TeleopWidget(QWidget*) - : Plugin(), currentThrottle(0.0), currentBrake(0.0), currentSteeringAngle(0.0), driving(false) { - qRegisterMetaType(); - - this->title = "TeleopWidget"; - - this->lineedit = new QLineEdit(); - this->lineedit->setText("teleop/0"); - - this->button = new QPushButton("Start Driving"); - - auto steeringAngleFixed = new QLabel("Steering Angle: "); - auto throttleValueFixed = new QLabel("Throttle Value: "); - auto brakeValueFixed = new QLabel("Brake Value: "); - - this->steeringAngleLabel = new QLabel("0.0"); - this->throttleValueLabel = new QLabel("0.0"); - this->brakeValueLabel = new QLabel("0.0"); - - auto layout = new QGridLayout; - layout->addWidget(this->lineedit, 0, 0); - layout->addWidget(this->button, 0, 1, 1, 2); - layout->addWidget(steeringAngleFixed, 1, 0); - layout->addWidget(this->steeringAngleLabel, 1, 1); - layout->addWidget(throttleValueFixed, 2, 0); - layout->addWidget(this->throttleValueLabel, 2, 1); - layout->addWidget(brakeValueFixed, 3, 0); - layout->addWidget(this->brakeValueLabel, 3, 1); - - this->setLayout(layout); - - QObject::connect(this->button, SIGNAL(clicked()), this, SLOT(StartDriving())); - - timer.start(10, this); -} - -void TeleopWidget::LoadConfig(const tinyxml2::XMLElement* _pluginElem) { - // Read configuration - if (_pluginElem) { - if (auto channelElem = _pluginElem->FirstChildElement("car_number")) { - std::string channelName = "teleop/" + std::string(channelElem->GetText()); - this->lineedit->setText(QString::fromStdString(channelName)); - } - } -} - -void TeleopWidget::StartDriving() { - if (this->driving) { - ignmsg << "Stop Driving" << std::endl; - this->button->setText("Start Driving"); - this->lineedit->setEnabled(true); - this->driving = false; - setFocus(); - } else { - ignmsg << "Start Driving" << std::endl; - auto lcmChannel = this->lineedit->text().toStdString(); - auto ignTopic = "/" + lcmChannel; - this->publisher_.reset(new ignition::transport::Node::Publisher()); - *(this->publisher_) = this->node_.Advertise(ignTopic); - this->button->setText("Stop Driving"); - this->lineedit->setEnabled(false); - this->driving = true; - setFocus(); - } -} - -///////////////////////////////////////////////// -TeleopWidget::~TeleopWidget() {} - -///////////////////////////////////////////////// -void TeleopWidget::mousePressEvent(QMouseEvent*) { setFocus(); } - -///////////////////////////////////////////////// -static void sec_and_nsec_now(int64_t& sec, int32_t& nsec) { - std::chrono::time_point now = std::chrono::system_clock::now(); - std::chrono::nanoseconds epoch = now.time_since_epoch(); - int64_t count = epoch.count(); - - nsec = static_cast(count % 1000000000l); - sec = (count - nsec) / 1000000000l; -} - -///////////////////////////////////////////////// -void TeleopWidget::timerEvent(QTimerEvent* event) { - if (event->timerId() == timer.timerId()) { - // do our stuff - if (this->driving) { - double last_throttle = this->currentThrottle; - double last_brake = this->currentBrake; - - if (!this->keepCurrentThrottle) { - if (this->throttleKeyPressed) { - computeClampAndSetThrottle(1.0); - } else { - computeClampAndSetThrottle(-6.0); - } - } - - if (!this->keepCurrentBrake) { - if (this->brakeKeyPressed) { - computeClampAndSetBrake(1.0); - } else { - computeClampAndSetBrake(-6.0); - } - } - - if (last_throttle != this->currentThrottle || last_brake != this->currentBrake || this->newSteeringAngle) { - ignition::msgs::AutomotiveDrivingCommand ignMsg; - - // We don't set the header here since the bridge completely ignores it - - int32_t nsec; - int64_t sec; - - sec_and_nsec_now(sec, nsec); - - ignMsg.mutable_time()->set_sec(sec); - ignMsg.mutable_time()->set_nsec(nsec); - - ignMsg.set_acceleration(this->currentThrottle - this->currentBrake); - ignMsg.set_theta(this->currentSteeringAngle); - - this->publisher_->Publish(ignMsg); - - this->steeringAngleLabel->setText(QString("%1").arg(this->currentSteeringAngle)); - this->throttleValueLabel->setText(QString("%1").arg(this->currentThrottle)); - this->brakeValueLabel->setText(QString("%1").arg(this->currentBrake)); - - this->newSteeringAngle = false; - } - } - } else { - QWidget::timerEvent(event); - } -} - -// Target velocity 60mph, i.e. ~26.8224 m/sec -static const double maxVelocity = 26.8224; -static const double throttleScale = maxVelocity / 300.0; - -static const double maxBrake = maxVelocity; -static const double brakeScale = throttleScale; - -// Maximum steering angle is 45 degrees ( -static const double Pi = 3.1415926535897931; -static const double DegToRad = Pi / 180.0; -static const double maxSteeringAngle = 45 * DegToRad; -static const double steeringButtonStepAngle = maxSteeringAngle / 100.0; - -///////////////////////////////////////////////// -void TeleopWidget::computeClampAndSetThrottle(double throttleGradient) { - double throttle = this->currentThrottle + throttleGradient * throttleScale; - if (throttle < 0.0) { - throttle = 0.0; - } else if (throttle > maxVelocity) { - throttle = maxVelocity; - } - this->currentThrottle = throttle; -} - -///////////////////////////////////////////////// -void TeleopWidget::computeClampAndSetBrake(double brakeGradient) { - double brake = this->currentBrake + brakeGradient * brakeScale; - if (brake < 0.0) { - brake = 0.0; - } else if (brake > maxBrake) { - brake = maxBrake; - } - this->currentBrake = brake; -} - -///////////////////////////////////////////////// -void TeleopWidget::computeClampAndSetSteeringAngle(double sign) { - double angle = this->currentSteeringAngle + steeringButtonStepAngle * sign; - if (angle > maxSteeringAngle) { - angle = maxSteeringAngle; - } else if (angle < -maxSteeringAngle) { - angle = -maxSteeringAngle; - } - this->currentSteeringAngle = angle; -} - -///////////////////////////////////////////////// -void TeleopWidget::keyPressEvent(QKeyEvent* _event) { - if (!driving) { - ignwarn << "Not driving, ignoring keypress" << std::endl; - Plugin::keyPressEvent(_event); - return; - } - - // The list of keys is here: http://doc.qt.io/qt-5/qt.html#Key-enum - if (_event->key() == Qt::Key_Left) { - computeClampAndSetSteeringAngle(1.0); - this->newSteeringAngle = true; - } else if (_event->key() == Qt::Key_Right) { - computeClampAndSetSteeringAngle(-1.0); - this->newSteeringAngle = true; - } else if (_event->key() == Qt::Key_Up) { - this->throttleKeyPressed = true; - this->keepCurrentThrottle = false; - } else if (_event->key() == Qt::Key_Down) { - this->brakeKeyPressed = true; - this->keepCurrentBrake = false; - } else { - // The Qt documentation at http://doc.qt.io/qt-5/qwidget.html#keyPressEvent - // says that you must call the base class if you don't handle the key, so - // do that here. - Plugin::keyPressEvent(_event); - return; - } -} - -///////////////////////////////////////////////// -void TeleopWidget::keyReleaseEvent(QKeyEvent* _event) { - if (!_event->isAutoRepeat()) { - if (_event->key() == Qt::Key_Space) { - this->keepCurrentThrottle = true; - this->keepCurrentBrake = true; - } else if (_event->key() == Qt::Key_Up) { - this->throttleKeyPressed = false; - } else if (_event->key() == Qt::Key_Down) { - this->brakeKeyPressed = false; - } else if (_event->key() == Qt::Key_Left || _event->key() == Qt::Key_Right) { - // do nothing - // this avoids the accel/brake values of not going down - // to zero after release when steering at the same time - } else { - Plugin::keyReleaseEvent(_event); - } - } - return; -} - -IGN_COMMON_REGISTER_SINGLE_PLUGIN(delphyne::gui::TeleopWidget, ignition::gui::Plugin) diff --git a/delphyne_gui/visualizer/teleop_widget.hh b/delphyne_gui/visualizer/teleop_widget.hh deleted file mode 100644 index 264645b7..00000000 --- a/delphyne_gui/visualizer/teleop_widget.hh +++ /dev/null @@ -1,117 +0,0 @@ -// Copyright 2017 Toyota Research Institute - -#ifndef DELPHYNE_GUI_TELEOPWIDGET_HH -#define DELPHYNE_GUI_TELEOPWIDGET_HH - -#include - -#include -#include - -namespace delphyne { -namespace gui { - -/// \class TeleopWidget -/// \brief This is a class that implements a simple ign-gui widget for -/// teleop-ing. -class TeleopWidget : public ignition::gui::Plugin { - Q_OBJECT - - public: - /// \brief Default constructor. - explicit TeleopWidget(QWidget* parent = 0); - - /// \brief Default Destructor. - virtual ~TeleopWidget(); - - /// \brief Callback to start the driving setup process - public slots: - void StartDriving(); - - /// \brief Notify that the response to the asynchronous service call - /// to the bridge returned. - signals: - void RepeatingDriveTopic(const ignition::msgs::Boolean& response, const bool result); - - /// \brief Callback to finish start driving - public slots: - void DriveTopicComplete(const ignition::msgs::Boolean& response, const bool result); - - protected: - virtual void keyPressEvent(QKeyEvent* _event) override; - virtual void keyReleaseEvent(QKeyEvent* _event) override; - void mousePressEvent(QMouseEvent* _event) override; - void timerEvent(QTimerEvent* event) override; - void LoadConfig(const tinyxml2::XMLElement* _pluginElem) override; - - private: - /// \internal - /// \brief A transport node. - ignition::transport::Node node_; - - /// \internal - /// \brief The ignition publisher used to send the updates - std::unique_ptr publisher_; - - /// \internal - /// \brief The current amount of throttle - double currentThrottle; - - /// \internal - /// \brief The current amount of brake - double currentBrake; - - /// \internal - /// \brief True if any valid key is pressed - bool throttleKeyPressed = false; - - /// \internal - /// \brief Whether the brake key is currently pressed - bool brakeKeyPressed = false; - - /// \internal - /// \brief True if keeping current throttle value - /// without reseting to zero is enabled. - bool keepCurrentThrottle = false; - - /// \internal - /// \brief True if keeping current brake value - /// without reseting to zero is enabled. - bool keepCurrentBrake = false; - - /// \internal - /// \brief Whether a new steering angle has been - /// computed since the last time the timer ran. - bool newSteeringAngle = false; - - /// \internal - /// \brief The current steering angle - double currentSteeringAngle; - - /// \internal - /// \brief The current state of the widget - bool driving; - - QLineEdit* lineedit; - QPushButton* button; - QLabel* steeringAngleLabel; - QLabel* throttleValueLabel; - QLabel* brakeValueLabel; - QBasicTimer timer; - - /// \internal - /// \brief Compute and set a valid throttle value - void computeClampAndSetThrottle(double throttleGradient); - - /// \internal - /// \brief Compute and set a valid brake value - void computeClampAndSetBrake(double brakeGradient); - - /// \internal - /// \brief Compute and set a valid steering angle value - void computeClampAndSetSteeringAngle(double sign); -}; -} // namespace gui -} // namespace delphyne - -#endif diff --git a/delphyne_gui/visualizer/visualizer0.cc b/delphyne_gui/visualizer/visualizer0.cc deleted file mode 100644 index e57fbd51..00000000 --- a/delphyne_gui/visualizer/visualizer0.cc +++ /dev/null @@ -1,169 +0,0 @@ -// Copyright 2017 Toyota Research Institute - -#include -#include -#include - -#include - -#include -#include -#include - -#ifndef Q_MOC_RUN -#include -#include -#include -#include -#endif - -#include "delphyne_gui/config.hh" - -#include "global_attributes.hh" - -namespace delphyne_gui { -namespace visualizer { -namespace { - -/// Constants. -constexpr char kVersionStr[] = "Visualizer 0.1.0"; -constexpr char kDefaultLayout[] = "layout_with_teleop.config"; - -///////////////////////////////////////////////// -/// \brief Get the path of the default configuration file for Delphyne. -/// \return The default configuration path. -std::string defaultConfigPath() { - std::string homePath; - ignition::common::env("HOME", homePath); - return ignition::common::joinPaths(homePath, ".delphyne", "delphyne.config"); -} - -///////////////////////////////////////////////// -int Main(int argc, const char* argv[]) { - static const std::string initialConfigFile = - ignition::common::joinPaths(DELPHYNE_INITIAL_CONFIG_PATH, kDefaultLayout); - - ignition::common::Console::SetVerbosity(3); - ignmsg << kVersionStr << std::endl; - - if (argc > 1) { - delphyne::gui::GlobalAttributes::ParseArguments(argc - 1, &(argv[1])); - } - - // If we run the visualizer as a child process (like a demo written in - // python), we need to ensure that it's not using a block buffer - // to display everything that goes to the stdout in realtime. - if (delphyne::gui::GlobalAttributes::HasArgument("use-line-buffer")) { - const std::string use_line_buffer_arg = delphyne::gui::GlobalAttributes::GetArgument("use-line-buffer"); - if (use_line_buffer_arg == "yes") { - setlinebuf(stdout); - } - } - - // Parse custom config file from args. - delphyne::utility::PackageManager* package_manager = delphyne::utility::PackageManager::Instance(); - if (delphyne::gui::GlobalAttributes::HasArgument("package")) { - package_manager->Use( - std::make_unique(delphyne::gui::GlobalAttributes::GetArgument("package"))); - } else { - package_manager->Use(std::make_unique()); - } - - // Initialize app - ignition::gui::initApp(); - - // Set the default location for saving user settings. - ignition::gui::setDefaultConfigPath(defaultConfigPath()); - - // Look for all plugins in the same place - ignition::gui::setPluginPathEnv("VISUALIZER_PLUGIN_PATH"); - - // Then look for plugins on compile-time defined path. - // Plugins installed by gazebo end up here - ignition::gui::addPluginPath(PLUGIN_INSTALL_PATH); - - // Attempt to load window layout from parsed arguments. - bool layout_loaded = delphyne::gui::GlobalAttributes::HasArgument("layout") && - ignition::gui::loadConfig(delphyne::gui::GlobalAttributes::GetArgument("layout")); - // If no layout was found, attempt to use the default config file. - layout_loaded = layout_loaded || ignition::gui::loadDefaultConfig(); - // If that's not available either, load it from initial config file. - layout_loaded = layout_loaded || ignition::gui::loadConfig(initialConfigFile); - // If no layout has been loaded so far, exit the application. - if (!layout_loaded) { - ignerr << "Unable to load a configuration file, exiting." << std::endl; - return 1; - } - - // Plugin injection pattern. Use `receiver-injected` for - // horizontal splits and `receiver/injected` for vertical - // splits. - std::regex injectionPattern{"(\\w+)\\s*(/|-)\\s*(\\w+)"}; - // Holds a tuple. - std::vector> pluginInjectionList; - if (delphyne::gui::GlobalAttributes::HasArgument("inject-plugin")) { - const std::string injectPluginArg = delphyne::gui::GlobalAttributes::GetArgument("inject-plugin"); - std::smatch match; - if (std::regex_search(injectPluginArg, match, injectionPattern)) { - const std::string& receiverPluginName = match[1]; - const Qt::Orientation splitOrientation = match[2] == "/" ? Qt::Vertical : Qt::Horizontal; - const std::string& injectedPluginName = match[3]; - pluginInjectionList.push_back( - std::make_tuple(receiverPluginName, injectedPluginName, injectedPluginName, splitOrientation)); - } else { - ignerr << "Ill formed --inject-plugin=" << injectPluginArg << " argument." - << " Missing '@'." << std::endl; - } - } - if (delphyne::gui::GlobalAttributes::HasArgument("plugin-name")) { - if (!delphyne::gui::GlobalAttributes::HasArgument("inject-plugin")) { - ignerr << "'--plugin-name' Option only available with plugin injection: '--inject-plugin' option" << std::endl; - } - std::get<2>(pluginInjectionList[0]) = delphyne::gui::GlobalAttributes::GetArgument("plugin-name"); - } - - // Create main window - ignition::gui::createMainWindow(); - - auto win = ignition::gui::mainWindow(); - win->setWindowTitle(kVersionStr); - - for (auto pluginInjectionEntry : pluginInjectionList) { - const std::string& receiverPluginName = std::get<0>(pluginInjectionEntry); - auto receiverWidget = win->findChild(QString(receiverPluginName.c_str())); - if (!receiverWidget) { - ignerr << "Unknown " << receiverPluginName << " plugin for injection. Skipping." << std::endl; - continue; - } - const std::string& injectedPluginName = std::get<1>(pluginInjectionEntry); - auto injectedWidget = win->findChild(QString(injectedPluginName.c_str())); - if (injectedWidget != nullptr) { - ignmsg << "Injected " << injectedPluginName << " plugin" - << " already found. Skipping." << std::endl; - continue; - } - if (!ignition::gui::loadPlugin(injectedPluginName)) { - continue; - } - ignition::gui::addPluginsToWindow(); - injectedWidget = win->findChild(QString(std::get<2>(pluginInjectionEntry).c_str())); - if (!injectedWidget) { - ignerr << "Plugin named " << std::get<2>(pluginInjectionEntry).c_str() << " couldn't be found." << std::endl; - continue; - } - const Qt::Orientation& splitOrientation = std::get<3>(pluginInjectionEntry); - win->splitDockWidget(receiverWidget, injectedWidget, splitOrientation); - } - - ignition::gui::runMainWindow(); - - ignition::gui::stop(); - - return 0; -} - -} // namespace -} // namespace visualizer -} // namespace delphyne_gui - -int main(int argc, const char* argv[]) { return delphyne_gui::visualizer::Main(argc, argv); }