From 2a076e91c6df7699284ab93e244894bb0f55336f Mon Sep 17 00:00:00 2001 From: Louise Poubel <louise@openrobotics.org> Date: Wed, 22 Dec 2021 10:46:14 -0800 Subject: [PATCH 01/17] =?UTF-8?q?=F0=9F=8E=88=204.14.0=20(#1261)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel <louise@openrobotics.org> --- CMakeLists.txt | 2 +- Changelog.md | 21 ++++++++++++++++++--- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9c10002405..5546ef954b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-gazebo4 VERSION 4.13.0) +project(ignition-gazebo4 VERSION 4.14.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index 881e09fb96..7804bff869 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,20 @@ ## Ignition Gazebo 4.x +### Ignition Gazebo 4.14.0 (2021-12-20) + +1. Support battery draining start via topics + * [Pull request #1255](https://github.com/ignitionrobotics/ign-gazebo/pull/1255) + +1. Make tests run as fast as possible + * [Pull request #1194](https://github.com/ignitionrobotics/ign-gazebo/pull/1194) + * [Pull request #1250](https://github.com/ignitionrobotics/ign-gazebo/pull/1250) + +1. Fix visualize lidar + * [Pull request #1224](https://github.com/ignitionrobotics/ign-gazebo/pull/1224) + +1. Disable user commands light test on macOS + * [Pull request #1204](https://github.com/ignitionrobotics/ign-gazebo/pull/1204) + ### Ignition Gazebo 4.13.0 (2021-11-15) 1. Prevent creation of spurious `<plugin>` elements when saving worlds @@ -202,7 +217,7 @@ 1. Support configuring particle scatter ratio in particle emitter system. * [Pull Request 674](https://github.com/ignitionrobotics/ign-gazebo/pull/674) -1. Fix diffuse and ambient values for ackermann example. +1. Fix diffuse and ambient values for ackermann example. * [Pull Request 707](https://github.com/ignitionrobotics/ign-gazebo/pull/707) 1. Scenebroadcaster sensors. @@ -223,7 +238,7 @@ 1. Ackermann Steering Plugin. * [Pull Request 618](https://github.com/ignitionrobotics/ign-gazebo/pull/618) -1. Remove bounding box when model is deleted +1. Remove bounding box when model is deleted * [Pull Request 675](https://github.com/ignitionrobotics/ign-gazebo/pull/675) 1. Cache link poses to improve performance. @@ -232,7 +247,7 @@ 1. Check empty world name in Scene3d. * [Pull Request 662](https://github.com/ignitionrobotics/ign-gazebo/pull/662) -1. All changes up to 3.8.0. +1. All changes up to 3.8.0. ### Ignition Gazebo 4.6.0 (2021-03-01) From 6f58c919a95e8ab03864d25af022599e64bc090f Mon Sep 17 00:00:00 2001 From: Louise Poubel <louise@openrobotics.org> Date: Tue, 18 Jan 2022 08:57:19 -0800 Subject: [PATCH 02/17] =?UTF-8?q?Prevent=20GzScene3D=20=F0=9F=92=A5=20if?= =?UTF-8?q?=20another=20scene=20is=20already=20loaded=20(#1294)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel <louise@openrobotics.org> --- src/gui/plugins/scene3d/GzScene3D.qml | 19 ++++-- src/gui/plugins/scene3d/Scene3D.cc | 87 +++++++++++++++++---------- src/gui/plugins/scene3d/Scene3D.hh | 44 +++++++++++++- 3 files changed, 112 insertions(+), 38 deletions(-) diff --git a/src/gui/plugins/scene3d/GzScene3D.qml b/src/gui/plugins/scene3d/GzScene3D.qml index 4411fb5d7e..6dea55f64e 100644 --- a/src/gui/plugins/scene3d/GzScene3D.qml +++ b/src/gui/plugins/scene3d/GzScene3D.qml @@ -14,16 +14,18 @@ * limitations under the License. * */ +import IgnGazebo 1.0 as IgnGazebo +import QtGraphicalEffects 1.0 import QtQuick 2.9 import QtQuick.Controls 2.2 import QtQuick.Dialogs 1.0 +import QtQuick.Layouts 1.3 import RenderWindow 1.0 -import QtGraphicalEffects 1.0 -import IgnGazebo 1.0 as IgnGazebo Rectangle { - width: 1000 - height: 800 + Layout.minimumWidth: 200 + Layout.minimumHeight: 200 + anchors.fill: parent /** * True to enable gamma correction @@ -38,6 +40,7 @@ Rectangle { anchors.fill: parent hoverEnabled: true acceptedButtons: Qt.NoButton + visible: GzScene3D.loadingError.length == 0 onEntered: { GzScene3D.OnFocusWindow() } @@ -50,6 +53,7 @@ Rectangle { id: renderWindow objectName: "renderWindow" anchors.fill: parent + visible: GzScene3D.loadingError.length == 0 /** * Message to be displayed over the render window @@ -120,4 +124,11 @@ Rectangle { standardButtons: Dialog.Ok } + Label { + anchors.fill: parent + anchors.margins: 10 + text: GzScene3D.loadingError + visible: (GzScene3D.loadingError.length > 0); + wrapMode: Text.WordWrap + } } diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 09ae8c5919..429294f7d4 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -1643,17 +1643,25 @@ void IgnRenderer::HandleMouseViewControl() } ///////////////////////////////////////////////// -void IgnRenderer::Initialize() +std::string IgnRenderer::Initialize() { if (this->initialized) - return; + return std::string(); + + // Only one engine / scene / user camera is currently supported. + // Fail gracefully even before getting to renderUtil. + if (!rendering::loadedEngines().empty()) + { + return "Currently only one plugin providing a 3D scene is supported at a " + "time."; + } this->dataPtr->renderUtil.SetUseCurrentGLContext(true); this->dataPtr->renderUtil.Init(); rendering::ScenePtr scene = this->dataPtr->renderUtil.Scene(); if (!scene) - return; + return "Failed to create a 3D scene."; auto root = scene->RootVisual(); @@ -1674,6 +1682,7 @@ void IgnRenderer::Initialize() this->dataPtr->rayQuery = this->dataPtr->camera->Scene()->CreateRayQuery(); this->initialized = true; + return std::string(); } ///////////////////////////////////////////////// @@ -2066,6 +2075,12 @@ RenderThread::RenderThread() qRegisterMetaType<std::string>(); } +///////////////////////////////////////////////// +void RenderThread::SetErrorCb(std::function<void(const QString&)> _cb) +{ + this->errorCb = _cb; +} + ///////////////////////////////////////////////// void RenderThread::RenderNext() { @@ -2074,7 +2089,12 @@ void RenderThread::RenderNext() if (!this->ignRenderer.initialized) { // Initialize renderer - this->ignRenderer.Initialize(); + auto loadingError = this->ignRenderer.Initialize(); + if (!loadingError.empty()) + { + this->errorCb(QString::fromStdString(loadingError)); + return; + } } // check if engine has been successfully initialized @@ -2092,18 +2112,24 @@ void RenderThread::RenderNext() ///////////////////////////////////////////////// void RenderThread::ShutDown() { - this->context->makeCurrent(this->surface); + if (this->context && this->surface) + this->context->makeCurrent(this->surface); this->ignRenderer.Destroy(); - this->context->doneCurrent(); - delete this->context; + if (this->context) + { + this->context->doneCurrent(); + delete this->context; + } // schedule this to be deleted only after we're done cleaning up - this->surface->deleteLater(); + if (this->surface) + this->surface->deleteLater(); // Stop event processing, move the thread to GUI and make sure it is deleted. - this->moveToThread(QGuiApplication::instance()->thread()); + if (this->ignRenderer.initialized) + this->moveToThread(QGuiApplication::instance()->thread()); } @@ -2203,16 +2229,6 @@ void TextureNode::PrepareNode() RenderWindowItem::RenderWindowItem(QQuickItem *_parent) : QQuickItem(_parent), dataPtr(new RenderWindowItemPrivate) { - // FIXME(anyone) Ogre 1/2 singletons crash when there's an attempt to load - // this plugin twice, so shortcut here. Ideally this would be caught at - // Ignition Rendering. - static bool done{false}; - if (done) - { - return; - } - done = true; - this->setAcceptedMouseButtons(Qt::AllButtons); this->setFlag(ItemHasContents); this->dataPtr->renderThread = new RenderThread(); @@ -2378,18 +2394,6 @@ Scene3D::~Scene3D() = default; ///////////////////////////////////////////////// void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) { - // FIXME(anyone) Ogre 1/2 singletons crash when there's an attempt to load - // this plugin twice, so shortcut here. Ideally this would be caught at - // Ignition Rendering. - static bool done{false}; - if (done) - { - ignerr << "Only one Scene3D is supported per process at the moment." - << std::endl; - return; - } - done = true; - auto renderWindow = this->PluginItem()->findChild<RenderWindowItem *>(); if (!renderWindow) { @@ -2397,6 +2401,8 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) << "Render window will not be created" << std::endl; return; } + renderWindow->SetErrorCb(std::bind(&Scene3D::SetLoadingError, this, + std::placeholders::_1)); if (this->title.empty()) this->title = "3D Scene"; @@ -2891,6 +2897,19 @@ void Scene3D::OnFocusWindow() renderWindow->forceActiveFocus(); } +///////////////////////////////////////////////// +QString Scene3D::LoadingError() const +{ + return this->loadingError; +} + +///////////////////////////////////////////////// +void Scene3D::SetLoadingError(const QString &_loadingError) +{ + this->loadingError = _loadingError; + this->LoadingErrorChanged(); +} + ///////////////////////////////////////////////// void RenderWindowItem::SetXYZSnap(const math::Vector3d &_xyz) { @@ -3180,6 +3199,12 @@ void RenderWindowItem::OnHovered(const ignition::math::Vector2i &_hoverPos) this->dataPtr->renderThread->ignRenderer.NewHoverEvent(_hoverPos); } +///////////////////////////////////////////////// +void RenderWindowItem::SetErrorCb(std::function<void(const QString&)> _cb) +{ + this->dataPtr->renderThread->SetErrorCb(_cb); +} + ///////////////////////////////////////////////// void RenderWindowItem::mousePressEvent(QMouseEvent *_e) { diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index 1f743a4323..37b95f5db7 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -56,10 +56,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { class Scene3DPrivate; class RenderUtil; - /// \brief Creates a new ignition rendering scene or adds a user-camera to an - /// existing scene. It is possible to orbit the camera around the scene with + /// \brief Creates an ignition rendering scene and user camera. + /// It is possible to orbit the camera around the scene with /// the mouse. Use other plugins to manage objects in the scene. /// + /// Only one plugin displaying an Ignition Rendering scene can be used at a + /// time. + /// /// ## Configuration /// /// * \<engine\> : Optional render engine name, defaults to 'ogre'. @@ -88,6 +91,14 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { NOTIFY ErrorPopupTextChanged ) + /// \brief Loading error message + Q_PROPERTY( + QString loadingError + READ LoadingError + WRITE SetLoadingError + NOTIFY LoadingErrorChanged + ) + /// \brief Constructor public: Scene3D(); @@ -185,6 +196,20 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// the connection to work on the QML side signals: void popupError(); + /// \brief Get the loading error string. + /// \return String explaining the loading error. If empty, there's no error. + public: Q_INVOKABLE QString LoadingError() const; + + /// \brief Set the loading error message. + /// \param[in] _loadingError Error message. + public: Q_INVOKABLE void SetLoadingError(const QString &_loadingError); + + /// \brief Notify that loading error has changed + signals: void LoadingErrorChanged(); + + /// \brief Loading error message + public: QString loadingError; + /// \internal /// \brief Pointer to private data. private: std::unique_ptr<Scene3DPrivate> dataPtr; @@ -210,7 +235,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: void Render(); /// \brief Initialize the render engine - public: void Initialize(); + /// \return Error message if initialization failed. If empty, no errors + /// occurred. + public: std::string Initialize(); /// \brief Destroy camera associated with this renderer public: void Destroy(); @@ -519,6 +546,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _size Size of the texture signals: void TextureReady(int _id, const QSize &_size); + /// \brief Set a callback to be called in case there are errors. + /// \param[in] _cb Error callback + public: void SetErrorCb(std::function<void(const QString &)> _cb); + + /// \brief Function to be called if there are errors. + public: std::function<void(const QString &)> errorCb; + /// \brief Offscreen surface to render to public: QOffscreenSurface *surface = nullptr; @@ -726,6 +760,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _entity Scoped name of entity. public slots: void OnContextMenuRequested(QString _entity); + /// \brief Set a callback to be called in case there are errors. + /// \param[in] _cb Error callback + public: void SetErrorCb(std::function<void(const QString &)> _cb); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr<RenderWindowItemPrivate> dataPtr; From 650b74658d0b8713de169b25a2e24c6d9c3e7ec3 Mon Sep 17 00:00:00 2001 From: Louise Poubel <louise@openrobotics.org> Date: Tue, 18 Jan 2022 08:59:02 -0800 Subject: [PATCH 03/17] Remove EachNew calls from sensor PreUpdates (#1281) Signed-off-by: Louise Poubel <louise@openrobotics.org> --- Migration.md | 4 ++ src/systems/air_pressure/AirPressure.cc | 46 ++++++++++++------ src/systems/altimeter/Altimeter.cc | 46 ++++++++++++------ src/systems/imu/Imu.cc | 52 ++++++++++++++------- src/systems/logical_camera/LogicalCamera.cc | 45 ++++++++++++------ src/systems/magnetometer/Magnetometer.cc | 47 +++++++++++++------ test/integration/air_pressure_system.cc | 6 ++- test/integration/altimeter_system.cc | 6 ++- test/integration/imu_system.cc | 6 ++- test/integration/logical_camera_system.cc | 30 +++++++----- test/integration/magnetometer_system.cc | 6 ++- 11 files changed, 211 insertions(+), 83 deletions(-) diff --git a/Migration.md b/Migration.md index 012dbafdb8..52e498a41b 100644 --- a/Migration.md +++ b/Migration.md @@ -5,6 +5,10 @@ Deprecated code produces compile-time warnings. These warning serve as notification to users that their code should be upgraded. The next major release will remove the deprecated code. +## Ignition Gazebo 3.12.0 to 3.X.X + +* Some sensors will only have the `SensorTopic` component after the 1st iteration. + ## Ignition Gazebo 2.x to 3.x * Use ign-rendering3, ign-sensors3 and ign-gui3. diff --git a/src/systems/air_pressure/AirPressure.cc b/src/systems/air_pressure/AirPressure.cc index cff6ea8858..4346cf1f1b 100644 --- a/src/systems/air_pressure/AirPressure.cc +++ b/src/systems/air_pressure/AirPressure.cc @@ -21,6 +21,7 @@ #include <string> #include <unordered_map> +#include <unordered_set> #include <utility> #include <ignition/plugin/Register.hh> @@ -49,30 +50,35 @@ using namespace systems; /// \brief Private AirPressure data class. class ignition::gazebo::systems::AirPressurePrivate { - /// \brief A map of air pressure entity to its vertical reference + /// \brief A map of air pressure entity to its sensor public: std::unordered_map<Entity, std::unique_ptr<sensors::AirPressureSensor>> entitySensorMap; /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; + /// \brief Keep list of sensors that were created during the previous + /// `PostUpdate`, so that components can be created during the next + /// `PreUpdate`. + public: std::unordered_set<Entity> newSensors; + /// True if the rendering component is initialized public: bool initialized = false; /// \brief Create sensor - /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU /// \param[in] _airPressure AirPressureSensor component. /// \param[in] _parent Parent entity component. public: void AddAirPressure( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::AirPressureSensor *_airPressure, const components::ParentEntity *_parent); /// \brief Create air pressure sensor - /// \param[in] _ecm Mutable reference to ECM. - public: void CreateAirPressureEntities(EntityComponentManager &_ecm); + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); /// \brief Update air pressure sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. @@ -98,7 +104,21 @@ void AirPressure::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { IGN_PROFILE("AirPressure::PreUpdate"); - this->dataPtr->CreateAirPressureEntities(_ecm); + + // Create components + for (auto entity : this->dataPtr->newSensors) + { + auto it = this->dataPtr->entitySensorMap.find(entity); + if (it == this->dataPtr->entitySensorMap.end()) + { + ignerr << "Entity [" << entity + << "] isn't in sensor map, this shouldn't happen." << std::endl; + continue; + } + // Set topic + _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic())); + } + this->dataPtr->newSensors.clear(); } ////////////////////////////////////////////////// @@ -116,6 +136,8 @@ void AirPressure::PostUpdate(const UpdateInfo &_info, << "s]. System may not work properly." << std::endl; } + this->dataPtr->CreateSensors(_ecm); + if (!_info.paused) { this->dataPtr->UpdateAirPressures(_ecm); @@ -134,7 +156,7 @@ void AirPressure::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void AirPressurePrivate::AddAirPressure( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::AirPressureSensor *_airPressure, const components::ParentEntity *_parent) @@ -171,15 +193,13 @@ void AirPressurePrivate::AddAirPressure( math::Pose3d sensorWorldPose = worldPose(_entity, _ecm); sensor->SetPose(sensorWorldPose); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); } ////////////////////////////////////////////////// -void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm) +void AirPressurePrivate::CreateSensors(const EntityComponentManager &_ecm) { IGN_PROFILE("AirPressurePrivate::CreateAirPressureEntities"); if (!this->initialized) @@ -190,7 +210,7 @@ void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm) const components::AirPressureSensor *_airPressure, const components::ParentEntity *_parent)->bool { - AddAirPressure(_ecm, _entity, _airPressure, _parent); + this->AddAirPressure(_ecm, _entity, _airPressure, _parent); return true; }); this->initialized = true; @@ -203,7 +223,7 @@ void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm) const components::AirPressureSensor *_airPressure, const components::ParentEntity *_parent)->bool { - AddAirPressure(_ecm, _entity, _airPressure, _parent); + this->AddAirPressure(_ecm, _entity, _airPressure, _parent); return true; }); } diff --git a/src/systems/altimeter/Altimeter.cc b/src/systems/altimeter/Altimeter.cc index 4cc12f12bc..1b34fc63c1 100644 --- a/src/systems/altimeter/Altimeter.cc +++ b/src/systems/altimeter/Altimeter.cc @@ -21,6 +21,7 @@ #include <string> #include <unordered_map> +#include <unordered_set> #include <utility> #include <ignition/common/Profiler.hh> @@ -51,30 +52,35 @@ using namespace systems; /// \brief Private Altimeter data class. class ignition::gazebo::systems::AltimeterPrivate { - /// \brief A map of altimeter entity to its vertical reference + /// \brief A map of altimeter entity to its sensor public: std::unordered_map<Entity, std::unique_ptr<sensors::AltimeterSensor>> entitySensorMap; /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; + /// \brief Keep list of sensors that were created during the previous + /// `PostUpdate`, so that components can be created during the next + /// `PreUpdate`. + public: std::unordered_set<Entity> newSensors; + /// True if the rendering component is initialized public: bool initialized = false; /// \brief Create sensor - /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU /// \param[in] _altimeter Altimeter component. /// \param[in] _parent Parent entity component. public: void AddAltimeter( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::Altimeter *_altimeter, const components::ParentEntity *_parent); /// \brief Create altimeter sensor - /// \param[in] _ecm Mutable reference to ECM. - public: void CreateAltimeterEntities(EntityComponentManager &_ecm); + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); /// \brief Update altimeter sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. @@ -99,7 +105,21 @@ void Altimeter::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { IGN_PROFILE("Altimeter::PreUpdate"); - this->dataPtr->CreateAltimeterEntities(_ecm); + + // Create components + for (auto entity : this->dataPtr->newSensors) + { + auto it = this->dataPtr->entitySensorMap.find(entity); + if (it == this->dataPtr->entitySensorMap.end()) + { + ignerr << "Entity [" << entity + << "] isn't in sensor map, this shouldn't happen." << std::endl; + continue; + } + // Set topic + _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic())); + } + this->dataPtr->newSensors.clear(); } ////////////////////////////////////////////////// @@ -116,6 +136,8 @@ void Altimeter::PostUpdate(const UpdateInfo &_info, << "s]. System may not work properly." << std::endl; } + this->dataPtr->CreateSensors(_ecm); + // Only update and publish if not paused. if (!_info.paused) { @@ -135,7 +157,7 @@ void Altimeter::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void AltimeterPrivate::AddAltimeter( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::Altimeter *_altimeter, const components::ParentEntity *_parent) @@ -173,15 +195,13 @@ void AltimeterPrivate::AddAltimeter( sensor->SetVerticalReference(verticalReference); sensor->SetPosition(verticalReference); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); } ////////////////////////////////////////////////// -void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm) +void AltimeterPrivate::CreateSensors(const EntityComponentManager &_ecm) { IGN_PROFILE("Altimeter::CreateAltimeterEntities"); if (!this->initialized) @@ -192,7 +212,7 @@ void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm) const components::Altimeter *_altimeter, const components::ParentEntity *_parent)->bool { - AddAltimeter(_ecm, _entity, _altimeter, _parent); + this->AddAltimeter(_ecm, _entity, _altimeter, _parent); return true; }); this->initialized = true; @@ -205,7 +225,7 @@ void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm) const components::Altimeter *_altimeter, const components::ParentEntity *_parent)->bool { - AddAltimeter(_ecm, _entity, _altimeter, _parent); + this->AddAltimeter(_ecm, _entity, _altimeter, _parent); return true; }); } diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index d883f6cc91..a6a3ecf7d9 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -17,9 +17,10 @@ #include "Imu.hh" +#include <string> #include <unordered_map> +#include <unordered_set> #include <utility> -#include <string> #include <ignition/plugin/Register.hh> @@ -56,6 +57,11 @@ class ignition::gazebo::systems::ImuPrivate /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; + /// \brief Keep list of sensors that were created during the previous + /// `PostUpdate`, so that components can be created during the next + /// `PreUpdate`. + public: std::unordered_set<Entity> newSensors; + /// \brief Keep track of world ID, which is equivalent to the scene's /// root visual. /// Defaults to zero, which is considered invalid by Ignition Gazebo. @@ -64,21 +70,21 @@ class ignition::gazebo::systems::ImuPrivate /// True if the rendering component is initialized public: bool initialized = false; - /// \brief Create IMU sensor - /// \param[in] _ecm Mutable reference to ECM. - public: void CreateImuEntities(EntityComponentManager &_ecm); + /// \brief Create IMU sensors in ign-sensors + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); /// \brief Update IMU sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. public: void Update(const EntityComponentManager &_ecm); /// \brief Create sensor - /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU /// \param[in] _imu IMU component. /// \param[in] _parent Parent entity component. - public: void addIMU( - EntityComponentManager &_ecm, + public: void AddSensor( + const EntityComponentManager &_ecm, const Entity _entity, const components::Imu *_imu, const components::ParentEntity *_parent); @@ -102,7 +108,21 @@ void Imu::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { IGN_PROFILE("Imu::PreUpdate"); - this->dataPtr->CreateImuEntities(_ecm); + + // Create components + for (auto entity : this->dataPtr->newSensors) + { + auto it = this->dataPtr->entitySensorMap.find(entity); + if (it == this->dataPtr->entitySensorMap.end()) + { + ignerr << "Entity [" << entity + << "] isn't in sensor map, this shouldn't happen." << std::endl; + continue; + } + // Set topic + _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic())); + } + this->dataPtr->newSensors.clear(); } ////////////////////////////////////////////////// @@ -119,6 +139,8 @@ void Imu::PostUpdate(const UpdateInfo &_info, << "s]. System may not work properly." << std::endl; } + this->dataPtr->CreateSensors(_ecm); + // Only update and publish if not paused. if (!_info.paused) { @@ -137,8 +159,8 @@ void Imu::PostUpdate(const UpdateInfo &_info, } ////////////////////////////////////////////////// -void ImuPrivate::addIMU( - EntityComponentManager &_ecm, +void ImuPrivate::AddSensor( + const EntityComponentManager &_ecm, const Entity _entity, const components::Imu *_imu, const components::ParentEntity *_parent) @@ -186,15 +208,13 @@ void ImuPrivate::addIMU( math::Pose3d p = worldPose(_entity, _ecm); sensor->SetOrientationReference(p.Rot()); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); } ////////////////////////////////////////////////// -void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm) +void ImuPrivate::CreateSensors(const EntityComponentManager &_ecm) { IGN_PROFILE("ImuPrivate::CreateImuEntities"); // Get World Entity @@ -214,7 +234,7 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm) const components::Imu *_imu, const components::ParentEntity *_parent)->bool { - addIMU(_ecm, _entity, _imu, _parent); + this->AddSensor(_ecm, _entity, _imu, _parent); return true; }); this->initialized = true; @@ -227,7 +247,7 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm) const components::Imu *_imu, const components::ParentEntity *_parent)->bool { - addIMU(_ecm, _entity, _imu, _parent); + this->AddSensor(_ecm, _entity, _imu, _parent); return true; }); } diff --git a/src/systems/logical_camera/LogicalCamera.cc b/src/systems/logical_camera/LogicalCamera.cc index 3b53fb4c0c..fbed671f66 100644 --- a/src/systems/logical_camera/LogicalCamera.cc +++ b/src/systems/logical_camera/LogicalCamera.cc @@ -22,6 +22,7 @@ #include <map> #include <string> #include <unordered_map> +#include <unordered_set> #include <utility> #include <ignition/common/Profiler.hh> @@ -59,23 +60,28 @@ class ignition::gazebo::systems::LogicalCameraPrivate /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; + /// \brief Keep list of sensors that were created during the previous + /// `PostUpdate`, so that components can be created during the next + /// `PreUpdate`. + public: std::unordered_set<Entity> newSensors; + /// True if the rendering component is initialized public: bool initialized = false; /// \brief Create sensor - /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU /// \param[in] _logicalCamera LogicalCamera component. /// \param[in] _parent Parent entity component. public: void AddLogicalCamera( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::LogicalCamera *_logicalCamera, const components::ParentEntity *_parent); /// \brief Create logicalCamera sensor - /// \param[in] _ecm Mutable reference to ECM. - public: void CreateLogicalCameraEntities(EntityComponentManager &_ecm); + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); /// \brief Update logicalCamera sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. @@ -101,7 +107,21 @@ void LogicalCamera::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { IGN_PROFILE("LogicalCamera::PreUpdate"); - this->dataPtr->CreateLogicalCameraEntities(_ecm); + + // Create components + for (auto entity : this->dataPtr->newSensors) + { + auto it = this->dataPtr->entitySensorMap.find(entity); + if (it == this->dataPtr->entitySensorMap.end()) + { + ignerr << "Entity [" << entity + << "] isn't in sensor map, this shouldn't happen." << std::endl; + continue; + } + // Set topic + _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic())); + } + this->dataPtr->newSensors.clear(); } ////////////////////////////////////////////////// @@ -118,6 +138,8 @@ void LogicalCamera::PostUpdate(const UpdateInfo &_info, << "s]. System may not work properly." << std::endl; } + this->dataPtr->CreateSensors(_ecm); + // Only update and publish if not paused. if (!_info.paused) { @@ -137,7 +159,7 @@ void LogicalCamera::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void LogicalCameraPrivate::AddLogicalCamera( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::LogicalCamera *_logicalCamera, const components::ParentEntity *_parent) @@ -172,16 +194,13 @@ void LogicalCameraPrivate::AddLogicalCamera( math::Pose3d sensorWorldPose = worldPose(_entity, _ecm); sensor->SetPose(sensorWorldPose); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); } ////////////////////////////////////////////////// -void LogicalCameraPrivate::CreateLogicalCameraEntities( - EntityComponentManager &_ecm) +void LogicalCameraPrivate::CreateSensors(const EntityComponentManager &_ecm) { IGN_PROFILE("LogicalCameraPrivate::CreateLogicalCameraEntities"); if (!this->initialized) @@ -192,7 +211,7 @@ void LogicalCameraPrivate::CreateLogicalCameraEntities( const components::LogicalCamera *_logicalCamera, const components::ParentEntity *_parent)->bool { - AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent); + this->AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent); return true; }); this->initialized = true; @@ -206,7 +225,7 @@ void LogicalCameraPrivate::CreateLogicalCameraEntities( const components::LogicalCamera *_logicalCamera, const components::ParentEntity *_parent)->bool { - AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent); + this->AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent); return true; }); } diff --git a/src/systems/magnetometer/Magnetometer.cc b/src/systems/magnetometer/Magnetometer.cc index ade0e3dd08..bf974caa82 100644 --- a/src/systems/magnetometer/Magnetometer.cc +++ b/src/systems/magnetometer/Magnetometer.cc @@ -19,6 +19,7 @@ #include <string> #include <unordered_map> +#include <unordered_set> #include <utility> #include <ignition/plugin/Register.hh> @@ -56,25 +57,30 @@ class ignition::gazebo::systems::MagnetometerPrivate /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; + /// \brief Keep list of sensors that were created during the previous + /// `PostUpdate`, so that components can be created during the next + /// `PreUpdate`. + public: std::unordered_set<Entity> newSensors; + /// True if the rendering component is initialized public: bool initialized = false; /// \brief Create sensor - /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _ecm Immutable reference to ECM. /// \param[in] _entity Entity of the IMU /// \param[in] _magnetometer Magnetometer component. /// \param[in] _worldField MagneticField component. /// \param[in] _parent Parent entity component. public: void AddMagnetometer( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::Magnetometer *_magnetometer, const components::MagneticField *_worldField, const components::ParentEntity *_parent); /// \brief Create magnetometer sensor - /// \param[in] _ecm Mutable reference to ECM. - public: void CreateMagnetometerEntities(EntityComponentManager &_ecm); + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); /// \brief Update magnetometer sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. @@ -100,7 +106,21 @@ void Magnetometer::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) { IGN_PROFILE("Magnetometer::PreUpdate"); - this->dataPtr->CreateMagnetometerEntities(_ecm); + + // Create components + for (auto entity : this->dataPtr->newSensors) + { + auto it = this->dataPtr->entitySensorMap.find(entity); + if (it == this->dataPtr->entitySensorMap.end()) + { + ignerr << "Entity [" << entity + << "] isn't in sensor map, this shouldn't happen." << std::endl; + continue; + } + // Set topic + _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic())); + } + this->dataPtr->newSensors.clear(); } ////////////////////////////////////////////////// @@ -117,6 +137,8 @@ void Magnetometer::PostUpdate(const UpdateInfo &_info, << "s]. System may not work properly." << std::endl; } + this->dataPtr->CreateSensors(_ecm); + // Only update and publish if not paused. if (!_info.paused) { @@ -136,7 +158,7 @@ void Magnetometer::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void MagnetometerPrivate::AddMagnetometer( - EntityComponentManager &_ecm, + const EntityComponentManager &_ecm, const Entity _entity, const components::Magnetometer *_magnetometer, const components::MagneticField *_worldField, @@ -178,16 +200,13 @@ void MagnetometerPrivate::AddMagnetometer( math::Pose3d p = worldPose(_entity, _ecm); sensor->SetWorldPose(p); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); } ////////////////////////////////////////////////// -void MagnetometerPrivate::CreateMagnetometerEntities( - EntityComponentManager &_ecm) +void MagnetometerPrivate::CreateSensors(const EntityComponentManager &_ecm) { IGN_PROFILE("MagnetometerPrivate::CreateMagnetometerEntities"); auto worldEntity = _ecm.EntityByComponents(components::World()); @@ -213,7 +232,8 @@ void MagnetometerPrivate::CreateMagnetometerEntities( const components::Magnetometer *_magnetometer, const components::ParentEntity *_parent)->bool { - AddMagnetometer(_ecm, _entity, _magnetometer, worldField, _parent); + this->AddMagnetometer(_ecm, _entity, _magnetometer, worldField, + _parent); return true; }); this->initialized = true; @@ -226,7 +246,8 @@ void MagnetometerPrivate::CreateMagnetometerEntities( const components::Magnetometer *_magnetometer, const components::ParentEntity *_parent)->bool { - AddMagnetometer(_ecm, _entity, _magnetometer, worldField, _parent); + this->AddMagnetometer(_ecm, _entity, _magnetometer, worldField, + _parent); return true; }); } diff --git a/test/integration/air_pressure_system.cc b/test/integration/air_pressure_system.cc index 51e9000d90..52ad96ab50 100644 --- a/test/integration/air_pressure_system.cc +++ b/test/integration/air_pressure_system.cc @@ -62,7 +62,7 @@ TEST_F(AirPressureTest, AirPressure) // Create a system that checks sensor topic test::Relay testSystem; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, const gazebo::EntityComponentManager &_ecm) { _ecm.Each<components::AirPressureSensor, components::Name>( @@ -75,6 +75,10 @@ TEST_F(AirPressureTest, AirPressure) auto sensorComp = _ecm.Component<components::Sensor>(_entity); EXPECT_NE(nullptr, sensorComp); + if (_info.iterations == 1) + return true; + + // This component is created on the 2nd PreUpdate auto topicComp = _ecm.Component<components::SensorTopic>(_entity); EXPECT_NE(nullptr, topicComp); if (topicComp) diff --git a/test/integration/altimeter_system.cc b/test/integration/altimeter_system.cc index 4b391963ac..bac80b7c25 100644 --- a/test/integration/altimeter_system.cc +++ b/test/integration/altimeter_system.cc @@ -80,7 +80,7 @@ TEST_F(AltimeterTest, ModelFalling) test::Relay testSystem; std::vector<math::Pose3d> poses; std::vector<math::Vector3d> velocities; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, const gazebo::EntityComponentManager &_ecm) { _ecm.Each<components::Altimeter, components::Name, @@ -100,6 +100,10 @@ TEST_F(AltimeterTest, ModelFalling) auto sensorComp = _ecm.Component<components::Sensor>(_entity); EXPECT_NE(nullptr, sensorComp); + if (_info.iterations == 1) + return true; + + // This component is created on the 2nd PreUpdate auto topicComp = _ecm.Component<components::SensorTopic>(_entity); EXPECT_NE(nullptr, topicComp); if (topicComp) diff --git a/test/integration/imu_system.cc b/test/integration/imu_system.cc index 68147f4e30..c38c3cf6c7 100644 --- a/test/integration/imu_system.cc +++ b/test/integration/imu_system.cc @@ -89,7 +89,7 @@ TEST_F(ImuTest, ModelFalling) std::vector<math::Pose3d> poses; std::vector<math::Vector3d> accelerations; std::vector<math::Vector3d> angularVelocities; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, const gazebo::EntityComponentManager &_ecm) { _ecm.Each<components::Imu, @@ -113,6 +113,10 @@ TEST_F(ImuTest, ModelFalling) auto sensorComp = _ecm.Component<components::Sensor>(_entity); EXPECT_NE(nullptr, sensorComp); + if (_info.iterations == 1) + return true; + + // This component is created on the 2nd PreUpdate auto topicComp = _ecm.Component<components::SensorTopic>(_entity); EXPECT_NE(nullptr, topicComp); if (topicComp) diff --git a/test/integration/logical_camera_system.cc b/test/integration/logical_camera_system.cc index 32bae02ef5..4abcf00700 100644 --- a/test/integration/logical_camera_system.cc +++ b/test/integration/logical_camera_system.cc @@ -91,7 +91,7 @@ TEST_F(LogicalCameraTest, LogicalCameraBox) // Create a system that checks sensor topics test::Relay testSystem; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, const gazebo::EntityComponentManager &_ecm) { _ecm.Each<components::LogicalCamera, components::Name>( @@ -105,11 +105,15 @@ TEST_F(LogicalCameraTest, LogicalCameraBox) auto sensorComp = _ecm.Component<components::Sensor>(_entity); EXPECT_NE(nullptr, sensorComp); - auto topicComp = - _ecm.Component<components::SensorTopic>(_entity); - EXPECT_NE(nullptr, topicComp); - if (topicComp) { - EXPECT_EQ(topic1, topicComp->Data()); + if (_info.iterations != 1) + { + auto topicComp = + _ecm.Component<components::SensorTopic>(_entity); + EXPECT_NE(nullptr, topicComp); + if (topicComp) + { + EXPECT_EQ(topic1, topicComp->Data()); + } } update1Checked = true; } @@ -119,11 +123,15 @@ TEST_F(LogicalCameraTest, LogicalCameraBox) auto sensorComp = _ecm.Component<components::Sensor>(_entity); EXPECT_NE(nullptr, sensorComp); - auto topicComp = - _ecm.Component<components::SensorTopic>(_entity); - EXPECT_NE(nullptr, topicComp); - if (topicComp) { - EXPECT_EQ(topic2, topicComp->Data()); + if (_info.iterations != 1) + { + auto topicComp = + _ecm.Component<components::SensorTopic>(_entity); + EXPECT_NE(nullptr, topicComp); + if (topicComp) + { + EXPECT_EQ(topic2, topicComp->Data()); + } } update2Checked = true; } diff --git a/test/integration/magnetometer_system.cc b/test/integration/magnetometer_system.cc index cf29f976f8..e17be9b9f7 100644 --- a/test/integration/magnetometer_system.cc +++ b/test/integration/magnetometer_system.cc @@ -81,7 +81,7 @@ TEST_F(MagnetometerTest, RotatedMagnetometer) test::Relay testSystem; std::vector<math::Pose3d> poses; - testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, const gazebo::EntityComponentManager &_ecm) { _ecm.Each<components::Magnetometer, @@ -98,6 +98,10 @@ TEST_F(MagnetometerTest, RotatedMagnetometer) auto sensorComp = _ecm.Component<components::Sensor>(_entity); EXPECT_NE(nullptr, sensorComp); + if (_info.iterations == 1) + return true; + + // This component is created on the 2nd PreUpdate auto topicComp = _ecm.Component<components::SensorTopic>(_entity); EXPECT_NE(nullptr, topicComp); if (topicComp) From 749884db3f031f6159101c1d98880df6ac2deb46 Mon Sep 17 00:00:00 2001 From: Louise Poubel <louise@openrobotics.org> Date: Fri, 21 Jan 2022 09:10:55 -0800 Subject: [PATCH 04/17] Buoyancy: fix center of volume's reference frame (#1302) Signed-off-by: Louise Poubel <louise@openrobotics.org> --- .../gazebo/components/CenterOfVolume.hh | 2 +- src/systems/buoyancy/Buoyancy.cc | 11 +- test/integration/buoyancy.cc | 76 +++++++++ test/worlds/center_of_volume.sdf | 160 ++++++++++++++++++ 4 files changed, 242 insertions(+), 7 deletions(-) create mode 100644 test/worlds/center_of_volume.sdf diff --git a/include/ignition/gazebo/components/CenterOfVolume.hh b/include/ignition/gazebo/components/CenterOfVolume.hh index 4bd2389101..2096d2d5d0 100644 --- a/include/ignition/gazebo/components/CenterOfVolume.hh +++ b/include/ignition/gazebo/components/CenterOfVolume.hh @@ -33,7 +33,7 @@ namespace components /// \brief A component for an entity's center of volume. Units are in meters. /// The Vector3 value indicates center of volume of an entity. The /// position of the center of volume is relative to the pose of the parent - /// entity. + /// entity, which is usually a link. using CenterOfVolume = Component<math::Vector3d, class CenterOfVolumeTag>; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.CenterOfVolume", CenterOfVolume) diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc index 81a880a21a..0e7a286583 100644 --- a/src/systems/buoyancy/Buoyancy.cc +++ b/src/systems/buoyancy/Buoyancy.cc @@ -151,7 +151,7 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, _entity, components::Collision()); double volumeSum = 0; - ignition::math::Vector3d weightedPosSum = + ignition::math::Vector3d weightedPosInLinkSum = ignition::math::Vector3d::Zero; // Compute the volume of the link by iterating over all the collision @@ -210,16 +210,15 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } volumeSum += volume; - math::Pose3d pose = worldPose(collision, _ecm); - weightedPosSum += volume * pose.Pos(); + auto poseInLink = _ecm.Component<components::Pose>(collision)->Data(); + weightedPosInLinkSum += volume * poseInLink.Pos(); } if (volumeSum > 0) { - // Store the center of volume - math::Pose3d linkWorldPose = worldPose(_entity, _ecm); + // Store the center of volume expressed in the link frame _ecm.CreateComponent(_entity, components::CenterOfVolume( - weightedPosSum / volumeSum - linkWorldPose.Pos())); + weightedPosInLinkSum / volumeSum)); // Store the volume _ecm.CreateComponent(_entity, components::Volume(volumeSum)); diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc index 77445fdd61..7acfee7a5e 100644 --- a/test/integration/buoyancy.cc +++ b/test/integration/buoyancy.cc @@ -20,8 +20,10 @@ #include <ignition/common/Console.hh> #include <ignition/common/Util.hh> +#include "ignition/gazebo/Util.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/TestFixture.hh" #include "ignition/gazebo/components/CenterOfVolume.hh" #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Name.hh" @@ -194,3 +196,77 @@ TEST_F(BuoyancyTest, Movement) server.Run(true, iterations, false); EXPECT_TRUE(finished); } + +///////////////////////////////////////////////// +TEST_F(BuoyancyTest, OffsetAndRotation) +{ + TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "center_of_volume.sdf")); + + std::size_t iterations{0}; + fixture.OnPostUpdate([&]( + const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + // Get links + auto noOffsets = entitiesFromScopedName("no_offset::link", _ecm); + ASSERT_EQ(1u, noOffsets.size()); + auto noOffset = *noOffsets.begin(); + EXPECT_NE(kNullEntity, noOffset); + + auto noOffsetRotateds = entitiesFromScopedName("no_offset_rotated::link", + _ecm); + ASSERT_EQ(1u, noOffsetRotateds.size()); + auto noOffsetRotated = *noOffsetRotateds.begin(); + EXPECT_NE(kNullEntity, noOffsetRotated); + + auto withOffsets = entitiesFromScopedName("com_cov_offset::link", _ecm); + ASSERT_EQ(1u, withOffsets.size()); + auto withOffset = *withOffsets.begin(); + EXPECT_NE(kNullEntity, withOffset); + + auto withOffsetRotateds = entitiesFromScopedName( + "com_cov_offset_rotated::link", _ecm); + ASSERT_EQ(1u, withOffsetRotateds.size()); + auto withOffsetRotated = *withOffsetRotateds.begin(); + EXPECT_NE(kNullEntity, withOffsetRotated); + + // Check CoVs have correct offsets + auto noOffsetCoV = _ecm.Component<components::CenterOfVolume>(noOffset); + ASSERT_NE(noOffsetCoV, nullptr); + EXPECT_EQ(math::Vector3d::Zero, noOffsetCoV->Data()); + + auto noOffsetRotatedCoV = _ecm.Component<components::CenterOfVolume>( + noOffsetRotated); + ASSERT_NE(noOffsetRotatedCoV, nullptr); + EXPECT_EQ(math::Vector3d::Zero, noOffsetRotatedCoV->Data()); + + auto withOffsetCoV = _ecm.Component<components::CenterOfVolume>(withOffset); + ASSERT_NE(withOffsetCoV, nullptr); + EXPECT_EQ(math::Vector3d::One, withOffsetCoV->Data()); + + auto withOffsetRotatedCoV = _ecm.Component<components::CenterOfVolume>( + withOffsetRotated); + ASSERT_NE(withOffsetRotatedCoV, nullptr); + EXPECT_EQ(math::Vector3d::One, withOffsetRotatedCoV->Data()); + + // Check that all objects are neutrally buoyant and stay still + auto noOffsetPose = worldPose(noOffset, _ecm); + EXPECT_EQ(math::Pose3d(), noOffsetPose); + + auto noOffsetRotatedPose = worldPose(noOffsetRotated, _ecm); + EXPECT_EQ(math::Pose3d(-3, 0, 0, 0.1, 0.2, 0.3), noOffsetRotatedPose); + + auto withOffsetPose = worldPose(withOffset, _ecm); + EXPECT_EQ(math::Pose3d(0, 3, 0, 0, 0, 0), withOffsetPose); + + auto withOffsetRotatedPose = worldPose(withOffsetRotated, _ecm); + EXPECT_EQ(math::Pose3d(-3, 3, 0, 0.1, 0.2, 0.3), withOffsetRotatedPose); + + iterations++; + }).Finalize(); + + std::size_t targetIterations{1000}; + fixture.Server()->Run(true, targetIterations, false); + EXPECT_EQ(targetIterations, iterations); +} diff --git a/test/worlds/center_of_volume.sdf b/test/worlds/center_of_volume.sdf new file mode 100644 index 0000000000..0972ca554d --- /dev/null +++ b/test/worlds/center_of_volume.sdf @@ -0,0 +1,160 @@ +<?xml version="1.0" ?> +<sdf version="1.6"> + <world name="center_of_volume"> + + <physics name="fast" type="ignored"> + <real_time_factor>0</real_time_factor> + </physics> + <plugin + filename="libignition-gazebo-physics-system.so" + name="ignition::gazebo::systems::Physics"> + </plugin> + <plugin + filename="libignition-gazebo-buoyancy-system.so" + name="ignition::gazebo::systems::Buoyancy"> + <uniform_fluid_density>1000</uniform_fluid_density> + </plugin> + + <model name='no_offset'> + <pose>0 0 0 0 0 0</pose> + <link name='link'> + <pose>0 0 0 0 0 0</pose> + <inertial> + <pose>0 0 0 0 0 0</pose> + <inertia> + <ixx>166.66</ixx> + <ixy>0</ixy> + <ixz>0</ixz> + <iyy>166.66</iyy> + <iyz>0</iyz> + <izz>166.66</izz> + </inertia> + <mass>1000.0</mass> + </inertial> + <visual name='visual'> + <pose>0 0 0 0 0 0</pose> + <geometry> + <box> + <size>1.0 1.0 1.0</size> + </box> + </geometry> + </visual> + <collision name='collision'> + <pose>0 0 0 0 0 0</pose> + <geometry> + <box> + <size>1.0 1.0 1.0</size> + </box> + </geometry> + </collision> + </link> + </model> + + <model name='no_offset_rotated'> + <pose>-3 0 0 0.1 0.2 0.3</pose> + <link name='link'> + <pose>0 0 0 0 0 0</pose> + <inertial> + <pose>0 0 0 0 0 0</pose> + <inertia> + <ixx>166.66</ixx> + <ixy>0</ixy> + <ixz>0</ixz> + <iyy>166.66</iyy> + <iyz>0</iyz> + <izz>166.66</izz> + </inertia> + <mass>1000.0</mass> + </inertial> + <visual name='visual'> + <pose>0 0 0 0 0 0</pose> + <geometry> + <box> + <size>1.0 1.0 1.0</size> + </box> + </geometry> + </visual> + <collision name='collision'> + <pose>0 0 0 0 0 0</pose> + <geometry> + <box> + <size>1.0 1.0 1.0</size> + </box> + </geometry> + </collision> + </link> + </model> + + <model name='com_cov_offset'> + <pose>0 3 0 0 0 0</pose> + <link name='link'> + <pose>0 0 0 0 0 0</pose> + <inertial> + <pose>1 1 1 0 0 0</pose> + <inertia> + <ixx>166.66</ixx> + <ixy>0</ixy> + <ixz>0</ixz> + <iyy>166.66</iyy> + <iyz>0</iyz> + <izz>166.66</izz> + </inertia> + <mass>1000.0</mass> + </inertial> + <visual name='visual'> + <pose>1 1 1 0 0 0</pose> + <geometry> + <box> + <size>1.0 1.0 1.0</size> + </box> + </geometry> + </visual> + <collision name='collision'> + <pose>1 1 1 0 0 0</pose> + <geometry> + <box> + <size>1.0 1.0 1.0</size> + </box> + </geometry> + </collision> + </link> + </model> + + <model name='com_cov_offset_rotated'> + <pose>-3 3 0 0.1 0.2 0.3</pose> + <link name='link'> + <pose>0 0 0 0 0 0</pose> + <inertial> + <pose>1 1 1 0 0 0</pose> + <inertia> + <ixx>166.66</ixx> + <ixy>0</ixy> + <ixz>0</ixz> + <iyy>166.66</iyy> + <iyz>0</iyz> + <izz>166.66</izz> + </inertia> + <mass>1000.0</mass> + </inertial> + <visual name='visual'> + <pose>1 1 1 0 0 0</pose> + <geometry> + <box> + <size>1.0 1.0 1.0</size> + </box> + </geometry> + </visual> + <collision name='collision'> + <pose>1 1 1 0 0 0</pose> + <geometry> + <box> + <size>1.0 1.0 1.0</size> + </box> + </geometry> + </collision> + </link> + </model> + + </world> +</sdf> + From 912e2cebec89a96fd8632ed5358d6036f7873ef6 Mon Sep 17 00:00:00 2001 From: Louise Poubel <louise@openrobotics.org> Date: Tue, 25 Jan 2022 11:36:36 -0800 Subject: [PATCH 05/17] Update source install instructions (#1311) Signed-off-by: Louise Poubel <louise@openrobotics.org> --- tutorials/install.md | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/tutorials/install.md b/tutorials/install.md index 97d4e598ac..3260f10154 100644 --- a/tutorials/install.md +++ b/tutorials/install.md @@ -59,33 +59,36 @@ feature which hasn't been released yet. ### Ubuntu 18.04 or above -1. Enable the Ignition software repositories: +1. Install tools + ``` + sudo apt install -y build-essential cmake g++-8 git gnupg lsb-release wget + ``` + +2. Enable the Ignition software repositories: ``` sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - sudo apt-get update ``` -2. Install package dependencies: +3. Clone repository ``` git clone https://github.com/ignitionrobotics/ign-gazebo -b ign-gazebo<#> - export SYSTEM_VERSION=bionic - sudo apt -y install \ - $(sort -u $(find . -iname 'packages-'`lsb_release -cs`'.apt' -o -iname 'packages.apt' | grep -v '/\.git/') | sed '/ignition\|sdf/d' | tr '\n' ' ') ``` -3. (Ubuntu 18.04 only) Configure gcc8 +4. Install package dependencies (including other Ignition libraries): ``` - sudo apt-get install g++-8 - sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-8 800 --slave /usr/bin/g++ g++ /usr/bin/g++-8 --slave /usr/bin/gcov gcov /usr/bin/gcov-8 + sudo apt -y install \ + $(sort -u $(find . -iname 'packages-'`lsb_release -cs`'.apt' -o -iname 'packages.apt' | tr '\n' ' ')) ``` -4. Clone the repository if you haven't already. +5. (Ubuntu 18.04 only) Configure gcc8 ``` - git clone https://github.com/ignitionrobotics/ign-gazebo -b ign-gazebo<#> + sudo apt-get install g++-8 + sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-8 800 --slave /usr/bin/g++ g++ /usr/bin/g++-8 --slave /usr/bin/gcov gcov /usr/bin/gcov-8 ``` -5. Configure and build. +6. Configure and build. ``` cd ign-gazebo mkdir build From ebb3a9ad8a7484eeb97bf2585f7731e7d4225f9b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty <arjo@openrobotics.org> Date: Fri, 28 Jan 2022 16:48:17 +0800 Subject: [PATCH 06/17] Log an error if JointPositionController cannot find the joint. (citadel retarget) (#1314) In the event a user enters the wrong name for a certain joint, the JointPositionController system will silently fail. This PR adds a simple error message that tells the user that the joint was not found. Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org> --- .../joint_position_controller/JointPositionController.cc | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index 067c622161..9530256eff 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -192,8 +192,17 @@ void JointPositionController::PreUpdate( this->dataPtr->model.JointByName(_ecm, this->dataPtr->jointName); } + // If the joint is still not found then warn the user, they may have entered + // the wrong joint name. if (this->dataPtr->jointEntity == kNullEntity) + { + static bool warned = false; + if(!warned) + ignerr << "Could not find joint with name [" + << this->dataPtr->jointName <<"]\n"; + warned = true; return; + } // Nothing left to do if paused. if (_info.paused) From 717a7e91b73e968d836689d23dcdbf2579e7fbce Mon Sep 17 00:00:00 2001 From: Ian Chen <ichen@osrfoundation.org> Date: Thu, 3 Feb 2022 17:40:22 -0800 Subject: [PATCH 07/17] Load and run visual plugin (system) on GUI side (#1275) * load and run visual plugins on gui end Signed-off-by: Ian Chen <ichen@osrfoundation.org> * scene update event emitted on both server and gui side Signed-off-by: Ian Chen <ichen@osrfoundation.org> * shader param update working Signed-off-by: Ian Chen <ichen@osrfoundation.org> * sim time, constants, full example working Signed-off-by: Ian Chen <ichen@osrfoundation.org> * add integration test Signed-off-by: Ian Chen <ichen@osrfoundation.org> * code cleanup Signed-off-by: Ian Chen <ichen@osrfoundation.org> * more code cleanup Signed-off-by: Ian Chen <ichen@osrfoundation.org> * style fixes and add some comments Signed-off-by: Ian Chen <ichen@osrfoundation.org> * review changes Signed-off-by: Ian Chen <ichen@osrfoundation.org> * require display for shader param test Signed-off-by: Ian Chen <ichen@osrfoundation.org> * style and comment Signed-off-by: Ian Chen <ichen@osrfoundation.org> --- examples/worlds/shader_param.sdf | 268 ++++++++++++ include/ignition/gazebo/components/Visual.hh | 54 +++ include/ignition/gazebo/gui/GuiEvents.hh | 24 ++ include/ignition/gazebo/rendering/Events.hh | 12 + .../ignition/gazebo/rendering/RenderUtil.hh | 4 + src/SdfEntityCreator.cc | 11 + src/SimulationRunner.hh | 3 - src/SimulationRunner_TEST.cc | 21 + src/gui/GuiEvents.cc | 30 ++ src/gui/GuiRunner.cc | 144 ++++++- src/gui/GuiRunner.hh | 10 + .../plugins/scene_manager/GzSceneManager.cc | 55 +++ src/rendering/RenderUtil.cc | 13 + src/systems/CMakeLists.txt | 1 + src/systems/sensors/Sensors.cc | 4 +- src/systems/shader_param/CMakeLists.txt | 8 + src/systems/shader_param/ShaderParam.cc | 396 ++++++++++++++++++ src/systems/shader_param/ShaderParam.hh | 100 +++++ test/integration/CMakeLists.txt | 1 + test/integration/shader_param_system.cc | 110 +++++ test/plugins/CMakeLists.txt | 1 + test/plugins/TestVisualSystem.cc | 23 + test/plugins/TestVisualSystem.hh | 81 ++++ test/worlds/plugins.sdf | 7 + 24 files changed, 1371 insertions(+), 10 deletions(-) create mode 100644 examples/worlds/shader_param.sdf create mode 100644 src/systems/shader_param/CMakeLists.txt create mode 100644 src/systems/shader_param/ShaderParam.cc create mode 100644 src/systems/shader_param/ShaderParam.hh create mode 100644 test/integration/shader_param_system.cc create mode 100644 test/plugins/TestVisualSystem.cc create mode 100644 test/plugins/TestVisualSystem.hh diff --git a/examples/worlds/shader_param.sdf b/examples/worlds/shader_param.sdf new file mode 100644 index 0000000000..db3d48ca78 --- /dev/null +++ b/examples/worlds/shader_param.sdf @@ -0,0 +1,268 @@ +<?xml version="1.0" ?> +<!-- + +This world contains a sphere model whose visual appearance is altered by the +ShaderParam visual plugin over time. + +--> +<sdf version="1.6"> + <world name="deformable_sphere"> + + <plugin + filename="ignition-gazebo-physics-system" + name="ignition::gazebo::systems::Physics"> + </plugin> + <plugin + filename="ignition-gazebo-sensors-system" + name="ignition::gazebo::systems::Sensors"> + <render_engine>ogre2</render_engine> + <background_color>0.8 0.8 0.8</background_color> + </plugin> + <plugin + filename="ignition-gazebo-scene-broadcaster-system" + name="ignition::gazebo::systems::SceneBroadcaster"> + </plugin> + <plugin + filename="ignition-gazebo-user-commands-system" + name="ignition::gazebo::systems::UserCommands"> + </plugin> + + <gui fullscreen="0"> + + <!-- 3D scene --> + <plugin filename="MinimalScene" name="3D View"> + <ignition-gui> + <title>3D View</title> + <property type="bool" key="showTitleBar">false</property> + <property type="string" key="state">docked</property> + </ignition-gui> + + <engine>ogre2</engine> + <scene>scene</scene> + <ambient_light>0.4 0.4 0.4</ambient_light> + <background_color>0.8 0.8 0.8</background_color> + <camera_pose>-6 0 6 0 0.5 0</camera_pose> + </plugin> + + <!-- Plugins that add functionality to the scene --> + <plugin filename="EntityContextMenuPlugin" name="Entity context menu"> + <ignition-gui> + <property key="state" type="string">floating</property> + <property key="width" type="double">5</property> + <property key="height" type="double">5</property> + <property key="showTitleBar" type="bool">false</property> + </ignition-gui> + </plugin> + <plugin filename="GzSceneManager" name="Scene Manager"> + <ignition-gui> + <property key="resizable" type="bool">false</property> + <property key="width" type="double">5</property> + <property key="height" type="double">5</property> + <property key="state" type="string">floating</property> + <property key="showTitleBar" type="bool">false</property> + </ignition-gui> + </plugin> + <plugin filename="InteractiveViewControl" name="Interactive view control"> + <ignition-gui> + <property key="resizable" type="bool">false</property> + <property key="width" type="double">5</property> + <property key="height" type="double">5</property> + <property key="state" type="string">floating</property> + <property key="showTitleBar" type="bool">false</property> + </ignition-gui> + </plugin> + <plugin filename="CameraTracking" name="Camera Tracking"> + <ignition-gui> + <property key="resizable" type="bool">false</property> + <property key="width" type="double">5</property> + <property key="height" type="double">5</property> + <property key="state" type="string">floating</property> + <property key="showTitleBar" type="bool">false</property> + </ignition-gui> + </plugin> + <!-- World control --> + <plugin filename="WorldControl" name="World control"> + <ignition-gui> + <title>World control</title> + <property type="bool" key="showTitleBar">false</property> + <property type="bool" key="resizable">false</property> + <property type="double" key="height">72</property> + <property type="double" key="width">121</property> + <property type="double" key="z">1</property> + + <property type="string" key="state">floating</property> + <anchors target="3D View"> + <line own="left" target="left"/> + <line own="bottom" target="bottom"/> + </anchors> + </ignition-gui> + + <play_pause>true</play_pause> + <step>true</step> + <start_paused>true</start_paused> + <use_event>true</use_event> + + </plugin> + + <!-- World statistics --> + <plugin filename="WorldStats" name="World stats"> + <ignition-gui> + <title>World stats</title> + <property type="bool" key="showTitleBar">false</property> + <property type="bool" key="resizable">false</property> + <property type="double" key="height">110</property> + <property type="double" key="width">290</property> + <property type="double" key="z">1</property> + + <property type="string" key="state">floating</property> + <anchors target="3D View"> + <line own="right" target="right"/> + <line own="bottom" target="bottom"/> + </anchors> + </ignition-gui> + + <sim_time>true</sim_time> + <real_time>true</real_time> + <real_time_factor>true</real_time_factor> + <iterations>true</iterations> + </plugin> + + <!-- Inspector --> + <plugin filename="ComponentInspector" name="Component inspector"> + <ignition-gui> + <property type="string" key="state">docked</property> + </ignition-gui> + </plugin> + + <!-- Entity tree --> + <plugin filename="EntityTree" name="Entity tree"> + <ignition-gui> + <property type="string" key="state">docked</property> + </ignition-gui> + </plugin> + + <plugin filename="ImageDisplay" name="Image Display"> + <ignition-gui> + <title>RGB camera</title> + <property key="state" type="string">floating</property> + <property type="double" key="width">350</property> + <property type="double" key="height">315</property> + </ignition-gui> + <topic>camera</topic> + <topic_picker>false</topic_picker> + </plugin> + <plugin filename="ImageDisplay" name="Image Display 2"> + <ignition-gui> + <title>Depth camera</title> + <property key="state" type="string">floating</property> + <property type="double" key="width">350</property> + <property type="double" key="height">315</property> + <property type="double" key="x">500</property> + </ignition-gui> + <topic>depth_camera</topic> + <topic_picker>false</topic_picker> + </plugin> + </gui> + + <scene> + <ambient>1.0 1.0 1.0</ambient> + <background>0.8 0.8 0.8</background> + </scene> + + <light type="directional" name="sun"> + <cast_shadows>true</cast_shadows> + <pose>0 0 10 0 0 0</pose> + <diffuse>0.8 0.8 0.8 1</diffuse> + <specular>0.6 0.6 0.6 1</specular> + <direction>-0.5 0.1 -0.9</direction> + </light> + + <model name="ground_plane"> + <static>true</static> + <link name="link"> + <collision name="collision"> + <geometry> + <plane> + <normal>0 0 1</normal> + <size>100 100</size> + </plane> + </geometry> + </collision> + <visual name="visual"> + <geometry> + <plane> + <normal>0 0 1</normal> + <size>100 100</size> + </plane> + </geometry> + <material> + <ambient>0.8 0.8 0.8 1</ambient> + <diffuse>0.8 0.8 0.8 1</diffuse> + <specular>0.8 0.8 0.8 1</specular> + </material> + </visual> + </link> + </model> + + <include> + <pose>0 0.0 0.5 0 0 0</pose> + <name>deformable_sphere</name> + <pose>3 0 0 0 0 0</pose> + <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/deformable_sphere</uri> + </include> + + <model name="camera"> + <static>true</static> + <pose>2.5 0 0.5 0 0.0 3.14</pose> + <link name="link"> + <pose>0.05 0.05 0.05 0 0 0</pose> + <collision name="collision"> + <geometry> + <box> + <size>0.1 0.1 0.1</size> + </box> + </geometry> + </collision> + <visual name="visual"> + <geometry> + <box> + <size>0.1 0.1 0.1</size> + </box> + </geometry> + </visual> + <sensor name="camera" type="camera"> + <camera> + <horizontal_fov>1.047</horizontal_fov> + <image> + <width>320</width> + <height>240</height> + </image> + <clip> + <near>0.1</near> + <far>100</far> + </clip> + </camera> + <always_on>1</always_on> + <update_rate>30</update_rate> + <topic>camera</topic> + </sensor> + <sensor name="depth_camera" type="depth_camera"> + <update_rate>10</update_rate> + <topic>depth_camera</topic> + <camera> + <horizontal_fov>1.05</horizontal_fov> + <image> + <width>320</width> + <height>240</height> + </image> + <clip> + <near>0.1</near> + <far>10.0</far> + </clip> + </camera> + </sensor> + </link> + </model> + + </world> +</sdf> diff --git a/include/ignition/gazebo/components/Visual.hh b/include/ignition/gazebo/components/Visual.hh index 9d90871a7b..de94818059 100644 --- a/include/ignition/gazebo/components/Visual.hh +++ b/include/ignition/gazebo/components/Visual.hh @@ -17,6 +17,11 @@ #ifndef IGNITION_GAZEBO_COMPONENTS_VISUAL_HH_ #define IGNITION_GAZEBO_COMPONENTS_VISUAL_HH_ +#include <string> + +#include <sdf/parser.hh> +#include <sdf/Element.hh> + #include <ignition/gazebo/components/Factory.hh> #include <ignition/gazebo/components/Component.hh> #include <ignition/gazebo/config.hh> @@ -27,11 +32,60 @@ namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers +{ + class SdfElementSerializer + { + /// \brief Serialization for `sdf::Model`. + /// \param[in] _out Output stream. + /// \param[in] _elem Visual plugin elem to stream + /// \return The stream. + public: static std::ostream &Serialize(std::ostream &_out, + const sdf::ElementPtr &_elem) + { + _out << "<?xml version=\"1.0\" ?>" + << "<sdf version='" << SDF_PROTOCOL_VERSION << "'>" + << _elem->ToString("") + << "</sdf>"; + return _out; + } + + /// \brief Deserialization for `sdf::Element`. + /// \param[in] _in Input stream. + /// \param[out] _elem Visual plugin elem to populate + /// \return The stream. + public: static std::istream &Deserialize(std::istream &_in, + sdf::ElementPtr &_elem) + { + std::string sdfStr(std::istreambuf_iterator<char>(_in), {}); + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + bool result = sdf::readString(sdfStr, sdfParsed); + if (!result) + { + ignerr << "Unable to deserialize sdf::ElementPtr" << std::endl; + return _in; + } + + _elem = sdfParsed->Root()->GetFirstElement(); + return _in; + } + }; +} + namespace components { /// \brief A component that identifies an entity as being a visual. using Visual = Component<NoData, class VisualTag>; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Visual", Visual) + + /// \brief A component that contains a visual plugin's SDF element. + using VisualPlugin = Component<sdf::ElementPtr, + class VisualPluginTag, + serializers::SdfElementSerializer>; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.VisualPlugin", + VisualPlugin) } } } diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index 4407d68598..afbb751879 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -25,8 +25,11 @@ #include <string> #include <utility> #include <vector> + #include <ignition/math/Vector3.hh> #include <ignition/utils/ImplPtr.hh> +#include <sdf/Element.hh> + #include "ignition/gazebo/gui/Export.hh" #include "ignition/gazebo/Entity.hh" #include "ignition/gazebo/config.hh" @@ -211,6 +214,27 @@ namespace events IGN_UTILS_IMPL_PTR(dataPtr) }; + /// \brief Event that notifies a visual plugin is to be loaded + class IGNITION_GAZEBO_GUI_VISIBLE VisualPlugin: public QEvent + { + /// \brief Constructor + /// \param[in] _entity Visual entity id + /// \param[in] _elem Visual plugin SDF element + public: explicit VisualPlugin(ignition::gazebo::Entity _entity, + const sdf::ElementPtr &_elem); + + /// \brief Get the entity to load the visual plugin for + public: ignition::gazebo::Entity Entity() const; + + /// \brief Get the sdf element of the visual plugin + public: sdf::ElementPtr Element() const; + + static const QEvent::Type kType = QEvent::Type(QEvent::User + 8); + + /// \internal + /// \brief Private data pointer + IGN_UTILS_IMPL_PTR(dataPtr) + }; } // namespace events } } // namespace gui diff --git a/include/ignition/gazebo/rendering/Events.hh b/include/ignition/gazebo/rendering/Events.hh index d17b3922c5..683c66f529 100644 --- a/include/ignition/gazebo/rendering/Events.hh +++ b/include/ignition/gazebo/rendering/Events.hh @@ -32,6 +32,18 @@ namespace ignition /// more information about events. namespace events { + /// \brief The render event is emitted when the the scene manager is + /// updated with contents from the ECM. This event is emitted + /// before the PreRender event on the server side in the rendering + /// thread. It is also accessible on the GUI side. + /// + /// For example: + /// \code + /// eventManager.Emit<ignition::gazebo::events::SceneUpdate>(); + /// \endcode + using SceneUpdate = ignition::common::EventT<void(void), + struct SceneUpdateTag>; + /// \brief The render event is emitted before rendering updates. /// The event is emitted in the rendering thread so rendering /// calls can ben make in this event callback diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index 6110b2083c..a757ec5378 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -214,6 +214,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _active True if active. public: void SetTransformActive(bool _active); + /// \brief Set the event manager to use + /// \param[in] _mgr Event manager to set to. + public: void SetEventManager(EventManager *_mgr); + /// \brief Private data pointer. private: std::unique_ptr<RenderUtilPrivate> dataPtr; }; diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index ae98f41ec7..d54e1f377f 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -749,6 +749,17 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Visual *_visual) components::Material(*_visual->Material())); } + // store the plugin in a component + if (_visual->Element()) + { + sdf::ElementPtr pluginElem = _visual->Element()->FindElement("plugin"); + if (pluginElem) + { + this->dataPtr->ecm->CreateComponent(visualEntity, + components::VisualPlugin(pluginElem)); + } + } + // Keep track of visuals so we can load their plugins after loading the // entire model and having its full scoped name. this->dataPtr->newVisuals[visualEntity] = _visual->Element(); diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index a52210425e..5da194e209 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -495,9 +495,6 @@ namespace ignition /// \brief Mutex to protect pendingSystems private: mutable std::mutex pendingSystemsMutex; - /// \brief Systems implementing Configure - private: std::vector<ISystemConfigure *> systemsConfigure; - /// \brief Systems implementing PreUpdate private: std::vector<ISystemPreUpdate *> systemsPreupdate; diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index cc9b33161a..17641a873c 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -1234,6 +1234,18 @@ TEST_P(SimulationRunnerTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) }); EXPECT_NE(kNullEntity, sensorId); + // Get visual entity + Entity visualId{kNullEntity}; + runner.EntityCompMgr().Each<ignition::gazebo::components::Visual>([&]( + const ignition::gazebo::Entity &_entity, + const ignition::gazebo::components::Visual *_visual)->bool + { + EXPECT_NE(nullptr, _visual); + visualId = _entity; + return true; + }); + EXPECT_NE(kNullEntity, visualId); + // Check component registered by world plugin std::string worldComponentName{"WorldPluginComponent"}; auto worldComponentId = ignition::common::hash64(worldComponentName); @@ -1258,6 +1270,14 @@ TEST_P(SimulationRunnerTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(sensorId, sensorComponentId)); + // Check component registered by visual plugin + std::string visualComponentName{"VisualPluginComponent"}; + auto visualComponentId = ignition::common::hash64(visualComponentName); + + EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(visualComponentId)); + EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(visualId, + visualComponentId)); + // Clang re-registers components between tests. If we don't unregister them // beforehand, the new plugin tries to create a storage type from a previous // plugin, causing a crash. @@ -1268,6 +1288,7 @@ TEST_P(SimulationRunnerTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) components::Factory::Instance()->Unregister(worldComponentId); components::Factory::Instance()->Unregister(modelComponentId); components::Factory::Instance()->Unregister(sensorComponentId); + components::Factory::Instance()->Unregister(visualComponentId); #endif } diff --git a/src/gui/GuiEvents.cc b/src/gui/GuiEvents.cc index ed195212ed..a89a5d8254 100644 --- a/src/gui/GuiEvents.cc +++ b/src/gui/GuiEvents.cc @@ -50,6 +50,15 @@ class ignition::gazebo::gui::events::ModelEditorAddEntity::Implementation public: ignition::gazebo::Entity parent; }; +class ignition::gazebo::gui::events::VisualPlugin::Implementation +{ + /// \brief Entity to load the visual plugin for + public: ignition::gazebo::Entity entity; + + /// \brief Sdf element of the visual plugin + public: sdf::ElementPtr element; +}; + using namespace ignition; using namespace gazebo; using namespace gui; @@ -132,3 +141,24 @@ QMap<QString, QString> &ModelEditorAddEntity::Data() { return this->dataPtr->data; } + +///////////////////////////////////////////////// +VisualPlugin::VisualPlugin(ignition::gazebo::Entity _entity, + const sdf::ElementPtr &_elem) : + QEvent(kType), dataPtr(utils::MakeImpl<Implementation>()) +{ + this->dataPtr->entity = _entity; + this->dataPtr->element = _elem; +} + +///////////////////////////////////////////////// +ignition::gazebo::Entity VisualPlugin::Entity() const +{ + return this->dataPtr->entity; +} + +///////////////////////////////////////////////// +sdf::ElementPtr VisualPlugin::Element() const +{ + return this->dataPtr->element; +} diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 50a2c3f075..2a70c033ac 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -15,6 +15,10 @@ * */ +#include <memory> +#include <utility> +#include <vector> + #include <ignition/common/Console.hh> #include <ignition/common/Profiler.hh> #include <ignition/fuel_tools/Interface.hh> @@ -28,7 +32,9 @@ #include "ignition/gazebo/components/components.hh" #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include <ignition/gazebo/gui/GuiEvents.hh> #include "ignition/gazebo/gui/GuiSystem.hh" +#include "ignition/gazebo/SystemLoader.hh" #include "GuiRunner.hh" @@ -68,6 +74,31 @@ class ignition::gazebo::GuiRunner::Implementation /// \brief Name of WorldControl service public: std::string controlService; + + /// \brief System loader for loading ign-gazebo systems + public: std::unique_ptr<SystemLoader> systemLoader; + + /// \brief Mutex to protect systemLoader + public: std::mutex systemLoadMutex; + + /// \brief Events containing visual plugins to load + public: std::vector<std::pair<ignition::gazebo::Entity, sdf::ElementPtr>> + visualPlugins; + + /// \brief Systems implementing PreUpdate + public: std::vector<SystemPluginPtr> systems; + + /// \brief Systems implementing PreUpdate + public: std::vector<ISystemPreUpdate *> systemsPreupdate; + + /// \brief Systems implementing Update + public: std::vector<ISystemUpdate *> systemsUpdate; + + /// \brief Systems implementing PostUpdate + public: std::vector<ISystemPostUpdate *> systemsPostupdate; + + /// \brief Manager of all events. + public: EventManager eventMgr; }; ///////////////////////////////////////////////// @@ -92,7 +123,7 @@ GuiRunner::GuiRunner(const std::string &_worldName) // so that an offset is not required this->dataPtr->ecm.SetEntityCreateOffset(math::MAX_I32 / 2); - auto win = gui::App()->findChild<ignition::gui::MainWindow *>(); + auto win = ignition::gui::App()->findChild<ignition::gui::MainWindow *>(); auto winWorldNames = win->property("worldNames").toStringList(); winWorldNames.append(QString::fromStdString(_worldName)); win->setProperty("worldNames", winWorldNames); @@ -136,7 +167,7 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) if (_event->type() == ignition::gui::events::WorldControl::kType) { auto worldControlEvent = - reinterpret_cast<gui::events::WorldControl *>(_event); + reinterpret_cast<ignition::gui::events::WorldControl *>(_event); if (worldControlEvent) { msgs::WorldControlState req; @@ -162,6 +193,20 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) this->dataPtr->node.Request(this->dataPtr->controlService, req, cb); } } + else if (_event->type() == ignition::gazebo::gui::events::VisualPlugin::kType) + { + auto visualPluginEvent = + reinterpret_cast<gui::events::VisualPlugin *>(_event); + if (visualPluginEvent) + { + std::lock_guard<std::mutex> lock(this->dataPtr->systemLoadMutex); + + Entity entity = visualPluginEvent->Entity(); + sdf::ElementPtr pluginElem = visualPluginEvent->Element(); + this->dataPtr->visualPlugins.push_back( + std::make_pair(entity, pluginElem)); + } + } // Standard event processing return QObject::eventFilter(_obj, _event); @@ -171,7 +216,7 @@ bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) void GuiRunner::RequestState() { // set up service for async state response callback - std::string id = std::to_string(gui::App()->applicationPid()); + std::string id = std::to_string(ignition::gui::App()->applicationPid()); std::string reqSrv = this->dataPtr->node.Options().NameSpace() + "/" + id + "/state_async"; auto reqSrvValid = transport::TopicUtils::AsValidTopic(reqSrv); @@ -225,7 +270,7 @@ void GuiRunner::OnStateAsyncService(const msgs::SerializedStepMap &_res) // todo(anyone) store reqSrv string in a member variable and use it here // and in RequestState() - std::string id = std::to_string(gui::App()->applicationPid()); + std::string id = std::to_string(ignition::gui::App()->applicationPid()); std::string reqSrv = this->dataPtr->node.Options().NameSpace() + "/" + id + "/state_async"; this->dataPtr->node.UnadvertiseSrv(reqSrv); @@ -264,7 +309,8 @@ void GuiRunner::OnStateQt(const msgs::SerializedStepMap &_msg) ///////////////////////////////////////////////// void GuiRunner::UpdatePlugins() { - auto plugins = gui::App()->findChildren<GuiSystem *>(); + // gui plugins + auto plugins = ignition::gui::App()->findChildren<GuiSystem *>(); for (auto plugin : plugins) { plugin->Update(this->dataPtr->updateInfo, this->dataPtr->ecm); @@ -272,4 +318,92 @@ void GuiRunner::UpdatePlugins() this->dataPtr->ecm.ClearRemovedComponents(); this->dataPtr->ecm.ClearNewlyCreatedEntities(); this->dataPtr->ecm.ProcessRemoveEntityRequests(); + + // ign-gazebo systems + this->LoadSystems(); + this->UpdateSystems(); +} + +///////////////////////////////////////////////// +void GuiRunner::LoadSystems() +{ + std::lock_guard<std::mutex> lock(this->dataPtr->systemLoadMutex); + // currently only support systems that are visual plugins + for (auto &visualPlugin : this->dataPtr->visualPlugins) + { + Entity entity = visualPlugin.first; + sdf::ElementPtr pluginElem = visualPlugin.second; + auto filename = pluginElem->Get<std::string>("filename"); + auto name = pluginElem->Get<std::string>("name"); + if (filename != "__default__" && name != "__default__") + { + std::optional<SystemPluginPtr> system; + if (!this->dataPtr->systemLoader) + this->dataPtr->systemLoader = std::make_unique<SystemLoader>(); + system = this->dataPtr->systemLoader->LoadPlugin( + filename, name, pluginElem); + if (system) + { + SystemPluginPtr sys = system.value(); + this->dataPtr->systems.push_back(sys); + this->dataPtr->systemsPreupdate.push_back( + sys->QueryInterface<ISystemPreUpdate>()); + this->dataPtr->systemsUpdate.push_back( + sys->QueryInterface<ISystemUpdate>()); + this->dataPtr->systemsPostupdate.push_back( + sys->QueryInterface<ISystemPostUpdate>()); + + auto sysConfigure = sys->QueryInterface<ISystemConfigure>(); + if (sysConfigure) + { + sysConfigure->Configure(entity, pluginElem, this->dataPtr->ecm, + this->dataPtr->eventMgr); + } + igndbg << "Loaded system [" << name + << "] for entity [" << entity << "] in GUI" + << std::endl; + } + } + } + this->dataPtr->visualPlugins.clear(); +} + +///////////////////////////////////////////////// +void GuiRunner::UpdateSystems() +{ + IGN_PROFILE("GuiRunner::UpdateSystems"); + + { + IGN_PROFILE("PreUpdate"); + for (auto& system : this->dataPtr->systemsPreupdate) + { + if (system) + system->PreUpdate(this->dataPtr->updateInfo, this->dataPtr->ecm); + } + } + + { + IGN_PROFILE("Update"); + for (auto& system : this->dataPtr->systemsUpdate) + { + if (system) + system->Update(this->dataPtr->updateInfo, this->dataPtr->ecm); + } + } + + { + IGN_PROFILE("PostUpdate"); + // \todo(anyone) Do PostUpdates in parallel + for (auto& system : this->dataPtr->systemsPostupdate) + { + if (system) + system->PostUpdate(this->dataPtr->updateInfo, this->dataPtr->ecm); + } + } +} + +///////////////////////////////////////////////// +EventManager &GuiRunner::GuiEventManager() const +{ + return this->dataPtr->eventMgr; } diff --git a/src/gui/GuiRunner.hh b/src/gui/GuiRunner.hh index 1a06098161..bc5a730553 100644 --- a/src/gui/GuiRunner.hh +++ b/src/gui/GuiRunner.hh @@ -25,6 +25,7 @@ #include <ignition/utils/ImplPtr.hh> #include "ignition/gazebo/config.hh" +#include "ignition/gazebo/EventManager.hh" #include "ignition/gazebo/gui/Export.hh" namespace ignition @@ -46,6 +47,9 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \brief Destructor public: ~GuiRunner() override; + /// \brief Get the event manager for the gui + public: EventManager &GuiEventManager() const; + // Documentation inherited protected: bool eventFilter(QObject *_obj, QEvent *_event) override; @@ -74,6 +78,12 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \todo(anyone) Move to GuiRunner::Implementation when porting to v5 private: Q_INVOKABLE void UpdatePlugins(); + /// \brief Load systems + private: void LoadSystems(); + + /// \brief Update systems + private: void UpdateSystems(); + /// \brief Pointer to private data. IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index cccb6caaa4..48c0735f10 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -14,10 +14,15 @@ * limitations under the License. * */ + +#include <map> #include <set> +#include "../../GuiRunner.hh" #include "GzSceneManager.hh" +#include <sdf/Element.hh> + #include <ignition/common/Profiler.hh> #include <ignition/gui/Application.hh> #include <ignition/gui/GuiEvents.hh> @@ -28,6 +33,7 @@ #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/gui/GuiEvents.hh" #include "ignition/gazebo/rendering/RenderUtil.hh" @@ -58,6 +64,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Mutex to protect gui event and system upate call race conditions /// for newEntities and removedEntities public: std::mutex newRemovedEntityMutex; + + /// \brief Indicates whether initial visual plugins have been loaded or not. + public: bool initializedVisualPlugins = false; }; } } @@ -102,6 +111,42 @@ void GzSceneManager::Update(const UpdateInfo &_info, this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); + // load visual plugin on gui side + std::map<Entity, sdf::ElementPtr> pluginElems; + if (!this->dataPtr->initializedVisualPlugins) + { + _ecm.Each<components::Visual, components::VisualPlugin>( + [&](const Entity &_entity, + const components::Visual *, + const components::VisualPlugin *_plugin)->bool + { + sdf::ElementPtr pluginElem = _plugin->Data(); + pluginElems[_entity] = _plugin->Data(); + return true; + }); + this->dataPtr->initializedVisualPlugins = true; + } + else + { + _ecm.EachNew<components::Visual, components::VisualPlugin>( + [&](const Entity &_entity, + const components::Visual *, + const components::VisualPlugin *_plugin)->bool + { + sdf::ElementPtr pluginElem = _plugin->Data(); + pluginElems[_entity] = _plugin->Data(); + return true; + }); + } + for (const auto &it : pluginElems) + { + ignition::gazebo::gui::events::VisualPlugin visualPluginEvent( + it.first, it.second); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild<ignition::gui::MainWindow *>(), + &visualPluginEvent); + } + // Emit entities created / removed event for gui::Plugins which don't have // direct access to the ECM. std::set<Entity> created; @@ -163,6 +208,16 @@ void GzSceneManagerPrivate::OnRender() return; this->renderUtil.SetScene(this->scene); + + auto runners = ignition::gui::App()->findChildren<GuiRunner *>(); + if (runners.empty() || runners[0] == nullptr) + { + ignerr << "Internal error: no GuiRunner found." << std::endl; + } + else + { + this->renderUtil.SetEventManager(&runners[0]->GuiEventManager()); + } } this->renderUtil.Update(); diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 3df544c588..0679db3769 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -89,6 +89,7 @@ #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/rendering/Events.hh" #include "ignition/gazebo/rendering/RenderUtil.hh" #include "ignition/gazebo/rendering/SceneManager.hh" #include "ignition/gazebo/rendering/MarkerManager.hh" @@ -193,6 +194,9 @@ class ignition::gazebo::RenderUtilPrivate const components::Name *_name, const components::ParentEntity *_parent); + /// \brief Event manager used for emitting render / scene events + public: EventManager *eventManager{nullptr}; + /// \brief Total time elapsed in simulation. This will not increase while /// paused. public: std::chrono::steady_clock::duration simTime{0}; @@ -1583,6 +1587,9 @@ void RenderUtil::Update() } } } + + if (this->dataPtr->eventManager) + this->dataPtr->eventManager->Emit<events::SceneUpdate>(); } ////////////////////////////////////////////////// @@ -3637,3 +3644,9 @@ void RenderUtilPrivate::CreateLight( this->newLights.push_back(std::make_tuple(_entity, _light->Data(), _name->Data(), _parent->Data())); } + +////////////////////////////////////////////////// +void RenderUtil::SetEventManager(EventManager *_mgr) +{ + this->dataPtr->eventManager = _mgr; +} diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 17514029f8..5054dad438 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -128,6 +128,7 @@ add_subdirectory(physics) add_subdirectory(pose_publisher) add_subdirectory(scene_broadcaster) add_subdirectory(sensors) +add_subdirectory(shader_param) add_subdirectory(thermal) add_subdirectory(thruster) add_subdirectory(touch_plugin) diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 5d802189b1..d5309a82c7 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -242,7 +242,6 @@ void SensorsPrivate::RunOnce() this->renderUtil.Update(); } - if (!this->activeSensors.empty()) { this->sensorMaskMutex.lock(); @@ -422,8 +421,9 @@ void Sensors::Configure(const Entity &/*_id*/, this->dataPtr->renderUtil.SetEnableSensors(true, std::bind(&Sensors::CreateSensor, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - this->dataPtr->renderUtil.SetRemoveSensorCb( + this->dataPtr->renderUtil.SetRemoveSensorCb( std::bind(&Sensors::RemoveSensor, this, std::placeholders::_1)); + this->dataPtr->renderUtil.SetEventManager(&_eventMgr); // parse sensor-specific data auto worldEntity = _ecm.EntityByComponents(components::World()); diff --git a/src/systems/shader_param/CMakeLists.txt b/src/systems/shader_param/CMakeLists.txt new file mode 100644 index 0000000000..0d14af33f3 --- /dev/null +++ b/src/systems/shader_param/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_system(shader-param + SOURCES + ShaderParam.cc + PUBLIC_LINK_LIBS + ${rendering_target} + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} +) diff --git a/src/systems/shader_param/ShaderParam.cc b/src/systems/shader_param/ShaderParam.cc new file mode 100644 index 0000000000..feab7709b2 --- /dev/null +++ b/src/systems/shader_param/ShaderParam.cc @@ -0,0 +1,396 @@ +/* + * Copyright (C) 2022 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 "ShaderParam.hh" + +#include <chrono> +#include <list> +#include <mutex> +#include <vector> +#include <string> + +#include <ignition/common/Profiler.hh> +#include <ignition/plugin/Register.hh> +#include <ignition/rendering/Material.hh> +#include <ignition/rendering/RenderingIface.hh> +#include <ignition/rendering/Scene.hh> +#include <ignition/rendering/ShaderParams.hh> +#include <ignition/rendering/Visual.hh> + +#include <sdf/Element.hh> + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/SourceFilePath.hh" +#include "ignition/gazebo/rendering/Events.hh" +#include "ignition/gazebo/rendering/RenderUtil.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::ShaderParamPrivate +{ + /// \brief Data structure for storing shader param info + public: class ShaderParamValue + { + /// \brief shader type: vertex or fragment + public: std::string shader; + + /// \brief variable type: int, float, float_array, int_array + /// \todo(anyone) support samplers + public: std::string type; + + /// \brief variable name of param + public: std::string name; + + /// \brief param value + public: std::string value; + }; + + /// \brief Path to vertex shader + public: std::string vertexShaderUri; + + /// \brief Path to fragment shader + public: std::string fragmentShaderUri; + + /// \brief Mutex to protect sim time updates. + public: std::mutex mutex; + + /// \brief Connection to pre-render event callback + public: ignition::common::ConnectionPtr connection{nullptr}; + + /// \brief Name of visual this plugin is attached to + public: std::string visualName; + + /// \brief Pointer to visual + public: rendering::VisualPtr visual; + + /// \brief Material used by this visual + public: rendering::MaterialPtr material; + + /// \brief Pointer to scene + public: rendering::ScenePtr scene; + + /// \brief Entity id of the visual + public: Entity entity = kNullEntity; + + /// \brief A list of shader params + public: std::vector<ShaderParamValue> shaderParams; + + /// \brief Time params that will be updated every iteration + public: std::vector<ShaderParamValue> timeParams; + + /// \brief Current sim time + public: std::chrono::steady_clock::duration currentSimTime; + + /// \brief All rendering operations must happen within this call + public: void OnUpdate(); +}; + +///////////////////////////////////////////////// +ShaderParam::ShaderParam() + : System(), dataPtr(std::make_unique<ShaderParamPrivate>()) +{ +} + +///////////////////////////////////////////////// +ShaderParam::~ShaderParam() +{ +} + +///////////////////////////////////////////////// +void ShaderParam::Configure(const Entity &_entity, + const std::shared_ptr<const sdf::Element> &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) +{ + IGN_PROFILE("ShaderParam::Configure"); + // Ugly, but needed because the sdf::Element::GetElement is not a const + // function and _sdf is a const shared pointer to a const sdf::Element. + auto sdf = const_cast<sdf::Element *>(_sdf.get()); + + if (sdf->HasElement("param")) + { + // loop and parse all shader params + sdf::ElementPtr paramElem = sdf->GetElement("param"); + while (paramElem) + { + if (!paramElem->HasElement("shader") || + !paramElem->HasElement("name")) + { + ignerr << "<param> must have <shader> and <name> sdf elements" + << std::endl; + paramElem = paramElem->GetNextElement("param"); + continue; + } + std::string shaderType = paramElem->Get<std::string>("shader"); + std::string paramName = paramElem->Get<std::string>("name"); + + std::string type = paramElem->Get<std::string>("type", "float").first; + std::string value = paramElem->Get<std::string>("value", "").first; + + ShaderParamPrivate::ShaderParamValue spv; + spv.shader = shaderType; + spv.name = paramName; + spv.value = value; + spv.type = type; + this->dataPtr->shaderParams.push_back(spv); + paramElem = paramElem->GetNextElement("param"); + } + } + + // parse path to shaders + if (sdf->HasElement("shader")) + { + sdf::ElementPtr shaderElem = sdf->GetElement("shader"); + if (!shaderElem->HasElement("vertex") || + !shaderElem->HasElement("fragment")) + { + ignerr << "<shader> must have <vertex> and <fragment> sdf elements" + << std::endl; + } + else + { + auto modelEntity = topLevelModel(_entity, _ecm); + auto modelPath = + _ecm.ComponentData<components::SourceFilePath>(modelEntity); + sdf::ElementPtr vertexElem = shaderElem->GetElement("vertex"); + this->dataPtr->vertexShaderUri = common::findFile( + asFullPath(vertexElem->Get<std::string>(), modelPath.value())); + sdf::ElementPtr fragmentElem = shaderElem->GetElement("fragment"); + this->dataPtr->fragmentShaderUri = common::findFile( + asFullPath(fragmentElem->Get<std::string>(), modelPath.value())); + } + } + + this->dataPtr->entity = _entity; + auto nameComp = _ecm.Component<components::Name>(_entity); + this->dataPtr->visualName = nameComp->Data(); + + // connect to the SceneUpdate event + // the callback is executed in the rendering thread so do all + // rendering operations in that thread + this->dataPtr->connection = + _eventMgr.Connect<ignition::gazebo::events::SceneUpdate>( + std::bind(&ShaderParamPrivate::OnUpdate, this->dataPtr.get())); +} + +////////////////////////////////////////////////// +void ShaderParam::PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &) +{ + IGN_PROFILE("ShaderParam::PreUpdate"); + std::lock_guard<std::mutex> lock(this->dataPtr->mutex); + this->dataPtr->currentSimTime = _info.simTime; +} + +////////////////////////////////////////////////// +void ShaderParamPrivate::OnUpdate() +{ + std::lock_guard<std::mutex> lock(this->mutex); + if (this->visualName.empty()) + return; + + if (!this->scene) + this->scene = rendering::sceneFromFirstRenderEngine(); + + if (!this->scene) + return; + + if (!this->visual) + { + // this does a breadth first search for visual with the entity id + // \todo(anyone) provide a helper function in RenderUtil to search for + // visual by entity id? + auto rootVis = scene->RootVisual(); + std::list<rendering::NodePtr> nodes; + nodes.push_back(rootVis); + while (!nodes.empty()) + { + auto n = nodes.front(); + nodes.pop_front(); + if (n && n->HasUserData("gazebo-entity")) + { + // RenderUti stores gazebo-entity user data as int + // \todo(anyone) Change this to uint64_t in Ignition H? + auto variant = n->UserData("gazebo-entity"); + const int *value = std::get_if<int>(&variant); + if (value && *value == static_cast<int>(this->entity)) + { + this->visual = std::dynamic_pointer_cast<rendering::Visual>(n); + break; + } + } + for (unsigned int i = 0; i < n->ChildCount(); ++i) + nodes.push_back(n->ChildByIndex(i)); + } + } + + if (!this->visual) + return; + + // get the material and set shaders + if (!this->material) + { + auto mat = scene->CreateMaterial(); + mat->SetVertexShader(this->vertexShaderUri); + mat->SetFragmentShader(this->fragmentShaderUri); + this->visual->SetMaterial(mat); + scene->DestroyMaterial(mat); + this->material = this->visual->Material(); + } + + if (!this->material) + return; + + // set the shader params read from SDF + // this is only done once + for (const auto & spv : this->shaderParams) + { + std::vector<std::string> values = common::split(spv.value, " "); + + int intValue = 0; + float floatValue = 0; + std::vector<float> floatArrayValue; + + rendering::ShaderParam::ParamType paramType = + rendering::ShaderParam::PARAM_NONE; + + rendering::ShaderParamsPtr params; + if (spv.shader == "fragment") + { + params = this->material->FragmentShaderParams(); + } + else if (spv.shader == "vertex") + { + params = this->material->VertexShaderParams(); + } + + // if no <value> is specified, this could be a constant + if (values.empty()) + { + // \todo handle args for constants + (*params)[spv.name] = intValue; + } + // float / int + else if (values.size() == 1u) + { + std::string str = values[0]; + + // TIME is reserved keyword for sim time + if (str == "TIME") + { + this->timeParams.push_back(spv); + continue; + } + + // if <type> is not empty, respect the specified type + if (!spv.type.empty()) + { + if (spv.type == "int") + { + intValue = std::stoi(str); + paramType = rendering::ShaderParam::PARAM_INT; + } + else if (spv.type == "float") + { + floatValue = std::stof(str); + paramType = rendering::ShaderParam::PARAM_FLOAT; + } + else + { + // \todo(anyone) support texture samplers + } + } + // else do our best guess at what the type is + else + { + std::string::size_type sz; + int n = std::stoi(str, &sz); + if ( sz == str.size()) + { + intValue = n; + paramType = rendering::ShaderParam::PARAM_INT; + } + else + { + floatValue = std::stof(str); + paramType = rendering::ShaderParam::PARAM_FLOAT; + } + } + } + // arrays + else + { + // int array + if (!spv.type.empty() && spv.type == "int_array") + { + for (const auto &v : values) + floatArrayValue.push_back(std::stoi(v)); + paramType = rendering::ShaderParam::PARAM_INT_BUFFER; + } + // treat everything else as float_array + else + { + for (const auto &v : values) + floatArrayValue.push_back(std::stof(v)); + paramType = rendering::ShaderParam::PARAM_FLOAT_BUFFER; + } + } + + // set the params + if (paramType == rendering::ShaderParam::PARAM_INT) + { + (*params)[spv.name] = intValue; + } + else if (paramType == rendering::ShaderParam::PARAM_FLOAT) + { + (*params)[spv.name] = floatValue; + } + else if (paramType == rendering::ShaderParam::PARAM_INT_BUFFER || + paramType == rendering::ShaderParam::PARAM_FLOAT_BUFFER) + { + (*params)[spv.name].InitializeBuffer(floatArrayValue.size()); + float *fv = &floatArrayValue[0]; + (*params)[spv.name].UpdateBuffer(fv); + } + } + this->shaderParams.clear(); + + // time variables need to be updated every iteration + for (const auto & spv : this->timeParams) + { + float floatValue = (std::chrono::duration_cast<std::chrono::nanoseconds>( + this->currentSimTime).count()) * 1e-9; + rendering::ShaderParamsPtr params; + if (spv.shader == "fragment") + params = this->material->FragmentShaderParams(); + else if (spv.shader == "vertex") + params = this->material->VertexShaderParams(); + (*params)[spv.name] = floatValue; + } +} + +IGNITION_ADD_PLUGIN(ShaderParam, + ignition::gazebo::System, + ShaderParam::ISystemConfigure, + ShaderParam::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(ShaderParam, + "ignition::gazebo::systems::ShaderParam") diff --git a/src/systems/shader_param/ShaderParam.hh b/src/systems/shader_param/ShaderParam.hh new file mode 100644 index 0000000000..d77e8bfd78 --- /dev/null +++ b/src/systems/shader_param/ShaderParam.hh @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2022 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 IGNITION_GAZEBO_SYSTEMS_SHADER_PARAM_HH_ +#define IGNITION_GAZEBO_SYSTEMS_SHADER_PARAM_HH_ + +#include <memory> + +#include "ignition/gazebo/System.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class ShaderParamPrivate; + + /// \brief A plugin for setting shaders to a visual and its params + /// + /// Plugin parameters: + /// + /// <shader> + /// <vertex> Path to vertex program + /// <fragment> Path to fragment program + /// <param> Shader parameter - can be repeated within plugin SDF element + /// <name> Name of uniform variable bound to the shader + /// <shader> Type of shader, i.e. vertex, fragment + /// <type> Variable type: float, int, float_array, int_array + /// <value> Value to set the shader parameter to. The vallue string can + /// be an int, float, or a space delimited array of ints or + /// floats. It can also be 'TIME', in which case the value will + /// be bound to sim time. + /// + /// Example usage: + /// + /// \verbatim + /// <plugin filename="ignition-gazebo-shader-param-system" + /// name="ignition::gazebo::systems::ShaderParam"> + /// <shader> + /// <vertex>materials/my_vs.glsl</vertex> + /// <fragment>materials/my_fs.glsl</fragment> + /// </shader> + /// <!-- Sets a fragment shader variable named "ambient" to red --> + /// <param> + /// <name>ambient</name> + /// <shader>fragment</shader> + /// <type>float_array</type> + /// <value>1.0 0.0 0.0 1.0</value> + /// </param> + /// </plugin> + /// \endverbatim + class ShaderParam + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: ShaderParam(); + + /// \brief Destructor + public: ~ShaderParam() override; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr<const sdf::Element> &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) final; + + /// Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr<ShaderParamPrivate> dataPtr; + }; + } +} +} +} + +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 6b712785a9..77197e73c3 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -76,6 +76,7 @@ set(tests_needing_display optical_tactile_plugin.cc rgbd_camera.cc sensors_system.cc + shader_param_system.cc thermal_system.cc thermal_sensor_system.cc ) diff --git a/test/integration/shader_param_system.cc b/test/integration/shader_param_system.cc new file mode 100644 index 0000000000..906133e937 --- /dev/null +++ b/test/integration/shader_param_system.cc @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2022 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 <gtest/gtest.h> + +#include <ignition/msgs/image.pb.h> + +#include <ignition/common/Console.hh> +#include <ignition/common/Util.hh> +#include <ignition/transport/Node.hh> +#include <ignition/utilities/ExtraTestMacros.hh> + +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Test ShaderParamTest system +class ShaderParamTest : public InternalFixture<::testing::Test> +{ +}; + +std::mutex mutex; +msgs::Image imageMsg; +unsigned char *imageBuffer = nullptr; + +///////////////////////////////////////////////// +void imageCb(const msgs::Image &_msg) +{ + mutex.lock(); + unsigned int channels = 3u; + unsigned int imageSamples = _msg.width() * _msg.height(); + unsigned int imageBufferSize = imageSamples * sizeof(unsigned char) + * channels; + + if (!imageBuffer) + imageBuffer = new unsigned char[imageSamples * channels]; + memcpy(imageBuffer, _msg.data().c_str(), imageBufferSize); + mutex.unlock(); +} + +///////////////////////////////////////////////// +// The test checks camera image data to verify that the sphere is using +// custom material shaders +TEST_F(ShaderParamTest, IGN_UTILS_TEST_DISABLED_ON_MAC(ShaderParam)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "examples", "worlds", "shader_param.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // subscribe to the image camera topic + transport::Node node; + node.Subscribe("/camera", &imageCb); + + // Run server and verify that we are receiving a message + // from the image camera + size_t iters100 = 100u; + server.Run(true, iters100, false); + + int sleep{0}; + int maxSleep{30}; + while (imageBuffer == nullptr && sleep < maxSleep) + { + std::this_thread::sleep_for(100ms); + sleep++; + } + EXPECT_LT(sleep, maxSleep); + ASSERT_NE(imageBuffer, nullptr); + + // shaders set the sphere color to red + unsigned int height = 320; + unsigned int width = 240; + + int mid = (height / 2 * width * 3u) + (width / 2 - 1) * 3u; + + // Lock access to buffer and don't release it + mutex.lock(); + int r = static_cast<int>(imageBuffer[mid]); + int g = static_cast<int>(imageBuffer[mid+1]); + int b = static_cast<int>(imageBuffer[mid+2]); + EXPECT_GT(r, g); + EXPECT_GT(r, b); + EXPECT_EQ(g, b); + + delete[] imageBuffer; +} diff --git a/test/plugins/CMakeLists.txt b/test/plugins/CMakeLists.txt index 19306775b4..554589094a 100644 --- a/test/plugins/CMakeLists.txt +++ b/test/plugins/CMakeLists.txt @@ -13,6 +13,7 @@ set (test_plugins EventTriggerSystem TestModelSystem TestSensorSystem + TestVisualSystem TestWorldSystem MockSystem Null diff --git a/test/plugins/TestVisualSystem.cc b/test/plugins/TestVisualSystem.cc new file mode 100644 index 0000000000..9cc3741117 --- /dev/null +++ b/test/plugins/TestVisualSystem.cc @@ -0,0 +1,23 @@ +/* + * Copyright (C) 2022 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 "TestVisualSystem.hh" + +#include <ignition/plugin/Register.hh> + +IGNITION_ADD_PLUGIN(ignition::gazebo::TestVisualSystem, + ignition::gazebo::System, + ignition::gazebo::TestVisualSystem::ISystemConfigure) diff --git a/test/plugins/TestVisualSystem.hh b/test/plugins/TestVisualSystem.hh new file mode 100644 index 0000000000..e8cdf4a45f --- /dev/null +++ b/test/plugins/TestVisualSystem.hh @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2022 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 IGNITION_GAZEBO_TEST_TESTVISUALSYSTEM_HH_ +#define IGNITION_GAZEBO_TEST_TESTVISUALSYSTEM_HH_ + +#include <ignition/gazebo/components/Component.hh> +#include <ignition/gazebo/components/Factory.hh> +#include <ignition/gazebo/System.hh> +#include <ignition/gazebo/config.hh> +#include <ignition/transport/Node.hh> + +namespace ignition +{ +namespace gazebo +{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ +using VisualPluginComponent = components::Component<int, + class VisualPluginComponentTag>; +IGN_GAZEBO_REGISTER_COMPONENT("VisualPluginComponent", + VisualPluginComponent) +} +} + +class TestVisualSystem : + public System, + public ISystemConfigure +{ + public: TestVisualSystem() + { + igndbg << "Constructing TestVisualSystem" << std::endl; + } + + public: ~TestVisualSystem() + { + igndbg << "Destroying TestVisualSystem" << std::endl; + } + + private: bool Service(msgs::StringMsg &_msg) + { + igndbg << "TestVisualSystem service called" << std::endl; + _msg.set_data("TestVisualSystem"); + return true; + } + + public: void Configure(const Entity &_entity, + const std::shared_ptr<const sdf::Element> &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventManager*/) override + { + igndbg << "Configuring TestVisualSystem" << std::endl; + auto value = _sdf->Get<int>("visual_key"); + _ecm.CreateComponent(_entity, + components::VisualPluginComponent(value)); + + // Create a test service + this->node.Advertise("/test/service/visual", + &TestVisualSystem::Service, this); + } + + private: transport::Node node; +}; +} +} + +#endif diff --git a/test/worlds/plugins.sdf b/test/worlds/plugins.sdf index 466215d186..d4668e294d 100644 --- a/test/worlds/plugins.sdf +++ b/test/worlds/plugins.sdf @@ -25,6 +25,13 @@ <sensor_key>456</sensor_key> </plugin> </sensor> + <visual name="visual"> + <plugin + filename="TestVisualSystem" + name="ignition::gazebo::TestVisualSystem"> + <sensor_key>890</sensor_key> + </plugin> + </visual> </link> </model> </world> From 986e30f411fba49f60111d708a7cf4cbf5c302a7 Mon Sep 17 00:00:00 2001 From: Ian Chen <ichen@osrfoundation.org> Date: Thu, 3 Feb 2022 17:41:53 -0800 Subject: [PATCH 08/17] Limit thruster system's input thrust cmd (#1318) * limit thrust cmd Signed-off-by: Ian Chen <ichen@osrfoundation.org> * check and error when max < min Signed-off-by: Ian Chen <ichen@osrfoundation.org> --- src/systems/thruster/Thruster.cc | 31 +++++++++++++++++++++++++++---- src/systems/thruster/Thruster.hh | 4 ++++ test/integration/thruster.cc | 22 ++++++++++++++++++++-- test/worlds/thruster_pid.sdf | 2 ++ test/worlds/thruster_vel_cmd.sdf | 2 ++ 5 files changed, 55 insertions(+), 6 deletions(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 6211434bc9..885f5ca209 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -79,12 +79,12 @@ class ignition::gazebo::systems::ThrusterPrivateData /// and writes the angular velocity directly to the joint. default: false public: bool velocityControl = false; - /// \brief Maximum input force [N] for the propellerController, default: 1000N - /// TODO(chapulina) Make it configurable from SDF. + /// \brief Maximum input force [N] for the propellerController, + /// default: 1000N public: double cmdMax = 1000; - /// \brief Minimum input force [N] for the propellerController, default: 1000N - /// TODO(chapulina) Make it configurable from SDF. + /// \brief Minimum input force [N] for the propellerController, + /// default: -1000N public: double cmdMin = -1000; /// \brief Thrust coefficient relating the propeller angular velocity to the @@ -206,6 +206,29 @@ void Thruster::Configure( enableComponent<components::WorldAngularVelocity>(_ecm, this->dataPtr->linkEntity); + double minThrustCmd = this->dataPtr->cmdMin; + double maxThrustCmd = this->dataPtr->cmdMax; + if (_sdf->HasElement("max_thrust_cmd")) + { + maxThrustCmd = _sdf->Get<double>("max_thrust_cmd"); + } + if (_sdf->HasElement("min_thrust_cmd")) + { + minThrustCmd = _sdf->Get<double>("min_thrust_cmd"); + } + if (maxThrustCmd < minThrustCmd) + { + ignerr << "<max_thrust_cmd> must be greater than or equal to " + << "<min_thrust_cmd>. Revert to using default values: " + << "min: " << this->dataPtr->cmdMin << ", " + << "max: " << this->dataPtr->cmdMax << std::endl; + } + else + { + this->dataPtr->cmdMax = maxThrustCmd; + this->dataPtr->cmdMin = minThrustCmd; + } + if (_sdf->HasElement("velocity_control")) { this->dataPtr->velocityControl = _sdf->Get<bool>("velocity_control"); diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index ec1a607d21..78eafb849b 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -64,6 +64,10 @@ namespace systems /// no units, defaults to 0.0] /// - <d_gain> - Derivative gain for joint PID controller. [Optional, /// no units, defaults to 0.0] + /// - <max_thrust_cmd> - Maximum thrust command. [Optional, + /// defaults to 1000N] + /// - <min_thrust_cmd> - Minimum thrust command. [Optional, + /// defaults to -1000N] /// /// ## Example /// An example configuration is installed with Gazebo. The example diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index af1ec159b8..d2333bc3f1 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -134,9 +134,24 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_LT(sleep, maxSleep); EXPECT_TRUE(pub.HasConnections()); - double force{300.0}; + // input force cmd - this should be capped to 0 + double forceCmd{-1000.0}; msgs::Double msg; - msg.set_data(force); + msg.set_data(forceCmd); + pub.Publish(msg); + + // Check no movement + fixture.Server()->Run(true, 100, false); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + EXPECT_DOUBLE_EQ(0.0, modelPoses.back().Pos().X()); + EXPECT_EQ(100u, modelPoses.size()); + EXPECT_EQ(100u, propellerAngVels.size()); + modelPoses.clear(); + propellerAngVels.clear(); + + // input force cmd this should be capped to 300 + forceCmd = 1000.0; + msg.set_data(forceCmd); pub.Publish(msg); // Check movement @@ -152,6 +167,9 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_EQ(100u * sleep, modelPoses.size()); EXPECT_EQ(100u * sleep, propellerAngVels.size()); + // max allowed force + double force{300.0}; + // F = m * a // s = a * t^2 / 2 // F = m * 2 * s / t^2 diff --git a/test/worlds/thruster_pid.sdf b/test/worlds/thruster_pid.sdf index c18becc7d0..540c73170b 100644 --- a/test/worlds/thruster_pid.sdf +++ b/test/worlds/thruster_pid.sdf @@ -104,6 +104,8 @@ <thrust_coefficient>0.004</thrust_coefficient> <fluid_density>1000</fluid_density> <propeller_diameter>0.2</propeller_diameter> + <max_thrust_cmd>300</max_thrust_cmd> + <min_thrust_cmd>0</min_thrust_cmd> </plugin> </model> diff --git a/test/worlds/thruster_vel_cmd.sdf b/test/worlds/thruster_vel_cmd.sdf index f78236a8ca..1850eac8f4 100644 --- a/test/worlds/thruster_vel_cmd.sdf +++ b/test/worlds/thruster_vel_cmd.sdf @@ -103,6 +103,8 @@ <fluid_density>950</fluid_density> <propeller_diameter>0.25</propeller_diameter> <velocity_control>true</velocity_control> + <max_thrust_cmd>300</max_thrust_cmd> + <min_thrust_cmd>0</min_thrust_cmd> </plugin> </model> From 756fa13630c1dc8cf11ec0e6f3f1ae82a77b6b50 Mon Sep 17 00:00:00 2001 From: Ian Chen <ichen@osrfoundation.org> Date: Fri, 4 Feb 2022 10:21:44 -0800 Subject: [PATCH 09/17] fix buoyancy test by increasing tol Signed-off-by: Ian Chen <ichen@osrfoundation.org> --- test/integration/buoyancy.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc index 9b2ca5d40d..b41f9e000b 100644 --- a/test/integration/buoyancy.cc +++ b/test/integration/buoyancy.cc @@ -156,7 +156,8 @@ TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RestoringMoments)) EXPECT_NEAR(maxUniformRoll, 0.11, 1e-1); EXPECT_NEAR(minUniformRoll, minGradedRoll, 1e-1); // Emperically derived - EXPECT_NEAR(minUniformRoll, -0.15, 1e-1); + // added extra tol (3e-5) after fixing center of volume's reference frame + EXPECT_NEAR(minUniformRoll, -0.15, 1e-1 + 3e-5); } ///////////////////////////////////////////////// From 1ef2c62889460ea464be970f3bb4006d28374aac Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty <arjo@openrobotics.org> Date: Mon, 7 Feb 2022 20:20:07 +0800 Subject: [PATCH 10/17] Fix weird indentation in `Link.hh` (#1324) There was some weird indentation going on in the `Link.hh` header file. This PR fixes it. Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org> --- include/ignition/gazebo/Link.hh | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/include/ignition/gazebo/Link.hh b/include/ignition/gazebo/Link.hh index 094567951b..944a013dbb 100644 --- a/include/ignition/gazebo/Link.hh +++ b/include/ignition/gazebo/Link.hh @@ -220,13 +220,13 @@ namespace ignition public: void EnableVelocityChecks(EntityComponentManager &_ecm, bool _enable = true) const; - /// \brief Get the angular acceleration of the body in the world frame. - /// \param[in] _ecm Entity-component manager. - /// \return Angular acceleration of the body in the world frame or - /// nullopt if acceleration checks aren't enabled. - /// \sa EnableAccelerationChecks - public: std::optional<math::Vector3d> WorldAngularAcceleration( - const EntityComponentManager &_ecm) const; + /// \brief Get the angular acceleration of the body in the world frame. + /// \param[in] _ecm Entity-component manager. + /// \return Angular acceleration of the body in the world frame or + /// nullopt if acceleration checks aren't enabled. + /// \sa EnableAccelerationChecks + public: std::optional<math::Vector3d> WorldAngularAcceleration( + const EntityComponentManager &_ecm) const; /// \brief Get the linear acceleration of the body in the world frame. /// \param[in] _ecm Entity-component manager. From 83f2f74c5e3da3b70fa0422192216b05d8d4814e Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty <arjo@openrobotics.org> Date: Tue, 8 Feb 2022 10:27:29 +0800 Subject: [PATCH 11/17] Adds a `Link::SetLinearVelocity()` method (#1323) This PR adds a Link::SetLinearVelocity() method. I foresee this method being useful for testing behaviour of systems like the hydrodynamics or liftDrag plugin which are dependent on velocity for their output forces. Signed-off-by: Arjo Chakravarty arjo@openrobotics.org --- include/ignition/gazebo/Link.hh | 7 +++++++ src/Link.cc | 20 +++++++++++++++++++ test/integration/link.cc | 34 +++++++++++++++++++++++++++++++++ 3 files changed, 61 insertions(+) diff --git a/include/ignition/gazebo/Link.hh b/include/ignition/gazebo/Link.hh index 944a013dbb..9dd3d5ccc7 100644 --- a/include/ignition/gazebo/Link.hh +++ b/include/ignition/gazebo/Link.hh @@ -220,6 +220,13 @@ namespace ignition public: void EnableVelocityChecks(EntityComponentManager &_ecm, bool _enable = true) const; + /// \brief Set the linear velocity on this link. If this is set, wrenches + /// on this link will be ignored for the current time step. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _vel Linear velocity to set in Link's Frame. + public: void SetLinearVelocity(EntityComponentManager &_ecm, + const math::Vector3d &_vel) const; + /// \brief Get the angular acceleration of the body in the world frame. /// \param[in] _ecm Entity-component manager. /// \return Angular acceleration of the body in the world frame or diff --git a/src/Link.cc b/src/Link.cc index 855adf4352..0d07f17140 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -26,6 +26,7 @@ #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/LinearAcceleration.hh" #include "ignition/gazebo/components/LinearVelocity.hh" +#include "ignition/gazebo/components/LinearVelocityCmd.hh" #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" @@ -246,6 +247,25 @@ void Link::EnableVelocityChecks(EntityComponentManager &_ecm, bool _enable) _enable); } +////////////////////////////////////////////////// +void Link::SetLinearVelocity(EntityComponentManager &_ecm, + const math::Vector3d &_vel) const +{ + auto vel = + _ecm.Component<components::LinearVelocityCmd>(this->dataPtr->id); + + if (vel == nullptr) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LinearVelocityCmd(_vel)); + } + else + { + vel->Data() = _vel; + } +} + ////////////////////////////////////////////////// std::optional<math::Vector3d> Link::WorldAngularAcceleration( const EntityComponentManager &_ecm) const diff --git a/test/integration/link.cc b/test/integration/link.cc index da64000838..53a1dc9f7c 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -29,6 +29,7 @@ #include <ignition/gazebo/components/Joint.hh> #include <ignition/gazebo/components/LinearAcceleration.hh> #include <ignition/gazebo/components/LinearVelocity.hh> +#include <ignition/gazebo/components/LinearVelocityCmd.hh> #include <ignition/gazebo/components/Link.hh> #include <ignition/gazebo/components/Model.hh> #include <ignition/gazebo/components/Name.hh> @@ -468,6 +469,39 @@ TEST_F(LinkIntegrationTest, LinkKineticEnergy) EXPECT_DOUBLE_EQ(100.0, *link.WorldKineticEnergy(ecm)); } +////////////////////////////////////////////////// +TEST_F(LinkIntegrationTest, LinkSetVelocity) +{ + EntityComponentManager ecm; + EventManager eventMgr; + SdfEntityCreator creator(ecm, eventMgr); + + auto eLink = ecm.CreateEntity(); + ecm.CreateComponent(eLink, components::Link()); + + Link link(eLink); + EXPECT_EQ(eLink, link.Entity()); + + ASSERT_TRUE(link.Valid(ecm)); + + // No LinearVelocityCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component<components::LinearVelocityCmd>(eLink)); + + math::Vector3d linVel(1, 0, 0); + link.SetLinearVelocity(ecm, linVel); + + // LinearVelocity cmd should exist + EXPECT_NE(nullptr, ecm.Component<components::LinearVelocityCmd>(eLink)); + EXPECT_EQ(linVel, + ecm.Component<components::LinearVelocityCmd>(eLink)->Data()); + + // Make sure the linear velocity is updated + math::Vector3d linVel2(0, 0, 0); + link.SetLinearVelocity(ecm, linVel2); + EXPECT_EQ(linVel2, + ecm.Component<components::LinearVelocityCmd>(eLink)->Data()); +} + ////////////////////////////////////////////////// TEST_F(LinkIntegrationTest, LinkAddWorldForce) { From 77dd8551e942c6f2a9dfbd6360a51cab9185ebab Mon Sep 17 00:00:00 2001 From: Ian Chen <ichen@osrfoundation.org> Date: Wed, 9 Feb 2022 13:42:39 -0800 Subject: [PATCH 12/17] Extend ShaderParam system to support textures (#1310) updates the ShaderParam system with the ability to parse texture params and pass them to custom shaders Signed-off-by: Ian Chen <ichen@osrfoundation.org> --- examples/worlds/shader_param.sdf | 40 ++--- src/systems/shader_param/ShaderParam.cc | 198 +++++++++++++++--------- 2 files changed, 131 insertions(+), 107 deletions(-) diff --git a/examples/worlds/shader_param.sdf b/examples/worlds/shader_param.sdf index db3d48ca78..1aab09d7d4 100644 --- a/examples/worlds/shader_param.sdf +++ b/examples/worlds/shader_param.sdf @@ -167,6 +167,8 @@ ShaderParam visual plugin over time. <scene> <ambient>1.0 1.0 1.0</ambient> <background>0.8 0.8 0.8</background> + <sky></sky> + <grid>false</grid> </scene> <light type="directional" name="sun"> @@ -177,43 +179,21 @@ ShaderParam visual plugin over time. <direction>-0.5 0.1 -0.9</direction> </light> - <model name="ground_plane"> - <static>true</static> - <link name="link"> - <collision name="collision"> - <geometry> - <plane> - <normal>0 0 1</normal> - <size>100 100</size> - </plane> - </geometry> - </collision> - <visual name="visual"> - <geometry> - <plane> - <normal>0 0 1</normal> - <size>100 100</size> - </plane> - </geometry> - <material> - <ambient>0.8 0.8 0.8 1</ambient> - <diffuse>0.8 0.8 0.8 1</diffuse> - <specular>0.8 0.8 0.8 1</specular> - </material> - </visual> - </link> - </model> - <include> - <pose>0 0.0 0.5 0 0 0</pose> <name>deformable_sphere</name> - <pose>3 0 0 0 0 0</pose> + <pose>0 0 1.5 0 0 0</pose> <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/deformable_sphere</uri> </include> + <include> + <name>waves</name> + <pose>0 0 0 0 0 0</pose> + <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/waves</uri> + </include> + <model name="camera"> <static>true</static> - <pose>2.5 0 0.5 0 0.0 3.14</pose> + <pose>2.5 0 1.5 0 0.0 3.14</pose> <link name="link"> <pose>0.05 0.05 0.05 0 0 0</pose> <collision name="collision"> diff --git a/src/systems/shader_param/ShaderParam.cc b/src/systems/shader_param/ShaderParam.cc index feab7709b2..973a7cac08 100644 --- a/src/systems/shader_param/ShaderParam.cc +++ b/src/systems/shader_param/ShaderParam.cc @@ -51,8 +51,8 @@ class ignition::gazebo::systems::ShaderParamPrivate /// \brief shader type: vertex or fragment public: std::string shader; - /// \brief variable type: int, float, float_array, int_array - /// \todo(anyone) support samplers + /// \brief variable type: int, float, float_array, int_array, + /// texture, texture_cube public: std::string type; /// \brief variable name of param @@ -60,6 +60,9 @@ class ignition::gazebo::systems::ShaderParamPrivate /// \brief param value public: std::string value; + + /// \brief Any additional arguments + public: std::vector<std::string> args; }; /// \brief Path to vertex shader @@ -98,6 +101,9 @@ class ignition::gazebo::systems::ShaderParamPrivate /// \brief Current sim time public: std::chrono::steady_clock::duration currentSimTime; + /// \brief Path to model + public: std::string modelPath; + /// \brief All rendering operations must happen within this call public: void OnUpdate(); }; @@ -149,11 +155,29 @@ void ShaderParam::Configure(const Entity &_entity, spv.name = paramName; spv.value = value; spv.type = type; + + if (paramElem->HasElement("arg")) + { + sdf::ElementPtr argElem = paramElem->GetElement("arg"); + while (argElem) + { + spv.args.push_back(argElem->Get<std::string>()); + argElem = argElem->GetNextElement("arg"); + } + } + this->dataPtr->shaderParams.push_back(spv); paramElem = paramElem->GetNextElement("param"); } } + if (this->dataPtr->modelPath.empty()) + { + auto modelEntity = topLevelModel(_entity, _ecm); + this->dataPtr->modelPath = + _ecm.ComponentData<components::SourceFilePath>(modelEntity).value(); + } + // parse path to shaders if (sdf->HasElement("shader")) { @@ -166,15 +190,14 @@ void ShaderParam::Configure(const Entity &_entity, } else { - auto modelEntity = topLevelModel(_entity, _ecm); - auto modelPath = - _ecm.ComponentData<components::SourceFilePath>(modelEntity); sdf::ElementPtr vertexElem = shaderElem->GetElement("vertex"); this->dataPtr->vertexShaderUri = common::findFile( - asFullPath(vertexElem->Get<std::string>(), modelPath.value())); + asFullPath(vertexElem->Get<std::string>(), + this->dataPtr->modelPath)); sdf::ElementPtr fragmentElem = shaderElem->GetElement("fragment"); this->dataPtr->fragmentShaderUri = common::findFile( - asFullPath(fragmentElem->Get<std::string>(), modelPath.value())); + asFullPath(fragmentElem->Get<std::string>(), + this->dataPtr->modelPath)); } } @@ -263,14 +286,12 @@ void ShaderParamPrivate::OnUpdate() // this is only done once for (const auto & spv : this->shaderParams) { - std::vector<std::string> values = common::split(spv.value, " "); - - int intValue = 0; - float floatValue = 0; - std::vector<float> floatArrayValue; - - rendering::ShaderParam::ParamType paramType = - rendering::ShaderParam::PARAM_NONE; + // TIME is reserved keyword for sim time + if (spv.value == "TIME") + { + this->timeParams.push_back(spv); + continue; + } rendering::ShaderParamsPtr params; if (spv.shader == "fragment") @@ -283,92 +304,115 @@ void ShaderParamPrivate::OnUpdate() } // if no <value> is specified, this could be a constant - if (values.empty()) + if (spv.value.empty()) + { + // \todo handle args for constants in ign-rendering + (*params)[spv.name] = 1; + continue; + } + + // handle texture params + if (spv.type == "texture") { - // \todo handle args for constants - (*params)[spv.name] = intValue; + unsigned int uvSetIndex = spv.args.empty() ? 0u : + static_cast<unsigned int>(std::stoul(spv.args[0])); + std::string texPath = common::findFile( + asFullPath(spv.value, this->modelPath)); + (*params)[spv.name].SetTexture(texPath, + rendering::ShaderParam::ParamType::PARAM_TEXTURE, uvSetIndex); } - // float / int - else if (values.size() == 1u) + else if (spv.type == "texture_cube") + { + unsigned int uvSetIndex = spv.args.empty() ? 0u : + static_cast<unsigned int>(std::stoul(spv.args[0])); + std::string texPath = common::findFile( + asFullPath(spv.value, this->modelPath)); + (*params)[spv.name].SetTexture(texPath, + rendering::ShaderParam::ParamType::PARAM_TEXTURE_CUBE, uvSetIndex); + } + // handle int, float, int_array, and float_array params + else { - std::string str = values[0]; + std::vector<std::string> values = common::split(spv.value, " "); - // TIME is reserved keyword for sim time - if (str == "TIME") - { - this->timeParams.push_back(spv); - continue; - } + int intValue = 0; + float floatValue = 0; + std::vector<float> floatArrayValue; - // if <type> is not empty, respect the specified type - if (!spv.type.empty()) + rendering::ShaderParam::ParamType paramType = + rendering::ShaderParam::PARAM_NONE; + + // float / int + if (values.size() == 1u) { - if (spv.type == "int") - { - intValue = std::stoi(str); - paramType = rendering::ShaderParam::PARAM_INT; - } - else if (spv.type == "float") + std::string str = values[0]; + + // if <type> is not empty, respect the specified type + if (!spv.type.empty()) { - floatValue = std::stof(str); - paramType = rendering::ShaderParam::PARAM_FLOAT; + if (spv.type == "int") + { + intValue = std::stoi(str); + paramType = rendering::ShaderParam::PARAM_INT; + } + else if (spv.type == "float") + { + floatValue = std::stof(str); + paramType = rendering::ShaderParam::PARAM_FLOAT; + } } + // else do our best guess at what the type is else { - // \todo(anyone) support texture samplers + std::string::size_type sz; + int n = std::stoi(str, &sz); + if ( sz == str.size()) + { + intValue = n; + paramType = rendering::ShaderParam::PARAM_INT; + } + else + { + floatValue = std::stof(str); + paramType = rendering::ShaderParam::PARAM_FLOAT; + } } } - // else do our best guess at what the type is + // arrays else { - std::string::size_type sz; - int n = std::stoi(str, &sz); - if ( sz == str.size()) + // int array + if (!spv.type.empty() && spv.type == "int_array") { - intValue = n; - paramType = rendering::ShaderParam::PARAM_INT; + for (const auto &v : values) + floatArrayValue.push_back(std::stoi(v)); + paramType = rendering::ShaderParam::PARAM_INT_BUFFER; } + // treat everything else as float_array else { - floatValue = std::stof(str); - paramType = rendering::ShaderParam::PARAM_FLOAT; + for (const auto &v : values) + floatArrayValue.push_back(std::stof(v)); + paramType = rendering::ShaderParam::PARAM_FLOAT_BUFFER; } } - } - // arrays - else - { - // int array - if (!spv.type.empty() && spv.type == "int_array") + + // set the params + if (paramType == rendering::ShaderParam::PARAM_INT) { - for (const auto &v : values) - floatArrayValue.push_back(std::stoi(v)); - paramType = rendering::ShaderParam::PARAM_INT_BUFFER; + (*params)[spv.name] = intValue; } - // treat everything else as float_array - else + else if (paramType == rendering::ShaderParam::PARAM_FLOAT) { - for (const auto &v : values) - floatArrayValue.push_back(std::stof(v)); - paramType = rendering::ShaderParam::PARAM_FLOAT_BUFFER; + (*params)[spv.name] = floatValue; + } + else if (paramType == rendering::ShaderParam::PARAM_INT_BUFFER || + paramType == rendering::ShaderParam::PARAM_FLOAT_BUFFER) + { + (*params)[spv.name].InitializeBuffer(floatArrayValue.size()); + float *fv = &floatArrayValue[0]; + (*params)[spv.name].UpdateBuffer(fv); } - } - - // set the params - if (paramType == rendering::ShaderParam::PARAM_INT) - { - (*params)[spv.name] = intValue; - } - else if (paramType == rendering::ShaderParam::PARAM_FLOAT) - { - (*params)[spv.name] = floatValue; - } - else if (paramType == rendering::ShaderParam::PARAM_INT_BUFFER || - paramType == rendering::ShaderParam::PARAM_FLOAT_BUFFER) - { - (*params)[spv.name].InitializeBuffer(floatArrayValue.size()); - float *fv = &floatArrayValue[0]; - (*params)[spv.name].UpdateBuffer(fv); } } this->shaderParams.clear(); From 49d0673f318fd1d29dc1a75e9be5de2f829217ab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= <caguero@openrobotics.org> Date: Sun, 13 Feb 2022 19:03:49 +0100 Subject: [PATCH 13/17] New trajectory follower system (#1332) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * New trajectory follower system Signed-off-by: Carlos Agüero <caguero@openrobotics.org> Signed-off-by: Ian Chen <ichen@osrfoundation.org> --- examples/worlds/shader_param.sdf | 1 - examples/worlds/trajectory_follower.sdf | 220 +++++++++++ src/systems/CMakeLists.txt | 1 + .../trajectory_follower/CMakeLists.txt | 6 + .../trajectory_follower/TrajectoryFollower.cc | 352 ++++++++++++++++++ .../trajectory_follower/TrajectoryFollower.hh | 152 ++++++++ 6 files changed, 731 insertions(+), 1 deletion(-) create mode 100644 examples/worlds/trajectory_follower.sdf create mode 100644 src/systems/trajectory_follower/CMakeLists.txt create mode 100644 src/systems/trajectory_follower/TrajectoryFollower.cc create mode 100644 src/systems/trajectory_follower/TrajectoryFollower.hh diff --git a/examples/worlds/shader_param.sdf b/examples/worlds/shader_param.sdf index 1aab09d7d4..25e8b12f14 100644 --- a/examples/worlds/shader_param.sdf +++ b/examples/worlds/shader_param.sdf @@ -184,7 +184,6 @@ ShaderParam visual plugin over time. <pose>0 0 1.5 0 0 0</pose> <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/deformable_sphere</uri> </include> - <include> <name>waves</name> <pose>0 0 0 0 0 0</pose> diff --git a/examples/worlds/trajectory_follower.sdf b/examples/worlds/trajectory_follower.sdf new file mode 100644 index 0000000000..128d0d1442 --- /dev/null +++ b/examples/worlds/trajectory_follower.sdf @@ -0,0 +1,220 @@ +<?xml version="1.0" ?> +<!-- + Ignition Gazebo trajectory follower plugin demo. +--> +<sdf version="1.6"> + <world name="trajectory_follower"> + + <physics name="1ms" type="ignored"> + <max_step_size>0.001</max_step_size> + <real_time_factor>1.0</real_time_factor> + </physics> + <plugin + filename="ignition-gazebo-physics-system" + name="ignition::gazebo::systems::Physics"> + </plugin> + <plugin + filename="ignition-gazebo-user-commands-system" + name="ignition::gazebo::systems::UserCommands"> + </plugin> + <plugin + filename="ignition-gazebo-scene-broadcaster-system" + name="ignition::gazebo::systems::SceneBroadcaster"> + </plugin> + + <light type="directional" name="sun"> + <cast_shadows>true</cast_shadows> + <pose>0 0 10 0 0 0</pose> + <diffuse>1 1 1 1</diffuse> + <specular>0.5 0.5 0.5 1</specular> + <attenuation> + <range>1000</range> + <constant>0.9</constant> + <linear>0.01</linear> + <quadratic>0.001</quadratic> + </attenuation> + <direction>-0.5 0.1 -0.9</direction> + </light> + + <model name="ground_plane"> + <static>true</static> + <link name="link"> + <collision name="collision"> + <geometry> + <plane> + <normal>0 0 1</normal> + <size>100 100</size> + </plane> + </geometry> + </collision> + <visual name="visual"> + <geometry> + <plane> + <normal>0 0 1</normal> + <size>100 100</size> + </plane> + </geometry> + <material> + <ambient>0.8 0.8 0.8 1</ambient> + <diffuse>0.8 0.8 0.8 1</diffuse> + <specular>0.8 0.8 0.8 1</specular> + </material> + </visual> + </link> + </model> + + <model name="box"> + <pose>2 0 0.5 0 0 0</pose> + <link name="box_link"> + <inertial> + <inertia> + <ixx>0.16666</ixx> + <ixy>0</ixy> + <ixz>0</ixz> + <iyy>0.16666</iyy> + <iyz>0</iyz> + <izz>0.16666</izz> + </inertia> + <mass>1.0</mass> + </inertial> + <collision name="box_collision"> + <geometry> + <box> + <size>1 1 1</size> + </box> + </geometry> + </collision> + + <visual name="box_visual"> + <geometry> + <box> + <size>1 1 1</size> + </box> + </geometry> + <material> + <ambient>1 0 0 1</ambient> + <diffuse>1 0 0 1</diffuse> + <specular>1 0 0 1</specular> + </material> + </visual> + </link> + + <plugin + filename="ignition-gazebo-trajectory-follower-system" + name="ignition::gazebo::systems::TrajectoryFollower"> + <link_name>box_link</link_name> + <loop>true</loop> + <force>10</force> + <torque>10</torque> + <waypoints> + <waypoint>2 0</waypoint> + <waypoint>7 0</waypoint> + </waypoints> + </plugin> + + </model> + + <model name="box_2"> + <pose>0 2 0.5 0 0 1.5708</pose> + <link name="box_link"> + <inertial> + <inertia> + <ixx>0.16666</ixx> + <ixy>0</ixy> + <ixz>0</ixz> + <iyy>0.16666</iyy> + <iyz>0</iyz> + <izz>0.16666</izz> + </inertia> + <mass>1.0</mass> + </inertial> + <collision name="box_collision"> + <geometry> + <box> + <size>1 1 1</size> + </box> + </geometry> + </collision> + + <visual name="box_visual"> + <geometry> + <box> + <size>1 1 1</size> + </box> + </geometry> + <material> + <ambient>1 0 0 1</ambient> + <diffuse>1 0 0 1</diffuse> + <specular>1 0 0 1</specular> + </material> + </visual> + </link> + + <plugin + filename="ignition-gazebo-trajectory-follower-system" + name="ignition::gazebo::systems::TrajectoryFollower"> + <link_name>box_link</link_name> + <loop>true</loop> + <force>10</force> + <torque>10</torque> + <line> + <direction>0</direction> + <length>5</length> + </line> + </plugin> + + </model> + + <model name="box_3"> + <pose>0 -2 0.5 0 0 -1.5708</pose> + <link name="box_link"> + <inertial> + <inertia> + <ixx>0.16666</ixx> + <ixy>0</ixy> + <ixz>0</ixz> + <iyy>0.16666</iyy> + <iyz>0</iyz> + <izz>0.16666</izz> + </inertia> + <mass>1.0</mass> + </inertial> + <collision name="box_collision"> + <geometry> + <box> + <size>1 1 1</size> + </box> + </geometry> + </collision> + + <visual name="box_visual"> + <geometry> + <box> + <size>1 1 1</size> + </box> + </geometry> + <material> + <ambient>1 0 0 1</ambient> + <diffuse>1 0 0 1</diffuse> + <specular>1 0 0 1</specular> + </material> + </visual> + </link> + + <plugin + filename="ignition-gazebo-trajectory-follower-system" + name="ignition::gazebo::systems::TrajectoryFollower"> + <link_name>box_link</link_name> + <loop>true</loop> + <force>10</force> + <torque>10</torque> + <line> + <direction>0</direction> + <length>5</length> + </line> + </plugin> + + </model> + + </world> +</sdf> diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 5054dad438..78ef2dc886 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -134,6 +134,7 @@ add_subdirectory(thruster) add_subdirectory(touch_plugin) add_subdirectory(track_controller) add_subdirectory(tracked_vehicle) +add_subdirectory(trajectory_follower) add_subdirectory(triggered_publisher) add_subdirectory(user_commands) add_subdirectory(velocity_control) diff --git a/src/systems/trajectory_follower/CMakeLists.txt b/src/systems/trajectory_follower/CMakeLists.txt new file mode 100644 index 0000000000..419aa55429 --- /dev/null +++ b/src/systems/trajectory_follower/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(trajectory-follower + SOURCES + TrajectoryFollower.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) diff --git a/src/systems/trajectory_follower/TrajectoryFollower.cc b/src/systems/trajectory_follower/TrajectoryFollower.cc new file mode 100644 index 0000000000..7ea2ccd6bb --- /dev/null +++ b/src/systems/trajectory_follower/TrajectoryFollower.cc @@ -0,0 +1,352 @@ +/* + * Copyright (C) 2022 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 <string> +#include <vector> + +#include <ignition/common/Profiler.hh> +#include <ignition/math/Angle.hh> +#include <ignition/math/Helpers.hh> +#include <ignition/math/Pose3.hh> +#include <ignition/math/Vector2.hh> +#include <ignition/math/Vector3.hh> +#include <ignition/plugin/Register.hh> +#include <sdf/sdf.hh> + +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Link.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" + +#include "TrajectoryFollower.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::TrajectoryFollowerPrivate +{ + /// \brief Initialize the plugin. + /// \param[in] _ecm Immutable reference to the EntityComponentManager. + /// \param[in] _sdf The SDF Element associated with this system plugin. + public: void Load(const EntityComponentManager &_ecm, + const sdf::ElementPtr &_sdf); + + /// \brief The link entity + public: ignition::gazebo::Link link; + + /// \brief Model interface + public: Model model{kNullEntity}; + + /// \brief The initial pose of the model relative to the world frame. + public: ignition::math::Pose3<double> modelPose; + + /// \brief True if the model should continue looping though the waypoints. + public: bool loopForever = false; + + /// \brief Linear force to apply to the model in its X direction. + public: double forceToApply = 60; + + /// \brief Torque to apply to the model to align it with the next goal. + public: double torqueToApply = 50; + + /// \brief When the model is at this distance or closer we won't try to move. + /// Units are in meters. + public: double rangeTolerance = 0.5; + + /// \brief When the model is at this angle or closer we won't try to rotate. + /// Units are in degrees. + public: double bearingTolerance = 2.0; + + /// \brief Use to sample waypoints around a circle. + public: unsigned int numSamples = 8u; + + /// \brief The next position to reach. + public: ignition::math::Vector3d nextGoal; + + /// \brief Vector containing waypoints as 3D vectors of doubles representing + /// X Y, where X and Y are local (Gazebo) coordinates. + public: std::vector<ignition::math::Vector2d> localWaypoints; + + /// \brief Initialization flag. + public: bool initialized{false}; + + /// \brief Copy of the sdf configuration used for this plugin + public: sdf::ElementPtr sdfConfig; +}; + +////////////////////////////////////////////////// +void TrajectoryFollowerPrivate::Load(const EntityComponentManager &_ecm, + const sdf::ElementPtr &_sdf) +{ + // Parse required elements. + if (!_sdf->HasElement("link_name")) + { + ignerr << "No <link_name> specified" << std::endl; + return; + } + + std::string linkName = _sdf->Get<std::string>("link_name"); + this->link = Link(this->model.LinkByName(_ecm, linkName)); + if (!this->link.Valid(_ecm)) + { + ignerr << "Could not find link named [" << linkName + << "] in model" << std::endl; + return; + } + + this->modelPose = ignition::gazebo::worldPose(this->link.Entity(), _ecm); + this->modelPose.Pos().Z() = 0; + + // Parse the optional <waypoints> element. + if (_sdf->HasElement("waypoints")) + { + auto waypointsElem = _sdf->GetElement("waypoints"); + + // We need at least one waypoint + if (!waypointsElem->HasElement("waypoint")) + { + ignerr << "TrajectoryFollower: Unable to find <waypoints><waypoint> " + << "element in SDF." << std::endl; + return; + } + auto waypointElem = waypointsElem->GetElement("waypoint"); + while (waypointElem) + { + ignition::math::Vector2d position = + waypointElem->Get<ignition::math::Vector2d>(); + + // Save the position. + this->localWaypoints.push_back(position); + + // Print some debugging messages + igndbg << "Waypoint, Local: X = " << position.X() + << " Y = " << position.Y() << std::endl; + + waypointElem = waypointElem->GetNextElement("waypoint"); + } + } + // If no waypoints present, check for the <circle> element and parse. + else if (_sdf->HasElement("circle")) + { + igndbg << "Circle element activated" << std::endl; + auto circleElem = _sdf->GetElement("circle"); + + if (!circleElem->HasElement("radius")) + { + ignerr << "No <circle><radius> specified" << std::endl; + return; + } + + // Parse the required <radius> field. + double radius = circleElem->Get<double>("radius"); + + // Get the current model position in global coordinates. Create + // local vectors that represent a path along a rough circle. + ignition::math::Vector2d position(this->modelPose.Pos().X(), + this->modelPose.Pos().Y()); + double angle = 0; + ignition::math::Vector2d vec(radius, 0); + for (unsigned int i = 0u; i < this->numSamples; ++i) + { + // Add the local vector to the current position. + // Store global position as a waypoint. + this->localWaypoints.push_back(position + vec); + angle += 2 * IGN_PI / this->numSamples; + vec.Set(radius * cos(angle), radius * sin(angle)); + igndbg << "Entered circle waypoint " << position + vec << std::endl; + } + } + // If no waypoints or circle, check for the <line> element and parse. + else if (_sdf->HasElement("line")) + { + auto lineElem = _sdf->GetElement("line"); + // Parse the required <direction> field. + if (!lineElem->HasElement("direction")) + { + ignerr << "No <line><direction> specified" << std::endl; + return; + } + ignition::math::Angle direction = + lineElem->Get<ignition::math::Angle>("direction"); + + // Parse the required <length> field. + if (!lineElem->HasElement("length")) + { + ignerr << "No <line><length> specified" << std::endl; + return; + } + auto length = lineElem->Get<double>("length"); + + // Create a relative vector in the direction of "direction" and of + // length "length". + ignition::math::Vector3d lineVec( + length * cos(direction.Radian()), + length * sin(direction.Radian()), 0); + ignition::math::Vector2d position(this->modelPose.Pos().X(), + this->modelPose.Pos().Y()); + // Add the initial model position and calculated endpoint as waypoints. + this->localWaypoints.push_back(position); + ignition::math::Vector3d p = this->modelPose.CoordPositionAdd(lineVec); + ignition::math::Vector2d p2D = {p.X(), p.Y()}; + this->localWaypoints.push_back(p2D); + igndbg << "Entered line waypoints " << position << ", " << p2D << std::endl; + } + + // Parse the optional <loop> element. + if (_sdf->HasElement("loop")) + this->loopForever = _sdf->Get<bool>("loop"); + + // Parse the optional <force> element. + if (_sdf->HasElement("force")) + this->forceToApply = _sdf->Get<double>("force"); + + // Parse the optional <torque> element. + if (_sdf->HasElement("torque")) + this->torqueToApply = _sdf->Get<double>("torque"); + + // Parse the optional <range_tolerance> element. + if (_sdf->HasElement("range_tolerance")) + this->rangeTolerance = _sdf->Get<double>("range_tolerance"); + + // Parse the optional <bearing_tolerance> element. + if (_sdf->HasElement("bearing_tolerance")) + this->bearingTolerance = _sdf->Get<double>("bearing_tolerance"); + + // If we have waypoints to visit, read the first one. + if (!this->localWaypoints.empty()) + { + this->nextGoal = + {this->localWaypoints.front().X(), this->localWaypoints.front().Y(), 0}; + } +} + +////////////////////////////////////////////////// +TrajectoryFollower::TrajectoryFollower() + : dataPtr(std::make_unique<TrajectoryFollowerPrivate>()) +{ +} + +////////////////////////////////////////////////// +void TrajectoryFollower::Configure(const Entity &_entity, + const std::shared_ptr<const sdf::Element> &_sdf, + EntityComponentManager &/*_ecm*/, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->model = Model(_entity); + this->dataPtr->sdfConfig = _sdf->Clone(); +} + +////////////////////////////////////////////////// +void TrajectoryFollower::PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("TrajectoryFollower::PreUpdate"); + + if (_info.paused) + return; + + if (!this->dataPtr->initialized) + { + // We call Load here instead of Configure because we can't be guaranteed + // that all entities have been created when Configure is called + this->dataPtr->Load(_ecm, this->dataPtr->sdfConfig); + this->dataPtr->initialized = true; + } + + // Nothing to do. + if (this->dataPtr->localWaypoints.empty()) + return; + + this->dataPtr->modelPose = ignition::gazebo::worldPose( + this->dataPtr->link.Entity(), _ecm); + this->dataPtr->modelPose.Pos().Z() = 0; + + // Direction vector to the goal from the model. + ignition::math::Vector3d direction = + this->dataPtr->nextGoal - this->dataPtr->modelPose.Pos(); + + // Direction vector in the local frame of the model. + ignition::math::Vector3d directionLocalFrame = + this->dataPtr->modelPose.Rot().RotateVectorReverse(direction); + + double range = directionLocalFrame.Length(); + ignition::math::Angle bearing( + atan2(directionLocalFrame.Y(), directionLocalFrame.X())); + bearing.Normalize(); + + // Waypoint reached! + if (range <= this->dataPtr->rangeTolerance) + { + // We always keep the last waypoint in the vector to keep the model + // "alive" in case it moves away from its goal. + if (this->dataPtr->localWaypoints.size() == 1) + return; + + if (this->dataPtr->loopForever) + { + // Rotate to the left. + std::rotate(this->dataPtr->localWaypoints.begin(), + this->dataPtr->localWaypoints.begin() + 1, + this->dataPtr->localWaypoints.end()); + } + else + { + // Remove the first waypoint. + this->dataPtr->localWaypoints.erase( + this->dataPtr->localWaypoints.begin()); + } + + this->dataPtr->nextGoal = { + this->dataPtr->localWaypoints.front().X(), + this->dataPtr->localWaypoints.front().Y(), 0}; + + return; + } + + // Transform from world to local frame. + auto comPose = this->dataPtr->link.WorldInertialPose(_ecm); + + // Transform the force and torque to the world frame. + // Move commands. The vehicle always move forward (X direction). + ignition::math::Vector3d forceWorld; + if (std::abs(bearing.Degree()) <= this->dataPtr->bearingTolerance) + { + forceWorld = (*comPose).Rot().RotateVector( + ignition::math::Vector3d(this->dataPtr->forceToApply, 0, 0)); + } + + ignition::math::Vector3d torqueWorld; + if (std::abs(bearing.Degree()) > this->dataPtr->bearingTolerance) + { + int sign = std::abs(bearing.Degree()) / bearing.Degree(); + torqueWorld = (*comPose).Rot().RotateVector( + ignition::math::Vector3d(0, 0, sign * this->dataPtr->torqueToApply)); + } + + // Apply the force and torque at COM. + this->dataPtr->link.AddWorldWrench(_ecm, forceWorld, torqueWorld); +} + +IGNITION_ADD_PLUGIN(TrajectoryFollower, + ignition::gazebo::System, + TrajectoryFollower::ISystemConfigure, + TrajectoryFollower::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(TrajectoryFollower, + "ignition::gazebo::systems::TrajectoryFollower") diff --git a/src/systems/trajectory_follower/TrajectoryFollower.hh b/src/systems/trajectory_follower/TrajectoryFollower.hh new file mode 100644 index 0000000000..c1d52aca06 --- /dev/null +++ b/src/systems/trajectory_follower/TrajectoryFollower.hh @@ -0,0 +1,152 @@ +/* + * Copyright (C) 2022 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 IGNITION_GAZEBO_SYSTEMS_TRAJECTORYFOLLOWER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_TRAJECTORYFOLLOWER_HH_ + +#include <ignition/gazebo/System.hh> +#include <memory> + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class TrajectoryFollowerPrivate; + + /// \brief A plugin that scripts the movement of a model based on waypoints. + /// Note that at this point, the trajectory is always in 2D (x, y). + /// The plugin applies a torque until the model is aligned with the next + /// waypoint. Then, it applies a force until the model is close enough to + /// the waypoint. + /// + /// This plugin loads a set of waypoints via SDF and traverse the waypoints + /// in sequence. The movement is generated applying force and torque to one of + /// of the model links. It's also possible to loop through the waypoints for + /// generating a never ending trajectory. + /// Waypoints may be inserted manually via the <waypoints> element, or + /// generated relative to the model's initial position via the <line> or + /// <circle> elements. Only one of these options should be used. + /// + /// This plugin requires the following SDF parameters: + /// * Required parameters: + /// <link_name>: The name of the link within the model where the force/torque + /// will be applied when moving the vehicle. + /// + /// * Optional parameters: + /// <loop>: When true, all waypoints will be visited continously in a + /// circular pattern. If false, the model will stop when the + /// last waypoint is reached. Note, that if the vehicle moves, + /// it will still try to reach the very last waypoint. + /// <waypoints>: Element specifying the set of waypoints that the + /// the model should navigate through. This block should contain + /// at least one of these blocks: + /// <waypoint>: This block should contain the X, Y of a + /// waypoint. + /// <range_tolerance>: The current waypoint is considered reached if the + /// distance to it is within +- this tolerance (m). + /// The default value is 0.5m. + /// <bearing_tolerance>: If the bearing to the current waypoint is within + /// +- this tolerance, a torque won't be applied (degree) + /// The default value is 2 deg. + /// <force>: The force to apply at every plugin iteration in the X direction + /// of the link (N). The default value is 60. + /// <torque>: The torque to apply at every plugin iteration in the Yaw + /// direction of the link (Nm). The default value is 50. + /// <line>: Element that indicates the model should travel in "line" mode. + /// The block should contain the relative direction and distance from + /// the initial position in which the vehicle should move, specified + /// in the world frame. + /// <direction>: Relative direction (radians) in the world frame for + /// the vehicle to travel. + /// <length>: Distance (meters) for the vehicle to travel. + /// <circle>: Element that indicates the model should travel in "circle" mode. + /// The block should contain the desired radius of the circle about + /// the vehicle's initial position + /// <radius>: Radius (meters) of circular path to travel. + /// + /// Here are three examples: + // <plugin + // filename="ignition-gazebo-trajectory-follower-system" + // name="ignition::gazebo::systems::TrajectoryFollower"> + // <link_name>base_link</link_name> + // <loop>true</loop> + // <waypoints> + // <waypoint>25 0</waypoint> + // <waypoint>15 0</waypoint> + // </waypoints> + // </plugin> + // <plugin + // filename="ignition-gazebo-trajectory-follower-system" + // name="ignition::gazebo::systems::TrajectoryFollower"> + // <link_name>base_link</link_name> + // <loop>true</loop> + // <line> + // <direction>0</direction> + // <length>5</length> + // </line> + // </plugin> + // <plugin + // filename="ignition-gazebo-trajectory-follower-system" + // name="ignition::gazebo::systems::TrajectoryFollower"> + // <link_name>base_link</link_name> + // <loop>true</loop> + // <circle> + // <radius>2</radius> + // </circle> + // </plugin> + class TrajectoryFollower + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: TrajectoryFollower(); + + /// \brief Destructor + public: ~TrajectoryFollower() override = default; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr<const sdf::Element> &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Check if an entity is enabled or not. + /// \param[in] _entity Target entity + /// \param[in] _ecm Entity component manager + /// \return True if buoyancy should be applied. + public: bool IsEnabled(Entity _entity, + const EntityComponentManager &_ecm) const; + + /// \brief Private data pointer + private: std::unique_ptr<TrajectoryFollowerPrivate> dataPtr; + }; + } +} +} +} + +#endif From 7901903d06a24b9a62d2e584241cfd4d95798f39 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero <jrivero@osrfoundation.org> Date: Mon, 14 Feb 2022 23:56:57 +0100 Subject: [PATCH 14/17] Prepare for releasing 6.5.0 (#1338) * Update changelog * Bump to 6.5.0 Signed-off-by: Jose Luis Rivero <jrivero@osrfoundation.org> --- CMakeLists.txt | 2 +- Changelog.md | 67 ++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 68 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ab770aa945..eaf570f512 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-gazebo6 VERSION 6.4.0) +project(ignition-gazebo6 VERSION 6.5.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index b2931c172b..36bfd47980 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,6 +2,73 @@ ### Ignition Gazebo 6.X.X (202X-XX-XX) +### Ignition Gazebo 6.5.0 (202X-XX-XX) +1. New trajectory follower system + * [Pull request #1332](https://github.com/ignitionrobotics/ign-gazebo/pull/1332) + +1. Extend ShaderParam system to support textures + * [Pull request #1310](https://github.com/ignitionrobotics/ign-gazebo/pull/1310) + +1. Adds a `Link::SetLinearVelocity()` method + * [Pull request #1323](https://github.com/ignitionrobotics/ign-gazebo/pull/1323) + +1. Fix weird indentation in `Link.hh` + * [Pull request #1324](https://github.com/ignitionrobotics/ign-gazebo/pull/1324) + +1. Limit thruster system's input thrust cmd + * [Pull request #1318](https://github.com/ignitionrobotics/ign-gazebo/pull/1318) + +1. Load and run visual plugin (system) on GUI side + * [Pull request #1275](https://github.com/ignitionrobotics/ign-gazebo/pull/1275) + +1. Log an error if JointPositionController cannot find the joint. (citadel retarget) + * [Pull request #1314](https://github.com/ignitionrobotics/ign-gazebo/pull/1314) + +1. Update source install instructions + * [Pull request #1311](https://github.com/ignitionrobotics/ign-gazebo/pull/1311) + +1. Document the `<topic>` option for JointPositionController. + * [Pull request #1309](https://github.com/ignitionrobotics/ign-gazebo/pull/1309) + +1. Fix typo in EntityComponentManager + * [Pull request #1304](https://github.com/ignitionrobotics/ign-gazebo/pull/1304) + +1. Buoyancy: fix center of volume's reference frame + * [Pull request #1302](https://github.com/ignitionrobotics/ign-gazebo/pull/1302) + +1. Fix graded buoyancy problems + * [Pull request #1297](https://github.com/ignitionrobotics/ign-gazebo/pull/1297) + +1. Add surface to buoyancy engine. (retarget fortress) + * [Pull request #1298](https://github.com/ignitionrobotics/ign-gazebo/pull/1298) + +1. Remove EachNew calls from sensor PreUpdates + * [Pull request #1281](https://github.com/ignitionrobotics/ign-gazebo/pull/1281) + +1. Prevent GzScene3D 💥 if another scene is already loaded + * [Pull request #1294](https://github.com/ignitionrobotics/ign-gazebo/pull/1294) + +1. Fix various typos on API documentation + * [Pull request #1291](https://github.com/ignitionrobotics/ign-gazebo/pull/1291) + +1. Optional orientation when spawning entity using spherical coordinates + * [Pull request #1263](https://github.com/ignitionrobotics/ign-gazebo/pull/1263) + +1. Cleanup update call for non-rendering sensors + * [Pull request #1282](https://github.com/ignitionrobotics/ign-gazebo/pull/1282) + +1. Documentation Error + * [Pull request #1285](https://github.com/ignitionrobotics/ign-gazebo/pull/1285) + +1. Min and max parameters for velocity, acceleration, and jerk apply to linear and angular separately. + * [Pull request #1229](https://github.com/ignitionrobotics/ign-gazebo/pull/1229) + +1. Add project() call to examples + * [Pull request #1274](https://github.com/ignitionrobotics/ign-gazebo/pull/1274) + +1. Implement /server_control::stop + * [Pull request #1240](https://github.com/ignitionrobotics/ign-gazebo/pull/1240) + ### Ignition Gazebo 6.4.0 (2021-01-13) 1. Disable more tests on Windows From 7313edea5a1ab75933cf23e40af8eef7ae94ea45 Mon Sep 17 00:00:00 2001 From: Louise Poubel <louise@openrobotics.org> Date: Wed, 9 Mar 2022 13:42:55 -0800 Subject: [PATCH 15/17] Enable WorldPose component on TrajectoryFollower (#1380) Signed-off-by: Louise Poubel <louise@openrobotics.org> --- src/systems/trajectory_follower/TrajectoryFollower.cc | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/systems/trajectory_follower/TrajectoryFollower.cc b/src/systems/trajectory_follower/TrajectoryFollower.cc index 7ea2ccd6bb..85c804e0d7 100644 --- a/src/systems/trajectory_follower/TrajectoryFollower.cc +++ b/src/systems/trajectory_follower/TrajectoryFollower.cc @@ -266,6 +266,7 @@ void TrajectoryFollower::PreUpdate( // We call Load here instead of Configure because we can't be guaranteed // that all entities have been created when Configure is called this->dataPtr->Load(_ecm, this->dataPtr->sdfConfig); + enableComponent<components::WorldPose>(_ecm, this->dataPtr->link.Entity()); this->dataPtr->initialized = true; } @@ -321,6 +322,12 @@ void TrajectoryFollower::PreUpdate( // Transform from world to local frame. auto comPose = this->dataPtr->link.WorldInertialPose(_ecm); + if (!comPose.has_value()) + { + ignerr << "Failed to get CoM pose for link [" + << this->dataPtr->link.Entity() << "]" << std::endl; + return; + } // Transform the force and torque to the world frame. // Move commands. The vehicle always move forward (X direction). From 9d8afdfaedafbf3e330cdd1e8426a9fccc2e2f06 Mon Sep 17 00:00:00 2001 From: Louise Poubel <louise@openrobotics.org> Date: Wed, 9 Mar 2022 13:49:33 -0800 Subject: [PATCH 16/17] Extend thruster limits to angular velocity test (#1380) Signed-off-by: Louise Poubel <louise@openrobotics.org> --- test/worlds/thruster_ang_vel_cmd.sdf | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test/worlds/thruster_ang_vel_cmd.sdf b/test/worlds/thruster_ang_vel_cmd.sdf index c345259735..28185e1ec5 100644 --- a/test/worlds/thruster_ang_vel_cmd.sdf +++ b/test/worlds/thruster_ang_vel_cmd.sdf @@ -104,6 +104,8 @@ <propeller_diameter>0.2</propeller_diameter> <velocity_control>true</velocity_control> <use_angvel_cmd>true</use_angvel_cmd> + <max_thrust_cmd>300</max_thrust_cmd> + <min_thrust_cmd>0</min_thrust_cmd> </plugin> </model> From bf857e2973dca4290dfabc3435ff646663affb58 Mon Sep 17 00:00:00 2001 From: Louise Poubel <louise@openrobotics.org> Date: Thu, 10 Mar 2022 08:58:02 -0800 Subject: [PATCH 17/17] Disable buoyancy offset test on Windows (#1380) Signed-off-by: Louise Poubel <louise@openrobotics.org> --- test/integration/buoyancy.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc index b41f9e000b..807b2c475f 100644 --- a/test/integration/buoyancy.cc +++ b/test/integration/buoyancy.cc @@ -401,7 +401,7 @@ TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(GradedBuoyancy)) } ///////////////////////////////////////////////// -TEST_F(BuoyancyTest, OffsetAndRotation) +TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(OffsetAndRotation)) { TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "center_of_volume.sdf"));