diff --git a/examples/worlds/visualize_frustum.sdf b/examples/worlds/visualize_frustum.sdf new file mode 100644 index 0000000000..57b776cf51 --- /dev/null +++ b/examples/worlds/visualize_frustum.sdf @@ -0,0 +1,444 @@ + + + + + + 0.001 + 1.0 + + + + + ogre2 + + + + + ogre2 + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + + + docked + + + + + + + docked + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 20 20 0.1 + + + + + + + 20 20 0.1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 -1 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + -4 0 0.325 0 0 0.0 + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + left_wheel_joint + right_wheel_joint + 1.25 + 0.3 + 1 + + + + + 0 0 0 0 0 1.57 + https://fuel.gazebosim.org/1.0/openrobotics/models/Playground + + + + true + + -6 0 6 0 0.5 0 + + 0.05 0.05 0.05 0 0 0 + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + " + 10 + + 0.55 + 5 + 1.04719755 + 1.778 + + 1 + true + logical_camera + true + + + + + diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 837f929c7a..593fb08202 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -159,3 +159,4 @@ add_subdirectory(view_angle) add_subdirectory(visualization_capabilities) add_subdirectory(visualize_contacts) add_subdirectory(visualize_lidar) +add_subdirectory(visualize_frustum) diff --git a/src/gui/plugins/visualize_frustum/CMakeLists.txt b/src/gui/plugins/visualize_frustum/CMakeLists.txt new file mode 100644 index 0000000000..d2cf3a4b9a --- /dev/null +++ b/src/gui/plugins/visualize_frustum/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_gui_plugin(VisualizeFrustum + SOURCES VisualizeFrustum.cc + QT_HEADERS VisualizeFrustum.hh + PRIVATE_LINK_LIBS + gz-rendering${GZ_RENDERING_VER}::core +) diff --git a/src/gui/plugins/visualize_frustum/VisualizeFrustum.cc b/src/gui/plugins/visualize_frustum/VisualizeFrustum.cc new file mode 100644 index 0000000000..20eeef19f5 --- /dev/null +++ b/src/gui/plugins/visualize_frustum/VisualizeFrustum.cc @@ -0,0 +1,440 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "VisualizeFrustum.hh" + +#include +#include +#include + +#include +#include + +#include +#include + +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/World.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Entity.hh" +#include "gz/sim/rendering/RenderUtil.hh" + +#include "gz/rendering/RenderTypes.hh" +#include "gz/rendering/RenderingIface.hh" +#include "gz/rendering/RenderEngine.hh" +#include "gz/rendering/Scene.hh" +#include "gz/rendering/FrustumVisual.hh" + +#include "gz/sim/components/Link.hh" +#include "gz/sim/components/Sensor.hh" +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/Util.hh" + +namespace gz +{ +namespace sim +{ +inline namespace GZ_SIM_VERSION_NAMESPACE +{ + /// \brief Private data class for VisualizeFrustum + class VisualizeFrustumPrivate + { + /// \brief Transport node + public: transport::Node node; + + /// \brief Scene Pointer + public: rendering::ScenePtr scene; + + /// \brief Pointer to FrustumVisual + public: rendering::FrustumVisualPtr frustum; + + /// \brief URI sequence to the frustum link + public: std::string frustumString{""}; + + /// \brief LaserScan message from sensor + public: msgs::LogicalCameraSensor msg; + + /// \brief Pose of the frustum visual + public: math::Pose3d frustumPose{math::Pose3d::Zero}; + + /// \brief Topic name to subscribe + public: std::string topicName{""}; + + /// \brief List of topics publishing LaserScan messages. + public: QStringList topicList; + + /// \brief Entity representing the sensor in the world + public: sim::Entity frustumEntity; + + /// \brief Mutex for variable mutated by the checkbox and spinboxes + /// callbacks. + /// The variables are: msg + public: std::mutex serviceMutex; + + /// \brief Initialization flag + public: bool initialized{false}; + + /// \brief Reset visual flag + public: bool resetVisual{false}; + + /// \brief frustum visual display dirty flag + public: bool visualDirty{false}; + + /// \brief frustum sensor entity dirty flag + public: bool frustumEntityDirty{true}; + }; +} +} +} + +using namespace gz; +using namespace sim; + +///////////////////////////////////////////////// +VisualizeFrustum::VisualizeFrustum() + : GuiSystem(), dataPtr(new VisualizeFrustumPrivate) +{ + // no ops +} + +///////////////////////////////////////////////// +VisualizeFrustum::~VisualizeFrustum() +{ + std::lock_guard lock(this->dataPtr->serviceMutex); + this->dataPtr->scene->DestroyVisual(this->dataPtr->frustum); +} + +///////////////////////////////////////////////// +void VisualizeFrustum::LoadFrustum() +{ + auto loadedEngNames = rendering::loadedEngines(); + if (loadedEngNames.empty()) + return; + + // assume there is only one engine loaded + auto engineName = loadedEngNames[0]; + if (loadedEngNames.size() > 1) + { + gzdbg << "More than one engine is available. " + << "VisualizeFrustum plugin will use engine [" + << engineName << "]" << std::endl; + } + auto engine = rendering::engine(engineName); + if (!engine) + { + gzerr << "Internal error: failed to load engine [" << engineName + << "]. VisualizeFrustum plugin won't work." << std::endl; + return; + } + + if (engine->SceneCount() == 0) + return; + + // assume there is only one scene + // load scene + auto scene = engine->SceneByIndex(0); + if (!scene) + { + gzerr << "Internal error: scene is null." << std::endl; + return; + } + + if (!scene->IsInitialized() || scene->VisualCount() == 0) + { + return; + } + + // Create frustum visual + gzdbg << "Creating frustum visual" << std::endl; + auto root = scene->RootVisual(); + + this->dataPtr->frustum = scene->CreateFrustumVisual(); + if (!this->dataPtr->frustum) + { + gzwarn << "Failed to create frustum, visualize frustum plugin won't work." + << std::endl; + + scene->DestroyVisual(this->dataPtr->frustum); + + gz::gui::App()->findChild< + gz::gui::MainWindow *>()->removeEventFilter(this); + } + else + { + this->dataPtr->scene = scene; + root->AddChild(this->dataPtr->frustum); + this->dataPtr->initialized = true; + } +} + +///////////////////////////////////////////////// +void VisualizeFrustum::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Visualize frustum"; + + gz::gui::App()->findChild< + gz::gui::MainWindow *>()->installEventFilter(this); +} + +///////////////////////////////////////////////// +bool VisualizeFrustum::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == gz::gui::events::Render::kType) + { + // This event is called in the RenderThread, so it's safe to make + // rendering calls here + + std::lock_guard lock(this->dataPtr->serviceMutex); + if (!this->dataPtr->initialized) + { + this->LoadFrustum(); + } + + if (this->dataPtr->frustum) + { + if (this->dataPtr->resetVisual) + { + this->dataPtr->resetVisual = false; + } + if (this->dataPtr->visualDirty) + { + this->dataPtr->frustum->SetWorldPose(this->dataPtr->frustumPose); + this->dataPtr->frustum->Update(); + this->dataPtr->visualDirty = false; + } + } + else + { + gzerr << "Frustum pointer is not set" << std::endl; + } + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +////////////////////////////////////////////////// +void VisualizeFrustum::Update(const UpdateInfo &, + EntityComponentManager &_ecm) +{ + GZ_PROFILE("VisualizeFrusum::Update"); + + std::lock_guard lock(this->dataPtr->serviceMutex); + + if (this->dataPtr->frustumEntityDirty) + { + auto frustumURIVec = common::split(common::trimmed( + this->dataPtr->frustumString), "::"); + if (frustumURIVec.size() > 0) + { + auto baseEntity = _ecm.EntityByComponents( + components::Name(frustumURIVec[0])); + if (!baseEntity) + { + gzerr << "Error entity " << frustumURIVec[0] + << " doesn't exist and cannot be used to set frustum visual pose" + << std::endl; + return; + } + else + { + auto parent = baseEntity; + bool success = false; + for (size_t i = 0u; i < frustumURIVec.size()-1; i++) + { + auto children = _ecm.EntitiesByComponents( + components::ParentEntity(parent)); + bool foundChild = false; + for (auto child : children) + { + std::string nextstring = frustumURIVec[i+1]; + auto comp = _ecm.Component(child); + if (!comp) + { + continue; + } + std::string childname = std::string( + comp->Data()); + if (nextstring.compare(childname) == 0) + { + parent = child; + foundChild = true; + if (i+1 == frustumURIVec.size()-1) + { + success = true; + } + break; + } + } + if (!foundChild) + { + gzerr << "The entity could not be found." + << "Error displaying frustum visual" <dataPtr->frustumEntity = parent; + this->dataPtr->frustumEntityDirty = false; + } + } + } + } + + // Only update frustumPose if the frustumEntity exists and the frustum is + // initialized and the sensor message is yet to arrive. + // + // If we update the worldpose on the physics thread **after** the sensor + // data arrives, the visual is offset from the obstacle if the sensor is + // moving fast. + if (!this->dataPtr->frustumEntityDirty && this->dataPtr->initialized && + !this->dataPtr->visualDirty) + { + this->dataPtr->frustumPose = worldPose(this->dataPtr->frustumEntity, _ecm); + } +} + +////////////////////////////////////////////////// +void VisualizeFrustum::OnTopic(const QString &_topicName) +{ + if (!this->dataPtr->topicName.empty() && + !this->dataPtr->node.Unsubscribe(this->dataPtr->topicName)) + { + gzerr << "Unable to unsubscribe from topic [" + << this->dataPtr->topicName <<"]" <dataPtr->topicName = _topicName.toStdString(); + std::lock_guard lock(this->dataPtr->serviceMutex); + + // Reset visualization + this->dataPtr->resetVisual = true; + + // Create new subscription + if (!this->dataPtr->node.Subscribe(this->dataPtr->topicName, + &VisualizeFrustum::OnScan, this)) + { + gzerr << "Unable to subscribe to topic [" + << this->dataPtr->topicName << "]\n"; + return; + } + this->dataPtr->visualDirty = false; +} + +////////////////////////////////////////////////// +void VisualizeFrustum::DisplayVisual(bool _value) +{ + std::lock_guard lock(this->dataPtr->serviceMutex); + this->dataPtr->frustum->SetVisible(_value); + gzerr << "Frustum Visual Display " << ((_value) ? "ON." : "OFF.") << std::endl; +} + +///////////////////////////////////////////////// +void VisualizeFrustum::OnRefresh() +{ + gzmsg << "Refreshing topic list for Logical Camera Sensor messages." << std::endl; + + // Clear + this->dataPtr->topicList.clear(); + + // Get updated list + std::vector allTopics; + this->dataPtr->node.TopicList(allTopics); + for (auto topic : allTopics) + { + std::vector publishers; + this->dataPtr->node.TopicInfo(topic, publishers); + for (auto pub : publishers) + { + if (pub.MsgTypeName() == "gz.msgs.LogicalCameraSensor") + { + this->dataPtr->topicList.push_back(QString::fromStdString(topic)); + break; + } + } + } + if (this->dataPtr->topicList.size() > 0) + { + this->OnTopic(this->dataPtr->topicList.at(0)); + } + + this->TopicListChanged(); +} + +///////////////////////////////////////////////// +QStringList VisualizeFrustum::TopicList() const +{ + return this->dataPtr->topicList; +} + +///////////////////////////////////////////////// +void VisualizeFrustum::SetTopicList(const QStringList &_topicList) +{ + this->dataPtr->topicList = _topicList; + this->TopicListChanged(); +} + +////////////////////////////////////////////////// +void VisualizeFrustum::OnScan(const msgs::LogicalCameraSensor &_msg) +{ + std::lock_guard lock(this->dataPtr->serviceMutex); + if (this->dataPtr->initialized) + { + this->dataPtr->msg = std::move(_msg); + + this->dataPtr->frustum->SetNear(this->dataPtr->msg.near_clip()); + this->dataPtr->frustum->SetFar(this->dataPtr->msg.far_clip()); + this->dataPtr->frustum->SetFOV(this->dataPtr->msg.horizontal_fov()); + this->dataPtr->frustum->SetAspectRatio(this->dataPtr->msg.aspect_ratio()); + + this->dataPtr->visualDirty = true; + + for (auto data_values : this->dataPtr->msg.header().data()) + { + if (data_values.key() == "frame_id") + { + if (this->dataPtr->frustumString.compare( + common::trimmed(data_values.value(0))) != 0) + { + this->dataPtr->frustumString = common::trimmed(data_values.value(0)); + this->dataPtr->frustumEntityDirty = true; + break; + } + } + } + } +} + +// Register this plugin +GZ_ADD_PLUGIN(gz::sim::VisualizeFrustum, + gz::gui::Plugin) diff --git a/src/gui/plugins/visualize_frustum/VisualizeFrustum.hh b/src/gui/plugins/visualize_frustum/VisualizeFrustum.hh new file mode 100644 index 0000000000..3205c3136c --- /dev/null +++ b/src/gui/plugins/visualize_frustum/VisualizeFrustum.hh @@ -0,0 +1,105 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef GZ_SIM_GUI_VISUALIZEFRUSTUM_HH_ +#define GZ_SIM_GUI_VISUALIZEFRUSTUM_HH_ + +#include + +#include "gz/msgs/logical_camera_sensor.pb.h" +#include "gz/sim/gui/GuiSystem.hh" +#include "gz/gui/qt.h" + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE +{ + class VisualizeFrustumPrivate; + + /// \brief Visualize the LaserScan message returned by the sensors. Use the + /// checkbox to turn visualization of non-hitting rays on or off and + /// the textfield to select the message to be visualised. The combobox is + /// used to select the type of visual for the sensor data. + class VisualizeFrustum : public gz::sim::GuiSystem + { + Q_OBJECT + + /// \brief Topic list + Q_PROPERTY( + QStringList topicList + READ TopicList + WRITE SetTopicList + NOTIFY TopicListChanged + ) + + /// \brief Constructor + public: VisualizeFrustum(); + + /// \brief Destructor + public: ~VisualizeFrustum() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation Inherited + public: bool eventFilter(QObject *_obj, QEvent *_event) override; + + // Documentation inherited + public: void Update(const UpdateInfo &, + EntityComponentManager &_ecm) override; + + /// \brief Callback function to get data from the message + /// \param[in]_msg FrustumSensor message + public: void OnScan(const msgs::LogicalCameraSensor &_msg); + + /// \brief Load the scene and attach FrustumVisual to the scene + public: void LoadFrustum(); + + /// \brief Get the topic list as a string + /// \return Message type + public: Q_INVOKABLE QStringList TopicList() const; + + /// \brief Set the topic list from a string, for example + /// 'gz.msgs.StringMsg' + /// \param[in] _topicList Message type + public: Q_INVOKABLE void SetTopicList(const QStringList &_topicList); + + /// \brief Notify that topic list has changed + signals: void TopicListChanged(); + + /// \brief Set topic to subscribe for FrustumSensor data + /// \param[in] _topicName Name of selected topic + public: Q_INVOKABLE void OnTopic(const QString &_topicName); + + /// \brief Set whether to display the frustum visual + /// \param[in] _value Boolean value for displaying the visual + public: Q_INVOKABLE void DisplayVisual(bool _value); + + /// \brief Callback when refresh button is pressed. + public: Q_INVOKABLE void OnRefresh(); + + /// \internal + /// \brief Pointer to private data + private: std::unique_ptr dataPtr; + }; +} +} +} +#endif diff --git a/src/gui/plugins/visualize_frustum/VisualizeFrustum.qml b/src/gui/plugins/visualize_frustum/VisualizeFrustum.qml new file mode 100644 index 0000000000..4c5be71a0e --- /dev/null +++ b/src/gui/plugins/visualize_frustum/VisualizeFrustum.qml @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 2.1 +import QtQuick.Dialogs 1.0 +import QtQuick.Controls.Material 2.1 +import QtQuick.Layouts 1.3 +import "qrc:/qml" + +GridLayout { + columns: 6 + columnSpacing: 10 + Layout.minimumWidth: 350 + Layout.minimumHeight: 400 + anchors.fill: parent + anchors.leftMargin: 10 + anchors.rightMargin: 10 + + property int tooltipDelay: 500 + property int tooltipTimeout: 1000 + + CheckBox { + Layout.alignment: Qt.AlignHCenter + id: displayVisual + Layout.columnSpan: 6 + Layout.fillWidth: true + text: qsTr("Display Frustum Visual") + checked: true + onClicked: { + VisualizeFrustum.DisplayVisual(checked) + } + } + + RoundButton { + Layout.columnSpan: 1 + text: "\u21bb" + Material.background: Material.primary + onClicked: { + combo.currentIndex = 0 + VisualizeFrustum.OnRefresh(); + } + ToolTip.visible: hovered + ToolTip.delay: tooltipDelay + ToolTip.timeout: tooltipTimeout + ToolTip.text: qsTr("Refresh list of topics publishing Logical Camera Sensor messages") + } + + ComboBox { + Layout.columnSpan: 5 + id: combo + Layout.fillWidth: true + model: VisualizeFrustum.topicList + currentIndex: 0 + onCurrentIndexChanged: { + if (currentIndex < 0) + return; + VisualizeFrustum.OnTopic(textAt(currentIndex)); + } + ToolTip.visible: hovered + ToolTip.delay: tooltipDelay + ToolTip.timeout: tooltipTimeout + ToolTip.text: qsTr("Gazebo Transport topics publishing Logical Camera Sensor messages") + } +} diff --git a/src/gui/plugins/visualize_frustum/VisualizeFrustum.qrc b/src/gui/plugins/visualize_frustum/VisualizeFrustum.qrc new file mode 100644 index 0000000000..a4001232f1 --- /dev/null +++ b/src/gui/plugins/visualize_frustum/VisualizeFrustum.qrc @@ -0,0 +1,5 @@ + + + VisualizeFrustum.qml + +