From df626586c00b32f0b152754518f39fa7250f1fc6 Mon Sep 17 00:00:00 2001 From: "Matias N. Goldberg" Date: Wed, 20 Apr 2022 20:49:11 -0300 Subject: [PATCH 01/28] Set simulation time to Rendering (#1415) * Set simulation time to Rendering Previously rendering was not being told of the current simulation time. This is needed for proper particle FXs simulation and some postprocessing effects that rely on time. Signed-off-by: Matias N. Goldberg Co-authored-by: Ian Chen --- src/rendering/RenderUtil.cc | 18 ++++++++++++++++++ src/systems/sensors/Sensors.cc | 1 + 2 files changed, 19 insertions(+) diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index ad61ace576..27924b3a87 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1031,6 +1031,8 @@ void RenderUtil::Update() return; this->dataPtr->updateMutex.lock(); + + this->dataPtr->scene->SetTime(this->dataPtr->simTime); auto newScenes = std::move(this->dataPtr->newScenes); auto newModels = std::move(this->dataPtr->newModels); auto newLinks = std::move(this->dataPtr->newLinks); @@ -2500,6 +2502,15 @@ void RenderUtil::Init() this->dataPtr->scene->SetSkyEnabled(this->dataPtr->skyEnabled); } } + + { + // HACK: Tell ign-rendering6 to listen to SetTime calls + // TODO(anyone) Remove this when linked against ign-rendering7 + this->dataPtr->scene->SetTime(std::chrono::nanoseconds(-1)); + IGN_ASSERT(this->dataPtr->scene->Time() != std::chrono::nanoseconds(-1), + "Please remove this snippet after merging with ign-rendering7"); + } + this->dataPtr->sceneManager.SetScene(this->dataPtr->scene); if (this->dataPtr->enableSensors) this->dataPtr->markerManager.SetTopic("/sensors/marker"); @@ -2574,6 +2585,13 @@ void RenderUtil::SetSceneName(const std::string &_name) void RenderUtil::SetScene(const rendering::ScenePtr &_scene) { this->dataPtr->scene = _scene; + { + // HACK: Tell ign-rendering6 to listen to SetTime calls + // TODO(anyone) Remove this when linked against ign-rendering7 + this->dataPtr->scene->SetTime(std::chrono::nanoseconds(-1)); + IGN_ASSERT(this->dataPtr->scene->Time() != std::chrono::nanoseconds(-1), + "Please remove this snippet after merging with ign-rendering7"); + } this->dataPtr->sceneManager.SetScene(_scene); this->dataPtr->engine = _scene == nullptr ? nullptr : _scene->Engine(); } diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 9a5e8239ec..e83d053e30 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -308,6 +308,7 @@ void SensorsPrivate::RunOnce() { IGN_PROFILE("PreRender"); this->eventManager->Emit(); + this->scene->SetTime(this->updateTime); // Update the scene graph manually to improve performance // We only need to do this once per frame It is important to call // sensors::RenderingSensor::SetManualSceneUpdate and set it to true From 9927a26287cfdb7c584b9fec334c994ae09cac0f Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 21 Apr 2022 13:32:59 -0700 Subject: [PATCH 02/28] Improve contact sensor / visualization performance (#1452) * add GetByPhysicsEntityId to improve performance of getting contact data Signed-off-by: Ian Chen * remove unused var Signed-off-by: Ian Chen --- src/systems/physics/EntityFeatureMap.hh | 23 +++++++++++++++++++- src/systems/physics/EntityFeatureMap_TEST.cc | 22 ++++++++++++------- src/systems/physics/Physics.cc | 17 ++++++++------- 3 files changed, 45 insertions(+), 17 deletions(-) diff --git a/src/systems/physics/EntityFeatureMap.hh b/src/systems/physics/EntityFeatureMap.hh index 933accc9fa..e29e509f86 100644 --- a/src/systems/physics/EntityFeatureMap.hh +++ b/src/systems/physics/EntityFeatureMap.hh @@ -203,6 +203,20 @@ namespace systems::physics_system return nullptr; } + /// \brief Get Gazebo entity that corresponds to the physics entity ID + /// \param[in] _id Physics entity ID + /// \return If found, returns the corresponding Gazebo entity. Otherwise, + /// kNullEntity + public: Entity GetByPhysicsId(std::size_t _id) const + { + auto it = this->entityByPhysId.find(_id); + if (it != this->entityByPhysId.end()) + { + return it->second; + } + return kNullEntity; + } + /// \brief Check whether there is a physics entity associated with the given /// Gazebo entity /// \param[in] _entity Gazebo entity. @@ -232,6 +246,7 @@ namespace systems::physics_system this->entityMap[_entity] = _physicsEntity; this->reverseMap[_physicsEntity] = _entity; this->physEntityById[_physicsEntity->EntityID()] = _physicsEntity; + this->entityByPhysId[_physicsEntity->EntityID()] = _entity; } /// \brief Remove entity from all associated maps @@ -244,6 +259,7 @@ namespace systems::physics_system { this->reverseMap.erase(it->second); this->physEntityById.erase(it->second->EntityID()); + this->entityByPhysId.erase(it->second->EntityID()); this->castCache.erase(_entity); this->entityMap.erase(it); return true; @@ -261,6 +277,7 @@ namespace systems::physics_system { this->entityMap.erase(it->second); this->physEntityById.erase(it->first->EntityID()); + this->entityByPhysId.erase(it->first->EntityID()); this->castCache.erase(it->second); this->reverseMap.erase(it); return true; @@ -282,7 +299,8 @@ namespace systems::physics_system public: std::size_t TotalMapEntryCount() const { return this->entityMap.size() + this->reverseMap.size() + - this->castCache.size() + this->physEntityById.size(); + this->castCache.size() + this->physEntityById.size() + + this->entityByPhysId.size(); } /// \brief Map from Gazebo entity to physics entities with required features @@ -295,6 +313,9 @@ namespace systems::physics_system /// with required features private: std::unordered_map physEntityById; + /// \brief Map of physics entity IDs to Gazebo entity + private: std::unordered_map entityByPhysId; + /// \brief Cache map from Gazebo entity to physics entities with optional /// features private: mutable std::unordered_map castCache; diff --git a/src/systems/physics/EntityFeatureMap_TEST.cc b/src/systems/physics/EntityFeatureMap_TEST.cc index dc1bddb898..f4c0e9d12f 100644 --- a/src/systems/physics/EntityFeatureMap_TEST.cc +++ b/src/systems/physics/EntityFeatureMap_TEST.cc @@ -124,17 +124,18 @@ TEST_F(EntityFeatureMapFixture, testMap.AddEntity(gazeboWorld1Entity, testWorld1); - // After adding the entity, there should be one entry each in three maps - EXPECT_EQ(3u, testMap.TotalMapEntryCount()); + // After adding the entity, there should be one entry each in four maps + EXPECT_EQ(4u, testMap.TotalMapEntryCount()); EXPECT_EQ(testWorld1, testMap.Get(gazeboWorld1Entity)); EXPECT_EQ(gazeboWorld1Entity, testMap.Get(testWorld1)); + EXPECT_EQ(gazeboWorld1Entity, testMap.GetByPhysicsId(testWorld1->EntityID())); // Cast to optional feature1 auto testWorld1Feature1 = testMap.EntityCast(gazeboWorld1Entity); ASSERT_NE(nullptr, testWorld1Feature1); // After the cast, there should be one more entry in the cache map. - EXPECT_EQ(4u, testMap.TotalMapEntryCount()); + EXPECT_EQ(5u, testMap.TotalMapEntryCount()); // Cast to optional feature2 auto testWorld1Feature2 = @@ -142,38 +143,43 @@ TEST_F(EntityFeatureMapFixture, ASSERT_NE(nullptr, testWorld1Feature2); // After the cast, the number of entries should remain the same because we // have not added an entity. - EXPECT_EQ(4u, testMap.TotalMapEntryCount()); + EXPECT_EQ(5u, testMap.TotalMapEntryCount()); // Add another entity WorldPtrType testWorld2 = this->engine->ConstructEmptyWorld("world2"); testMap.AddEntity(gazeboWorld2Entity, testWorld2); - EXPECT_EQ(7u, testMap.TotalMapEntryCount()); + EXPECT_EQ(9u, testMap.TotalMapEntryCount()); EXPECT_EQ(testWorld2, testMap.Get(gazeboWorld2Entity)); EXPECT_EQ(gazeboWorld2Entity, testMap.Get(testWorld2)); + EXPECT_EQ(gazeboWorld2Entity, testMap.GetByPhysicsId(testWorld2->EntityID())); auto testWorld2Feature1 = testMap.EntityCast(testWorld2); ASSERT_NE(nullptr, testWorld2Feature1); // After the cast, there should be one more entry in the cache map. - EXPECT_EQ(8u, testMap.TotalMapEntryCount()); + EXPECT_EQ(10u, testMap.TotalMapEntryCount()); auto testWorld2Feature2 = testMap.EntityCast(testWorld2); ASSERT_NE(nullptr, testWorld2Feature2); // After the cast, the number of entries should remain the same because we // have not added an entity. - EXPECT_EQ(8u, testMap.TotalMapEntryCount()); + EXPECT_EQ(10u, testMap.TotalMapEntryCount()); // Remove entitites testMap.Remove(gazeboWorld1Entity); EXPECT_FALSE(testMap.HasEntity(gazeboWorld1Entity)); EXPECT_EQ(nullptr, testMap.Get(gazeboWorld1Entity)); EXPECT_EQ(gazebo::kNullEntity, testMap.Get(testWorld1)); - EXPECT_EQ(4u, testMap.TotalMapEntryCount()); + EXPECT_EQ(gazebo::kNullEntity, + testMap.GetByPhysicsId(testWorld1->EntityID())); + EXPECT_EQ(5u, testMap.TotalMapEntryCount()); testMap.Remove(testWorld2); EXPECT_FALSE(testMap.HasEntity(gazeboWorld2Entity)); EXPECT_EQ(nullptr, testMap.Get(gazeboWorld2Entity)); EXPECT_EQ(gazebo::kNullEntity, testMap.Get(testWorld2)); + EXPECT_EQ(gazebo::kNullEntity, testMap.GetByPhysicsId( + testWorld2->EntityID())); EXPECT_EQ(0u, testMap.TotalMapEntryCount()); } diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 77ce0c9ea6..506833f662 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -3227,15 +3227,16 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) // Note that we are temporarily storing pointers to elements in this // ("allContacts") container. Thus, we must make sure it doesn't get destroyed // until the end of this function. - auto allContacts = worldCollisionFeature->GetContactsFromLastStep(); + auto allContacts = + std::move(worldCollisionFeature->GetContactsFromLastStep()); + for (const auto &contactComposite : allContacts) { const auto &contact = contactComposite.Get(); auto coll1Entity = - this->entityCollisionMap.Get(ShapePtrType(contact.collision1)); + this->entityCollisionMap.GetByPhysicsId(contact.collision1->EntityID()); auto coll2Entity = - this->entityCollisionMap.Get(ShapePtrType(contact.collision2)); - + this->entityCollisionMap.GetByPhysicsId(contact.collision2->EntityID()); if (coll1Entity != kNullEntity && coll2Entity != kNullEntity) { @@ -3334,10 +3335,10 @@ void PhysicsPrivate::EnableContactSurfaceCustomization(const Entity &_world) Feature::ContactSurfaceParams &_params) { const auto &contact = _contact.Get(); - auto coll1Entity = this->entityCollisionMap.Get( - ShapePtrType(contact.collision1)); - auto coll2Entity = this->entityCollisionMap.Get( - ShapePtrType(contact.collision2)); + auto coll1Entity = this->entityCollisionMap.GetByPhysicsId( + contact.collision1->EntityID()); + auto coll2Entity = this->entityCollisionMap.GetByPhysicsId( + contact.collision2->EntityID()); // check if at least one of the entities wants contact surface // customization From f034b159d0714e870bb9ad9bb7f3f04269163a98 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 21 Apr 2022 21:09:33 -0700 Subject: [PATCH 03/28] bump rendering dep version (#1455) Signed-off-by: Ian Chen --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 91cf178c08..27af15fe8e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -147,7 +147,7 @@ set(IGN_SENSORS_VER ${ignition-sensors6_VERSION_MAJOR}) #-------------------------------------- # Find ignition-rendering -ign_find_package(ignition-rendering6 REQUIRED) +ign_find_package(ignition-rendering6 REQUIRED VERSION 6.3) set(IGN_RENDERING_VER ${ignition-rendering6_VERSION_MAJOR}) #-------------------------------------- From 07bbeb2216eb6c7d0ebcc1f19134a20fa39b2615 Mon Sep 17 00:00:00 2001 From: David Lechner Date: Tue, 26 Apr 2022 06:59:58 -0500 Subject: [PATCH 04/28] python: remove semicolons (#1459) Python doesn't use semicolons (although it does ignore them). Signed-off-by: David Lechner --- examples/scripts/python_api/testFixture.py | 2 +- python/test/testFixture_TEST.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/scripts/python_api/testFixture.py b/examples/scripts/python_api/testFixture.py index caedebdc84..e7b42552d6 100644 --- a/examples/scripts/python_api/testFixture.py +++ b/examples/scripts/python_api/testFixture.py @@ -48,7 +48,7 @@ def on_pre_udpate_cb(_info, _ecm): pre_iterations += 1 if first_iteration: first_iteration = False - world_e = world_entity(_ecm); + world_e = world_entity(_ecm) print('World entity is ', world_e) w = World(world_e) v = w.gravity(_ecm) diff --git a/python/test/testFixture_TEST.py b/python/test/testFixture_TEST.py index b48c501330..24d3479135 100644 --- a/python/test/testFixture_TEST.py +++ b/python/test/testFixture_TEST.py @@ -40,7 +40,7 @@ def on_post_udpate_cb(_info, _ecm): def on_pre_udpate_cb(_info, _ecm): global pre_iterations pre_iterations += 1 - world_e = world_entity(_ecm); + world_e = world_entity(_ecm) self.assertEqual(1, world_e) w = World(world_e) v = w.gravity(_ecm) From 0a5828e53735cae7e3ee264410d7d69e3ac2954e Mon Sep 17 00:00:00 2001 From: David Lechner Date: Tue, 26 Apr 2022 10:55:16 -0500 Subject: [PATCH 05/28] python: release GIL when running server (#1458) Since running the server can be a blocking function, we need to release the GIL so that other Python threads can run. This also means we no longer need to poll for the server to finish running by setting the blocking argument to True. Signed-off-by: David Lechner --- examples/scripts/python_api/testFixture.py | 6 +----- python/src/ignition/gazebo/Server.cc | 1 + python/test/testFixture_TEST.py | 6 +----- tutorials/python_interfaces.md | 4 +--- 4 files changed, 4 insertions(+), 13 deletions(-) diff --git a/examples/scripts/python_api/testFixture.py b/examples/scripts/python_api/testFixture.py index e7b42552d6..dabe30ae44 100644 --- a/examples/scripts/python_api/testFixture.py +++ b/examples/scripts/python_api/testFixture.py @@ -23,7 +23,6 @@ # python3 examples/scripts/python_api/helperFixture.py import os -import time from ignition.common import set_verbosity from ignition.gazebo import TestFixture, World, world_entity @@ -75,10 +74,7 @@ def on_post_udpate_cb(_info, _ecm): helper.finalize() server = helper.server() -server.run(False, 1000, False) - -while(server.is_running()): - time.sleep(0.1) +server.run(True, 1000, False) print('iterations ', iterations) print('post_iterations ', post_iterations) diff --git a/python/src/ignition/gazebo/Server.cc b/python/src/ignition/gazebo/Server.cc index 917560a8ae..7bdd116399 100644 --- a/python/src/ignition/gazebo/Server.cc +++ b/python/src/ignition/gazebo/Server.cc @@ -35,6 +35,7 @@ void defineGazeboServer(pybind11::object module) .def(pybind11::init()) .def( "run", &ignition::gazebo::Server::Run, + pybind11::call_guard(), "Run the server. By default this is a non-blocking call, " " which means the server runs simulation in a separate thread. Pass " " in true to the _blocking argument to run the server in the current " diff --git a/python/test/testFixture_TEST.py b/python/test/testFixture_TEST.py index 24d3479135..ea04e75e6e 100644 --- a/python/test/testFixture_TEST.py +++ b/python/test/testFixture_TEST.py @@ -13,7 +13,6 @@ # limitations under the License. import os -import time import unittest from ignition.common import set_verbosity @@ -56,10 +55,7 @@ def on_udpate_cb(_info, _ecm): helper.finalize() server = helper.server() - server.run(False, 1000, False) - - while(server.is_running()): - time.sleep(0.1) + server.run(True, 1000, False) self.assertEqual(1000, pre_iterations) self.assertEqual(1000, iterations) diff --git a/tutorials/python_interfaces.md b/tutorials/python_interfaces.md index 477a95dc29..33fe8cb3de 100644 --- a/tutorials/python_interfaces.md +++ b/tutorials/python_interfaces.md @@ -37,9 +37,7 @@ helper.finalize() - **Step 5**: Run the server ```python -server.run(False, 1000, False) -while(server.is_running()): - time.sleep(0.1) +server.run(True, 1000, False) ``` # Run the example From 5994f6dd18423c08c79d3fd5f9e9b1a264146529 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 26 Apr 2022 19:20:10 -0500 Subject: [PATCH 06/28] Add repo specific issue templates (#1461) Signed-off-by: Addisu Z. Taddese --- .github/ISSUE_TEMPLATE/bug_report.md | 72 +++++++++++++++++++++++ .github/ISSUE_TEMPLATE/feature_request.md | 23 ++++++++ 2 files changed, 95 insertions(+) create mode 100644 .github/ISSUE_TEMPLATE/bug_report.md create mode 100644 .github/ISSUE_TEMPLATE/feature_request.md diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 0000000000..8e30646e38 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,72 @@ +--- +name: Bug report +about: Report a bug +labels: bug +--- + + + +## Environment +* OS Version: +* Source or binary build? + + +* If this is a GUI or sensor rendering bug, describe your GPU and rendering system. Otherwise delete this section. + - Rendering plugin: [ogre | ogre2]. + - [ ] Sensor rendering error. + - [ ] GUI rendering error. + - EGL headless mode: + - [ ] Running in EGL headless mode + - Generally, mention all circumstances that might affect rendering capabilities: + - [ ] running on a dual GPU machine (integrated GPU + discrete GPU) + - [ ] running on a multi-GPU machine (it has multiple discrete GPUs) + - [ ] running on real hardware + - [ ] running in virtual machine + - [ ] running in Docker/Singularity + - [ ] running remotely (e.g. via SSH) + - [ ] running in a cloud + - [ ] using VirtualGL, XVFB, Xdummy, XVNC or other indirect rendering utilities + - [ ] GPU is concurrently used for other tasks + - [ ] desktop acceleration + - [ ] video decoding (i.e. a playing Youtube video) + - [ ] video encoding + - [ ] CUDA/ROCm computations (Tensorflow, Torch, Caffe running) + - [ ] multiple simulators running at the same time + - [ ] other... + - Rendering system info: + - On Linux, provide the outputs of the following commands: + ```bash + LANG=C lspci -nn | grep VGA # might require installing pciutils + echo "$DISPLAY" + LANG=C glxinfo -B | grep -i '\(direct rendering\|opengl\|profile\)' # might require installing mesa-utils package + ps aux | grep Xorg + sudo env LANG=C X -version # if you don't have root access, try to tell the version of Xorg e.g. via package manager + ``` + - On Windows, run `dxdiag` and report the GPU-related information. + - On Mac OS, open a terminal and type `system_profiler SPDisplaysDataType`. Copy the output here. + + - [ ] Please, attach the ogre.log or ogre2.log file from `~/.ignition/rendering` + +
+ +``` +# paste log here +``` + +
+ +## Description +* Expected behavior: +* Actual behavior: + +## Steps to reproduce + + +1. +2. +3. + +## Output + diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 0000000000..87233a479a --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,23 @@ +--- +name: Feature request +about: Request a new feature +labels: enhancement +--- + + + +## Desired behavior + + +## Alternatives considered + + +## Implementation suggestion + + +## Additional context + From b6186a8f75fe7a60fe0999cdfde96a797655920b Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 2 May 2022 09:09:45 -0700 Subject: [PATCH 07/28] Fix running simulation with no world specified on the command line (#1463) Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- include/ignition/gazebo/ServerConfig.hh | 3 ++- src/ServerConfig.cc | 3 +++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index 7311d092e8..2afae36d32 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -172,7 +172,8 @@ namespace ignition /// Setting the SDF file will override any value set by `SetSdfString`. /// /// \param[in] _file Full path to an SDF file. - /// \return (reserved for future use) + /// \return True if the file was set, false if the file was not set. + /// The file will not be set if the provide _file string is empty. public: bool SetSdfFile(const std::string &_file); /// \brief Get the SDF file that has been set. An empty string will be diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 1024eb5d11..d410168d09 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -327,6 +327,9 @@ ServerConfig::~ServerConfig() = default; ////////////////////////////////////////////////// bool ServerConfig::SetSdfFile(const std::string &_file) { + if (_file.empty()) + return false; + this->dataPtr->source = ServerConfig::SourceType::kSdfFile; this->dataPtr->sdfFile = _file; this->dataPtr->sdfString = ""; From c756b607dfb6f5033d72074563137ad5fd5368c8 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 2 May 2022 20:13:01 -0700 Subject: [PATCH 08/28] Skip serializing nested model with //pose/@relative_to attribute (#1454) This PR prevents serialization of a model component if it has the //pose/@relative_to attribute to avoid flooding the console with error messages when there are multiple models with nested models using this attribute. Since model deserialization with //pose/@relative_to never worked, this should not affect existing behavior. I believe the model serialization / deserialization functionality is mainly used by the component editor. Related issue: #1071 Signed-off-by: Ian Chen --- examples/worlds/sensors_demo.sdf | 1 + include/ignition/gazebo/components/Model.hh | 31 +++++++++++++++++++-- 2 files changed, 29 insertions(+), 3 deletions(-) diff --git a/examples/worlds/sensors_demo.sdf b/examples/worlds/sensors_demo.sdf index 31374a1a3e..7eeba49c10 100644 --- a/examples/worlds/sensors_demo.sdf +++ b/examples/worlds/sensors_demo.sdf @@ -380,6 +380,7 @@ 320 240 + L8 0.1 diff --git a/include/ignition/gazebo/components/Model.hh b/include/ignition/gazebo/components/Model.hh index 5ba0420d94..c75f79f436 100644 --- a/include/ignition/gazebo/components/Model.hh +++ b/include/ignition/gazebo/components/Model.hh @@ -46,13 +46,38 @@ namespace serializers sdf::ElementPtr modelElem = _model.Element(); if (!modelElem) { - ignerr << "Unable to serialize sdf::Model" << std::endl; + ignwarn << "Unable to serialize sdf::Model" << std::endl; return _out; } + bool skip = false; + if (modelElem->HasElement("pose")) + { + sdf::ElementPtr poseElem = modelElem->GetElement("pose"); + if (poseElem->HasAttribute("relative_to")) + { + // Skip serializing models with //pose/@relative_to attribute + // since deserialization will fail. This could be a nested model. + // see https://github.com/ignitionrobotics/ign-gazebo/issues/1071 + // Once https://github.com/ignitionrobotics/sdformat/issues/820 is + // resolved, there should be an API that returns sdf::Errors objects + // instead of printing console msgs so it would be easier to ignore + // specific errors in Deserialize. + static bool warned = false; + if (!warned) + { + ignwarn << "Skipping serialization / deserialization for models " + << "with //pose/@relative_to attribute." + << std::endl; + warned = true; + } + skip = true; + } + } + _out << "" << "" - << modelElem->ToString("") + << (skip ? std::string() : modelElem->ToString("")) << ""; return _out; } @@ -70,7 +95,7 @@ namespace serializers sdf::Errors errors = root.LoadSdfString(sdf); if (!root.Model()) { - ignerr << "Unable to unserialize sdf::Model" << std::endl; + ignwarn << "Unable to deserialize sdf::Model" << std::endl; return _in; } From 3b0f9117cc6e4d370731ca9f7608c07a1288f715 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 3 May 2022 14:09:41 -0700 Subject: [PATCH 09/28] Fix finding DART on macOS (#1469) * Find DART without QUIET * Find DART with CONFIG, require ign-cmake 2.12 On macOS, find_package(DART) will resolve to a find-module for the Dart programming language. Use CONFIG to ensure that the DART physics library is found. The CONFIG parameter was added in igntion-cmake2 2.12.0. * Fix cmake CMP0074 warning Signed-off-by: Steve Peters --- CMakeLists.txt | 2 +- test/integration/CMakeLists.txt | 7 ++++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 27af15fe8e..4b9ff4329a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,7 @@ project(ignition-gazebo6 VERSION 6.9.0) # Find ignition-cmake #============================================================================ # If you get an error at this line, you need to install ignition-cmake -find_package(ignition-cmake2 2.8.0 REQUIRED) +find_package(ignition-cmake2 2.12.0 REQUIRED) #============================================================================ # Configure the project diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 8c4a0ea0e4..be910f8ab5 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -128,7 +128,12 @@ ign_build_tests(TYPE INTEGRATION # For INTEGRATION_physics_system, we need to check what version of DART is # available so that we can disable tests that are unsupported by the particular # version of physics engine -ign_find_package(DART QUIET) +cmake_policy(PUSH) +if (POLICY CMP0074) + cmake_policy(SET CMP0074 NEW) +endif() +ign_find_package(DART CONFIG) +cmake_policy(POP) if (DART_FOUND) # Only adding include directories, no need to link against DART to check version target_include_directories(INTEGRATION_physics_system SYSTEM PRIVATE ${DART_INCLUDE_DIRS}) From 66fa61a1c2b018fa0a4d1080ef89c7cc9d321cea Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 4 May 2022 03:34:54 -0700 Subject: [PATCH 10/28] revert format change (#1468) Signed-off-by: Ian Chen --- examples/worlds/sensors_demo.sdf | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/worlds/sensors_demo.sdf b/examples/worlds/sensors_demo.sdf index 7eeba49c10..31374a1a3e 100644 --- a/examples/worlds/sensors_demo.sdf +++ b/examples/worlds/sensors_demo.sdf @@ -380,7 +380,6 @@ 320 240 - L8 0.1 From e3d8ae71874452ae126dfaf7c407faab267b1165 Mon Sep 17 00:00:00 2001 From: oogee Date: Wed, 4 May 2022 19:20:00 +0300 Subject: [PATCH 11/28] SceneBroadcaster: Use double for state publish frequency instead of int (#1417) Signed-off-by: Theodoros Tyrovouzis --- .../scene_broadcaster/SceneBroadcaster.cc | 31 +++++++++------ test/integration/scene_broadcaster_system.cc | 39 +++++++++++++++++++ 2 files changed, 58 insertions(+), 12 deletions(-) diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index ad168d2811..ed98ffe076 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -224,9 +224,9 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate /// paused. The not-paused (a.ka. running) period has a key=false and a /// default update rate of 60Hz. The paused period has a key=true and a /// default update rate of 30Hz. - public: std::map>> - statePublishPeriod{{false, std::chrono::milliseconds(1000/60)}, - {true, std::chrono::milliseconds(1000/30)}}; + public: std::map>> + statePublishPeriod{{false, std::chrono::duration>(1000/60.0)}, + {true, std::chrono::duration>(1000/30.0)}}; /// \brief Flag used to indicate if the state service was called. public: bool stateServiceRequest{false}; @@ -261,16 +261,23 @@ void SceneBroadcaster::Configure( auto readHertz = _sdf->Get("dynamic_pose_hertz", 60); this->dataPtr->dyPoseHertz = readHertz.first; - auto stateHerz = _sdf->Get("state_hertz", 60); - this->dataPtr->statePublishPeriod[false] = - std::chrono::duration>( - std::chrono::milliseconds(1000/stateHerz.first)); + auto stateHertz = _sdf->Get("state_hertz", 60); + if (stateHertz.first > 0.0) + { + this->dataPtr->statePublishPeriod[false] = + std::chrono::duration>(1000 / stateHertz.first); - // Set the paused update rate to half of the running update rate. - this->dataPtr->statePublishPeriod[true] = - std::chrono::duration>( - std::chrono::milliseconds(1000 / - static_cast(std::max(stateHerz.first * 0.5, 1.0)))); + // Set the paused update rate to half of the running update rate. + this->dataPtr->statePublishPeriod[true] = + std::chrono::duration>(1000 / (stateHertz.first * 0.5)); + } + else + { + using secs_double = std::chrono::duration>; + ignerr << "SceneBroadcaster state_hertz must be positive, using default (" << + 1.0 / secs_double(this->dataPtr->statePublishPeriod[false]).count() << + "Hz)\n"; + } // Add to graph { diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index b62b4d1995..5fb506fd1e 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -874,6 +874,45 @@ TEST_P(SceneBroadcasterTest, EXPECT_GE(receivedStates, 7); } +///////////////////////////////////////////////// +// Tests https://github.com/ignitionrobotics/ign-gazebo/issues/1414 +TEST_P(SceneBroadcasterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(DecimalStateHertz)) +{ + // Start server + std::string sdfStr = R"( + + + + + 0.001 + 1.0 + + + + + 0.4 + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + +)"; + ignition::gazebo::ServerConfig serverConfig; + serverConfig.SetSdfString(sdfStr); + + gazebo::Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Run server + server.Run(true, 1, false); +} + // Run multiple times INSTANTIATE_TEST_SUITE_P(ServerRepeat, SceneBroadcasterTest, ::testing::Range(1, 2)); From 212aaed7ebf277be5f528d1d38b89e1a3f6723c2 Mon Sep 17 00:00:00 2001 From: Aditya Pande Date: Wed, 4 May 2022 13:03:03 -0700 Subject: [PATCH 12/28] Test case to check if velocity limits are applied to joints (#1445) * Added test case for DART 6.10 and above Signed-off-by: Aditya Co-authored-by: Steve Peters --- .../scene_broadcaster/SceneBroadcaster.cc | 17 +- test/integration/physics_system.cc | 68 +++++ test/integration/scene_broadcaster_system.cc | 3 +- test/worlds/joint_velocity_limit.sdf | 265 ++++++++++++++++++ 4 files changed, 346 insertions(+), 7 deletions(-) create mode 100644 test/worlds/joint_velocity_limit.sdf diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index ed98ffe076..d6d16803c0 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -225,8 +225,11 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate /// default update rate of 60Hz. The paused period has a key=true and a /// default update rate of 30Hz. public: std::map>> - statePublishPeriod{{false, std::chrono::duration>(1000/60.0)}, - {true, std::chrono::duration>(1000/30.0)}}; + statePublishPeriod{ + {false, std::chrono::duration>(1000/60.0)}, + {true, std::chrono::duration>(1000/30.0)}}; /// \brief Flag used to indicate if the state service was called. public: bool stateServiceRequest{false}; @@ -265,17 +268,19 @@ void SceneBroadcaster::Configure( if (stateHertz.first > 0.0) { this->dataPtr->statePublishPeriod[false] = - std::chrono::duration>(1000 / stateHertz.first); + std::chrono::duration>( + 1000 / stateHertz.first); // Set the paused update rate to half of the running update rate. this->dataPtr->statePublishPeriod[true] = - std::chrono::duration>(1000 / (stateHertz.first * 0.5)); + std::chrono::duration>(1000/ + (stateHertz.first * 0.5)); } else { using secs_double = std::chrono::duration>; - ignerr << "SceneBroadcaster state_hertz must be positive, using default (" << - 1.0 / secs_double(this->dataPtr->statePublishPeriod[false]).count() << + ignerr << "SceneBroadcaster state_hertz must be positive, using default (" + << 1.0 / secs_double(this->dataPtr->statePublishPeriod[false]).count() << "Hz)\n"; } diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index 2f6d6ca94c..f2aff19a24 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -2196,3 +2196,71 @@ TEST_F(PhysicsSystemFixture, EXPECT_NEAR(0.0, wrench.torque().z(), 1e-3); } } + +///////////////////////////////////////////////// +// Test that joint velocity limit is applied +TEST_F(PhysicsSystemFixtureWithDart6_10, + IGN_UTILS_TEST_DISABLED_ON_WIN32(JointVelocityLimitTest)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/joint_velocity_limit.sdf"; + serverConfig.SetSdfFile(sdfFile); + + auto server = std::make_unique(serverConfig); + EXPECT_FALSE(server->Running()); + EXPECT_FALSE(*server->Running(0)); + + server->SetUpdatePeriod(1ns); + + test::Relay testSystem; + + const std::size_t nIters{600}; + testSystem.OnPreUpdate( + [&](const gazebo::UpdateInfo &_info, gazebo::EntityComponentManager &_ecm) + { + // Create components, if they don't exist on the first iteration + if (_info.iterations == 1) + { + for (const auto &e : _ecm.EntitiesByComponents(components::Joint())) + { + if (!_ecm.Component(e)) + { + _ecm.CreateComponent(e, components::JointVelocity()); + } + } + } + }); + + testSystem.OnPostUpdate( + [&](const gazebo::UpdateInfo &_info, + const gazebo::EntityComponentManager &_ecm) + { + // At nIters iterations, check angular velocity of each of the joints + if (_info.iterations == nIters) + { + int count = 0; + for (const auto &e : _ecm.EntitiesByComponents(components::Joint())) + { + auto *jointVel = _ecm.Component(e); + EXPECT_NE(nullptr, jointVel); + EXPECT_FALSE(jointVel->Data().empty()); + if (jointVel->Data().size() > 0) + { + ++count; + // Joint velocity should lie between + // - (kVelocityLimit + kTolerance) and + // + (kVelocityLimit + kTolerance) + const double kVelocityLimit = 1.0; + const double kTolerance = 1e-6; + EXPECT_NEAR(jointVel->Data()[0], 0, kVelocityLimit + kTolerance); + } + } + EXPECT_EQ(count, 2); + } + }); + + server->AddSystem(testSystem.systemPtr); + server->Run(true, nIters, false); +} diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index 5fb506fd1e..83ddc6cb60 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -876,7 +876,8 @@ TEST_P(SceneBroadcasterTest, ///////////////////////////////////////////////// // Tests https://github.com/ignitionrobotics/ign-gazebo/issues/1414 -TEST_P(SceneBroadcasterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(DecimalStateHertz)) +TEST_P(SceneBroadcasterTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(DecimalStateHertz)) { // Start server std::string sdfStr = R"( diff --git a/test/worlds/joint_velocity_limit.sdf b/test/worlds/joint_velocity_limit.sdf new file mode 100644 index 0000000000..829f1000c0 --- /dev/null +++ b/test/worlds/joint_velocity_limit.sdf @@ -0,0 +1,265 @@ + + + + + 0.001 + 1.0 + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 3 0 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + 0.25 1.0 2.1 -2 0 0 + 0 + + 0 0 0.5 0 0 0 + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + 1 + + + + + + upper_link + lower_link + + 1.0 0 0 + + 1 + + + + + + + + From 270da60b9387d3751d8d65eafd60f6459b450b00 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 5 May 2022 14:44:32 -0700 Subject: [PATCH 13/28] Remove dead ign.cc file (#1474) Signed-off-by: Louise Poubel --- src/cmd/ign.cc | 461 ------------------------------------------------- 1 file changed, 461 deletions(-) delete mode 100644 src/cmd/ign.cc diff --git a/src/cmd/ign.cc b/src/cmd/ign.cc deleted file mode 100644 index ac3ff8932d..0000000000 --- a/src/cmd/ign.cc +++ /dev/null @@ -1,461 +0,0 @@ -/* - * Copyright (C) 2019 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 -#include -#include -#include -#include - -#include -#include -#include - -#include "ignition/gazebo/config.hh" -#include "ignition/gazebo/Server.hh" -#include "ignition/gazebo/ServerConfig.hh" -#include "gui/GuiRunner.hh" -#include "ign.hh" - -////////////////////////////////////////////////// -extern "C" char *ignitionGazeboVersion() -{ - return strdup(IGNITION_GAZEBO_VERSION_FULL); -} - -////////////////////////////////////////////////// -extern "C" char *gazeboVersionHeader() -{ - return strdup(IGNITION_GAZEBO_VERSION_HEADER); -} - -////////////////////////////////////////////////// -extern "C" void cmdVerbosity( - const char *_verbosity) -{ - ignition::common::Console::SetVerbosity(std::atoi(_verbosity)); -} - -////////////////////////////////////////////////// -extern "C" const char *worldInstallDir() -{ - return IGN_GAZEBO_WORLD_INSTALL_DIR; -} - -////////////////////////////////////////////////// -extern "C" int runServer(const char *_sdfString, - int _iterations, int _run, float _hz, int _levels, const char *_networkRole, - int _networkSecondaries, int _record, const char *_recordPath, - int _logOverwrite, const char *_playback, const char *_file) -{ - ignition::gazebo::ServerConfig serverConfig; - - // Path for logs - std::string recordPathMod = serverConfig.LogRecordPath(); - - // Initialize console log - if ((_recordPath != nullptr && std::strlen(_recordPath) > 0) || _record > 0) - { - if (_playback != nullptr && std::strlen(_playback) > 0) - { - ignerr << "Both record and playback are specified. Only specify one.\n"; - return -1; - } - - serverConfig.SetUseLogRecord(true); - - // If a record path is specified - if (_recordPath != nullptr && std::strlen(_recordPath) > 0) - { - recordPathMod = std::string(_recordPath); - - // Check if path or compressed file with same prefix exists - if (ignition::common::exists(recordPathMod)) - { - // Overwrite if flag specified - if (_logOverwrite > 0) - { - bool recordMsg = false; - // Remove files before initializing console log files on top of them - if (ignition::common::exists(recordPathMod)) - { - recordMsg = true; - ignition::common::removeAll(recordPathMod); - } - - // Create log file before printing any messages so they can be logged - ignLogInit(recordPathMod, "server_console.log"); - - if (recordMsg) - { - ignmsg << "Log path already exists on disk! Existing files will " - << "be overwritten." << std::endl; - ignmsg << "Removing existing path [" << recordPathMod << "]\n"; - } - } - // Otherwise rename to unique path - else - { - // Remove the separator at end of path - if (!std::string(1, recordPathMod.back()).compare( - ignition::common::separator(""))) - { - recordPathMod = recordPathMod.substr(0, recordPathMod.length() - 1); - } - recordPathMod = ignition::common::uniqueDirectoryPath(recordPathMod); - - ignLogInit(recordPathMod, "server_console.log"); - ignwarn << "Log path already exists on disk! " - << "Recording instead to [" << recordPathMod << "]" << std::endl; - } - } - else - { - ignLogInit(recordPathMod, "server_console.log"); - } - } - // Empty record path specified. Use default. - else - { - // Create log file before printing any messages so they can be logged - ignLogInit(recordPathMod, "server_console.log"); - ignmsg << "Recording states to default path [" << recordPathMod << "]" - << std::endl; - - serverConfig.SetLogRecordPath(recordPathMod); - } - } - else - { - ignLogInit(serverConfig.LogRecordPath(), "server_console.log"); - } - - serverConfig.SetLogRecordPath(recordPathMod); - - ignmsg << "Ignition Gazebo Server v" << IGNITION_GAZEBO_VERSION_FULL - << std::endl; - - // Set the SDF string to user - if (_sdfString != nullptr && std::strlen(_sdfString) > 0) - { - if (!serverConfig.SetSdfString(_sdfString)) - { - ignerr << "Failed to set SDF string [" << _sdfString << "]" << std::endl; - return -1; - } - } - serverConfig.SetSdfFile(_file); - - // Set the update rate. - if (_hz > 0.0) - serverConfig.SetUpdateRate(_hz); - - // Set whether levels should be used. - if (_levels > 0) - { - ignmsg << "Using the level system\n"; - serverConfig.SetUseLevels(true); - } - - if (_networkRole && std::strlen(_networkRole) > 0) - { - ignmsg << "Using the distributed simulation and levels systems\n"; - serverConfig.SetNetworkRole(_networkRole); - serverConfig.SetNetworkSecondaries(_networkSecondaries); - serverConfig.SetUseLevels(true); - } - - if (_playback != nullptr && std::strlen(_playback) > 0) - { - if (_sdfString != nullptr && std::strlen(_sdfString) > 0) - { - ignerr << "Both an SDF file and playback flag are specified. " - << "Only specify one.\n"; - return -1; - } - else - { - ignmsg << "Playing back states" << _playback << std::endl; - serverConfig.SetLogPlaybackPath(ignition::common::absPath( - std::string(_playback))); - } - } - - // Create the Gazebo server - ignition::gazebo::Server server(serverConfig); - - // Run the server - server.Run(true, _iterations, _run == 0); - - igndbg << "Shutting down server" << std::endl; - return 0; -} - -////////////////////////////////////////////////// -extern "C" int runGui(const char *_guiConfig) -{ - ignition::common::SignalHandler sigHandler; - bool sigKilled = false; - sigHandler.AddCallback([&](const int /*_sig*/) - { - sigKilled = true; - }); - - ignmsg << "Ignition Gazebo GUI v" << IGNITION_GAZEBO_VERSION_FULL - << std::endl; - - int argc = 0; - char **argv = nullptr; - - // Initialize Qt app - ignition::gui::Application app(argc, argv); - app.AddPluginPath(IGN_GAZEBO_GUI_PLUGIN_INSTALL_DIR); - - // add import path so we can load custom modules - app.Engine()->addImportPath(IGN_GAZEBO_GUI_PLUGIN_INSTALL_DIR); - - // Set default config file for Gazebo - std::string defaultConfigDir; - ignition::common::env(IGN_HOMEDIR, defaultConfigDir); - defaultConfigDir = ignition::common::joinPaths(defaultConfig, ".ignition", - "gazebo", IGNITION_GAZEBO_MAJOR_VERSION_STR); - - auto defaultConfig = ignition::common::joinPaths(defaultConfigDir, - "gui.config"); - - app.SetDefaultConfigPath(defaultConfig); - - // Customize window - auto mainWin = app.findChild(); - auto win = mainWin->QuickWindow(); - win->setProperty("title", "Gazebo"); - - // Instantiate GazeboDrawer.qml file into a component - QQmlComponent component(app.Engine(), ":/Gazebo/GazeboDrawer.qml"); - auto gzDrawerItem = qobject_cast(component.create(context)); - if (gzDrawerItem) - { - // C++ ownership - QQmlEngine::setObjectOwnership(gzDrawerItem, QQmlEngine::CppOwnership); - - // Add to main window - auto parentDrawerItem = win->findChild("sideDrawer"); - gzDrawerItem->setParentItem(parentDrawerItem); - gzDrawerItem->setParent(app.Engine()); - } - else - { - ignerr << "Failed to instantiate custom drawer, drawer will be empty" - << std::endl; - } - - // Get list of worlds - ignition::transport::Node node; - - bool executed{false}; - bool result{false}; - unsigned int timeout{5000}; - std::string service{"/gazebo/worlds"}; - ignition::msgs::StringMsg_V worldsMsg; - - // This loop is here to allow the server time to download resources. - // \todo(nkoenig) Async resource download. Search for "Async resource - // download in `src/Server.cc` for corresponding todo item. This todo is - // resolved when this while loop can be removed. - while (!sigKilled && !executed) - { - igndbg << "GUI requesting list of world names. The server may be busy " - << "downloading resources. Please be patient." << std::endl; - executed = node.Request(service, timeout, worldsMsg, result); - } - - // Only print error message if a sigkill was not received. - if (!sigKilled) - { - if (!executed) - ignerr << "Timed out when getting world names." << std::endl; - else if (!result) - ignerr << "Failed to get world names." << std::endl; - } - - if (!executed || !result || worldsMsg.data().empty()) - return false; - - // Remove warning suppression in v6 -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - std::vector runners; -#ifndef _WIN32 -# pragma GCC diagnostic pop -#else -# pragma warning(pop) -#endif - - // Configuration file from command line - if (_guiConfig != nullptr && std::strlen(_guiConfig) > 0) - { - if (!app.LoadConfig(_guiConfig)) - { - ignwarn << "Failed to load config file[" << _guiConfig << "]." - << std::endl; - } - - // Use the first world name with the config file - // TODO(anyone) Most of ign-gazebo's transport API includes the world name, - // which makes it complicated to mix configurations across worlds. - // We could have a way to use world-agnostic topics like Gazebo-classic's ~ - - // Remove warning suppression in v6 -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - auto runner = new ignition::gazebo::GuiRunner(worldsMsg.data(0)); -#ifndef _WIN32 -# pragma GCC diagnostic pop -#else -# pragma warning(pop) -#endif - runner->connect(&app, &ignition::gui::Application::PluginAdded, runner, - &ignition::gazebo::GuiRunner::OnPluginAdded); - runners.push_back(runner); - runner->setParent(ignition::gui::App()); - } - // GUI configuration from SDF (request to server) - else - { - // TODO(anyone) Parallelize this if multiple worlds becomes an important use - // case. - for (int w = 0; w < worldsMsg.data_size(); ++w) - { - const auto &worldName = worldsMsg.data(w); - - // Request GUI info for each world - result = false; - service = std::string("/world/" + worldName + "/gui/info"); - - igndbg << "Requesting GUI from [" << service << "]..." << std::endl; - - // Request and block - ignition::msgs::GUI res; - executed = node.Request(service, timeout, res, result); - - if (!executed) - ignerr << "Service call timed out for [" << service << "]" << std::endl; - else if (!result) - ignerr << "Service call failed for [" << service << "]" << std::endl; - - for (int p = 0; p < res.plugin_size(); ++p) - { - const auto &plugin = res.plugin(p); - const auto &fileName = plugin.filename(); - std::string pluginStr = "" + - plugin.innerxml() + ""; - - tinyxml2::XMLDocument pluginDoc; - pluginDoc.Parse(pluginStr.c_str()); - - app.LoadPlugin(fileName, - pluginDoc.FirstChildElement("plugin")); - } - - // GUI runner - auto runner = new ignition::gazebo::GuiRunner(worldName); - runner->connect(&app, &ignition::gui::Application::PluginAdded, runner, - &ignition::gazebo::GuiRunner::OnPluginAdded); - runner->setParent(ignition::gui::App()); - runners.push_back(runner); - } - mainWin->configChanged(); - } - - if (runners.empty()) - { - ignerr << "Failed to start a GUI runner." << std::endl; - return -1; - } - - // If no plugins have been added, load default config file - auto plugins = mainWin->findChildren(); - if (plugins.empty()) - { - // Check if there's a default config file under - // ~/.ignition/gazebo and use that. If there isn't, copy - // the installed file there first. - if (!ignition::common::exists(defaultConfig)) - { - auto installedConfig = ignition::common::joinPaths( - IGNITION_GAZEBO_GUI_CONFIG_PATH, "gui.config"); - - if (!ignition::common::createDirectories(defaultConfigDir)) - { - ignerr << "Failed to create directory [" << defaultConfigDir - << "]." << std::endl; - return -1; - } - - if (!ignition::common::exists(installedConfig)) - { - ignerr << "Failed to copy installed config [" << installedConfig - << "] to default config [" << defaultConfig << "]." - << "(file " << installedConfig << " doesn't exist)" - << std::endl; - return -1; - } - - if (!ignition::common::copyFile(installedConfig, defaultConfig)) - { - ignerr << "Failed to copy installed config [" << installedConfig - << "] to default config [" << defaultConfig << "]." - << std::endl; - return -1; - } - else - { - ignmsg << "Copied installed config [" << installedConfig - << "] to default config [" << defaultConfig << "]." - << std::endl; - } - } - - // Also set ~/.ignition/gazebo/ver/gui.config as the default path - if (!app.LoadConfig(defaultConfig)) - { - ignerr << "Failed to load config file[" << _guiConfig << "]." - << std::endl; - return -1; - } - } - - // Run main window. - // This blocks until the window is closed or we receive a SIGINT - app.exec(); - - for (auto runner : runners) - delete runner; - runners.clear(); - - igndbg << "Shutting down GUI" << std::endl; - return 0; -} From 52d12f18315d83370a166d73c8bc6fe7108f282e Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 6 May 2022 10:01:39 -0700 Subject: [PATCH 14/28] Extend Multicoptor Control system to include nested model inertial params (#1450) Updated the multicoptor velocity control system's inertial computation to include nested models. With this change, the controller will now be able to generate more thrusts when a higher motor_constant is specified. Before this change, the drone will not be able to take-off. This is useful when adding payloads (in the form of nested models) with heavy mass to the drone platform Signed-off-by: Ian Chen --- .../MulticopterVelocityControl.cc | 43 +- .../MulticopterVelocityControl.hh | 7 + test/integration/multicopter.cc | 63 +++ .../quadcopter_velocity_control_nested.sdf | 452 ++++++++++++++++++ 4 files changed, 551 insertions(+), 14 deletions(-) create mode 100644 test/worlds/quadcopter_velocity_control_nested.sdf diff --git a/src/systems/multicopter_control/MulticopterVelocityControl.cc b/src/systems/multicopter_control/MulticopterVelocityControl.cc index 9eb4b248b4..17fc2b56ff 100644 --- a/src/systems/multicopter_control/MulticopterVelocityControl.cc +++ b/src/systems/multicopter_control/MulticopterVelocityControl.cc @@ -36,6 +36,7 @@ #include "ignition/gazebo/components/Gravity.hh" #include "ignition/gazebo/components/Inertial.hh" #include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/Link.hh" @@ -92,20 +93,7 @@ void MulticopterVelocityControl::Configure(const Entity &_entity, VehicleParameters vehicleParams; math::Inertiald vehicleInertial; - // Compute the vehicle's moment of inertia and mass assuming that all the - // links in the model belong to the vehicle. - for (const Entity &link : - _ecm.ChildrenByComponents(this->model.Entity(), components::Link())) - { - auto inertial = _ecm.Component(link); - if (nullptr == inertial) - { - ignerr << "Could not find inertial component on on link " - << this->comLinkName << std::endl; - return; - } - vehicleInertial += inertial->Data(); - } + vehicleInertial = this->VehicleInertial(_ecm, this->model.Entity()); vehicleParams.mass = vehicleInertial.MassMatrix().Mass(); vehicleParams.inertia = math::eigen3::convert(vehicleInertial.Moi()); @@ -318,6 +306,33 @@ void MulticopterVelocityControl::Configure(const Entity &_entity, this->initialized = true; } +////////////////////////////////////////////////// +math::Inertiald MulticopterVelocityControl::VehicleInertial( + const EntityComponentManager &_ecm, Entity _entity) +{ + math::Inertiald vehicleInertial; + + for (const Entity &link : + _ecm.ChildrenByComponents(_entity, components::Link())) + { + auto inertial = _ecm.Component(link); + if (nullptr == inertial) + { + ignerr << "Could not find inertial component on link " + << this->comLinkName << std::endl; + return vehicleInertial; + } + vehicleInertial += inertial->Data(); + } + + for (const Entity &modelEnt : + _ecm.ChildrenByComponents(_entity, components::Model())) + { + vehicleInertial += this->VehicleInertial(_ecm, modelEnt); + } + return vehicleInertial; +} + ////////////////////////////////////////////////// void MulticopterVelocityControl::PreUpdate( const ignition::gazebo::UpdateInfo &_info, diff --git a/src/systems/multicopter_control/MulticopterVelocityControl.hh b/src/systems/multicopter_control/MulticopterVelocityControl.hh index 0fe9d7ece6..c146ae3845 100644 --- a/src/systems/multicopter_control/MulticopterVelocityControl.hh +++ b/src/systems/multicopter_control/MulticopterVelocityControl.hh @@ -185,6 +185,13 @@ namespace systems ignition::gazebo::EntityComponentManager &_ecm, const Eigen::VectorXd &_vels); + /// \brief Get the vehicle inertial from child links and nested models + /// \param[in] _ecm Immutable reference to the EntityComponentManager + /// \param[in] _entity Model entity to get inertial for + private: math::Inertiald VehicleInertial( + const ignition::gazebo::EntityComponentManager &_ecm, + Entity _entity); + /// \brief Model interface private: Model model{kNullEntity}; diff --git a/test/integration/multicopter.cc b/test/integration/multicopter.cc index 6062e84630..669a2cfe47 100644 --- a/test/integration/multicopter.cc +++ b/test/integration/multicopter.cc @@ -34,6 +34,7 @@ #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/Model.hh" #include "ignition/gazebo/Server.hh" @@ -317,3 +318,65 @@ TEST_F(MulticopterTest, }); server->Run(true, 10, false); } + +///////////////////////////////////////////////// +TEST_F(MulticopterTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(MulticopterVelocityControlNestedModel)) +{ + // test that the drone is able to take off when carrying a payload + // (nexted model) with extra mass. + + // Start server + auto server = + this->StartServer("/test/worlds/quadcopter_velocity_control_nested.sdf"); + + test::Relay testSystem; + transport::Node node; + auto cmdVel = node.Advertise("/X3/gazebo/command/twist"); + + // Add the system + server->AddSystem(testSystem.systemPtr); + server->Run(true, 1, false); + + // get pose of drone in post update + math::Pose3d x3Pose; + testSystem.OnPostUpdate( + [&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto x3Ent = _ecm.EntityByComponents( + components::Model(), components::Name("X3")); + ASSERT_NE(kNullEntity, x3Ent); + + auto poseComp = _ecm.Component(x3Ent); + if (poseComp) + x3Pose = poseComp->Data(); + }); + + server->Run(true, 100, false); + + // check initial z pos + double initialZ = x3Pose.Pos().Z(); + EXPECT_GT(0.1, initialZ); + + // run for a few interations and verify drone is still on the ground + server->Run(true, 100, false); + EXPECT_NEAR(initialZ, x3Pose.Pos().Z(), 1e-3); + + // send linear z vel for drone to take off + msgs::Twist msg; + msgs::Set(msg.mutable_linear(), math::Vector3d(0, 0, 5)); + cmdVel.Publish(msg); + + // verify drone continues to fly higher over the duration of 1 second + double zHeight = x3Pose.Pos().Z(); + for (unsigned int i = 0; i < 10; ++i) + { + server->Run(true, 100, false); + EXPECT_LT(zHeight, x3Pose.Pos().Z()); + zHeight = x3Pose.Pos().Z(); + } + + // one last check to verify drone is at least 5 meters off the ground + EXPECT_LT(5.0, x3Pose.Pos().Z()); +} diff --git a/test/worlds/quadcopter_velocity_control_nested.sdf b/test/worlds/quadcopter_velocity_control_nested.sdf new file mode 100644 index 0000000000..8a1e2879fb --- /dev/null +++ b/test/worlds/quadcopter_velocity_control_nested.sdf @@ -0,0 +1,452 @@ + + + + + 0 + + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 0 0 0.053302 0 0 0 + + + 1.5 + + 0.0347563 + 0 + 0 + 0.07 + 0 + 0.0977 + + + + + + 0.30 0.42 0.11 + + + + + + + 0.15 0.21 0.11 + + + + + + 0.13 -0.22 0.023 0 -0 0 + + 0.005 + + 9.75e-07 + 0 + 0 + 4.17041e-05 + 0 + 4.26041e-05 + + + + + + 0.005 + 0.1 + + + + + + + 0.2 + 0.01 + + + + 0 0 1 1 + + + + + rotor_0 + base_link + + 0 0 1 + + -1e+16 + 1e+16 + + + + + -0.13 0.2 0.023 0 -0 0 + + 0.005 + + 9.75e-07 + 0 + 0 + 4.17041e-05 + 0 + 4.26041e-05 + + + + + + 0.005 + 0.1 + + + + + + + 0.2 + 0.01 + + + + 1 0 0 1 + + + + + rotor_1 + base_link + + 0 0 1 + + -1e+16 + 1e+16 + + + + + 0.13 0.22 0.023 0 -0 0 + + 0.005 + + 9.75e-07 + 0 + 0 + 4.17041e-05 + 0 + 4.26041e-05 + + + + + + 0.005 + 0.1 + + + + + + + 0.2 + 0.01 + + + + 0 0 1 1 + + + + + rotor_2 + base_link + + 0 0 1 + + -1e+16 + 1e+16 + + + + + -0.13 -0.2 0.023 0 -0 0 + + 0.005 + + 9.75e-07 + 0 + 0 + 4.17041e-05 + 0 + 4.26041e-05 + + + + + + 0.005 + 0.1 + + + + + + + 0.2 + 0.01 + + + + 1 0 0 1 + + + + + rotor_3 + base_link + + 0 0 1 + + -1e+16 + 1e+16 + + + + + + + + 2 + + 0.00166667 + 0 + 0 + 0.00166667 + 0 + 0.00166667 + + + + 0.025 0 0 0 1.57079 0 + + + 0.05 + 0.05 + + + + + 0.025 0 0 0 1.57079 0 + + + 0.05 + 0.05 + + + + 0.05 0.05 0.05 + 0.8 0.8 0.8 + + + + + + 2 + + 0.00166667 + 0 + 0 + 0.00166667 + 0 + 0.00166667 + + + + 0.0505 0 0 0 1.57079 0 + + + 0.04 + 0.01 + + + + + 0.0505 0 0 0 1.57079 0 + + + 0.04 + 0.01 + + + + 0.02 0.02 0.02 + 0.3 0.3 0.3 + + + + + link + link_1 + + + + base_link + nested_payload::link + + + + + X3 + rotor_0_joint + rotor_0 + ccw + 0.0125 + 0.025 + 8000.0 + 2.54858e-05 + 0.016 + gazebo/command/motor_speed + 0 + 8.06428e-05 + 1e-06 + motor_speed/0 + 1 + velocity + + + X3 + rotor_1_joint + rotor_1 + ccw + 0.0125 + 0.025 + 8000.0 + 2.54858e-05 + 0.016 + gazebo/command/motor_speed + 1 + 8.06428e-05 + 1e-06 + motor_speed/1 + 1 + velocity + + + X3 + rotor_2_joint + rotor_2 + cw + 0.0125 + 0.025 + 8000.0 + 2.54858e-05 + 0.016 + gazebo/command/motor_speed + 2 + 8.06428e-05 + 1e-06 + motor_speed/2 + 1 + velocity + + + X3 + rotor_3_joint + rotor_3 + cw + 0.0125 + 0.025 + 8000.0 + 2.54858e-05 + 0.016 + gazebo/command/motor_speed + 3 + 8.06428e-05 + 1e-06 + motor_speed/3 + 1 + velocity + + + X3 + gazebo/command/twist + enable + base_link + 2.7 2.7 2.7 + 10 10 0.15 + 0.4 0.52 0.18 + 2 2 2 + + + + rotor_0_joint + 8.54858e-06 + 0.016 + 1 + + + rotor_1_joint + 8.54858e-06 + 0.016 + 1 + + + rotor_2_joint + 8.54858e-06 + 0.016 + -1 + + + rotor_3_joint + 8.54858e-06 + 0.016 + -1 + + + + + + From c7e0b21d2d23ad5aff4121ea9f7b6c52db18fd2d Mon Sep 17 00:00:00 2001 From: William Lew Date: Fri, 6 May 2022 14:15:40 -0700 Subject: [PATCH 15/28] Camera trigger integration test (#1384) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Added trigger camera world Signed-off-by: William Lew * Added triggered camera world Signed-off-by: William Lew * Added instructions to trigger camera Signed-off-by: William Lew * Added triggerd camera integration test Signed-off-by: William Lew Co-authored-by: Alejandro Hernández Cordero --- examples/worlds/triggered_camera_sensor.sdf | 323 ++++++++++++++++++++ test/integration/CMakeLists.txt | 3 +- test/integration/triggered_camera.cc | 109 +++++++ test/worlds/triggered_camera_sensor.sdf | 57 ++++ 4 files changed, 491 insertions(+), 1 deletion(-) create mode 100644 examples/worlds/triggered_camera_sensor.sdf create mode 100644 test/integration/triggered_camera.cc create mode 100644 test/worlds/triggered_camera_sensor.sdf diff --git a/examples/worlds/triggered_camera_sensor.sdf b/examples/worlds/triggered_camera_sensor.sdf new file mode 100644 index 0000000000..1b7d450934 --- /dev/null +++ b/examples/worlds/triggered_camera_sensor.sdf @@ -0,0 +1,323 @@ + + + + + + 0.001 + 1.0 + + + + + ogre + + + + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + true + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + docked + + + + + + + docked + + + + + + + docked + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 -1 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + true + 3 0 0.5 0 0 0 + + + + + 0.125 + + + + + + + 0.125 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + false + + + + + + 4 0 1.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + true + camera/trigger + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 1 + 30 + true + camera + + + + + + 0 1 3 0.0 0.0 1.57 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone + + + + + diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index be910f8ab5..730d0e12b8 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -88,8 +88,9 @@ set(tests_needing_display sensors_system.cc sensors_system_battery.cc shader_param_system.cc - thermal_system.cc thermal_sensor_system.cc + thermal_system.cc + triggered_camera.cc ) # Add systems that need a valid display here. diff --git a/test/integration/triggered_camera.cc b/test/integration/triggered_camera.cc new file mode 100644 index 0000000000..928abea90c --- /dev/null +++ b/test/integration/triggered_camera.cc @@ -0,0 +1,109 @@ +/* + * 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 + +#ifdef _MSC_VER +#pragma warning(push, 0) +#endif +#include +#ifdef _MSC_VER +#pragma warning(pop) +#endif + +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/test_config.hh" +#include +#include +#include +#include +#include + +#include "plugins/MockSystem.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Test TriggeredCameraTest system +class TriggeredCameraTest : public InternalFixture<::testing::Test> +{ +}; + +std::mutex mutex; +msgs::Image imageMsg; +unsigned char *imageBuffer = nullptr; + +///////////////////////////////////////////////// +void imageCb(const msgs::Image &_msg) +{ + ASSERT_EQ(msgs::PixelFormatType::RGB_INT8, + _msg.pixel_format_type()); + + mutex.lock(); + unsigned int imageSamples = _msg.width() * _msg.height() * 3; + + if (!imageBuffer) + imageBuffer = new unsigned char[imageSamples]; + memcpy(imageBuffer, _msg.data().c_str(), imageSamples); + mutex.unlock(); +} + +///////////////////////////////////////////////// +// The test checks the Triggered Camera readings +TEST_F(TriggeredCameraTest, + IGN_UTILS_TEST_DISABLED_ON_MAC(TriggeredCameraBox)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "triggered_camera_sensor.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(100us); + server.Run(false, 0, false); + + // Subscribe to the image topic + transport::Node node; + node.Subscribe("/camera", &imageCb); + + ignition::transport::Node triggerNode; + std::string triggerTopic = + "/camera/trigger"; + + auto pub = triggerNode.Advertise(triggerTopic); + ignition::msgs::Boolean msg; + msg.set_data(true); + + int sleep{0}; + int maxSleep{30}; + while (imageBuffer == nullptr && sleep < maxSleep) + { + pub.Publish(msg); + std::this_thread::sleep_for(100ms); + sleep++; + } + EXPECT_LT(sleep, maxSleep); + ASSERT_NE(imageBuffer, nullptr); + + delete[] imageBuffer; +} diff --git a/test/worlds/triggered_camera_sensor.sdf b/test/worlds/triggered_camera_sensor.sdf new file mode 100644 index 0000000000..1cbc641fed --- /dev/null +++ b/test/worlds/triggered_camera_sensor.sdf @@ -0,0 +1,57 @@ + + + + + .001 + 0 + + + + + ogre2 + 1 0 0 1 + + + + true + 0 0 1.0 0 0 0 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + true + camera/trigger + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 30 + camera + + + + + From 06381eaa22754fd89e631e23c37e4b4a27aaec3f Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Thu, 12 May 2022 11:16:11 -0400 Subject: [PATCH 16/28] fix fuel url (#1479) Signed-off-by: Mabel Zhang --- examples/worlds/multi_lrauv_race.sdf | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/worlds/multi_lrauv_race.sdf b/examples/worlds/multi_lrauv_race.sdf index 717bf86180..3cba72cbf3 100644 --- a/examples/worlds/multi_lrauv_race.sdf +++ b/examples/worlds/multi_lrauv_race.sdf @@ -65,7 +65,7 @@ See https://github.com/ignitionrobotics/ign-gazebo/pull/730 --> -5 0 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/mabel/models/Turquoise turbidity generator + https://fuel.ignitionrobotics.org/1.0/mabelzhang/models/Turquoise turbidity generator @@ -352,12 +352,12 @@ 0 0 -1 0 0 3.1415926 - https://fuel.ignitionrobotics.org/1.0/mabel/models/ABCSign_5m + https://fuel.ignitionrobotics.org/1.0/mabelzhang/models/ABCSign_5m start_line 0 -25 -1 0 0 3.1415926 - https://fuel.ignitionrobotics.org/1.0/mabel/models/ABCSign_5m + https://fuel.ignitionrobotics.org/1.0/mabelzhang/models/ABCSign_5m finish_line From 738579df58c7dd5a3b2c3a53d9b55e9b3410ae98 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 12 May 2022 08:17:55 -0700 Subject: [PATCH 17/28] Extruded 2D polyline geometries (#1456) Signed-off-by: Louise Poubel --- examples/worlds/polylines.sdf | 233 +++++++++++++++++++++++++++++++++ src/Conversions.cc | 37 ++++++ src/Conversions_TEST.cc | 39 +++++- src/rendering/SceneManager.cc | 22 ++++ src/systems/physics/Physics.cc | 130 ++++++++++++++++-- 5 files changed, 446 insertions(+), 15 deletions(-) create mode 100644 examples/worlds/polylines.sdf diff --git a/examples/worlds/polylines.sdf b/examples/worlds/polylines.sdf new file mode 100644 index 0000000000..5178eb54ca --- /dev/null +++ b/examples/worlds/polylines.sdf @@ -0,0 +1,233 @@ + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + -5 0 5 0 1.57 0 + + + 0 0 0.5 0 0 0 + 1.0 + + 0.1666 + 0 + 0 + 0.1666 + 0 + 0.1666 + + + + + + + -0.5 -0.5 + -0.5 0.5 + 0.5 0.5 + 0 0 + 0.5 -0.5 + 1 + + + + + + + + -0.5 -0.5 + -0.5 0.5 + 0.5 0.5 + 0 0 + 0.5 -0.5 + 1 + + + + 1.0 0 0 1 + 1.0 0 0 1 + 1.0 0 0 1 + + + + + + + -5 -3 5 0 1.3 0 + + + 0 0 2 0 0 0 + + + + + -0.3 0.5 + 0.3 0.3 + 0.3 -0.3 + -0.3 -0.5 + -0.5 0 + 4 + + + + + + + + -0.3 0.5 + 0.3 0.3 + 0.3 -0.3 + -0.3 -0.5 + -0.5 0 + 4 + + + + 0 1.0 0 1 + 0 1.0 0 1 + 0 1.0 0 1 + + + + + + + -5 2 5 0 1.57 0 + + + 0.5 0.5 0.75 0 0 0 + + + + + 0 0 + 0 1 + 1 1 + 1 0 + 1.5 + + + + + + + + 0 0 + 0 1 + 1 1 + 1 0 + 1.5 + + + + 0 1.0 1.0 1 + 0 1.0 1.0 1 + 0 1.0 1.0 1 + + + + + + + -5 2 5 0 1.57 0 + + + 2.2 1.5 0.5 0 0 0 + + + + + 2.27467 1.0967 + 1.81094 2.35418 + 2.74009 2.35418 + 1.0 + + + 2.08173 0.7599 + 2.4693 0.7599 + 3.4323 3.28672 + 3.07689 3.28672 + 2.84672 2.63851 + 1.7077 2.63851 + 1.47753 3.28672 + 1.11704 3.28672 + 1.0 + + + + + + + + 2.27467 1.0967 + 1.81094 2.35418 + 2.74009 2.35418 + 1.0 + + + 2.08173 0.7599 + 2.4693 0.7599 + 3.4323 3.28672 + 3.07689 3.28672 + 2.84672 2.63851 + 1.7077 2.63851 + 1.47753 3.28672 + 1.11704 3.28672 + 1.0 + + + + 1.0 0 1.0 1 + 1.0 0 1.0 1 + 1.0 0 1.0 1 + + + + + + diff --git a/src/Conversions.cc b/src/Conversions.cc index 88b5b2817f..64c9097bdd 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -56,6 +56,7 @@ #include #include #include +#include #include #include @@ -188,6 +189,20 @@ msgs::Geometry ignition::gazebo::convert(const sdf::Geometry &_in) meshMsg->set_submesh(meshSdf->Submesh()); meshMsg->set_center_submesh(meshSdf->CenterSubmesh()); } + else if (_in.Type() == sdf::GeometryType::POLYLINE && + !_in.PolylineShape().empty()) + { + out.set_type(msgs::Geometry::POLYLINE); + for (const auto &polyline : _in.PolylineShape()) + { + auto polylineMsg = out.add_polyline(); + polylineMsg->set_height(polyline.Height()); + for (const auto &point : polyline.Points()) + { + msgs::Set(polylineMsg->add_point(), point); + } + } + } else { ignerr << "Geometry type [" << static_cast(_in.Type()) @@ -252,6 +267,28 @@ sdf::Geometry ignition::gazebo::convert(const msgs::Geometry &_in) out.SetMeshShape(meshShape); } + else if (_in.type() == msgs::Geometry::POLYLINE && _in.polyline_size() > 0) + { + out.SetType(sdf::GeometryType::POLYLINE); + + std::vector polylines; + + for (auto i = 0; i < _in.polyline().size(); ++i) + { + auto polylineMsg = _in.polyline(i); + sdf::Polyline polylineShape; + polylineShape.SetHeight(polylineMsg.height()); + + for (auto j = 0; j < polylineMsg.point().size(); ++j) + { + polylineShape.AddPoint(msgs::Convert(polylineMsg.point(j))); + } + + polylines.push_back(polylineShape); + } + + out.SetPolylineShape(polylines); + } else { ignerr << "Geometry type [" << static_cast(_in.type()) diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index 71ee1c639b..d3d906b277 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -424,6 +425,40 @@ TEST(Conversions, GeometryPlane) EXPECT_EQ(math::Vector3d::UnitY, newGeometry.PlaneShape()->Normal()); } +///////////////////////////////////////////////// +TEST(Conversions, GeometryPolyline) +{ + sdf::Geometry geometry; + geometry.SetType(sdf::GeometryType::POLYLINE); + + sdf::Polyline polylineShape; + polylineShape.SetHeight(1.23); + polylineShape.AddPoint({4.5, 6.7}); + polylineShape.AddPoint({8.9, 10.11}); + geometry.SetPolylineShape({polylineShape}); + + auto geometryMsg = convert(geometry); + EXPECT_EQ(msgs::Geometry::POLYLINE, geometryMsg.type()); + ASSERT_EQ(1, geometryMsg.polyline_size()); + EXPECT_DOUBLE_EQ(1.23, geometryMsg.polyline(0).height()); + ASSERT_EQ(2, geometryMsg.polyline(0).point_size()); + EXPECT_DOUBLE_EQ(4.5, geometryMsg.polyline(0).point(0).x()); + EXPECT_DOUBLE_EQ(6.7, geometryMsg.polyline(0).point(0).y()); + EXPECT_DOUBLE_EQ(8.9, geometryMsg.polyline(0).point(1).x()); + EXPECT_DOUBLE_EQ(10.11, geometryMsg.polyline(0).point(1).y()); + + auto newGeometry = convert(geometryMsg); + EXPECT_EQ(sdf::GeometryType::POLYLINE, newGeometry.Type()); + ASSERT_FALSE(newGeometry.PolylineShape().empty()); + ASSERT_EQ(1u, newGeometry.PolylineShape().size()); + EXPECT_DOUBLE_EQ(1.23, newGeometry.PolylineShape()[0].Height()); + ASSERT_EQ(2u, newGeometry.PolylineShape()[0].PointCount()); + EXPECT_DOUBLE_EQ(4.5, newGeometry.PolylineShape()[0].PointByIndex(0)->X()); + EXPECT_DOUBLE_EQ(6.7, newGeometry.PolylineShape()[0].PointByIndex(0)->Y()); + EXPECT_DOUBLE_EQ(8.9, newGeometry.PolylineShape()[0].PointByIndex(1)->X()); + EXPECT_DOUBLE_EQ(10.11, newGeometry.PolylineShape()[0].PointByIndex(1)->Y()); +} + ///////////////////////////////////////////////// TEST(Conversions, Inertial) { @@ -543,7 +578,7 @@ TEST(Conversions, Atmosphere) } ///////////////////////////////////////////////// -TEST(CONVERSIONS, MagnetometerSensor) +TEST(Conversions, MagnetometerSensor) { sdf::Sensor sensor; sensor.SetName("my_sensor"); @@ -583,7 +618,7 @@ TEST(CONVERSIONS, MagnetometerSensor) } ///////////////////////////////////////////////// -TEST(CONVERSIONS, AltimeterSensor) +TEST(Conversions, AltimeterSensor) { sdf::Sensor sensor; sensor.SetName("my_sensor"); diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index a5f360259a..a38ed7ffe8 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -33,6 +34,7 @@ #include #include #include +#include #include #include @@ -450,6 +452,26 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, geom = this->dataPtr->scene->CreateMesh(descriptor); scale = _geom.MeshShape()->Scale(); } + else if (_geom.Type() == sdf::GeometryType::POLYLINE) + { + std::vector> vertices; + for (const auto &polyline : _geom.PolylineShape()) + { + vertices.push_back(polyline.Points()); + } + + std::string name("POLYLINE_" + common::Uuid().String()); + + auto meshManager = ignition::common::MeshManager::Instance(); + meshManager->CreateExtrudedPolyline(name, vertices, + _geom.PolylineShape()[0].Height()); + + rendering::MeshDescriptor descriptor; + descriptor.meshName = name; + descriptor.mesh = meshManager->MeshByName(name); + + geom = this->dataPtr->scene->CreateMesh(descriptor); + } else { ignerr << "Unsupported geometry type" << std::endl; diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 647250b4bf..1ec9b9c43d 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -33,6 +33,7 @@ #include #include #include +#include #include #include #include @@ -73,6 +74,7 @@ #include #include #include +#include #include #include @@ -177,6 +179,31 @@ class ignition::gazebo::systems::PhysicsPrivate /// \param[in] _ecm Constant reference to ECM. public: void CreatePhysicsEntities(const EntityComponentManager &_ecm); + + /// \brief Create world entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateWorldEntities(const EntityComponentManager &_ecm); + + /// \brief Create model entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateModelEntities(const EntityComponentManager &_ecm); + + /// \brief Create link entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateLinkEntities(const EntityComponentManager &_ecm); + + /// \brief Create collision entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateCollisionEntities(const EntityComponentManager &_ecm); + + /// \brief Create joint entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateJointEntities(const EntityComponentManager &_ecm); + + /// \brief Create Battery entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateBatteryEntities(const EntityComponentManager &_ecm); + /// \brief Remove physics entities if they are removed from the ECM /// \param[in] _ecm Constant reference to ECM. public: void RemovePhysicsEntities(const EntityComponentManager &_ecm); @@ -664,6 +691,18 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) ////////////////////////////////////////////////// void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) +{ + this->CreateWorldEntities(_ecm); + this->CreateModelEntities(_ecm); + this->CreateLinkEntities(_ecm); + // We don't need to add visuals to the physics engine. + this->CreateCollisionEntities(_ecm); + this->CreateJointEntities(_ecm); + this->CreateBatteryEntities(_ecm); +} + +////////////////////////////////////////////////// +void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm) { // Get all the new worlds _ecm.EachNew( @@ -689,7 +728,11 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) return true; }); +} +////////////////////////////////////////////////// +void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) +{ _ecm.EachNew( [&](const Entity &_entity, @@ -817,7 +860,11 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) return true; }); +} +////////////////////////////////////////////////// +void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) +{ _ecm.EachNew( [&](const Entity &_entity, @@ -871,10 +918,11 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) return true; }); +} - // We don't need to add visuals to the physics engine. - - // collisions +////////////////////////////////////////////////// +void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) +{ _ecm.EachNew( @@ -953,6 +1001,55 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) math::eigen3::convert(_pose->Data()), math::eigen3::convert(meshSdf->Scale())); } + else if (_geom->Data().Type() == sdf::GeometryType::POLYLINE) + { + auto polylineSdf = _geom->Data().PolylineShape(); + if (polylineSdf.empty()) + { + ignwarn << "Polyline geometry for collision [" << _name->Data() + << "] missing polylines." << std::endl; + return true; + } + + std::vector> vertices; + for (const auto &polyline : _geom->Data().PolylineShape()) + { + vertices.push_back(polyline.Points()); + } + + std::string name("POLYLINE_" + common::Uuid().String()); + auto meshManager = ignition::common::MeshManager::Instance(); + meshManager->CreateExtrudedPolyline(name, vertices, + _geom->Data().PolylineShape()[0].Height()); + + auto polyline = meshManager->MeshByName(name); + if (nullptr == polyline) + { + ignwarn << "Failed to create polyline for collision [" + << _name->Data() << "]." << std::endl; + return true; + } + + auto linkMeshFeature = + this->entityLinkMap.EntityCast(_parent->Data()); + if (!linkMeshFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to process polyline geometries, but the" + << " physics engine doesn't support feature " + << "[AttachMeshShapeFeature]. Polylines will be ignored." + << std::endl; + informed = true; + } + return true; + } + + collisionPtrPhys = linkMeshFeature->AttachMeshShape(_name->Data(), + *polyline, + math::eigen3::convert(_pose->Data())); + } else { auto linkCollisionFeature = @@ -1003,8 +1100,11 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) topLevelModel(_entity, _ecm))); return true; }); +} - // joints +////////////////////////////////////////////////// +void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) +{ _ecm.EachNew( - [&](const Entity & _entity, const components::BatterySoC *)->bool - { - // Parent entity of battery is model entity - this->entityOffMap.insert(std::make_pair( - _ecm.ParentEntity(_entity), false)); - return true; - }); - // Detachable joints _ecm.EachNew( [&](const Entity &_entity, @@ -1222,6 +1313,19 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) }); } +////////////////////////////////////////////////// +void PhysicsPrivate::CreateBatteryEntities(const EntityComponentManager &_ecm) +{ + _ecm.EachNew( + [&](const Entity & _entity, const components::BatterySoC *)->bool + { + // Parent entity of battery is model entity + this->entityOffMap.insert(std::make_pair( + _ecm.ParentEntity(_entity), false)); + return true; + }); +} + ////////////////////////////////////////////////// void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) { From 7225f383919a1aa71cdafa9558fb3895f9f66f11 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 12 May 2022 09:27:23 -0700 Subject: [PATCH 18/28] View polyline collisions on the GUI (#1481) Signed-off-by: Louise Poubel --- .../VisualizationCapabilities.cc | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc index 731cd22bc4..e995949bab 100644 --- a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc +++ b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -65,6 +66,7 @@ #include #include #include +#include #include #include "ignition/gazebo/components/CastShadows.hh" @@ -1295,6 +1297,26 @@ rendering::GeometryPtr VisualizationCapabilitiesPrivate::CreateGeometry( } scale = _geom.HeightmapShape()->Size(); } + else if (_geom.Type() == sdf::GeometryType::POLYLINE) + { + std::vector> vertices; + for (const auto &polyline : _geom.PolylineShape()) + { + vertices.push_back(polyline.Points()); + } + + std::string name("POLYLINE_" + common::Uuid().String()); + + auto meshManager = ignition::common::MeshManager::Instance(); + meshManager->CreateExtrudedPolyline(name, vertices, + _geom.PolylineShape()[0].Height()); + + rendering::MeshDescriptor descriptor; + descriptor.meshName = name; + descriptor.mesh = meshManager->MeshByName(name); + + geom = this->scene->CreateMesh(descriptor); + } else { ignerr << "Unsupported geometry type" << std::endl; From ce4241cdb8dd8bb566b7c2cde2e3be05757e97ae Mon Sep 17 00:00:00 2001 From: Guillaume Jacquenot Date: Sat, 14 May 2022 21:50:22 +0200 Subject: [PATCH 19/28] :books: Fixed broken URL link to gazebo documentation (#1486) Signed-off-by: Guillaume Jacquenot --- examples/scripts/python_api/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/scripts/python_api/README.md b/examples/scripts/python_api/README.md index a828853737..620e2bb7be 100644 --- a/examples/scripts/python_api/README.md +++ b/examples/scripts/python_api/README.md @@ -3,4 +3,4 @@ This example shows how to use Gazebo's Python API. For more information, see the -[Python interfaces](https://ignitionrobotics.org/api/gazebo/6.4/python_interfaces.html) tutorial. +[Python interfaces](https://ignitionrobotics.org/api/gazebo/6.9/python_interfaces.html) tutorial. From a4b7e8553b3fa2193fba7c37e62ea22f9399181b Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 16 May 2022 13:46:33 -0700 Subject: [PATCH 20/28] Delete unused gazebo.hh.in (#1490) Signed-off-by: Steve Peters --- include/ignition/gazebo/gazebo.hh.in | 3 --- 1 file changed, 3 deletions(-) delete mode 100644 include/ignition/gazebo/gazebo.hh.in diff --git a/include/ignition/gazebo/gazebo.hh.in b/include/ignition/gazebo/gazebo.hh.in deleted file mode 100644 index d9d633cc59..0000000000 --- a/include/ignition/gazebo/gazebo.hh.in +++ /dev/null @@ -1,3 +0,0 @@ -// Automatically generated -#include /config.hh> -${ign_headers} From f4198be19ae432bbcc584b73887178320e69ed90 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 17 May 2022 20:16:43 +0200 Subject: [PATCH 21/28] Adding rssi (#1482) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adding rssi. Signed-off-by: Carlos Agüero --- src/systems/rf_comms/RFComms.cc | 15 ++++++++++++++- test/integration/rf_comms.cc | 3 +++ 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/systems/rf_comms/RFComms.cc b/src/systems/rf_comms/RFComms.cc index 199d6f55ce..c18cb6b20f 100644 --- a/src/systems/rf_comms/RFComms.cc +++ b/src/systems/rf_comms/RFComms.cc @@ -15,6 +15,8 @@ * */ +#include + #include #include #include @@ -447,7 +449,18 @@ void RFComms::Step( #endif if (sendPacket) - _newRegistry[msg->dst_address()].inboundMsgs.push_back(msg); + { + // We create a copy of the outbound message because each destination + // might have a different rssi value. + auto inboundMsg = std::make_shared(*msg); + + // Add rssi. + auto *rssiPtr = inboundMsg->mutable_header()->add_data(); + rssiPtr->set_key("rssi"); + rssiPtr->add_value(std::to_string(rssi)); + + _newRegistry[msg->dst_address()].inboundMsgs.push_back(inboundMsg); + } } } diff --git a/test/integration/rf_comms.cc b/test/integration/rf_comms.cc index a15cd222d4..2cbb7491aa 100644 --- a/test/integration/rf_comms.cc +++ b/test/integration/rf_comms.cc @@ -65,7 +65,10 @@ TEST_F(RFCommsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RFComms)) ignition::msgs::StringMsg receivedMsg; receivedMsg.ParseFromString(_msg.data()); + EXPECT_EQ(expected, receivedMsg.data()); + ASSERT_GT(_msg.header().data_size(), 0); + EXPECT_EQ("rssi", _msg.header().data(0).key()); msgCounter++; }; From f3686aa139a63dd93c8edee64402f8d471a363a7 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 19 May 2022 15:16:23 +0800 Subject: [PATCH 22/28] Fix Documentation Header. (#1501) Seems like some weird unicode issue was causing The documentation header not to be parsed as documentation. Signed-off-by: Arjo Chakravarty --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index d5b19186a9..e949910270 100644 --- a/README.md +++ b/README.md @@ -123,7 +123,7 @@ export IGN_CONFIG_PATH=$HOME/.ignition/tools/configs This issue is tracked [here](https://github.com/ignitionrobotics/ign-tools/issues/8). -# Documentation +# Documentation See the [installation tutorial](https://ignitionrobotics.org/api/gazebo/6.0/install.html). From 62272db983d929d1601960ab7219d01a51b2c3ec Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 20 May 2022 10:38:45 +0800 Subject: [PATCH 23/28] Makes thruster stop when battery runs out. (#1495) Previously the thruster system would not stop when the battery was out. Now if a battery is attached to the model, the thruster will automatically stop once the battery runs out. Signed-off-by: Arjo Chakravarty --- src/systems/thruster/Thruster.cc | 68 +++++++++++++--- src/systems/thruster/Thruster.hh | 17 ++-- test/integration/thruster.cc | 50 +++++++++--- test/worlds/thruster_battery.sdf | 133 +++++++++++++++++++++++++++++++ 4 files changed, 241 insertions(+), 27 deletions(-) create mode 100644 test/worlds/thruster_battery.sdf diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 885f5ca209..92a74bb3a7 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -27,6 +27,7 @@ #include #include "ignition/gazebo/components/AngularVelocity.hh" +#include "ignition/gazebo/components/BatterySoC.hh" #include "ignition/gazebo/components/ChildLinkName.hh" #include "ignition/gazebo/components/JointAxis.hh" #include "ignition/gazebo/components/JointVelocityCmd.hh" @@ -54,6 +55,12 @@ class ignition::gazebo::systems::ThrusterPrivateData /// \brief Desired propeller angular velocity in rad / s public: double propellerAngVel = 0.0; + /// \brief Enabled or not + public: bool enabled = true; + + /// \brief Model entity + public: ignition::gazebo::Entity modelEntity; + /// \brief The link entity which will spin public: ignition::gazebo::Entity linkEntity; @@ -97,13 +104,18 @@ class ignition::gazebo::systems::ThrusterPrivateData /// \brief Diameter of propeller in m, default: 0.02 public: double propellerDiameter = 0.02; - /// \brief callback for handling thrust update - public: void OnCmdThrust(const ignition::msgs::Double &_msg); + /// \brief Callback for handling thrust update + public: void OnCmdThrust(const msgs::Double &_msg); - /// \brief function which computes angular velocity from thrust + /// \brief Function which computes angular velocity from thrust /// \param[in] _thrust Thrust in N /// \return Angular velocity in rad/s public: double ThrustToAngularVec(double _thrust); + + /// \brief Returns a boolean if the battery has sufficient charge to continue + /// \return True if battery is charged, false otherwise. If no battery found, + /// returns true. + public: bool HasSufficientBattery(const EntityComponentManager &_ecm) const; }; ///////////////////////////////////////////////// @@ -115,13 +127,14 @@ Thruster::Thruster(): ///////////////////////////////////////////////// void Thruster::Configure( - const ignition::gazebo::Entity &_entity, + const Entity &_entity, const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::EventManager &/*_eventMgr*/) + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) { // Create model object, to access convenient functions - auto model = ignition::gazebo::Model(_entity); + this->dataPtr->modelEntity = _entity; + auto model = Model(_entity); auto modelName = model.Name(_ecm); // Get namespace @@ -277,10 +290,10 @@ void Thruster::Configure( } ///////////////////////////////////////////////// -void ThrusterPrivateData::OnCmdThrust(const ignition::msgs::Double &_msg) +void ThrusterPrivateData::OnCmdThrust(const msgs::Double &_msg) { std::lock_guard lock(mtx); - this->thrust = ignition::math::clamp(ignition::math::fixnan(_msg.data()), + this->thrust = math::clamp(math::fixnan(_msg.data()), this->cmdMin, this->cmdMax); // Thrust is proportional to the Rotation Rate squared @@ -303,6 +316,28 @@ double ThrusterPrivateData::ThrustToAngularVec(double _thrust) return propAngularVelocity; } +///////////////////////////////////////////////// +bool ThrusterPrivateData::HasSufficientBattery( + const EntityComponentManager &_ecm) const +{ + bool result = true; + _ecm.Each([&]( + const Entity &_entity, + const components::BatterySoC *_data + ){ + if(_ecm.ParentEntity(_entity) == this->modelEntity) + { + if(_data->Data() <= 0) + { + result = false; + } + } + + return true; + }); + return result; +} + ///////////////////////////////////////////////// void Thruster::PreUpdate( const ignition::gazebo::UpdateInfo &_info, @@ -311,6 +346,11 @@ void Thruster::PreUpdate( if (_info.paused) return; + if (!this->dataPtr->enabled) + { + return; + } + ignition::gazebo::Link link(this->dataPtr->linkEntity); auto pose = worldPose(this->dataPtr->linkEntity, _ecm); @@ -367,10 +407,18 @@ void Thruster::PreUpdate( unitVector * torque); } +///////////////////////////////////////////////// +void Thruster::PostUpdate(const UpdateInfo &/*unused*/, + const EntityComponentManager &_ecm) +{ + this->dataPtr->enabled = this->dataPtr->HasSufficientBattery(_ecm); +} + IGNITION_ADD_PLUGIN( Thruster, System, Thruster::ISystemConfigure, - Thruster::ISystemPreUpdate) + Thruster::ISystemPreUpdate, + Thruster::ISystemPostUpdate) IGNITION_ADD_PLUGIN_ALIAS(Thruster, "ignition::gazebo::systems::Thruster") diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index 78eafb849b..96b397ea45 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -89,25 +89,30 @@ namespace systems /// ``` /// The vehicle should move in a circle. class Thruster: - public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPreUpdate + public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate { /// \brief Constructor public: Thruster(); /// Documentation inherited public: void Configure( - const ignition::gazebo::Entity &_entity, + const Entity &_entity, const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::EventManager &/*_eventMgr*/) override; + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) override; /// Documentation inherited public: void PreUpdate( const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) override; + /// Documentation inherited + public: void PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm); + /// \brief Private data pointer private: std::unique_ptr dataPtr; }; diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index d2333bc3f1..1d28df8d5d 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -155,17 +155,20 @@ void ThrusterTest::TestWorld(const std::string &_world, pub.Publish(msg); // Check movement - for (sleep = 0; modelPoses.back().Pos().X() < 5.0 && sleep < maxSleep; - ++sleep) + if (_namespace != "lowbattery") { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - fixture.Server()->Run(true, 100, false); - } - EXPECT_LT(sleep, maxSleep); - EXPECT_LT(5.0, modelPoses.back().Pos().X()); + for (sleep = 0; modelPoses.back().Pos().X() < 5.0 && sleep < maxSleep; + ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + fixture.Server()->Run(true, 100, false); + } + EXPECT_LT(sleep, maxSleep); + EXPECT_LT(5.0, modelPoses.back().Pos().X()); - EXPECT_EQ(100u * sleep, modelPoses.size()); - EXPECT_EQ(100u * sleep, propellerAngVels.size()); + EXPECT_EQ(100u * sleep, modelPoses.size()); + EXPECT_EQ(100u * sleep, propellerAngVels.size()); + } // max allowed force double force{300.0}; @@ -178,6 +181,14 @@ void ThrusterTest::TestWorld(const std::string &_world, double xTol{1e-2}; for (unsigned int i = 0; i < modelPoses.size(); ++i) { + if (_namespace == "lowbattery" && i > 545) + { + // Battery discharged should not accelerate + EXPECT_NEAR(modelPoses[i-1].Pos().X() - modelPoses[i-2].Pos().X(), + modelPoses[i].Pos().X() - modelPoses[i-1].Pos().X(), 1e-6); + continue; + } + auto pose = modelPoses[i]; auto time = dt * i; EXPECT_NEAR(force * time * time / (2 * mass), pose.Pos().X(), xTol); @@ -188,7 +199,7 @@ void ThrusterTest::TestWorld(const std::string &_world, // The joint velocity command adds some roll to the body which the PID // wrench doesn't - if (_namespace == "custom") + if (_namespace == "custom" || _namespace == "lowbattery") EXPECT_NEAR(0.0, pose.Rot().Roll(), 0.1); else EXPECT_NEAR(0.0, pose.Rot().Roll(), _baseTol); @@ -205,7 +216,14 @@ void ThrusterTest::TestWorld(const std::string &_world, // It takes a few iterations to reach the speed if (i > 25) { - EXPECT_NEAR(omega, angVel.X(), omegaTol) << i; + if (_namespace == "lowbattery" && i > 545) + { + EXPECT_NEAR(0.0, angVel.X(), _baseTol); + } + else + { + EXPECT_NEAR(omega, angVel.X(), omegaTol) << i; + } } EXPECT_NEAR(0.0, angVel.Y(), _baseTol); EXPECT_NEAR(0.0, angVel.Z(), _baseTol); @@ -234,3 +252,13 @@ TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(VelocityControl)) this->TestWorld(world, "custom", 0.005, 950, 0.25, 1e-2); } +///////////////////////////////////////////////// +TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(BatteryIntegration)) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "thruster_battery.sdf"); + + // Tolerance is high because the joint command disturbs the vehicle body + this->TestWorld(world, "lowbattery", 0.005, 950, 0.25, 1e-2); +} + diff --git a/test/worlds/thruster_battery.sdf b/test/worlds/thruster_battery.sdf new file mode 100644 index 0000000000..3cf5a3495e --- /dev/null +++ b/test/worlds/thruster_battery.sdf @@ -0,0 +1,133 @@ + + + + + + + 0 + + + + 0 0 0 + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + + 0 0 0 0 1.57 0 + + 100 + + 33.89 + 0 + 0 + 33.89 + 0 + 1.125 + + + + + + 2 + 0.15 + + + + + + + -1.05 0 0 0 0 0 + + 0.1 + + 0.000354167 + 0 + 0 + 0.000021667 + 0 + 0.000334167 + + + + + + 0.01 0.25 0.05 + + + + + + + body + propeller + + 1 0 0 + + -1e+12 + 1e+12 + 1e6 + 1e6 + + + + + + lowbattery + propeller_joint + 0.005 + 950 + 0.25 + true + 300 + 0 + + + + linear_battery + 14.4 + 14.4 + true + + 0.00005 + 400 + + 0.003064 + + 28.8 + + + + true + true + 42.1 + + + + + From 3b6c0f2a0bc71c5ff391852615609deec3c03674 Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Mon, 23 May 2022 13:19:37 -0700 Subject: [PATCH 24/28] Updating hydrodynamics plugin description (#1502) Signed-off-by: Dharini Dutia --- src/systems/hydrodynamics/Hydrodynamics.cc | 2 +- src/systems/hydrodynamics/Hydrodynamics.hh | 30 +++++++++++----------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/systems/hydrodynamics/Hydrodynamics.cc b/src/systems/hydrodynamics/Hydrodynamics.cc index 767a7e036d..5d278790f8 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.cc +++ b/src/systems/hydrodynamics/Hydrodynamics.cc @@ -251,7 +251,7 @@ void Hydrodynamics::Configure( if (!_sdf->HasElement("link_name")) { - ignerr << "You musk specify a for the hydrodynamic" + ignerr << "You must specify a for the hydrodynamic" << " plugin to act upon"; return; } diff --git a/src/systems/hydrodynamics/Hydrodynamics.hh b/src/systems/hydrodynamics/Hydrodynamics.hh index a06b37be7c..f9f87cfe4c 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.hh +++ b/src/systems/hydrodynamics/Hydrodynamics.hh @@ -41,27 +41,27 @@ namespace systems /// plugin), etc. /// /// # System Parameters - /// The exact description of these parameters can be found on p. 37 of - /// Fossen's book. They are used to calculate added mass, linear and quadratic - /// drag and coriolis force. + /// The exact description of these parameters can be found on p. 37 and + /// p. 43 of Fossen's book. They are used to calculate added mass, linear and + /// quadratic drag and coriolis force. /// * - Added mass in x direction [kg] /// * - Added mass in y direction [kg] /// * - Added mass in z direction [kg] /// * - Added mass in roll direction [kgm^2] /// * - Added mass in pitch direction [kgm^2] /// * - Added mass in yaw direction [kgm^2] - /// * - Stability derivative, 2nd order, x component [kg/m] - /// * - Stability derivative, 1st order, x component [kg] - /// * - Stability derivative, 2nd order, y component [kg/m] - /// * - Stability derivative, 1st order, y component [kg] - /// * - Stability derivative, 2nd order, z component [kg/m] - /// * - Stability derivative, 1st order, z component [kg] - /// * - Stability derivative, 2nd order, roll component [kg/m^2] - /// * - Stability derivative, 1st order, roll component [kg/m] - /// * - Stability derivative, 2nd order, pitch component [kg/m^2] - /// * - Stability derivative, 1st order, pitch component [kg/m] - /// * - Stability derivative, 2nd order, yaw component [kg/m^2] - /// * - Stability derivative, 1st order, yaw component [kg/m] + /// * - Quadratic damping, 2nd order, x component [kg/m] + /// * - Linear damping, 1st order, x component [kg] + /// * - Quadratic damping, 2nd order, y component [kg/m] + /// * - Linear damping, 1st order, y component [kg] + /// * - Quadratic damping, 2nd order, z component [kg/m] + /// * - Linear damping, 1st order, z component [kg] + /// * - Quadratic damping, 2nd order, roll component [kg/m^2] + /// * - Linear damping, 1st order, roll component [kg/m] + /// * - Quadratic damping, 2nd order, pitch component [kg/m^2] + /// * - Linear damping, 1st order, pitch component [kg/m] + /// * - Quadratic damping, 2nd order, yaw component [kg/m^2] + /// * - Linear damping, 1st order, yaw component [kg/m] /// Additionally the system also supports the following parameters: /// * - The density of the fluid its moving in. /// Defaults to 998kgm^-3. [kgm^-3, deprecated] From 26f67fa9ae9cd2cbd03a21dbafee41fb16688806 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 31 May 2022 07:32:56 -0700 Subject: [PATCH 25/28] Scene update resource finder (#1508) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Added resource finder and add scene information to broadcaster Signed-off-by: Nate Koenig * Added tests Signed-off-by: Nate Koenig * update documentation Signed-off-by: Nate Koenig * codecheck Signed-off-by: Nate Koenig * Update ServerPrivate.hh * Updates Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig Co-authored-by: Carlos Agüero --- src/ServerPrivate.cc | 73 +++++++++++++++++++ src/ServerPrivate.hh | 22 ++++++ src/Server_TEST.cc | 72 ++++++++++++++++++ .../scene_broadcaster/SceneBroadcaster.cc | 17 ++++- test/integration/scene_broadcaster_system.cc | 38 ++++++++++ test/worlds/conveyor.sdf | 2 + 6 files changed, 222 insertions(+), 2 deletions(-) diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 7bd36065ce..55f8784154 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -356,6 +356,21 @@ void ServerPrivate::SetupTransport() << "]" << std::endl; } + // Advertise a service that returns the full path, on the Gazebo server's + // host machine, based on a provided URI. + std::string resolvePathService{"/gazebo/resource_paths/resolve"}; + if (this->node.Advertise(resolvePathService, + &ServerPrivate::ResourcePathsResolveService, this)) + { + ignmsg << "Resource path resolve service on [" << resolvePathService << "]." + << std::endl; + } + else + { + ignerr << "Something went wrong, failed to advertise [" << getPathService + << "]" << std::endl; + } + std::string pathTopic{"/gazebo/resource_paths"}; this->pathPub = this->node.Advertise(pathTopic); @@ -485,6 +500,64 @@ bool ServerPrivate::ResourcePathsService( return true; } +////////////////////////////////////////////////// +bool ServerPrivate::ResourcePathsResolveService( + const ignition::msgs::StringMsg &_req, + ignition::msgs::StringMsg &_res) +{ + // Get the request + std::string req = _req.data(); + + // Handle the case where the request is already a valid path + if (common::exists(req)) + { + _res.set_data(req); + return true; + } + + // Try Fuel + std::string path = + fuel_tools::fetchResourceWithClient(req, *this->fuelClient.get()); + if (!path.empty() && common::exists(path)) + { + _res.set_data(path); + return true; + } + + // Check for the file:// prefix. + std::string prefix = "file://"; + if (req.find(prefix) == 0) + { + req = req.substr(prefix.size()); + // Check to see if the path exists + if (common::exists(req)) + { + _res.set_data(req); + return true; + } + } + + // Check for the model:// prefix + prefix = "model://"; + if (req.find(prefix) == 0) + req = req.substr(prefix.size()); + + // Checkout resource paths + std::vector gzPaths = resourcePaths(); + for (const std::string &gzPath : gzPaths) + { + std::string fullPath = common::joinPaths(gzPath, req); + if (common::exists(fullPath)) + { + _res.set_data(fullPath); + return true; + } + } + + // Otherwise the resource could not be found + return false; +} + ////////////////////////////////////////////////// std::string ServerPrivate::FetchResource(const std::string &_uri) { diff --git a/src/ServerPrivate.hh b/src/ServerPrivate.hh index b7f23c26d9..db13460131 100644 --- a/src/ServerPrivate.hh +++ b/src/ServerPrivate.hh @@ -117,6 +117,28 @@ namespace ignition /// \return True if successful. private: bool ResourcePathsService(ignition::msgs::StringMsg_V &_res); + /// \brief Callback for a resource path resolve service. This service + /// will return the full path to a provided resource's URI. An empty + /// string and return value of false will be used if the resource could + /// not be found. + /// + /// Fuel will be checked and then the GZ_GAZEBO_RESOURCE_PATH environment + /// variable paths. This service will not check for files relative to + /// working directory of the Gazebo server. + /// + /// \param[in] _req Request filled with a resource URI to resolve. + /// Example values could be: + /// * https://URI_TO_A_FUEL_RESOURCE + /// * model://MODLE_NAME/meshes/MESH_NAME + /// * file://PATH/TO/FILE + /// + /// \param[out] _res Response filled with the resolved path, or empty + /// if the resource could not be found. + /// \return True if successful, false otherwise. + private: bool ResourcePathsResolveService( + const ignition::msgs::StringMsg &_req, + ignition::msgs::StringMsg &_res); + /// \brief Callback for server control service. /// \param[out] _req The control request. /// \param[out] _res Whether the request was successfully fullfilled. diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index a2c6d1803d..619e08b2b1 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -1033,6 +1033,78 @@ TEST_P(ServerFixture, AddResourcePaths) } } +///////////////////////////////////////////////// +TEST_P(ServerFixture, ResolveResourcePaths) +{ + ignition::common::setenv("IGN_GAZEBO_RESOURCE_PATH", ""); + ignition::common::setenv("SDF_PATH", ""); + ignition::common::setenv("IGN_FILE_PATH", ""); + + ServerConfig serverConfig; + gazebo::Server server(serverConfig); + + EXPECT_FALSE(*server.Running(0)); + + auto test = std::function( + [&](const std::string &_uri, const std::string &_expected, bool _found) + { + transport::Node node; + msgs::StringMsg req, res; + bool result{false}; + bool executed{false}; + int sleep{0}; + int maxSleep{30}; + + req.set_data(_uri); + while (!executed && sleep < maxSleep) + { + igndbg << "Requesting /gazebo/resource_paths/resolve" << std::endl; + executed = node.Request("/gazebo/resource_paths/resolve", req, 100, + res, result); + sleep++; + } + EXPECT_TRUE(executed); + EXPECT_EQ(_found, result); + EXPECT_EQ(_expected, res.data()) << "Expected[" << _expected + << "] Received[" << res.data() << "]"; + }); + + // Make sure the resource path is clear + ignition::common::setenv("IGN_GAZEBO_RESOURCE_PATH", ""); + + // An absolute path should return the same absolute path + test(PROJECT_SOURCE_PATH, PROJECT_SOURCE_PATH, true); + + // An absolute path, with the file:// prefix, should return the absolute path + test(std::string("file://") + + PROJECT_SOURCE_PATH, PROJECT_SOURCE_PATH, true); + + // A non-absolute path with no RESOURCE_PATH should not find the resource + test(common::joinPaths("test", "worlds", "plugins.sdf"), "", false); + + // Try again, this time with a RESOURCE_PATH + common::setenv("IGN_GAZEBO_RESOURCE_PATH", PROJECT_SOURCE_PATH); + test(common::joinPaths("test", "worlds", "plugins.sdf"), + common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "plugins.sdf"), + true); + // With the file:// prefix should also work + test(std::string("file://") + + common::joinPaths("test", "worlds", "plugins.sdf"), + common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "plugins.sdf"), + true); + + // The model:// URI should not resolve + test("model://include_nested/model.sdf", "", false); + ignition::common::setenv("IGN_GAZEBO_RESOURCE_PATH", + common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "models")); + // The model:// URI should now resolve because the RESOURCE_PATH has been + // updated. + test("model://include_nested/model.sdf", + common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "models", + "include_nested", "model.sdf"), true); +} + // Run multiple times. We want to make sure that static globals don't cause // problems. INSTANTIATE_TEST_SUITE_P(ServerRepeat, ServerFixture, ::testing::Range(1, 2)); diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index d6d16803c0..91cdf15e34 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -51,6 +51,7 @@ #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/RgbdCamera.hh" +#include "ignition/gazebo/components/Scene.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/Static.hh" #include "ignition/gazebo/components/ThermalCamera.hh" @@ -63,6 +64,7 @@ #include #include #include +#include #include using namespace std::chrono_literals; @@ -236,6 +238,10 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate /// \brief A list of async state requests public: std::unordered_set stateRequests; + + /// \brief Store SDF scene information so that it can be inserted into + /// scene message. + public: sdf::Scene sdfScene; }; ////////////////////////////////////////////////// @@ -302,8 +308,12 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, if (_manager.HasNewEntities()) this->dataPtr->SceneGraphAddEntities(_manager); - // Populate pose message - // TODO(louise) Get from SDF + // Store the Scene component data, which holds sdf::Scene so that we can + // populate the scene info messages. + auto sceneComp = + _manager.Component(this->dataPtr->worldEntity); + if (sceneComp) + this->dataPtr->sdfScene = sceneComp->Data(); // Create and send pose update if transport connections exist. if (this->dataPtr->dyPosePub.HasConnections() || @@ -615,6 +625,7 @@ bool SceneBroadcasterPrivate::SceneInfoService(ignition::msgs::Scene &_res) _res.Clear(); // Populate scene message + _res.CopyFrom(convert(this->sdfScene)); // Add models AddModels(&_res, this->worldEntity, this->sceneGraph); @@ -1012,6 +1023,8 @@ void SceneBroadcasterPrivate::SceneGraphAddEntities( this->SetupTransport(this->worldName); msgs::Scene sceneMsg; + // Populate scene message + sceneMsg.CopyFrom(convert(this->sdfScene)); AddModels(&sceneMsg, this->worldEntity, newGraph); diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index 83ddc6cb60..41ca64e113 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -914,6 +914,44 @@ TEST_P(SceneBroadcasterTest, server.Run(true, 1, false); } +///////////////////////////////////////////////// +TEST_P(SceneBroadcasterTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(SceneInfoHasSceneSdf)) +{ + // Start server + ignition::gazebo::ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + common::joinPaths("/", "test", "worlds", "conveyor.sdf")); + + gazebo::Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Run server + server.Run(true, 1, false); + + // Create requester + transport::Node node; + + bool result{false}; + unsigned int timeout{5000}; + ignition::msgs::Scene res; + + EXPECT_TRUE(node.Request("/world/default/scene/info", timeout, res, result)); + EXPECT_TRUE(result); + + ASSERT_TRUE(res.has_ambient()); + EXPECT_EQ(math::Color(1.0, 1.0, 1.0, 1.0), msgs::Convert(res.ambient())); + + ASSERT_TRUE(res.has_background()); + EXPECT_EQ(math::Color(0.8, 0.8, 0.8, 1.0), msgs::Convert(res.background())); + + EXPECT_TRUE(res.shadows()); + EXPECT_FALSE(res.grid()); + EXPECT_FALSE(res.has_fog()); + EXPECT_FALSE(res.has_sky()); +} + // Run multiple times INSTANTIATE_TEST_SUITE_P(ServerRepeat, SceneBroadcasterTest, ::testing::Range(1, 2)); diff --git a/test/worlds/conveyor.sdf b/test/worlds/conveyor.sdf index f33ba10c3f..033c32dc0a 100644 --- a/test/worlds/conveyor.sdf +++ b/test/worlds/conveyor.sdf @@ -24,6 +24,8 @@ 1.0 1.0 1.0 0.8 0.8 0.8 + true + false From 67c41c91d60ae9ede4b7242d72896e76e9ff72a5 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 1 Jun 2022 12:51:45 -0700 Subject: [PATCH 26/28] System inspector GUI widget (#1404) Signed-off-by: Louise Poubel Co-authored-by: Nate Koenig --- include/ignition/gazebo/Conversions.hh | 33 +++ .../gazebo/components/SystemPluginInfo.hh | 47 ++++ src/Conversions.cc | 71 ++++-- src/Conversions_TEST.cc | 49 ++++ src/SystemManager.cc | 44 +++- src/SystemManager.hh | 9 +- src/SystemManager_TEST.cc | 58 +++++ .../component_inspector/CMakeLists.txt | 2 + .../component_inspector/ComponentInspector.cc | 7 + .../ComponentInspector.qml | 8 + .../ComponentInspector.qrc | 1 + .../ExpandingTypeHeader.qml | 13 + .../component_inspector/SystemPluginInfo.cc | 71 ++++++ .../component_inspector/SystemPluginInfo.hh | 54 ++++ .../component_inspector/SystemPluginInfo.qml | 235 ++++++++++++++++++ .../component_inspector/TypeHeader.qml | 6 +- .../component_inspector_editor/Pose3d.qml | 2 +- 17 files changed, 678 insertions(+), 32 deletions(-) create mode 100644 include/ignition/gazebo/components/SystemPluginInfo.hh create mode 100644 src/gui/plugins/component_inspector/SystemPluginInfo.cc create mode 100644 src/gui/plugins/component_inspector/SystemPluginInfo.hh create mode 100644 src/gui/plugins/component_inspector/SystemPluginInfo.qml diff --git a/include/ignition/gazebo/Conversions.hh b/include/ignition/gazebo/Conversions.hh index 985bec6a97..e39b579340 100644 --- a/include/ignition/gazebo/Conversions.hh +++ b/include/ignition/gazebo/Conversions.hh @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -709,6 +710,38 @@ namespace ignition /// \return Particle emitter message. template<> sdf::ParticleEmitter convert(const msgs::ParticleEmitter &_in); + + /// \brief Generic conversion from an SDF element to another type. + /// \param[in] _in SDF element. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const sdf::Element &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from an SDF element to a plugin message. + /// \param[in] _in SDF element. + /// \return Plugin message. + template<> + msgs::Plugin convert(const sdf::Element &_in); + + /// \brief Generic conversion from an SDF plugin to another type. + /// \param[in] _in SDF plugin. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const sdf::Plugin &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from an SDF plugin to a plugin message. + /// \param[in] _in SDF plugin. + /// \return Plugin message. + template<> + msgs::Plugin convert(const sdf::Plugin &_in); } } } diff --git a/include/ignition/gazebo/components/SystemPluginInfo.hh b/include/ignition/gazebo/components/SystemPluginInfo.hh new file mode 100644 index 0000000000..365886c1e4 --- /dev/null +++ b/include/ignition/gazebo/components/SystemPluginInfo.hh @@ -0,0 +1,47 @@ +/* + * 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_COMPONENTS_SYSTEMINFO_HH_ +#define IGNITION_GAZEBO_COMPONENTS_SYSTEMINFO_HH_ + +#include +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief This component holds information about all the system plugins that + /// are attached to an entity. The content of each system is populated the + /// moment the plugin is instantiated and isn't modified throughout + /// simulation. + using SystemPluginInfo = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.SystemPluginInfo", + SystemPluginInfo) +} +} +} +} + +#endif diff --git a/src/Conversions.cc b/src/Conversions.cc index b2670983cb..28f07167d3 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -711,28 +711,13 @@ msgs::GUI ignition::gazebo::convert(const sdf::Gui &_in) out.set_fullscreen(_in.Fullscreen()); // Set gui plugins - auto elem = _in.Element(); - if (elem && elem->HasElement("plugin")) + for (uint64_t i = 0; i < _in.PluginCount(); ++i) { - auto pluginElem = elem->GetElement("plugin"); - while (pluginElem) - { - auto pluginMsg = out.add_plugin(); - pluginMsg->set_name(pluginElem->Get("name")); - pluginMsg->set_filename(pluginElem->Get("filename")); - - std::stringstream ss; - for (auto innerElem = pluginElem->GetFirstElement(); - innerElem; innerElem = innerElem->GetNextElement("")) - { - ss << innerElem->ToString(""); - } - pluginMsg->set_innerxml(ss.str()); - pluginElem = pluginElem->GetNextElement("plugin"); - } + auto pluginMsg = out.add_plugin(); + pluginMsg->CopyFrom(convert(*_in.PluginByIndex(i))); } - if (elem->HasElement("camera")) + if (_in.Element()->HasElement("camera")) { ignwarn << " can't be converted yet" << std::endl; } @@ -1759,3 +1744,51 @@ sdf::ParticleEmitter ignition::gazebo::convert(const msgs::ParticleEmitter &_in) return out; } + +////////////////////////////////////////////////// +template<> +IGNITION_GAZEBO_VISIBLE +msgs::Plugin ignition::gazebo::convert(const sdf::Element &_in) +{ + msgs::Plugin result; + + if (_in.GetName() != "plugin") + { + ignerr << "Tried to convert SDF [" << _in.GetName() + << "] into [plugin]" << std::endl; + return result; + } + + result.set_name(_in.Get("name")); + result.set_filename(_in.Get("filename")); + + std::stringstream ss; + for (auto innerElem = _in.GetFirstElement(); innerElem; + innerElem = innerElem->GetNextElement("")) + { + ss << innerElem->ToString(""); + } + result.set_innerxml(ss.str()); + + return result; +} + +////////////////////////////////////////////////// +template<> +IGNITION_GAZEBO_VISIBLE +msgs::Plugin ignition::gazebo::convert(const sdf::Plugin &_in) +{ + msgs::Plugin result; + + result.set_name(_in.Name()); + result.set_filename(_in.Filename()); + + std::stringstream ss; + for (auto content : _in.Contents()) + { + ss << content->ToString(""); + } + result.set_innerxml(ss.str()); + + return result; +} diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index 9123dd497c..4dc334fd6f 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -1060,3 +1061,51 @@ TEST(Conversions, ParticleEmitter) EXPECT_EQ(emitter2.RawPose(), emitter.RawPose()); EXPECT_FLOAT_EQ(emitter2.ScatterRatio(), emitter.ScatterRatio()); } + +///////////////////////////////////////////////// +TEST(Conversions, PluginElement) +{ + sdf::Root root; + root.LoadSdfString("" + "" + " " + " 0.5" + " " + ""); + + auto world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + auto worldElem = world->Element(); + ASSERT_NE(nullptr, worldElem); + + auto pluginElem = worldElem->GetElement("plugin"); + ASSERT_NE(nullptr, pluginElem); + + auto pluginMsg = convert(*(pluginElem.get())); + EXPECT_EQ("plum", pluginMsg.filename()); + EXPECT_EQ("peach", pluginMsg.name()); + + EXPECT_NE(pluginMsg.innerxml().find("0.5"), + std::string::npos); +} + +///////////////////////////////////////////////// +TEST(Conversions, Plugin) +{ + sdf::Plugin pluginSdf; + pluginSdf.SetName("peach"); + pluginSdf.SetFilename("plum"); + + auto content = std::make_shared(); + content->SetName("avocado"); + content->AddValue("double", "0.5", false, ""); + pluginSdf.InsertContent(content); + + auto pluginMsg = convert(pluginSdf); + EXPECT_EQ("plum", pluginMsg.filename()); + EXPECT_EQ("peach", pluginMsg.name()); + + EXPECT_NE(pluginMsg.innerxml().find("0.5"), + std::string::npos) << pluginMsg.innerxml(); +} diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 5f75fc3ec9..902804a59d 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -15,6 +15,8 @@ * */ +#include "ignition/gazebo/components/SystemPluginInfo.hh" +#include "ignition/gazebo/Conversions.hh" #include "SystemManager.hh" using namespace ignition; @@ -120,6 +122,29 @@ void SystemManager::AddSystemImpl( SystemInternal _system, std::shared_ptr _sdf) { + // Add component + if (this->entityCompMgr && kNullEntity != _system.parentEntity) + { + msgs::Plugin_V systemInfoMsg; + auto systemInfoComp = + this->entityCompMgr->Component( + _system.parentEntity); + if (systemInfoComp) + { + systemInfoMsg = systemInfoComp->Data(); + } + if (_sdf) + { + auto pluginMsg = systemInfoMsg.add_plugins(); + pluginMsg->CopyFrom(convert(*_sdf.get())); + } + + this->entityCompMgr->SetComponentData( + _system.parentEntity, systemInfoMsg); + this->entityCompMgr->SetChanged(_system.parentEntity, + components::SystemPluginInfo::typeId); + } + // Configure the system, if necessary if (_system.configure && this->entityCompMgr && this->eventMgr) { @@ -160,16 +185,15 @@ const std::vector& SystemManager::SystemsPostUpdate() ////////////////////////////////////////////////// std::vector SystemManager::TotalByEntity(Entity _entity) { + auto checkEntity = [&](const SystemInternal &_system) + { + return _system.parentEntity == _entity; + }; + std::vector result; - for (auto system : this->systems) - { - if (system.parentEntity == _entity) - result.push_back(system); - } - for (auto system : this->pendingSystems) - { - if (system.parentEntity == _entity) - result.push_back(system); - } + std::copy_if(this->systems.begin(), this->systems.end(), + std::back_inserter(result), checkEntity); + std::copy_if(this->pendingSystems.begin(), this->pendingSystems.end(), + std::back_inserter(result), checkEntity); return result; } diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 1d83272787..bb6db6e593 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -112,13 +112,20 @@ namespace ignition /// \return Vector of systems. public: std::vector TotalByEntity(Entity _entity); + /// \brief Implementation for AddSystem functions that takes an SDF + /// element. This calls the AddSystemImpl that accepts an SDF Plugin. + /// \param[in] _system Generic representation of a system. + /// \param[in] _sdf SDF element. + private: void AddSystemImpl(SystemInternal _system, + std::shared_ptr _sdf); + /// \brief Implementation for AddSystem functions. This only adds systems /// to a queue, the actual addition is performed by `AddSystemToRunner` at /// the appropriate time. /// \param[in] _system Generic representation of a system. /// \param[in] _sdf SDF received from AddSystem. private: void AddSystemImpl(SystemInternal _system, - std::shared_ptr _sdf); + const sdf::Plugin &_sdf); /// \brief All the systems. private: std::vector systems; diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc index 454f53e45a..b53d63d24c 100644 --- a/src/SystemManager_TEST.cc +++ b/src/SystemManager_TEST.cc @@ -21,6 +21,7 @@ #include "ignition/gazebo/System.hh" #include "ignition/gazebo/SystemLoader.hh" #include "ignition/gazebo/Types.hh" +#include "ignition/gazebo/components/SystemPluginInfo.hh" #include "ignition/gazebo/test_config.hh" // NOLINT(build/include) #include "SystemManager.hh" @@ -202,3 +203,60 @@ TEST(SystemManager, AddSystemEcm) EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size()); } +///////////////////////////////////////////////// +TEST(SystemManager, AddSystemWithInfo) +{ + auto loader = std::make_shared(); + + EntityComponentManager ecm; + auto entity = ecm.CreateEntity(); + EXPECT_NE(kNullEntity, entity); + + auto eventManager = EventManager(); + + SystemManager systemMgr(loader, &ecm, &eventManager); + + // No element, no SystemPluginInfo component + auto configSystem = std::make_shared(); + systemMgr.AddSystem(configSystem, entity, nullptr); + + // Element becomes SystemPluginInfo component + auto pluginElem = std::make_shared(); + sdf::initFile("plugin.sdf", pluginElem); + sdf::readString("" + " " + " 0.5" + " " + "", pluginElem); + + auto updateSystem = std::make_shared(); + systemMgr.AddSystem(updateSystem, entity, pluginElem); + + int entityCount{0}; + ecm.Each( + [&](const Entity &_entity, + const components::SystemPluginInfo *_systemInfoComp) -> bool + { + EXPECT_EQ(entity, _entity); + + EXPECT_NE(nullptr, _systemInfoComp); + if (nullptr == _systemInfoComp) + return true; + + auto pluginsMsg = _systemInfoComp->Data(); + EXPECT_EQ(1, pluginsMsg.plugins().size()); + if (1u != pluginsMsg.plugins().size()) + return true; + + auto pluginMsg = pluginsMsg.plugins(0); + EXPECT_EQ("plum", pluginMsg.filename()); + EXPECT_EQ("peach", pluginMsg.name()); + EXPECT_NE(pluginMsg.innerxml().find("0.5"), + std::string::npos); + + entityCount++; + return true; + }); + EXPECT_EQ(1, entityCount); +} + diff --git a/src/gui/plugins/component_inspector/CMakeLists.txt b/src/gui/plugins/component_inspector/CMakeLists.txt index d4bc795abc..53119092b1 100644 --- a/src/gui/plugins/component_inspector/CMakeLists.txt +++ b/src/gui/plugins/component_inspector/CMakeLists.txt @@ -2,7 +2,9 @@ gz_add_gui_plugin(ComponentInspector SOURCES ComponentInspector.cc Pose3d.cc + SystemPluginInfo.cc QT_HEADERS ComponentInspector.hh Pose3d.hh + SystemPluginInfo.hh ) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 5bf8124493..4185b826fb 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -62,6 +62,7 @@ #include "ignition/gazebo/components/SourceFilePath.hh" #include "ignition/gazebo/components/SphericalCoordinates.hh" #include "ignition/gazebo/components/Static.hh" +#include "ignition/gazebo/components/SystemPluginInfo.hh" #include "ignition/gazebo/components/ThreadPitch.hh" #include "ignition/gazebo/components/Transparency.hh" #include "ignition/gazebo/components/Visual.hh" @@ -73,6 +74,7 @@ #include "ComponentInspector.hh" #include "Pose3d.hh" +#include "SystemPluginInfo.hh" namespace ignition::gazebo { @@ -114,6 +116,9 @@ namespace ignition::gazebo /// \brief Handles all components displayed as a 3D pose. public: std::unique_ptr pose3d; + + /// \brief Handles all system info components. + public: std::unique_ptr systemInfo; }; } @@ -475,6 +480,8 @@ void ComponentInspector::LoadConfig(const tinyxml2::XMLElement *) // Type-specific handlers this->dataPtr->pose3d = std::make_unique(this); + this->dataPtr->systemInfo = + std::make_unique(this); } ////////////////////////////////////////////////// diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 90a1e53dcb..21f43e9506 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -245,6 +245,14 @@ Rectangle { delegate: Loader { id: loader + + /** + * Most items can access model.data directly, but items like ListView, + * which have their own `model` property, can't. They can use + * `componentData` instead. + */ + property var componentData: model.data + source: delegateQml(model) } } diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qrc b/src/gui/plugins/component_inspector/ComponentInspector.qrc index 79b7d8a30d..24b9a85cfb 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qrc +++ b/src/gui/plugins/component_inspector/ComponentInspector.qrc @@ -12,6 +12,7 @@ Pose3d.qml SphericalCoordinates.qml String.qml + SystemPluginInfo.qml TypeHeader.qml Vector3d.qml plottable_icon.svg diff --git a/src/gui/plugins/component_inspector/ExpandingTypeHeader.qml b/src/gui/plugins/component_inspector/ExpandingTypeHeader.qml index c233ecf775..f36a0c6f6f 100644 --- a/src/gui/plugins/component_inspector/ExpandingTypeHeader.qml +++ b/src/gui/plugins/component_inspector/ExpandingTypeHeader.qml @@ -34,6 +34,9 @@ Rectangle { // Left indentation property int indentation: 10 + // Expose TypeHeader's custom tooltip + property alias customToolTip: typeHeader.customToolTip + RowLayout { anchors.fill: parent Item { @@ -55,7 +58,17 @@ Rectangle { Layout.fillWidth: true } } + ToolTip { + visible: ma.containsMouse + delay: tooltipDelay + text: typeHeader.customToolTip.length > 0 ? + typeHeader.customToolTip : typeHeader.tooltipText(componentData) + y: typeHeader.y - 30 + enter: null + exit: null + } MouseArea { + id: ma anchors.fill: parent hoverEnabled: true cursorShape: Qt.PointingHandCursor diff --git a/src/gui/plugins/component_inspector/SystemPluginInfo.cc b/src/gui/plugins/component_inspector/SystemPluginInfo.cc new file mode 100644 index 0000000000..e8ecf8a932 --- /dev/null +++ b/src/gui/plugins/component_inspector/SystemPluginInfo.cc @@ -0,0 +1,71 @@ +/* + * 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 + +#include +#include + +#include "SystemPluginInfo.hh" +#include "ComponentInspector.hh" +#include "Types.hh" + +using namespace ignition; +using namespace gazebo; +using namespace inspector; + +///////////////////////////////////////////////// +SystemPluginInfo::SystemPluginInfo(ComponentInspector *_inspector) +{ + _inspector->Context()->setContextProperty("SystemPluginInfoImpl", this); + this->inspector = _inspector; + + this->inspector->AddUpdateViewCb(components::SystemPluginInfo::typeId, + std::bind(&SystemPluginInfo::UpdateView, this, std::placeholders::_1, + std::placeholders::_2)); +} + +///////////////////////////////////////////////// +void SystemPluginInfo::UpdateView(const EntityComponentManager &_ecm, + QStandardItem *_item) +{ + auto comp = _ecm.Component( + this->inspector->GetEntity()); + if (nullptr == _item || nullptr == comp) + return; + + auto msg = comp->Data(); + + _item->setData(QString("SystemPluginInfo"), + ComponentsModel::RoleNames().key("dataType")); + + QList pluginList; + for (int i = 0; i < msg.plugins().size(); ++i) + { + QList dataList; + dataList.push_back( + QVariant(QString::fromStdString(msg.plugins(i).name()))); + dataList.push_back( + QVariant(QString::fromStdString(msg.plugins(i).filename()))); + dataList.push_back( + QVariant(QString::fromStdString(msg.plugins(i).innerxml()))); + pluginList.push_back(dataList); + } + + _item->setData(pluginList, + ComponentsModel::RoleNames().key("data")); +} + diff --git a/src/gui/plugins/component_inspector/SystemPluginInfo.hh b/src/gui/plugins/component_inspector/SystemPluginInfo.hh new file mode 100644 index 0000000000..43c2698c9c --- /dev/null +++ b/src/gui/plugins/component_inspector/SystemPluginInfo.hh @@ -0,0 +1,54 @@ +/* + * 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_GUI_COMPONENTINSPECTOR_SYSTEMINFO_HH_ +#define IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_SYSTEMINFO_HH_ + +#include "ignition/gazebo/EntityComponentManager.hh" + +#include +#include + +namespace ignition +{ +namespace gazebo +{ +class ComponentInspector; +namespace inspector +{ + /// \brief A class that handles SystemPluginInfo components. + class SystemPluginInfo : public QObject + { + Q_OBJECT + + /// \brief Constructor + /// \param[in] _inspector The component inspector. + public: explicit SystemPluginInfo(ComponentInspector *_inspector); + + /// \brief Callback when there are ECM updates. + /// \param[in] _ecm Immutable reference to the ECM. + /// \param[in] _item Item to update. + public: void UpdateView(const EntityComponentManager &_ecm, + QStandardItem *_item); + + /// \brief Pointer to the component inspector. This is used to add + /// callbacks. + private: ComponentInspector *inspector{nullptr}; + }; +} +} +} +#endif diff --git a/src/gui/plugins/component_inspector/SystemPluginInfo.qml b/src/gui/plugins/component_inspector/SystemPluginInfo.qml new file mode 100644 index 0000000000..2e4d487b7f --- /dev/null +++ b/src/gui/plugins/component_inspector/SystemPluginInfo.qml @@ -0,0 +1,235 @@ +/* + * 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. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 1.4 +import QtQuick.Controls 2.2 +import QtQuick.Controls.Material 2.1 +import QtQuick.Layouts 1.3 +import QtQuick.Controls.Styles 1.4 +import "qrc:/ComponentInspector" + +Rectangle { + id: systemInfoComponent + height: header.height + content.height + width: componentInspector.width + color: index % 2 == 0 ? lightGrey : darkGrey + + // Left indentation + property int indentation: 10 + + // Horizontal margins + property int margin: 5 + + // Light text color according to theme + property string propertyColor: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + + // Dark text color according to theme + property string valueColor: Material.theme == Material.Light ? "black" : "white" + + Column { + anchors.fill: parent + + ExpandingTypeHeader { + id: header + customToolTip: "Information about system plugins attached to this entity" + } + + // Content + Rectangle { + id: content + property bool show: false + width: parent.width + height: show ? column.height : 0 + clip: true + color: "transparent" + + Behavior on height { + NumberAnimation { + duration: 200; + easing.type: Easing.InOutQuad + } + } + + ListView { + id: column + height: contentHeight + model: componentData + width: content.width + spacing: 5 + + delegate: Column { + width: content.width + height: pluginHeader.height + pluginContent.height + + Rectangle { + id: pluginHeader + width: content.width + color: "transparent" + height: 20 + RowLayout { + anchors.fill: parent + Item { + width: margin * 2 + indentation + } + Image { + id: icon + sourceSize.height: indentation + sourceSize.width: indentation + fillMode: Image.Pad + Layout.alignment : Qt.AlignVCenter + source: pluginContent.show ? + "qrc:/Gazebo/images/minus.png" : "qrc:/Gazebo/images/plus.png" + } + Text { + height: parent.height + text: modelData[0].substring(modelData[0].lastIndexOf('::') + 2) + Layout.alignment : Qt.AlignVCenter + leftPadding: margin + color: propertyColor + font.pointSize: 12 + } + Item { + Layout.fillWidth: true + } + } + MouseArea { + anchors.fill: pluginHeader + hoverEnabled: true + cursorShape: Qt.PointingHandCursor + onClicked: { + pluginContent.show = !pluginContent.show + } + onEntered: { + pluginHeader.color = highlightColor + } + onExited: { + pluginHeader.color = "transparent" + } + } + } + GridLayout { + id: pluginContent + property bool show: false + width: parent.width + height: show ? 20 * 4 + innerXmlText.height : 0 + clip: true + columns: 4 + + Behavior on height { + NumberAnimation { + duration: 200; + easing.type: Easing.InOutQuad + } + } + Item { + Layout.rowSpan: 4 + width: margin * 4 + indentation * 2 + } + Text { + height: 20 + text: "Name" + color: propertyColor + font.pointSize: 12 + } + Text { + height: 20 + text: modelData[0] + color: valueColor + font.pointSize: 12 + clip: true + elide: Text.ElideLeft + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + + ToolTip { + visible: maName.containsMouse + delay: Qt.styleHints.mousePressAndHoldInterval + text: modelData[0] + enter: null + exit: null + } + MouseArea { + id: maName + anchors.fill: parent + hoverEnabled: true + } + } + Item { + Layout.rowSpan: 4 + width: margin + } + Text { + height: 20 + text: "Filename" + color: propertyColor + font.pointSize: 12 + } + Text { + height: 20 + text: modelData[1] + color: valueColor + font.pointSize: 12 + clip: true + elide: Text.ElideLeft + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + + ToolTip { + visible: maFilename.containsMouse + delay: Qt.styleHints.mousePressAndHoldInterval + text: modelData[1] + enter: null + exit: null + } + MouseArea { + id: maFilename + anchors.fill: parent + hoverEnabled: true + } + } + Text { + height: 20 + text: "Inner XML" + color: propertyColor + font.pointSize: 12 + Layout.columnSpan: 2 + } + Rectangle { + id: innerXmlRectangle + Layout.columnSpan: 2 + Layout.fillWidth: true + height: innerXmlText.paintedHeight + color: Material.background + TextEdit { + id: innerXmlText + width: parent.width + text: modelData[2].length == 0 ? "N/A" : modelData[2].trim() + color: valueColor + textFormat: TextEdit.PlainText + font.pointSize: 10 + clip: true + readOnly: true + wrapMode: Text.WordWrap + selectByMouse: true + } + } + } + } + } + } + } +} diff --git a/src/gui/plugins/component_inspector/TypeHeader.qml b/src/gui/plugins/component_inspector/TypeHeader.qml index 5cb0b39e2b..d8dd9221d5 100644 --- a/src/gui/plugins/component_inspector/TypeHeader.qml +++ b/src/gui/plugins/component_inspector/TypeHeader.qml @@ -27,6 +27,10 @@ Rectangle { width: headerText.width color: "transparent" + // Custom tooltip text. If not set, the component's type name and ID will be + // shown. + property string customToolTip: "" + function tooltipText(_model) { if (model === null) return "Unknown component" @@ -50,7 +54,7 @@ Rectangle { ToolTip { visible: ma.containsMouse delay: tooltipDelay - text: tooltipText(model) + text: customToolTip.length > 0 ? customToolTip : typeHeader.tooltipText(componentData) y: typeHeader.y - 30 enter: null exit: null diff --git a/src/gui/plugins/component_inspector_editor/Pose3d.qml b/src/gui/plugins/component_inspector_editor/Pose3d.qml index 8b2acf20ac..40e3bf9ba9 100644 --- a/src/gui/plugins/component_inspector_editor/Pose3d.qml +++ b/src/gui/plugins/component_inspector_editor/Pose3d.qml @@ -315,7 +315,7 @@ Rectangle { id: zSpin Layout.fillWidth: true height: 40 - numberValue: model.data[2] + numberValue: model.data[2] minValue: -Number.MAX_VALUE maxValue: Number.MAX_VALUE stepValue: 0.1 From b26a5734d6e6c77fad7488a1d51325e4ca5094f3 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 1 Jun 2022 12:52:47 -0700 Subject: [PATCH 27/28] Optimize sensor updates (#1480) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ian Chen Co-authored-by: Michael Carroll Co-authored-by: Alejandro Hernández Cordero --- src/systems/air_pressure/AirPressure.cc | 18 ++++++ src/systems/altimeter/Altimeter.cc | 18 ++++++ src/systems/force_torque/ForceTorque.cc | 18 ++++++ src/systems/imu/Imu.cc | 18 ++++++ src/systems/logical_camera/LogicalCamera.cc | 19 ++++++ src/systems/magnetometer/Magnetometer.cc | 18 ++++++ src/systems/navsat/NavSat.cc | 18 ++++++ src/systems/sensors/Sensors.cc | 66 +++++++++++++++++++++ 8 files changed, 193 insertions(+) diff --git a/src/systems/air_pressure/AirPressure.cc b/src/systems/air_pressure/AirPressure.cc index 6f4188ede7..a2fcb5704a 100644 --- a/src/systems/air_pressure/AirPressure.cc +++ b/src/systems/air_pressure/AirPressure.cc @@ -140,6 +140,24 @@ void AirPressure::PostUpdate(const UpdateInfo &_info, if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the AirPressurePrivate::UpdatePressures function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->UpdateAirPressures(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/altimeter/Altimeter.cc b/src/systems/altimeter/Altimeter.cc index d72ca9847a..3152c0c7e7 100644 --- a/src/systems/altimeter/Altimeter.cc +++ b/src/systems/altimeter/Altimeter.cc @@ -141,6 +141,24 @@ void Altimeter::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the AltimeterPrivate::UpdateAltimeters function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->UpdateAltimeters(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/force_torque/ForceTorque.cc b/src/systems/force_torque/ForceTorque.cc index e1668fbf71..7ebd91c3d2 100644 --- a/src/systems/force_torque/ForceTorque.cc +++ b/src/systems/force_torque/ForceTorque.cc @@ -132,6 +132,24 @@ void ForceTorque::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the ForceTorquePrivate::Update function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->Update(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index 2199854e2e..5afbd67ae0 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -145,6 +145,24 @@ void Imu::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the ImuPrivate::Update function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->Update(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/logical_camera/LogicalCamera.cc b/src/systems/logical_camera/LogicalCamera.cc index b573f5214b..5c21813f25 100644 --- a/src/systems/logical_camera/LogicalCamera.cc +++ b/src/systems/logical_camera/LogicalCamera.cc @@ -143,6 +143,25 @@ void LogicalCamera::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the LogicalCameraPrivate::UpdateLogicalCameras + // function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->UpdateLogicalCameras(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/magnetometer/Magnetometer.cc b/src/systems/magnetometer/Magnetometer.cc index b46b77a424..2ecad15692 100644 --- a/src/systems/magnetometer/Magnetometer.cc +++ b/src/systems/magnetometer/Magnetometer.cc @@ -142,6 +142,24 @@ void Magnetometer::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the MagnetometerPrivate::Update function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->Update(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/navsat/NavSat.cc b/src/systems/navsat/NavSat.cc index faa8f4f38f..2eea382d9e 100644 --- a/src/systems/navsat/NavSat.cc +++ b/src/systems/navsat/NavSat.cc @@ -137,6 +137,24 @@ void NavSat::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the NavSat::Implementation::Update function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->Update(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index e83d053e30..69222d2209 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -186,6 +187,11 @@ class ignition::gazebo::systems::SensorsPrivate /// \param[in] _ecm Entity component manager public: void UpdateBatteryState(const EntityComponentManager &_ecm); + /// \brief Check if sensor has subscribers + /// \param[in] _sensor Sensor to check + /// \return True if the sensor has subscribers, false otherwise + public: bool HasConnections(sensors::RenderingSensor *_sensor) const; + /// \brief Use to optionally set the background color. public: std::optional backgroundColor; @@ -316,12 +322,34 @@ void SensorsPrivate::RunOnce() this->scene->PreRender(); } + // disable sensors that have no subscribers to prevent doing unnecessary + // work + std::unordered_set tmpDisabledSensors; + this->sensorMaskMutex.lock(); + for (auto id : this->sensorIds) + { + sensors::Sensor *s = this->sensorManager.Sensor(id); + auto rs = dynamic_cast(s); + if (rs->IsActive() && !this->HasConnections(rs)) + { + rs->SetActive(false); + tmpDisabledSensors.insert(rs); + } + } + this->sensorMaskMutex.unlock(); + { // publish data IGN_PROFILE("RunOnce"); this->sensorManager.RunOnce(this->updateTime); } + // re-enble sensors + for (auto &rs : tmpDisabledSensors) + { + rs->SetActive(true); + } + { IGN_PROFILE("PostRender"); // Update the scene graph manually to improve performance @@ -805,6 +833,44 @@ std::string Sensors::CreateSensor(const Entity &_entity, return sensor->Name(); } +////////////////////////////////////////////////// +bool SensorsPrivate::HasConnections(sensors::RenderingSensor *_sensor) const +{ + if (!_sensor) + return true; + + // \todo(iche033) Remove this function once a virtual + // sensors::RenderingSensor::HasConnections function is available + { + auto s = dynamic_cast(_sensor); + if (s) + return s->HasConnections(); + } + { + auto s = dynamic_cast(_sensor); + if (s) + return s->HasConnections(); + } + { + auto s = dynamic_cast(_sensor); + if (s) + return s->HasConnections(); + } + { + auto s = dynamic_cast(_sensor); + if (s) + return s->HasConnections(); + } + { + auto s = dynamic_cast(_sensor); + if (s) + return s->HasConnections(); + } + ignwarn << "Unable to check connection count for sensor: " << _sensor->Name() + << ". Unknown sensor type." << std::endl; + return true; +} + IGNITION_ADD_PLUGIN(Sensors, System, Sensors::ISystemConfigure, Sensors::ISystemUpdate, From 3d0867f6ed9b3faa84482e5275527a997652e2ed Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 2 Jun 2022 14:15:04 -0700 Subject: [PATCH 28/28] Remove ign-rendering SetTime hack (#1514) Signed-off-by: Louise Poubel --- include/gz/sim/components/Model.hh | 2 +- src/rendering/RenderUtil.cc | 7 ------- src/systems/thruster/Thruster.hh | 2 +- 3 files changed, 2 insertions(+), 9 deletions(-) diff --git a/include/gz/sim/components/Model.hh b/include/gz/sim/components/Model.hh index c2816ff0dc..6aa829ab8d 100644 --- a/include/gz/sim/components/Model.hh +++ b/include/gz/sim/components/Model.hh @@ -46,7 +46,7 @@ namespace serializers sdf::ElementPtr modelElem = _model.Element(); if (!modelElem) { - gzerr << "Unable to serialize sdf::Model" << std::endl; + gzwarn << "Unable to serialize sdf::Model" << std::endl; return _out; } diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 26b700b306..0c0ba40fbd 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -2620,13 +2620,6 @@ void RenderUtil::SetSceneName(const std::string &_name) void RenderUtil::SetScene(const rendering::ScenePtr &_scene) { this->dataPtr->scene = _scene; - { - // HACK: Tell ign-rendering6 to listen to SetTime calls - // TODO(anyone) Remove this when linked against ign-rendering7 - this->dataPtr->scene->SetTime(std::chrono::nanoseconds(-1)); - IGN_ASSERT(this->dataPtr->scene->Time() != std::chrono::nanoseconds(-1), - "Please remove this snippet after merging with ign-rendering7"); - } this->dataPtr->sceneManager.SetScene(_scene); this->dataPtr->engine = _scene == nullptr ? nullptr : _scene->Engine(); } diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index 45829f436a..2ca04b5156 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -126,7 +126,7 @@ namespace systems /// Documentation inherited public: void PostUpdate(const UpdateInfo &_info, - const EntityComponentManager &_ecm); + const EntityComponentManager &_ecm) override; /// \brief Private data pointer private: std::unique_ptr dataPtr;