From e42e3060f88f24926adf5bbf6f6436c13bbb4eef Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 16 Nov 2021 20:02:26 +0800 Subject: [PATCH 1/9] Don't waite on new inertia Signed-off-by: Arjo Chakravarty --- src/systems/buoyancy/Buoyancy.cc | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc index ff91c83471..de16a13b53 100644 --- a/src/systems/buoyancy/Buoyancy.cc +++ b/src/systems/buoyancy/Buoyancy.cc @@ -441,11 +441,7 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, { auto newPose = enableComponent(_ecm, _entity); newPose |= enableComponent(_ecm, _entity); - if (newPose) - { - // Skip entity if WorldPose and inertial are not yet ready - return true; - } + // World pose of the link. math::Pose3d linkWorldPose = worldPose(_entity, _ecm); @@ -477,6 +473,13 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, else if (this->dataPtr->buoyancyType == BuoyancyPrivate::BuoyancyType::GRADED_BUOYANCY) { + if (newPose) + { + // Skip entity if WorldPose and inertial are not yet ready + // TODO(arjo): Find a way of disabling gravity effects for + // this first iteration. + return true; + } std::vector collisions = _ecm.ChildrenByComponents( _entity, components::Collision()); this->dataPtr->buoyancyForces.clear(); @@ -521,12 +524,13 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } } } - } - auto [force, torque] = this->dataPtr->ResolveForces( + auto [force, torque] = this->dataPtr->ResolveForces( link.WorldInertialPose(_ecm).value()); - // Apply the wrench to the link. This wrench is applied in the - // Physics System. - link.AddWorldWrench(_ecm, force, torque); + // Apply the wrench to the link. This wrench is applied in the + // Physics System. + link.AddWorldWrench(_ecm, force, torque); + } + return true; }); } From ee908927d64c9d08a946c93d0ea75d467324d923 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 16 Nov 2021 19:46:24 -0800 Subject: [PATCH 2/9] Update new / removed entity GUI events (#1213) Signed-off-by: Louise Poubel --- include/ignition/gazebo/gui/GuiEvents.hh | 72 +++++++--------- src/gui/CMakeLists.txt | 2 + src/gui/GuiEvents.cc | 85 +++++++++++++++++++ src/gui/GuiEvents_TEST.cc | 67 +++++++++++++++ src/gui/plugins/entity_tree/EntityTree.cc | 6 +- .../plugins/scene_manager/GzSceneManager.cc | 19 +++-- .../plugins/select_entities/SelectEntities.cc | 8 +- 7 files changed, 209 insertions(+), 50 deletions(-) create mode 100644 src/gui/GuiEvents.cc create mode 100644 src/gui/GuiEvents_TEST.cc diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index d05645a49e..8bac275993 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -23,6 +23,8 @@ #include #include #include +#include +#include "ignition/gazebo/gui/Export.hh" #include "ignition/gazebo/Entity.hh" #include "ignition/gazebo/config.hh" @@ -38,7 +40,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace events { /// \brief Event that notifies when new entities have been selected. - class EntitiesSelected : public QEvent + class IGNITION_GAZEBO_GUI_VISIBLE EntitiesSelected : public QEvent { /// \brief Constructor /// \param[in] _entities All the selected entities @@ -76,7 +78,7 @@ namespace events }; /// \brief Event that notifies when all entities have been deselected. - class DeselectAllEntities : public QEvent + class IGNITION_GAZEBO_GUI_VISIBLE DeselectAllEntities : public QEvent { /// \brief Constructor /// \param[in] _fromUser True if the event was directly generated by the @@ -100,68 +102,60 @@ namespace events private: bool fromUser{false}; }; - /// \brief Event that contains newly created and removed entities - class AddedRemovedEntities : public QEvent + /// \brief Event that contains entities newly created or removed from the + /// GUI, but that aren't present on the server yet. + /// \sa NewRemovedEntities + class IGNITION_GAZEBO_GUI_VISIBLE GuiNewRemovedEntities : public QEvent { /// \brief Constructor /// \param[in] _newEntities Set of newly created entities /// \param[in] _removedEntities Set of recently removed entities - public: AddedRemovedEntities(const std::set &_newEntities, - const std::set &_removedEntities) - : QEvent(kType), newEntities(_newEntities), - removedEntities(_removedEntities) - { - } + public: GuiNewRemovedEntities(const std::set &_newEntities, + const std::set &_removedEntities); /// \brief Get the set of newly created entities - public: const std::set &NewEntities() const - { - return this->newEntities; - } + public: const std::set &NewEntities() const; /// \brief Get the set of recently removed entities - public: const std::set &RemovedEntities() const - { - return this->removedEntities; - } + public: const std::set &RemovedEntities() const; /// \brief Unique type for this event. static const QEvent::Type kType = QEvent::Type(QEvent::User + 3); - /// \brief Set of newly created entities - private: std::set newEntities; - - /// \brief Set of recently removed entities - private: std::set removedEntities; + /// \internal + /// \brief Private data pointer + IGN_UTILS_IMPL_PTR(dataPtr) }; - /// \brief Event that notifies when new entities have been removed. - class RemovedEntities : public QEvent + /// \brief Event that notifies when new entities have been created or removed + /// on the server. This is a duplication of what `GuiSystem`s would get from + /// `EachNew` / `EachRemoved` ECM calls. + /// \sa GuiNewRemovedEntities + class IGNITION_GAZEBO_GUI_VISIBLE NewRemovedEntities : public QEvent { /// \brief Constructor - /// \param[in] _entities All the removed entities - public: explicit RemovedEntities(const std::vector &_entities) - : QEvent(kType), entities(_entities) - { - } + /// \param[in] _newEntities Set of newly created entities + /// \param[in] _removedEntities Set of recently removed entities + public: NewRemovedEntities(const std::set &_newEntities, + const std::set &_removedEntities); - /// \brief Get the data sent with the event. - /// \return The entities being removed. - public: std::vector Data() const - { - return this->entities; - } + /// \brief Get the set of newly created entities + public: const std::set &NewEntities() const; + + /// \brief Get the set of recently removed entities + public: const std::set &RemovedEntities() const; /// \brief Unique type for this event. static const QEvent::Type kType = QEvent::Type(QEvent::User + 4); - /// \brief The removed entities. - private: std::vector entities; + /// \internal + /// \brief Private data pointer + IGN_UTILS_IMPL_PTR(dataPtr) }; /// \brief True if a transform control is currently active (translate / /// rotate / scale). False if we're in selection mode. - class TransformControlModeActive : public QEvent + class IGNITION_GAZEBO_GUI_VISIBLE TransformControlModeActive : public QEvent { /// \brief Constructor /// \param[in] _tranformModeActive is the transform control mode active diff --git a/src/gui/CMakeLists.txt b/src/gui/CMakeLists.txt index fa9d276b0f..ae0de7d3da 100644 --- a/src/gui/CMakeLists.txt +++ b/src/gui/CMakeLists.txt @@ -1,6 +1,7 @@ set (gui_sources AboutDialogHandler.cc Gui.cc + GuiEvents.cc GuiFileHandler.cc GuiRunner.cc PathManager.cc @@ -8,6 +9,7 @@ set (gui_sources set (gtest_sources Gui_TEST.cc + GuiEvents_TEST.cc ) add_subdirectory(plugins) diff --git a/src/gui/GuiEvents.cc b/src/gui/GuiEvents.cc new file mode 100644 index 0000000000..dd89de306f --- /dev/null +++ b/src/gui/GuiEvents.cc @@ -0,0 +1,85 @@ +/* + * Copyright (C) 2021 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 "ignition/gazebo/gui/GuiEvents.hh" + +class ignition::gazebo::gui::events::GuiNewRemovedEntities::Implementation +{ + /// \brief Set of newly created entities + public: std::set newEntities; + + /// \brief Set of recently removed entities + public: std::set removedEntities; +}; + +class ignition::gazebo::gui::events::NewRemovedEntities::Implementation +{ + /// \brief Set of newly created entities + public: std::set newEntities; + + /// \brief Set of recently removed entities + public: std::set removedEntities; +}; + +using namespace ignition; +using namespace gazebo; +using namespace gui; +using namespace events; + +///////////////////////////////////////////////// +GuiNewRemovedEntities::GuiNewRemovedEntities( + const std::set &_newEntities, + const std::set &_removedEntities) + : QEvent(kType), dataPtr(utils::MakeImpl()) +{ + this->dataPtr->newEntities = _newEntities; + this->dataPtr->removedEntities = _removedEntities; +} + +///////////////////////////////////////////////// +const std::set &GuiNewRemovedEntities::NewEntities() const +{ + return this->dataPtr->newEntities; +} + +///////////////////////////////////////////////// +const std::set &GuiNewRemovedEntities::RemovedEntities() const +{ + return this->dataPtr->removedEntities; +} + +///////////////////////////////////////////////// +NewRemovedEntities::NewRemovedEntities( + const std::set &_newEntities, + const std::set &_removedEntities) + : QEvent(kType), dataPtr(utils::MakeImpl()) +{ + this->dataPtr->newEntities = _newEntities; + this->dataPtr->removedEntities = _removedEntities; +} + +///////////////////////////////////////////////// +const std::set &NewRemovedEntities::NewEntities() const +{ + return this->dataPtr->newEntities; +} + +///////////////////////////////////////////////// +const std::set &NewRemovedEntities::RemovedEntities() const +{ + return this->dataPtr->removedEntities; +} diff --git a/src/gui/GuiEvents_TEST.cc b/src/gui/GuiEvents_TEST.cc new file mode 100644 index 0000000000..cb0bebc999 --- /dev/null +++ b/src/gui/GuiEvents_TEST.cc @@ -0,0 +1,67 @@ +/* + * Copyright (C) 2021 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 "ignition/gazebo/test_config.hh" +#include "ignition/gazebo/gui/GuiEvents.hh" + +using namespace ignition; +using namespace gazebo; +using namespace gui; + +///////////////////////////////////////////////// +TEST(GuiEventsTest, GuiNewRemovedEntities) +{ + events::GuiNewRemovedEntities event({1, 2, 3}, {4, 5}); + + EXPECT_LT(QEvent::User, event.type()); + + auto addedEntities = event.NewEntities(); + EXPECT_EQ(3u, addedEntities.size()); + EXPECT_NE(addedEntities.find(1), addedEntities.end()); + EXPECT_NE(addedEntities.find(2), addedEntities.end()); + EXPECT_NE(addedEntities.find(3), addedEntities.end()); + EXPECT_EQ(addedEntities.find(100), addedEntities.end()); + + auto removedEntities = event.RemovedEntities(); + EXPECT_EQ(2u, removedEntities.size()); + EXPECT_NE(removedEntities.find(4), removedEntities.end()); + EXPECT_NE(removedEntities.find(5), removedEntities.end()); + EXPECT_EQ(removedEntities.find(6), removedEntities.end()); +} + +///////////////////////////////////////////////// +TEST(GuiEventsTest, NewRemovedEntities) +{ + events::NewRemovedEntities event({1, 2, 3}, {4, 5}); + + EXPECT_LT(QEvent::User, event.type()); + + auto addedEntities = event.NewEntities(); + EXPECT_EQ(3u, addedEntities.size()); + EXPECT_NE(addedEntities.find(1), addedEntities.end()); + EXPECT_NE(addedEntities.find(2), addedEntities.end()); + EXPECT_NE(addedEntities.find(3), addedEntities.end()); + EXPECT_EQ(addedEntities.find(100), addedEntities.end()); + + auto removedEntities = event.RemovedEntities(); + EXPECT_EQ(2u, removedEntities.size()); + EXPECT_NE(removedEntities.find(4), removedEntities.end()); + EXPECT_NE(removedEntities.find(5), removedEntities.end()); + EXPECT_EQ(removedEntities.find(6), removedEntities.end()); +} diff --git a/src/gui/plugins/entity_tree/EntityTree.cc b/src/gui/plugins/entity_tree/EntityTree.cc index de909caa38..a1eb39a2e1 100644 --- a/src/gui/plugins/entity_tree/EntityTree.cc +++ b/src/gui/plugins/entity_tree/EntityTree.cc @@ -556,13 +556,15 @@ bool EntityTree::eventFilter(QObject *_obj, QEvent *_event) } } else if (_event->type() == - ignition::gazebo::gui::events::AddedRemovedEntities::kType) + ignition::gazebo::gui::events::GuiNewRemovedEntities::kType) { std::lock_guard lock(this->dataPtr->newRemovedEntityMutex); auto addedRemovedEvent = - reinterpret_cast(_event); + reinterpret_cast(_event); if (addedRemovedEvent) { + // TODO(chapulina) Make these entities visually different from entities + // created from the server. for (auto entity : addedRemovedEvent->NewEntities()) this->dataPtr->newEntities.insert(entity); diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index 802c350575..88710b0e9b 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -17,7 +17,7 @@ #include "GzSceneManager.hh" -#include +#include #include #include @@ -85,16 +85,25 @@ void GzSceneManager::Update(const UpdateInfo &_info, this->dataPtr->renderUtil.UpdateECM(_info, _ecm); this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); - // Emit entities removed event - std::vector removed; + // Emit entities created / removed event for gui::Plugins which don't have + // direct access to the ECM. + std::set created; + _ecm.EachNew( + [&](const Entity &_entity, const components::Name *)->bool + { + created.insert(_entity); + return true; + }); + std::set removed; _ecm.EachRemoved( [&](const Entity &_entity, const components::Name *)->bool { - removed.push_back(_entity); + removed.insert(_entity); return true; }); - ignition::gazebo::gui::events::RemovedEntities removedEvent(removed); + ignition::gazebo::gui::events::NewRemovedEntities removedEvent( + created, removed); ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), &removedEvent); diff --git a/src/gui/plugins/select_entities/SelectEntities.cc b/src/gui/plugins/select_entities/SelectEntities.cc index c08dc0963b..d3513888f7 100644 --- a/src/gui/plugins/select_entities/SelectEntities.cc +++ b/src/gui/plugins/select_entities/SelectEntities.cc @@ -574,13 +574,13 @@ bool SelectEntities::eventFilter(QObject *_obj, QEvent *_event) } } else if (_event->type() == - ignition::gazebo::gui::events::RemovedEntities::kType) + ignition::gazebo::gui::events::NewRemovedEntities::kType) { if (!this->dataPtr->wireBoxes.empty()) { - auto removedEvent = - reinterpret_cast(_event); - for (auto &entity : removedEvent->Data()) + auto event = + reinterpret_cast(_event); + for (auto &entity : event->RemovedEntities()) { auto wireBoxIt = this->dataPtr->wireBoxes.find(entity); if (wireBoxIt != this->dataPtr->wireBoxes.end()) From adebcf5730ce1969c3c1bac821cfb18f9e16c30c Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 17 Nov 2021 08:01:27 -0800 Subject: [PATCH 3/9] =?UTF-8?q?=F0=9F=8E=88=206.2.0=20(#1214)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel --- CMakeLists.txt | 2 +- Changelog.md | 99 ++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 100 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 240e35338d..94162ecd4a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-gazebo6 VERSION 6.1.0) +project(ignition-gazebo6 VERSION 6.2.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index caccd2cdfe..b4a2011901 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,104 @@ ## Ignition Gazebo 6.x +### Ignition Gazebo 6.X.X (202X-XX-XX) + +### Ignition Gazebo 6.2.0 (2021-11-16) + +1. Configurable joint state publisher's topic + * [Pull request #1076](https://github.com/ignitionrobotics/ign-gazebo/pull/1076) + +1. Thruster plugin: add tests and velocity control + * [Pull request #1190](https://github.com/ignitionrobotics/ign-gazebo/pull/1190) + +1. Prevent creation of spurious `` elements when saving worlds + * [Pull request #1192](https://github.com/ignitionrobotics/ign-gazebo/pull/1192) + +1. Add `sdfString` to `ServerConfig`'s copy constructor. + * [Pull request #1185](https://github.com/ignitionrobotics/ign-gazebo/pull/1185) + +1. Added support for tracked vehicles + * [Pull request #869](https://github.com/ignitionrobotics/ign-gazebo/pull/869) + +1. Add components to dynamically set joint limits + * [Pull request #847](https://github.com/ignitionrobotics/ign-gazebo/pull/847) + +1. Remove bounding box when entities are removed + * [Pull request #1053](https://github.com/ignitionrobotics/ign-gazebo/pull/1053) + * [Pull request #1213](https://github.com/ignitionrobotics/ign-gazebo/pull/1213) + +1. Fix updating component from state + * [Pull request #1181](https://github.com/ignitionrobotics/ign-gazebo/pull/1181) + +1. Extend odom publisher to allow 3D + * [Pull request #1180](https://github.com/ignitionrobotics/ign-gazebo/pull/1180) + +1. Support copy/paste + * [Pull request #1013](https://github.com/ignitionrobotics/ign-gazebo/pull/1013) + +1. Tweaks install instructions + * [Pull request #1078](https://github.com/ignitionrobotics/ign-gazebo/pull/1078) + +1. Publish 10 world stats msgs/sec instead of 5 + * [Pull request #1163](https://github.com/ignitionrobotics/ign-gazebo/pull/1163) + +1. Add functionality to add entities via the entity tree + * [Pull request #1101](https://github.com/ignitionrobotics/ign-gazebo/pull/1101) + +1. Get updated GUI ECM info when a user presses 'play' + * [Pull request #1109](https://github.com/ignitionrobotics/ign-gazebo/pull/1109) + +1. Create expanding type header to reduce code duplication + * [Pull request #1169](https://github.com/ignitionrobotics/ign-gazebo/pull/1169) + +1. `minimal_scene.sdf` example: add `camera_clip` params + * [Pull request #1166](https://github.com/ignitionrobotics/ign-gazebo/pull/1166) + +1. Sensor systems work if loaded after sensors + * [Pull request #1104](https://github.com/ignitionrobotics/ign-gazebo/pull/1104) + +1. Support printing sensors using `ign model` + * [Pull request #1157](https://github.com/ignitionrobotics/ign-gazebo/pull/1157) + +1. Set camera clipping plane distances from the GUI + * [Pull request #1162](https://github.com/ignitionrobotics/ign-gazebo/pull/1162) + +1. Fix generation of systems library symlinks in build directory + * [Pull request #1160](https://github.com/ignitionrobotics/ign-gazebo/pull/1160) + +1. Add a default value for `isHeadlessRendering`. + * [Pull request #1151](https://github.com/ignitionrobotics/ign-gazebo/pull/1151) + +1. Component inspector + + 1. Edit material colors + * [Pull request #1123](https://github.com/ignitionrobotics/ign-gazebo/pull/1123) + * [Pull request #1186](https://github.com/ignitionrobotics/ign-gazebo/pull/1186) + + 1. Fix integers and floats + * [Pull request #1143](https://github.com/ignitionrobotics/ign-gazebo/pull/1143) + + 1. Prevent a segfault when updating + * [Pull request #1167](https://github.com/ignitionrobotics/ign-gazebo/pull/1167) + + 1. Use `uint64_t` for Entity IDs + * [Pull request #1144](https://github.com/ignitionrobotics/ign-gazebo/pull/1144) + +1. Support setting the background color for sensors + * [Pull request #1147](https://github.com/ignitionrobotics/ign-gazebo/pull/1147) + +1. Select top level entity not visual + * [Pull request #1150](https://github.com/ignitionrobotics/ign-gazebo/pull/1150) + +1. Update create entity offset on GUI side + * [Pull request #1145](https://github.com/ignitionrobotics/ign-gazebo/pull/1145) + +1. Update Select Entities GUI plugin to use Entity type + * [Pull request #1146](https://github.com/ignitionrobotics/ign-gazebo/pull/1146) + +1. Notify other GUI plugins of added/removed entities via GUI events + * [Pull request #1138](https://github.com/ignitionrobotics/ign-gazebo/pull/1138) + * [Pull request #1213](https://github.com/ignitionrobotics/ign-gazebo/pull/1213) + ### Ignition Gazebo 6.1.0 (2021-10-25) 1. Updates to camera video record from subt From 96ed1efff86af0ce5d3322fdad1bf3196896e881 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 17 Nov 2021 13:48:58 -0600 Subject: [PATCH 4/9] Fix the force-torque sensor update rate (#1159) Signed-off-by: Louise Poubel --- examples/worlds/sensors.sdf | 70 +++++++++++++++++++++++++ src/systems/force_torque/ForceTorque.cc | 3 +- test/integration/force_torque_system.cc | 34 +++++++----- test/worlds/force_torque.sdf | 6 +-- 4 files changed, 94 insertions(+), 19 deletions(-) diff --git a/examples/worlds/sensors.sdf b/examples/worlds/sensors.sdf index eefd6a8ee8..b07bd89925 100644 --- a/examples/worlds/sensors.sdf +++ b/examples/worlds/sensors.sdf @@ -35,6 +35,10 @@ filename="ignition-gazebo-magnetometer-system" name="ignition::gazebo::systems::Magnetometer"> + + @@ -186,5 +190,71 @@ + + + true + + + + 0 0 2.0 0 0 0 + + + 0.100000 + 0.000000 + 0.000000 + 0.100000 + 0.000000 + 0.100000 + + 10.000000 + + + + + 0.100000 + + + + + 0 0 -0.75 0 0 0 + + + 0.0100000 + 1.5 + + + + + + + 0.100000 + + + + + + world + base_plate + + + base_plate + link_1 + 0 0 -1.5 0 0 0 + + + -1.57079 + 1.57079 + + + 0.000000 + 0.000000 + + 1.000000 0.000000 0.000000 + + + 10 + + + diff --git a/src/systems/force_torque/ForceTorque.cc b/src/systems/force_torque/ForceTorque.cc index 56f78a4e21..e1668fbf71 100644 --- a/src/systems/force_torque/ForceTorque.cc +++ b/src/systems/force_torque/ForceTorque.cc @@ -136,8 +136,7 @@ void ForceTorque::PostUpdate(const UpdateInfo &_info, for (auto &it : this->dataPtr->entitySensorMap) { - // Update measurement time - it.second->Update(_info.simTime); + it.second.get()->sensors::Sensor::Update(_info.simTime, false); } } diff --git a/test/integration/force_torque_system.cc b/test/integration/force_torque_system.cc index d3fce78236..a6f769e0ed 100644 --- a/test/integration/force_torque_system.cc +++ b/test/integration/force_torque_system.cc @@ -54,18 +54,21 @@ TEST_F(ForceTorqueTest, MeasureWeight) EXPECT_FALSE(*server.Running(0)); server.SetUpdatePeriod(1us); - size_t iters = 1000u; + // Having iters exactly in sync with update rate can lead to a race condition + // in the test between simulation and transport + size_t iters = 999u; + size_t updates = 100u; std::vector wrenches; - wrenches.reserve(iters); + wrenches.reserve(updates); std::mutex wrenchMutex; std::condition_variable cv; auto wrenchCb = std::function( - [&wrenchMutex, &wrenches, &cv, iters](const auto &_msg) + [&wrenchMutex, &wrenches, &cv, updates](const auto &_msg) { std::lock_guard lock(wrenchMutex); wrenches.push_back(_msg); - if (wrenches.size() >= iters) + if (wrenches.size() >= updates) { cv.notify_all(); } @@ -80,8 +83,8 @@ TEST_F(ForceTorqueTest, MeasureWeight) { std::unique_lock lock(wrenchMutex); - cv.wait_for(lock, 30s, [&] { return wrenches.size() >= iters; }); - ASSERT_EQ(iters, wrenches.size()); + cv.wait_for(lock, 30s, [&] { return wrenches.size() >= updates; }); + ASSERT_EQ(updates, wrenches.size()); const double kSensorMass = 0.2; const double kWeightMass = 10; @@ -109,18 +112,21 @@ TEST_F(ForceTorqueTest, SensorPoseOffset) EXPECT_FALSE(*server.Running(0)); server.SetUpdatePeriod(1us); - size_t iters = 1000u; + // Having iters exactly in sync with update rate can lead to a race condition + // in the test between simulation and transport + size_t iters = 999u; + size_t updates = 100u; std::vector wrenches; - wrenches.reserve(iters); + wrenches.reserve(updates); std::mutex wrenchMutex; std::condition_variable cv; auto wrenchCb = std::function( - [&wrenchMutex, &wrenches, &cv, iters](const auto &_msg) + [&wrenchMutex, &wrenches, &cv, updates](const auto &_msg) { std::lock_guard lock(wrenchMutex); wrenches.push_back(_msg); - if (wrenches.size() >= iters) + if (wrenches.size() >= updates) { cv.notify_all(); } @@ -138,8 +144,8 @@ TEST_F(ForceTorqueTest, SensorPoseOffset) const double kGravity = 9.8; { std::unique_lock lock(wrenchMutex); - cv.wait_for(lock, 30s, [&] { return wrenches.size() >= iters; }); - ASSERT_EQ(iters, wrenches.size()); + cv.wait_for(lock, 30s, [&] { return wrenches.size() >= updates; }); + ASSERT_EQ(updates, wrenches.size()); const double kMomentArm = 0.1; const auto &wrench = wrenches.back(); @@ -157,8 +163,8 @@ TEST_F(ForceTorqueTest, SensorPoseOffset) ASSERT_EQ(2 * iters, *server.IterationCount()); { std::unique_lock lock(wrenchMutex); - cv.wait_for(lock, 30s, [&] { return wrenches.size() >= iters; }); - ASSERT_EQ(iters, wrenches.size()); + cv.wait_for(lock, 30s, [&] { return wrenches.size() >= updates; }); + ASSERT_EQ(updates, wrenches.size()); const auto &wrench = wrenches.back(); diff --git a/test/worlds/force_torque.sdf b/test/worlds/force_torque.sdf index fe9e177245..bc394430b2 100644 --- a/test/worlds/force_torque.sdf +++ b/test/worlds/force_torque.sdf @@ -132,7 +132,7 @@ base sensor_plate - 30 + 100 true true force_torque1 @@ -262,7 +262,7 @@ sensor_plate 0.1 0 0 0 0 0 - 30 + 100 true true force_torque2 @@ -392,7 +392,7 @@ sensor_plate 0 0 0.0 30 0 0 - 30 + 100 true true force_torque3 From adfb751542419476f38bcd2217445a53650065dd Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 24 Nov 2021 14:41:50 +0800 Subject: [PATCH 5/9] codecheck Signed-off-by: Arjo Chakravarty --- src/systems/buoyancy/Buoyancy.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc index de16a13b53..fc544941b3 100644 --- a/src/systems/buoyancy/Buoyancy.cc +++ b/src/systems/buoyancy/Buoyancy.cc @@ -441,7 +441,6 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, { auto newPose = enableComponent(_ecm, _entity); newPose |= enableComponent(_ecm, _entity); - // World pose of the link. math::Pose3d linkWorldPose = worldPose(_entity, _ecm); @@ -530,7 +529,7 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, // Physics System. link.AddWorldWrench(_ecm, force, torque); } - + return true; }); } From eb842e00187cdbf945e941ab75c30a56351dcd25 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 24 Nov 2021 16:00:04 +0800 Subject: [PATCH 6/9] fix unit tests Signed-off-by: Arjo Chakravarty --- test/integration/buoyancy.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc index 5ab6ba4ca2..481c7d29ee 100644 --- a/test/integration/buoyancy.cc +++ b/test/integration/buoyancy.cc @@ -57,7 +57,7 @@ TEST_F(BuoyancyTest, UniformWorldMovement) using namespace std::chrono_literals; server.SetUpdatePeriod(1ns); - std::size_t iterations = 1001; + std::size_t iterations = 1000; bool finished = false; test::Relay testSystem; From 525572fcd013cc16bb454e445dc99c8fdcb491b3 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 29 Nov 2021 08:55:15 -0800 Subject: [PATCH 7/9] Increase maximum values in ViewAngle widget (#1221) Signed-off-by: Louise Poubel --- src/gui/plugins/view_angle/ViewAngle.qml | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/gui/plugins/view_angle/ViewAngle.qml b/src/gui/plugins/view_angle/ViewAngle.qml index 9f7213f488..bdf494d73c 100644 --- a/src/gui/plugins/view_angle/ViewAngle.qml +++ b/src/gui/plugins/view_angle/ViewAngle.qml @@ -233,8 +233,8 @@ ColumnLayout { Layout.row: 0 Layout.column: 1 value: ViewAngle.camPose[0] - maximumValue: 1000000 - minimumValue: -1000000 + maximumValue: Number.MAX_VALUE + minimumValue: -Number.MAX_VALUE decimals: 6 stepSize: 0.01 onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) @@ -252,8 +252,8 @@ ColumnLayout { Layout.row: 1 Layout.column: 1 value: ViewAngle.camPose[1] - maximumValue: 1000000 - minimumValue: -1000000 + maximumValue: Number.MAX_VALUE + minimumValue: -Number.MAX_VALUE decimals: 6 stepSize: 0.01 onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) @@ -271,8 +271,8 @@ ColumnLayout { Layout.row: 2 Layout.column: 1 value: ViewAngle.camPose[2] - maximumValue: 1000000 - minimumValue: -1000000 + maximumValue: Number.MAX_VALUE + minimumValue: -Number.MAX_VALUE decimals: 6 stepSize: 0.01 onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) @@ -383,7 +383,7 @@ ColumnLayout { Layout.row: 0 Layout.column: 3 value: ViewAngle.camClipDist[1] - maximumValue: 1000000 + maximumValue: Number.MAX_VALUE minimumValue: nearClip.value decimals: 6 stepSize: 0.01 From 442fbfd2b5ce71fdde7fad48ba4605c63dba4d3b Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 1 Dec 2021 15:34:20 -0800 Subject: [PATCH 8/9] Update SdfGenerator to save link and sensor data to file (#1201) * update more model properties and add code to update link properties Signed-off-by: Ian Chen * update sensors, and add test Signed-off-by: Ian Chen * Suggestion to #1170 Signed-off-by: Nate Koenig * update to use new api Signed-off-by: Ian Chen * use updated api Signed-off-by: Ian Chen * require sdf 12.2 Signed-off-by: Nate Koenig * Update SdfGenerator to save light data to file (#1209) * save lights Signed-off-by: Ian Chen * fix doc Signed-off-by: Ian Chen * use updated api Signed-off-by: Ian Chen * Update SdfGenerator to save joint data to file (#1220) * save joints Signed-off-by: Ian Chen * use joinPaths Signed-off-by: Ian Chen * Only output thread_pitch for screw joints Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig Co-authored-by: Nate Koenig * Fix test Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig --- CMakeLists.txt | 2 +- src/SdfGenerator.cc | 512 ++++++++++++++++++++++++- src/SdfGenerator.hh | 43 +++ src/SdfGenerator_TEST.cc | 66 +++- test/integration/save_world.cc | 528 ++++++++++++++++++++++++++ test/worlds/joint_sensor.sdf | 107 ++++++ test/worlds/non_rendering_sensors.sdf | 176 +++++++++ test/worlds/save_world.sdf | 15 + test/worlds/sensor.sdf | 45 +++ 9 files changed, 1481 insertions(+), 13 deletions(-) create mode 100644 test/worlds/joint_sensor.sdf create mode 100644 test/worlds/non_rendering_sensors.sdf diff --git a/CMakeLists.txt b/CMakeLists.txt index 94162ecd4a..859f59c740 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -50,7 +50,7 @@ set(CMAKE_POLICY_DEFAULT_CMP0077 NEW) # as protobuf could be find transitively by any dependency set(protobuf_MODULE_COMPATIBLE TRUE) -ign_find_package(sdformat12 REQUIRED) +ign_find_package(sdformat12 REQUIRED VERSION 12.2) set(SDF_VER ${sdformat12_VERSION_MAJOR}) #-------------------------------------- diff --git a/src/SdfGenerator.cc b/src/SdfGenerator.cc index e2ca63bfc1..9f8b9286c2 100644 --- a/src/SdfGenerator.cc +++ b/src/SdfGenerator.cc @@ -26,12 +26,37 @@ #include #include "ignition/gazebo/Util.hh" +#include "ignition/gazebo/components/AirPressureSensor.hh" +#include "ignition/gazebo/components/Altimeter.hh" +#include "ignition/gazebo/components/Camera.hh" +#include "ignition/gazebo/components/ChildLinkName.hh" +#include "ignition/gazebo/components/ContactSensor.hh" +#include "ignition/gazebo/components/DepthCamera.hh" +#include "ignition/gazebo/components/ForceTorque.hh" +#include "ignition/gazebo/components/GpuLidar.hh" +#include "ignition/gazebo/components/Imu.hh" +#include "ignition/gazebo/components/Inertial.hh" +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/JointType.hh" #include "ignition/gazebo/components/Light.hh" +#include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/LogicalCamera.hh" +#include "ignition/gazebo/components/Magnetometer.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/ParentLinkName.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/RgbdCamera.hh" +#include "ignition/gazebo/components/SelfCollide.hh" +#include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/SourceFilePath.hh" +#include "ignition/gazebo/components/SegmentationCamera.hh" +#include "ignition/gazebo/components/Static.hh" +#include "ignition/gazebo/components/ThermalCamera.hh" +#include "ignition/gazebo/components/ThreadPitch.hh" +#include "ignition/gazebo/components/WindMode.hh" #include "ignition/gazebo/components/World.hh" @@ -277,7 +302,7 @@ namespace sdf_generator // First remove child entities of whose names can be changed during // simulation (eg. models). Then we add them back from the data in the // ECM. - // TODO(addisu) Remove actors and lights + // TODO(addisu) Remove actors std::vector toRemove; if (_elem->HasElement("model")) { @@ -287,6 +312,15 @@ namespace sdf_generator toRemove.push_back(modelElem); } } + if (_elem->HasElement("light")) + { + for (auto lightElem = _elem->GetElement("light"); lightElem; + lightElem = lightElem->GetNextElement("light")) + { + toRemove.push_back(lightElem); + } + } + for (const auto &e : toRemove) { _elem->RemoveChild(e); @@ -294,6 +328,7 @@ namespace sdf_generator auto worldDir = common::parentPath(worldSdf->Data().Element()->FilePath()); + // models _ecm.Each( [&](const Entity &_modelEntity, const components::Model *, const components::ModelSdf *_modelSdf) @@ -386,6 +421,21 @@ namespace sdf_generator return true; }); + // lights + _ecm.Each( + [&](const Entity &_lightEntity, + const components::Light *, + const components::ParentEntity *_parent) -> bool + { + if (_parent->Data() != _entity) + return true; + + auto lightElem = _elem->AddElement("light"); + updateLightElement(lightElem, _ecm, _lightEntity); + + return true; + }); + return true; } @@ -397,10 +447,12 @@ namespace sdf_generator if (!copySdf(_ecm.Component(_entity), _elem)) return false; - // Update sdf based current components. Here are the list of components to - // be updated: + // Update sdf based on current components. Here are the list of components + // to be updated: // - Name // - Pose + // - Static + // - SelfCollide // This list is to be updated as other components become updateable during // simulation auto *nameComp = _ecm.Component(_entity); @@ -421,19 +473,463 @@ namespace sdf_generator } poseElem->Set(poseComp->Data()); + // static + auto *staticComp = _ecm.Component(_entity); + if (staticComp) + _elem->GetElement("static")->Set(staticComp->Data()); + + // self collide + auto *selfCollideComp = _ecm.Component(_entity); + if (selfCollideComp) + _elem->GetElement("self_collide")->Set(selfCollideComp->Data()); + const auto *pathComp = _ecm.Component(_entity); - if (_elem->HasElement("link") && nullptr != pathComp) + if (_elem->HasElement("link")) + { + if (nullptr != pathComp) + { + // Update relative URIs to use absolute paths. Relative URIs work fine + // in included models, but they have to be converted to absolute URIs + // when the included model is expanded. + relativeToAbsoluteUri(_elem, common::parentPath(pathComp->Data())); + } + + // update links + sdf::ElementPtr linkElem = _elem->GetElement("link"); + while (linkElem) + { + std::string linkName = linkElem->Get("name"); + auto linkEnt = _ecm.EntityByComponents( + components::ParentEntity(_entity), components::Name(linkName)); + if (linkEnt != kNullEntity) + updateLinkElement(linkElem, _ecm, linkEnt); + linkElem = linkElem->GetNextElement("link"); + } + } + + if (_elem->HasElement("joint")) + { + // update joints + sdf::ElementPtr jointElem = _elem->GetElement("joint"); + while (jointElem) + { + std::string jointName = jointElem->Get("name"); + auto jointEnt = _ecm.EntityByComponents( + components::ParentEntity(_entity), components::Name(jointName)); + if (jointEnt != kNullEntity) + updateJointElement(jointElem, _ecm, jointEnt); + jointElem = jointElem->GetNextElement("joint"); + } + } + + return true; + } + + ///////////////////////////////////////////////// + bool updateLinkElement(const sdf::ElementPtr &_elem, + const EntityComponentManager &_ecm, + const Entity &_entity) + { + // Update sdf based on current components. Here are the list of components + // to be updated: + // - Name + // - Pose + // - Inertial + // - WindMode + // This list is to be updated as other components become updateable during + // simulation + auto *nameComp = _ecm.Component(_entity); + _elem->GetAttribute("name")->Set(nameComp->Data()); + + auto *poseComp = _ecm.Component(_entity); + + auto poseElem = _elem->GetElement("pose"); + + // Remove all attributes of poseElem + for (const auto *attrName : {"relative_to", "degrees", "rotation_format"}) + { + sdf::ParamPtr attr = poseElem->GetAttribute(attrName); + if (nullptr != attr) + { + attr->Reset(); + } + } + poseElem->Set(poseComp->Data()); + + // inertial + auto inertialComp = _ecm.Component(_entity); + if (inertialComp) { - // Update relative URIs to use absolute paths. Relative URIs work fine in - // included models, but they have to be converted to absolute URIs when - // the included model is expanded. - relativeToAbsoluteUri(_elem, common::parentPath(pathComp->Data())); + math::Inertiald inertial = inertialComp->Data(); + sdf::ElementPtr inertialElem = _elem->GetElement("inertial"); + inertialElem->GetElement("pose")->Set(inertial.Pose()); + const math::MassMatrix3d &massMatrix = inertial.MassMatrix(); + inertialElem->GetElement("mass")->Set(massMatrix.Mass()); + sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia"); + inertiaElem->GetElement("ixx")->Set(massMatrix.Ixx()); + inertiaElem->GetElement("ixy")->Set(massMatrix.Ixy()); + inertiaElem->GetElement("ixz")->Set(massMatrix.Ixz()); + inertiaElem->GetElement("iyy")->Set(massMatrix.Iyy()); + inertiaElem->GetElement("iyz")->Set(massMatrix.Iyz()); + inertiaElem->GetElement("izz")->Set(massMatrix.Izz()); } + + // wind mode + auto windModeComp = _ecm.Component(_entity); + if (windModeComp) + { + bool windMode = windModeComp->Data(); + _elem->GetElement("enable_wind")->Set(windMode); + } + + // update sensors + if (_elem->HasElement("sensor")) + { + sdf::ElementPtr sensorElem = _elem->GetElement("sensor"); + while (sensorElem) + { + std::string sensorName = sensorElem->Get("name"); + auto sensorEnt = _ecm.EntityByComponents( + components::ParentEntity(_entity), components::Name(sensorName)); + if (sensorEnt != kNullEntity) + updateSensorElement(sensorElem, _ecm, sensorEnt); + sensorElem = sensorElem->GetNextElement("sensor"); + } + } + + // update lights + if (_elem->HasElement("light")) + { + sdf::ElementPtr lightElem = _elem->GetElement("light"); + while (lightElem) + { + std::string lightName = lightElem->Get("name"); + auto lightEnt = _ecm.EntityByComponents( + components::ParentEntity(_entity), components::Name(lightName)); + if (lightEnt != kNullEntity) + updateLightElement(lightElem, _ecm, lightEnt); + lightElem = lightElem->GetNextElement("light"); + } + } + return true; } + ///////////////////////////////////////////////// + bool updateSensorElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity) + { + // Update sdf based on current components. + // This list is to be updated as other components become updateable during + // simulation + auto updateSensorNameAndPose = [&] + { + // override name and pose sdf element using values from ECM + auto *nameComp = _ecm.Component(_entity); + _elem->GetAttribute("name")->Set(nameComp->Data()); + + auto *poseComp = _ecm.Component(_entity); + auto poseElem = _elem->GetElement("pose"); + + // Remove all attributes of poseElem + for (const auto *attrName : {"relative_to", "degrees", "rotation_format"}) + { + sdf::ParamPtr attr = poseElem->GetAttribute(attrName); + if (nullptr != attr) + { + attr->Reset(); + } + } + poseElem->Set(poseComp->Data()); + return true; + }; + + // camera + auto camComp = _ecm.Component(_entity); + if (camComp) + { + const sdf::Sensor &sensor = camComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // depth camera + auto depthCamComp = _ecm.Component(_entity); + if (depthCamComp) + { + const sdf::Sensor &sensor = depthCamComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // thermal camera + auto thermalCamComp = _ecm.Component(_entity); + if (thermalCamComp) + { + const sdf::Sensor &sensor = thermalCamComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // logical camera + auto logicalCamComp = _ecm.Component(_entity); + if (logicalCamComp) + { + // components::LogicalCamera holds an sdf::ElementPtr instead of an + // sdf::Sensor + _elem = logicalCamComp->Data(); + return updateSensorNameAndPose(); + } + // segmentation camera + auto segmentationCamComp = + _ecm.Component(_entity); + if (segmentationCamComp) + { + const sdf::Sensor &sensor = segmentationCamComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + + // gpu lidar + auto gpuLidarComp = _ecm.Component(_entity); + if (gpuLidarComp) + { + const sdf::Sensor &sensor = gpuLidarComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // altimeter + auto altimeterComp = _ecm.Component(_entity); + if (altimeterComp) + { + const sdf::Sensor &sensor = altimeterComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // contact + auto contactComp = _ecm.Component(_entity); + if (contactComp) + { + // components::ContactSensor holds an sdf::ElementPtr instead of an + // sdf::Sensor + _elem = contactComp->Data(); + return updateSensorNameAndPose(); + } + // air pressure + auto airPressureComp = + _ecm.Component(_entity); + if (airPressureComp) + { + const sdf::Sensor &sensor = airPressureComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // force torque + auto forceTorqueComp = _ecm.Component(_entity); + if (forceTorqueComp) + { + const sdf::Sensor &sensor = forceTorqueComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // imu + auto imuComp = _ecm.Component(_entity); + if (imuComp) + { + const sdf::Sensor &sensor = imuComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // magnetometer + auto magnetometerComp = + _ecm.Component(_entity); + if (magnetometerComp) + { + const sdf::Sensor &sensor = magnetometerComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + + return true; + } + + ///////////////////////////////////////////////// + bool updateLightElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity) + { + // Update sdf based on the light component + auto updateLightNameAndPose = [&] + { + // override name and pose sdf element using values from ECM + auto *nameComp = _ecm.Component(_entity); + _elem->GetAttribute("name")->Set(nameComp->Data()); + + auto *poseComp = _ecm.Component(_entity); + auto poseElem = _elem->GetElement("pose"); + + // Remove all attributes of poseElem + for (const auto *attrName : {"relative_to", "degrees", "rotation_format"}) + { + sdf::ParamPtr attr = poseElem->GetAttribute(attrName); + if (nullptr != attr) + { + attr->Reset(); + } + } + poseElem->Set(poseComp->Data()); + return true; + }; + + // light + auto lightComp = _ecm.Component(_entity); + if (lightComp) + { + const sdf::Light &light = lightComp->Data(); + _elem->Copy(light.ToElement()); + return updateLightNameAndPose(); + } + return true; + } + + ///////////////////////////////////////////////// + bool updateJointElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity) + { + // Update sdf based on the joint component + auto updateJointNameAndPose = [&] + { + // override name and pose sdf element using values from ECM + auto *nameComp = _ecm.Component(_entity); + _elem->GetAttribute("name")->Set(nameComp->Data()); + + auto *poseComp = _ecm.Component(_entity); + auto poseElem = _elem->GetElement("pose"); + + // Remove all attributes of poseElem + for (const auto *attrName : {"relative_to", "degrees", "rotation_format"}) + { + sdf::ParamPtr attr = poseElem->GetAttribute(attrName); + if (nullptr != attr) + { + attr->Reset(); + } + } + poseElem->Set(poseComp->Data()); + return true; + }; + + // joint + auto jointComp = _ecm.Component(_entity); + if (!jointComp) + { + return false; + } + + // joint type + auto jointTypeComp = _ecm.Component(_entity); + sdf::JointType jointType = jointTypeComp->Data(); + if (jointTypeComp) + { + std::string jointTypeStr = "invalid"; + switch (jointType) + { + case sdf::JointType::BALL: + jointTypeStr = "ball"; + break; + case sdf::JointType::CONTINUOUS: + jointTypeStr = "continuous"; + break; + case sdf::JointType::FIXED: + jointTypeStr = "fixed"; + break; + case sdf::JointType::PRISMATIC: + jointTypeStr = "prismatic"; + break; + case sdf::JointType::GEARBOX: + jointTypeStr = "gearbox"; + break; + case sdf::JointType::REVOLUTE: + jointTypeStr = "revolute"; + break; + case sdf::JointType::REVOLUTE2: + jointTypeStr = "revolute2"; + break; + case sdf::JointType::SCREW: + jointTypeStr = "screw"; + break; + case sdf::JointType::UNIVERSAL: + jointTypeStr = "universal"; + break; + default: + break; + } + _elem->GetAttribute("type")->Set(jointTypeStr); + } + + // parent + auto parentLinkNameComp = + _ecm.Component(_entity); + if (parentLinkNameComp) + { + _elem->GetElement("parent")->Set(parentLinkNameComp->Data()); + } + // child + auto childLinkNameComp = _ecm.Component(_entity); + if (childLinkNameComp) + { + _elem->GetElement("child")->Set(childLinkNameComp->Data()); + } + // thread pitch + auto threadPitchComp = _ecm.Component(_entity); + if (threadPitchComp && jointType == sdf::JointType::SCREW) + { + _elem->GetElement("thread_pitch")->Set(threadPitchComp->Data()); + } + // axis + auto jointAxisComp = _ecm.Component(_entity); + if (jointAxisComp) + { + const sdf::JointAxis axis = jointAxisComp->Data(); + _elem->GetElement("axis")->Copy(axis.ToElement()); + } + // axis2 + auto jointAxis2Comp = _ecm.Component(_entity); + if (jointAxis2Comp) + { + const sdf::JointAxis axis2 = jointAxis2Comp->Data(); + _elem->GetElement("axis2")->Copy(axis2.ToElement(1u)); + } + + // sensors + // remove existing ones in sdf element and add new ones from ECM. + std::vector toRemove; + if (_elem->HasElement("sensor")) + { + for (auto sensorElem = _elem->GetElement("sensor"); sensorElem; + sensorElem = sensorElem->GetNextElement("sensor")) + { + toRemove.push_back(sensorElem); + } + } + for (const auto &e : toRemove) + { + _elem->RemoveChild(e); + } + + auto sensorEntities = _ecm.EntitiesByComponents( + components::ParentEntity(_entity), components::Sensor()); + + for (const auto &sensorEnt : sensorEntities) + { + sdf::ElementPtr sensorElem = _elem->AddElement("sensor"); + updateSensorElement(sensorElem, _ecm, sensorEnt); + } + + return updateJointNameAndPose(); + } + ///////////////////////////////////////////////// /// \brief Checks if a string is a number /// \param[in] _str The string to check diff --git a/src/SdfGenerator.hh b/src/SdfGenerator.hh index e580fe53e8..83a7844c29 100644 --- a/src/SdfGenerator.hh +++ b/src/SdfGenerator.hh @@ -99,6 +99,49 @@ namespace sdf_generator const EntityComponentManager &_ecm, const Entity &_entity, const std::string &_uri); + /// \brief Update an sdf::Element of a link. + /// Intended for internal use. + /// \input[in, out] _elem sdf::Element to update + /// \input[in] _ecm Immutable reference to the Entity Component Manager + /// \input[in] _entity Link entity + /// \returns true if update succeeded. + IGNITION_GAZEBO_VISIBLE + bool updateLinkElement(const sdf::ElementPtr &_elem, + const EntityComponentManager &_ecm, + const Entity &_entity); + + /// \brief Update an sdf::Element of a sensor. + /// Intended for internal use. + /// \input[in, out] _elem sdf::Element to update + /// \input[in] _ecm Immutable reference to the Entity Component Manager + /// \input[in] _entity Sensor entity + /// \returns true if update succeeded. + IGNITION_GAZEBO_VISIBLE + bool updateSensorElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity); + + /// \brief Update an sdf::Element of a light. + /// Intended for internal use. + /// \input[in, out] _elem sdf::Element to update + /// \input[in] _ecm Immutable reference to the Entity Component Manager + /// \input[in] _entity Light entity + /// \returns true if update succeeded. + IGNITION_GAZEBO_VISIBLE + bool updateLightElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity); + + /// \brief Update an sdf::Element of a joint. + /// Intended for internal use. + /// \input[in, out] _elem sdf::Element to update + /// \input[in] _ecm Immutable reference to the Entity Component Manager + /// \input[in] _entity joint entity + /// \returns true if update succeeded. + IGNITION_GAZEBO_VISIBLE + bool updateJointElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity); } // namespace sdf_generator } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE } // namespace gazebo diff --git a/src/SdfGenerator_TEST.cc b/src/SdfGenerator_TEST.cc index aace52588f..3599fda3ef 100644 --- a/src/SdfGenerator_TEST.cc +++ b/src/SdfGenerator_TEST.cc @@ -117,6 +117,43 @@ static testing::AssertionResult isSubset(const sdf::ElementPtr &_elemA, << "'"; } } + else if (valA->GetTypeName() == "double") + { + double dblA, dblB; + valA->Get(dblA); + valB->Get(dblB); + if (!math::equal(dblA, dblB)) + { + return testing::AssertionFailure() + << "Mismatch in value as double: '" << dblA << "' vs '" + << dblB << "'"; + } + } + else if (valA->GetTypeName() == "float") + { + float fltA, fltB; + valA->Get(fltA); + valB->Get(fltB); + if (!math::equal(fltA, fltB)) + { + return testing::AssertionFailure() + << "Mismatch in value as float: '" << fltA << "' vs '" + << fltB << "'"; + } + } + else if (valA->GetTypeName() == "bool") + { + bool boolA, boolB; + valA->Get(boolA); + valB->Get(boolB); + if (boolA != boolB) + { + return testing::AssertionFailure() + << "Mismatch in value as bool: '" << boolA << "' vs '" + << boolB << "'"; + } + + } else if (valA->GetAsString() != valB->GetAsString()) { return testing::AssertionFailure() @@ -148,6 +185,27 @@ static testing::AssertionResult isSubset(const sdf::ElementPtr &_elemA, if (!result) { + // Ignore missing pose values if the pose is zero. + sdf::ParamPtr childValA = childElemA->GetValue(); + if (childValA->GetTypeName() == "pose") + { + math::Pose3d childValPose; + childValA->Get(childValPose); + if (childValPose == math::Pose3d::Zero) + return testing::AssertionSuccess(); + } + else if (childValA->GetTypeName() == "bool") + { + bool childValBool; + childValA->Get(childValBool); + if (!childValBool && (childElemA->GetName() == "static" || + childElemA->GetName() == "self_collide" || + childElemA->GetName() == "enable_wind" )) + { + return testing::AssertionSuccess(); + } + } + return testing::AssertionFailure() << "No matching child element in element B for child element '" << childElemA->GetName() << "' in element A"; @@ -163,15 +221,15 @@ TEST(CompareElements, CompareWithDuplicateElements) const std::string m1Sdf = R"( - 0 0 0 0 0 0 + 1 0 0 0 0 0 )"; const std::string m1CompTestSdf = R"( - 0 0 0 0 0 0 - 0 0 0 0 0 0 + 1 0 0 0 0 0 + 0 1 0 0 0 0 )"; @@ -271,7 +329,7 @@ class ModelElementFixture : public ElementUpdateFixture auto elem = std::make_shared(); sdf::initFile("model.sdf", elem); - updateModelElement(elem, this->ecm, model); + EXPECT_TRUE(updateModelElement(elem, this->ecm, model)); return elem; } diff --git a/test/integration/save_world.cc b/test/integration/save_world.cc index 7f9f7072e2..0e3964b052 100644 --- a/test/integration/save_world.cc +++ b/test/integration/save_world.cc @@ -20,10 +20,21 @@ #include #include +#include +#include +#include #include +#include +#include +#include +#include +#include #include +#include +#include #include #include +#include #include #include @@ -428,6 +439,523 @@ TEST_F(SdfGeneratorFixture, ModelWithNestedIncludes) EXPECT_TRUE(err.empty()); } +///////////////////////////////////////////////// +TEST_F(SdfGeneratorFixture, WorldWithSensors) +{ + this->LoadWorld("test/worlds/non_rendering_sensors.sdf"); + + const std::string worldGenSdfRes = + this->RequestGeneratedSdf("non_rendering_sensors"); + + sdf::Root root; + sdf::Errors err = root.LoadSdfString(worldGenSdfRes); + EXPECT_TRUE(err.empty()); + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + EXPECT_TRUE(world->ModelNameExists("model")); + auto *model = world->ModelByName("model"); + ASSERT_NE(nullptr, model); + auto *link = model->LinkByName("link"); + ASSERT_NE(nullptr, link); + + // altimeter + { + auto *sensor = link->SensorByName("altimeter_sensor"); + ASSERT_NE(nullptr, sensor); + const sdf::Altimeter *altimeter = sensor->AltimeterSensor(); + ASSERT_NE(nullptr, altimeter); + const sdf::Noise &posNoise = altimeter->VerticalPositionNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, posNoise.Type()); + EXPECT_DOUBLE_EQ(0.1, posNoise.Mean()); + EXPECT_DOUBLE_EQ(0.2, posNoise.StdDev()); + + const sdf::Noise &velNoise = altimeter->VerticalVelocityNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, velNoise.Type()); + EXPECT_DOUBLE_EQ(2.3, velNoise.Mean()); + EXPECT_DOUBLE_EQ(4.5, velNoise.StdDev()); + } + + // contact sensor + { + // contact sensor does not have an SDF DOM class + auto *sensor = link->SensorByName("contact_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(4, 5, 6, 0, 0, 0), sensor->RawPose()); + EXPECT_EQ(sdf::SensorType::CONTACT, sensor->Type()); + } + + // force torque + { + auto *sensor = link->SensorByName("force_torque_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(10, 11, 12, 0, 0, 0), sensor->RawPose()); + const sdf::ForceTorque *forceTorque = sensor->ForceTorqueSensor(); + ASSERT_NE(nullptr, forceTorque); + EXPECT_EQ(sdf::ForceTorqueFrame::CHILD, forceTorque->Frame()); + EXPECT_EQ(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD, + forceTorque->MeasureDirection()); + const sdf::Noise &forceXNoise = forceTorque->ForceXNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN_QUANTIZED, forceXNoise.Type()); + EXPECT_DOUBLE_EQ(0.02, forceXNoise.Mean()); + EXPECT_DOUBLE_EQ(0.0005, forceXNoise.StdDev()); + const sdf::Noise &torqueYNoise = forceTorque->TorqueYNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, torqueYNoise.Type()); + EXPECT_DOUBLE_EQ(0.009, torqueYNoise.Mean()); + EXPECT_DOUBLE_EQ(0.0000985, torqueYNoise.StdDev()); + } + + // imu + { + auto *sensor = link->SensorByName("imu_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(4, 5, 6, 0, 0, 0), sensor->RawPose()); + const sdf::Imu *imu = sensor->ImuSensor(); + ASSERT_NE(nullptr, imu); + const sdf::Noise &linAccXNoise = imu->LinearAccelerationXNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, linAccXNoise.Type()); + EXPECT_DOUBLE_EQ(0.0, linAccXNoise.Mean()); + EXPECT_DOUBLE_EQ(0.1, linAccXNoise.StdDev()); + EXPECT_DOUBLE_EQ(0.2, linAccXNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(1.0, linAccXNoise.DynamicBiasCorrelationTime()); + const sdf::Noise &linAccYNoise = imu->LinearAccelerationYNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, linAccYNoise.Type()); + EXPECT_DOUBLE_EQ(1.0, linAccYNoise.Mean()); + EXPECT_DOUBLE_EQ(1.1, linAccYNoise.StdDev()); + EXPECT_DOUBLE_EQ(1.2, linAccYNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(2.0, linAccYNoise.DynamicBiasCorrelationTime()); + const sdf::Noise &linAccZNoise = imu->LinearAccelerationZNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, linAccZNoise.Type()); + EXPECT_DOUBLE_EQ(2.0, linAccZNoise.Mean()); + EXPECT_DOUBLE_EQ(2.1, linAccZNoise.StdDev()); + EXPECT_DOUBLE_EQ(2.2, linAccZNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(3.0, linAccZNoise.DynamicBiasCorrelationTime()); + const sdf::Noise &angVelXNoise = imu->AngularVelocityXNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, angVelXNoise.Type()); + EXPECT_DOUBLE_EQ(3.0, angVelXNoise.Mean()); + EXPECT_DOUBLE_EQ(3.1, angVelXNoise.StdDev()); + EXPECT_DOUBLE_EQ(4.2, angVelXNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(4.0, angVelXNoise.DynamicBiasCorrelationTime()); + const sdf::Noise &angVelYNoise = imu->AngularVelocityYNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, angVelYNoise.Type()); + EXPECT_DOUBLE_EQ(4.0, angVelYNoise.Mean()); + EXPECT_DOUBLE_EQ(4.1, angVelYNoise.StdDev()); + EXPECT_DOUBLE_EQ(5.2, angVelYNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(5.0, angVelYNoise.DynamicBiasCorrelationTime()); + const sdf::Noise &angVelZNoise = imu->AngularVelocityZNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, angVelZNoise.Type()); + EXPECT_DOUBLE_EQ(5.0, angVelZNoise.Mean()); + EXPECT_DOUBLE_EQ(5.1, angVelZNoise.StdDev()); + EXPECT_DOUBLE_EQ(6.2, angVelZNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(6.0, angVelZNoise.DynamicBiasCorrelationTime()); + + EXPECT_EQ("ENU", imu->Localization()); + EXPECT_EQ("linka", imu->CustomRpyParentFrame()); + EXPECT_EQ(math::Vector3d::UnitY, imu->CustomRpy()); + EXPECT_EQ("linkb", imu->GravityDirXParentFrame()); + EXPECT_EQ(math::Vector3d::UnitZ, imu->GravityDirX()); + EXPECT_FALSE(imu->OrientationEnabled()); + } + + // logical camera + { + // logical camera sensor does not have an SDF DOM class + auto *sensor = link->SensorByName("logical_camera_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(7, 8, 9, 0, 0, 0), sensor->RawPose()); + EXPECT_EQ(sdf::SensorType::LOGICAL_CAMERA, sensor->Type()); + } + + // magnetometer + { + auto *sensor = link->SensorByName("magnetometer_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(10, 11, 12, 0, 0, 0), sensor->RawPose()); + const sdf::Magnetometer *magnetometer = sensor->MagnetometerSensor(); + ASSERT_NE(nullptr, magnetometer); + const sdf::Noise &xNoise = magnetometer->XNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, xNoise.Type()); + EXPECT_DOUBLE_EQ(0.1, xNoise.Mean()); + EXPECT_DOUBLE_EQ(0.2, xNoise.StdDev()); + const sdf::Noise &yNoise = magnetometer->YNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, yNoise.Type()); + EXPECT_DOUBLE_EQ(1.2, yNoise.Mean()); + EXPECT_DOUBLE_EQ(2.3, yNoise.StdDev()); + const sdf::Noise &zNoise = magnetometer->ZNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, zNoise.Type()); + EXPECT_DOUBLE_EQ(3.4, zNoise.Mean()); + EXPECT_DOUBLE_EQ(5.6, zNoise.StdDev()); + } + + // air pressure + { + auto *sensor = link->SensorByName("air_pressure_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(10, 20, 30, 0, 0, 0), sensor->RawPose()); + const sdf::AirPressure *airPressure = sensor->AirPressureSensor(); + ASSERT_NE(nullptr, airPressure); + EXPECT_DOUBLE_EQ(123.4, airPressure->ReferenceAltitude()); + const sdf::Noise &noise = airPressure->PressureNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, noise.Type()); + EXPECT_DOUBLE_EQ(3.4, noise.Mean()); + EXPECT_DOUBLE_EQ(5.6, noise.StdDev()); + } + + tinyxml2::XMLDocument genSdfDoc; + genSdfDoc.Parse(worldGenSdfRes.c_str()); + ASSERT_NE(nullptr, genSdfDoc.RootElement()); + auto genWorld = genSdfDoc.RootElement()->FirstChildElement("world"); + ASSERT_NE(nullptr, genWorld); +} + +///////////////////////////////////////////////// +TEST_F(SdfGeneratorFixture, WorldWithRenderingSensors) +{ + this->LoadWorld("test/worlds/sensor.sdf"); + + const std::string worldGenSdfRes = + this->RequestGeneratedSdf("camera_sensor"); + + sdf::Root root; + sdf::Errors err = root.LoadSdfString(worldGenSdfRes); + EXPECT_TRUE(err.empty()); + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + // camera + { + EXPECT_TRUE(world->ModelNameExists("camera")); + auto *model = world->ModelByName("camera"); + ASSERT_NE(nullptr, model); + EXPECT_EQ(1u, model->LinkCount()); + + auto *link = model->LinkByName("link"); + ASSERT_NE(nullptr, link); + math::MassMatrix3d massMatrix(0.1, + math::Vector3d( 0.000166667, 0.000166667, 0.000166667), + math::Vector3d::Zero); + math::Inertiald inertial(massMatrix, math::Pose3d::Zero); + EXPECT_EQ(inertial, link->Inertial()); + + auto *cameraSensor = link->SensorByName("camera"); + ASSERT_NE(nullptr, cameraSensor); + EXPECT_EQ("camera", cameraSensor->Topic()); + const sdf::Camera *camera = cameraSensor->CameraSensor(); + ASSERT_NE(nullptr, camera); + EXPECT_DOUBLE_EQ(1.047, camera->HorizontalFov().Radian()); + EXPECT_EQ(320u, camera->ImageWidth()); + EXPECT_EQ(240u, camera->ImageHeight()); + EXPECT_DOUBLE_EQ(0.1, camera->NearClip()); + EXPECT_DOUBLE_EQ(100, camera->FarClip()); + const sdf::Noise &noise = camera->ImageNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN_QUANTIZED, noise.Type()); + EXPECT_DOUBLE_EQ(0.01, noise.Mean()); + EXPECT_DOUBLE_EQ(0.0002, noise.StdDev()); + } + + EXPECT_TRUE(world->ModelNameExists("default_topics")); + auto *model = world->ModelByName("default_topics"); + ASSERT_NE(nullptr, model); + // gpu lidar + { + auto *gpuLidarLink = model->LinkByName("gpu_lidar_link"); + ASSERT_NE(nullptr, gpuLidarLink); + auto *gpuLidarSensor = gpuLidarLink->SensorByName("gpu_lidar"); + const sdf::Lidar *lidar = gpuLidarSensor->LidarSensor(); + EXPECT_EQ(640u, lidar->HorizontalScanSamples()); + EXPECT_DOUBLE_EQ(1.0, lidar->HorizontalScanResolution()); + EXPECT_NEAR(-1.396263, lidar->HorizontalScanMinAngle().Radian(), 1e-5); + EXPECT_NEAR(1.396263, lidar->HorizontalScanMaxAngle().Radian(), 1e-5); + EXPECT_EQ(1u, lidar->VerticalScanSamples()); + EXPECT_DOUBLE_EQ(0.01, lidar->VerticalScanResolution()); + EXPECT_DOUBLE_EQ(0.0, lidar->VerticalScanMinAngle().Radian()); + EXPECT_DOUBLE_EQ(0.0, lidar->VerticalScanMaxAngle().Radian()); + EXPECT_DOUBLE_EQ(0.08, lidar->RangeMin()); + EXPECT_DOUBLE_EQ(10.0, lidar->RangeMax()); + EXPECT_DOUBLE_EQ(0.01, lidar->RangeResolution()); + } + + // depth camera + { + auto *depthLink = model->LinkByName("depth_camera_link"); + ASSERT_NE(nullptr, depthLink); + auto *depthSensor = depthLink->SensorByName("depth_camera"); + ASSERT_NE(nullptr, depthSensor); + const sdf::Camera *camera = depthSensor->CameraSensor(); + ASSERT_NE(nullptr, camera); + EXPECT_DOUBLE_EQ(1.05, camera->HorizontalFov().Radian()); + EXPECT_EQ(256u, camera->ImageWidth()); + EXPECT_EQ(256u, camera->ImageHeight()); + EXPECT_EQ("R_FLOAT32", camera->PixelFormatStr()); + EXPECT_DOUBLE_EQ(0.1, camera->NearClip()); + EXPECT_DOUBLE_EQ(10, camera->FarClip()); + EXPECT_DOUBLE_EQ(0.05, camera->DepthNearClip()); + EXPECT_DOUBLE_EQ(9.0, camera->DepthFarClip()); + } + + // rgbd camera + { + auto *rgbdLink = model->LinkByName("rgbd_camera_link"); + ASSERT_NE(nullptr, rgbdLink); + auto *rgbdSensor = rgbdLink->SensorByName("rgbd_camera"); + ASSERT_NE(nullptr, rgbdSensor); + const sdf::Camera *camera = rgbdSensor->CameraSensor(); + ASSERT_NE(nullptr, camera); + EXPECT_DOUBLE_EQ(1.05, camera->HorizontalFov().Radian()); + EXPECT_EQ(256u, camera->ImageWidth()); + EXPECT_EQ(256u, camera->ImageHeight()); + EXPECT_DOUBLE_EQ(0.1, camera->NearClip()); + EXPECT_DOUBLE_EQ(10, camera->FarClip()); + } + + // thermal camera + { + auto *thermalLink = model->LinkByName("thermal_camera_link"); + ASSERT_NE(nullptr, thermalLink); + auto *thermalSensor = thermalLink->SensorByName("thermal_camera"); + ASSERT_NE(nullptr, thermalSensor); + const sdf::Camera *camera = thermalSensor->CameraSensor(); + ASSERT_NE(nullptr, camera); + EXPECT_DOUBLE_EQ(1.15, camera->HorizontalFov().Radian()); + EXPECT_EQ(300u, camera->ImageWidth()); + EXPECT_EQ(200u, camera->ImageHeight()); + EXPECT_DOUBLE_EQ(0.14, camera->NearClip()); + EXPECT_DOUBLE_EQ(120.0, camera->FarClip()); + } + + // segmentation camera + { + auto *segmentationLink = model->LinkByName("segmentation_camera_link"); + ASSERT_NE(nullptr, segmentationLink); + auto *segmentationSensor = + segmentationLink->SensorByName("segmentation_camera"); + ASSERT_NE(nullptr, segmentationSensor); + const sdf::Camera *camera = segmentationSensor->CameraSensor(); + ASSERT_NE(nullptr, camera); + EXPECT_DOUBLE_EQ(1.0, camera->HorizontalFov().Radian()); + EXPECT_EQ(320u, camera->ImageWidth()); + EXPECT_EQ(240u, camera->ImageHeight()); + EXPECT_DOUBLE_EQ(0.11, camera->NearClip()); + EXPECT_DOUBLE_EQ(20.0, camera->FarClip()); + EXPECT_EQ("panoptic", camera->SegmentationType()); + } + + tinyxml2::XMLDocument genSdfDoc; + genSdfDoc.Parse(worldGenSdfRes.c_str()); + ASSERT_NE(nullptr, genSdfDoc.RootElement()); + auto genWorld = genSdfDoc.RootElement()->FirstChildElement("world"); + ASSERT_NE(nullptr, genWorld); +} + +///////////////////////////////////////////////// +TEST_F(SdfGeneratorFixture, WorldWithLights) +{ + this->LoadWorld("test/worlds/lights.sdf"); + + const std::string worldGenSdfRes = + this->RequestGeneratedSdf("lights"); + + sdf::Root root; + sdf::Errors err = root.LoadSdfString(worldGenSdfRes); + EXPECT_TRUE(err.empty()); + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + // directional light in the world + { + const sdf::Light *light = world->LightByIndex(0u); + EXPECT_EQ("directional", light->Name()); + EXPECT_EQ(sdf::LightType::DIRECTIONAL, light->Type()); + EXPECT_EQ(ignition::math::Pose3d(0, 0, 10, 0, 0, 0), + light->RawPose()); + EXPECT_EQ(std::string(), light->PoseRelativeTo()); + EXPECT_TRUE(light->CastShadows()); + EXPECT_EQ(ignition::math::Color(0.8f, 0.8f, 0.8f, 1), + light->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.2f, 0.2f, 0.2f, 1), + light->Specular()); + EXPECT_DOUBLE_EQ(100, light->AttenuationRange()); + EXPECT_DOUBLE_EQ(0.9, light->ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.01, light->LinearAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.001, light->QuadraticAttenuationFactor()); + EXPECT_EQ(ignition::math::Vector3d(0.5, 0.2, -0.9), + light->Direction()); + } + // point light in the world + { + const sdf::Light *light = world->LightByIndex(1u); + EXPECT_EQ("point", light->Name()); + EXPECT_EQ(sdf::LightType::POINT, light->Type()); + EXPECT_EQ(ignition::math::Pose3d(0, -1.5, 3, 0, 0, 0), + light->RawPose()); + EXPECT_FALSE(light->CastShadows()); + EXPECT_EQ(ignition::math::Color(1.0f, 0.0f, 0.0f, 1), + light->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.1f, 0.1f, 0.1f, 1), + light->Specular()); + EXPECT_DOUBLE_EQ(4, light->AttenuationRange()); + EXPECT_DOUBLE_EQ(0.2, light->ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.5, light->LinearAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.01, light->QuadraticAttenuationFactor()); + } + // spot light in the world + { + const sdf::Light *light = world->LightByIndex(2u); + EXPECT_EQ("spot", light->Name()); + EXPECT_EQ(sdf::LightType::SPOT, light->Type()); + EXPECT_EQ(ignition::math::Pose3d(0, 1.5, 3, 0, 0, 0), + light->RawPose()); + EXPECT_EQ(std::string(), light->PoseRelativeTo()); + EXPECT_FALSE(light->CastShadows()); + EXPECT_EQ(ignition::math::Color(0.0f, 1.0f, 0.0f, 1), + light->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.2f, 0.2f, 0.2f, 1), + light->Specular()); + EXPECT_DOUBLE_EQ(5, light->AttenuationRange()); + EXPECT_DOUBLE_EQ(0.3, light->ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.4, light->LinearAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.001, light->QuadraticAttenuationFactor()); + EXPECT_EQ(ignition::math::Vector3d(0.0, 0.0, -1.0), + light->Direction()); + EXPECT_DOUBLE_EQ(0.1, light->SpotInnerAngle().Radian()); + EXPECT_DOUBLE_EQ(0.5, light->SpotOuterAngle().Radian()); + EXPECT_DOUBLE_EQ(0.8, light->SpotFalloff()); + } + + // get model + EXPECT_TRUE(world->ModelNameExists("sphere")); + auto *model = world->ModelByName("sphere"); + ASSERT_NE(nullptr, model); + EXPECT_EQ(1u, model->LinkCount()); + + // get link + auto *link = model->LinkByName("sphere_link"); + ASSERT_NE(nullptr, link); + + // light attached to link + { + const sdf::Light *light = link->LightByName("link_light_point"); + EXPECT_EQ("link_light_point", light->Name()); + EXPECT_EQ(ignition::math::Pose3d(0.0, 0.0, 1.0, 0, 0, 0), + light->RawPose()); + EXPECT_EQ(sdf::LightType::POINT, light->Type()); + EXPECT_FALSE(light->CastShadows()); + EXPECT_EQ(ignition::math::Color(0.0f, 0.0f, 1.0f, 1), + light->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.1f, 0.1f, 0.1f, 1), + light->Specular()); + EXPECT_DOUBLE_EQ(2, light->AttenuationRange()); + EXPECT_DOUBLE_EQ(0.05, light->ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.02, light->LinearAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.01, light->QuadraticAttenuationFactor()); + } + + tinyxml2::XMLDocument genSdfDoc; + genSdfDoc.Parse(worldGenSdfRes.c_str()); + ASSERT_NE(nullptr, genSdfDoc.RootElement()); + auto genWorld = genSdfDoc.RootElement()->FirstChildElement("world"); + ASSERT_NE(nullptr, genWorld); +} + +///////////////////////////////////////////////// +TEST_F(SdfGeneratorFixture, ModelWithJoints) +{ + this->LoadWorld(ignition::common::joinPaths("test", "worlds", + "joint_sensor.sdf")); + + const std::string worldGenSdfRes = + this->RequestGeneratedSdf("joint_sensor"); + + sdf::Root root; + sdf::Errors err = root.LoadSdfString(worldGenSdfRes); + EXPECT_TRUE(err.empty()); + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + EXPECT_EQ(1u, world->ModelCount()); + + EXPECT_TRUE(world->ModelNameExists("model")); + auto *model = world->ModelByName("model"); + ASSERT_NE(nullptr, model); + EXPECT_EQ(2u, model->LinkCount()); + auto *link1 = model->LinkByName("link1"); + ASSERT_NE(nullptr, link1); + auto *link2 = model->LinkByName("link2"); + ASSERT_NE(nullptr, link2); + EXPECT_EQ(1u, model->JointCount()); + auto *joint = model->JointByName("joint"); + ASSERT_NE(nullptr, joint); + + EXPECT_EQ("link1", joint->ParentLinkName()); + EXPECT_EQ("link2", joint->ChildLinkName()); + EXPECT_EQ(sdf::JointType::REVOLUTE2, joint->Type()); + + // Get the first axis + const sdf::JointAxis *axis = joint->Axis(); + ASSERT_NE(nullptr, axis); + ASSERT_NE(nullptr, axis->Element()); + + // Get the second axis + const sdf::JointAxis *axis2 = joint->Axis(1); + ASSERT_NE(nullptr, axis2); + + EXPECT_EQ(ignition::math::Vector3d::UnitZ, axis->Xyz()); + EXPECT_EQ(ignition::math::Vector3d::UnitY, axis2->Xyz()); + + EXPECT_EQ("__model__", axis->XyzExpressedIn()); + EXPECT_TRUE(axis2->XyzExpressedIn().empty()); + + EXPECT_DOUBLE_EQ(-0.5, axis->Lower()); + EXPECT_DOUBLE_EQ(0.5, axis->Upper()); + EXPECT_DOUBLE_EQ(-1.0, axis2->Lower()); + EXPECT_DOUBLE_EQ(1.0, axis2->Upper()); + + EXPECT_DOUBLE_EQ(123.4, axis->Effort()); + EXPECT_DOUBLE_EQ(0.5, axis2->Effort()); + + EXPECT_DOUBLE_EQ(12.0, axis->MaxVelocity()); + EXPECT_DOUBLE_EQ(200.0, axis2->MaxVelocity()); + + EXPECT_DOUBLE_EQ(0.1, axis->Damping()); + EXPECT_DOUBLE_EQ(0.0, axis2->Damping()); + + EXPECT_DOUBLE_EQ(0.2, axis->Friction()); + EXPECT_DOUBLE_EQ(0.0, axis2->Friction()); + + EXPECT_DOUBLE_EQ(1.3, axis->SpringReference()); + EXPECT_DOUBLE_EQ(0.0, axis2->SpringReference()); + + EXPECT_DOUBLE_EQ(10.6, axis->SpringStiffness()); + EXPECT_DOUBLE_EQ(0.0, axis2->SpringStiffness()); + + // sensor + const sdf::Sensor *forceTorqueSensor = + joint->SensorByName("force_torque_sensor"); + ASSERT_NE(nullptr, forceTorqueSensor); + EXPECT_EQ("force_torque_sensor", forceTorqueSensor->Name()); + EXPECT_EQ(sdf::SensorType::FORCE_TORQUE, forceTorqueSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0), + forceTorqueSensor->RawPose()); + auto forceTorqueSensorObj = forceTorqueSensor->ForceTorqueSensor(); + ASSERT_NE(nullptr, forceTorqueSensorObj); + EXPECT_EQ(sdf::ForceTorqueFrame::PARENT, forceTorqueSensorObj->Frame()); + EXPECT_EQ(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD, + forceTorqueSensorObj->MeasureDirection()); + + EXPECT_DOUBLE_EQ(0.0, forceTorqueSensorObj->ForceXNoise().Mean()); + EXPECT_DOUBLE_EQ(0.1, forceTorqueSensorObj->ForceXNoise().StdDev()); + EXPECT_DOUBLE_EQ(1.0, forceTorqueSensorObj->ForceYNoise().Mean()); + EXPECT_DOUBLE_EQ(1.1, forceTorqueSensorObj->ForceYNoise().StdDev()); + EXPECT_DOUBLE_EQ(2.0, forceTorqueSensorObj->ForceZNoise().Mean()); + EXPECT_DOUBLE_EQ(2.1, forceTorqueSensorObj->ForceZNoise().StdDev()); + + EXPECT_DOUBLE_EQ(3.0, forceTorqueSensorObj->TorqueXNoise().Mean()); + EXPECT_DOUBLE_EQ(3.1, forceTorqueSensorObj->TorqueXNoise().StdDev()); + EXPECT_DOUBLE_EQ(4.0, forceTorqueSensorObj->TorqueYNoise().Mean()); + EXPECT_DOUBLE_EQ(4.1, forceTorqueSensorObj->TorqueYNoise().StdDev()); + EXPECT_DOUBLE_EQ(5.0, forceTorqueSensorObj->TorqueZNoise().Mean()); + EXPECT_DOUBLE_EQ(5.1, forceTorqueSensorObj->TorqueZNoise().StdDev()); +} + ///////////////////////////////////////////////// /// Main int main(int _argc, char **_argv) diff --git a/test/worlds/joint_sensor.sdf b/test/worlds/joint_sensor.sdf new file mode 100644 index 0000000000..f96cdeefe0 --- /dev/null +++ b/test/worlds/joint_sensor.sdf @@ -0,0 +1,107 @@ + + + + + + + + + + + + + link2 + link1 + + 0 0 1 + 0.5 + true + + -0.5 + 0.5 + 123.4 + 12 + + + 0.1 + 0.2 + 1.3 + 10.6 + + + + 0 1 0 + 1.5 + false + + -1 + 1 + 0.5 + 200 + + + + + 1 + + 0 + 0.2 + + + + + 10 11 12 0 0 0 + + parent + parent_to_child + + + + 0 + 0.1 + + + + + 1 + 1.1 + + + + + 2 + 2.1 + + + + + + + 3 + 3.1 + + + + + 4 + 4.1 + + + + + 5 + 5.1 + + + + + + + + + + diff --git a/test/worlds/non_rendering_sensors.sdf b/test/worlds/non_rendering_sensors.sdf new file mode 100644 index 0000000000..f5fac8c8aa --- /dev/null +++ b/test/worlds/non_rendering_sensors.sdf @@ -0,0 +1,176 @@ + + + + + ogre2 + + + + + + + 0 0 3 0 0 0 + + + + + 0.1 + 0.2 + + + + + 2.3 + 4.5 + + + + + + + 4 5 6 0 0 0 + true + + + + 10 11 12 0 0 0 + + child + parent_to_child + + + + 0.02 + 0.0005 + + + + + + + 0.009 + 0.0000985 + + + + + + + + 4 5 6 0 0 0 + + + + + 0 + 0.1 + 0.2 + 1 + + + + + 1 + 1.1 + 1.2 + 2 + + + + + 2 + 2.1 + 2.2 + 3 + + + + + + + 3 + 3.1 + 4.2 + 4 + + + + + 4 + 4.1 + 5.2 + 5 + + + + + 5 + 5.1 + 6.2 + 6 + + + + + ENU + 0 1 0 + 0 0 1 + + false + + + + + 7 8 9 0 0 0 + + 0.1 + 100.1 + 1.33 + 1.234 + + + + + 10 11 12 0 0 0 + + + + 0.1 + 0.2 + + + + + 1.2 + 2.3 + + + + + 3.4 + 5.6 + + + + + + + 10 20 30 0 0 0 + + 123.4 + + + 3.4 + 5.6 + + + + + + + + diff --git a/test/worlds/save_world.sdf b/test/worlds/save_world.sdf index e6a431d4e6..226ef45cb5 100644 --- a/test/worlds/save_world.sdf +++ b/test/worlds/save_world.sdf @@ -13,6 +13,21 @@ filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands"> + 0 0 -9.8 + 5.5645e-6 22.8758e-6 -42.3884e-6 + + + + 0.4 0.4 0.4 1.0 + .7 .7 .7 1 + true + + + + 0.001 + 1.0 + 1000 + 10 0 0 0 0 0 diff --git a/test/worlds/sensor.sdf b/test/worlds/sensor.sdf index aebb32104c..1587a32736 100644 --- a/test/worlds/sensor.sdf +++ b/test/worlds/sensor.sdf @@ -64,6 +64,11 @@ 0.1 100 + + gaussian_quantized + 0.01 + 0.0002 + 1 30 @@ -131,6 +136,12 @@ 0.1 10.0 + + + 0.05 + 9.0 + + @@ -150,6 +161,40 @@ + + + + + 1.15 + + 300 + 200 + + + 0.14 + 120.0 + + + + + + + + + 1.0 + + 320 + 240 + + + 0.11 + 20.0 + + panoptic + + + + From f2927775e7fb7128ff04b2818e94c209eaf8726d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 2 Dec 2021 11:13:19 +0100 Subject: [PATCH 9/9] Added sensor plugin to be able to visualize camera in plane_propeller_demo.sdf (#1226) Signed-off-by: ahcorde --- examples/worlds/plane_propeller_demo.sdf | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/examples/worlds/plane_propeller_demo.sdf b/examples/worlds/plane_propeller_demo.sdf index d7f104cbf0..3604385a09 100644 --- a/examples/worlds/plane_propeller_demo.sdf +++ b/examples/worlds/plane_propeller_demo.sdf @@ -24,6 +24,10 @@ filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster"> + + true