diff --git a/CMakeLists.txt b/CMakeLists.txt index d502043b77..801f5067fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(gz-sim8 VERSION 8.6.0) +project(gz-sim8 VERSION 8.7.0) set (GZ_DISTRIBUTION "Harmonic") #============================================================================ diff --git a/Changelog.md b/Changelog.md index b17cc44204..15f7042e5b 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,82 @@ ## Gazebo Sim 8.x +### Gazebo Sim 8.7.0 (2024-11-08) + +1. Fix crash when multicopter motor system is attached to an empty model + * [Pull request #2653](https://github.com/gazebosim/gz-sim/pull/2653) + +1. Backport SystemConfigurePriority to harmonic + * [Pull request #2661](https://github.com/gazebosim/gz-sim/pull/2661) + +1. Break out server constructor + * [Pull request #2638](https://github.com/gazebosim/gz-sim/pull/2638) + +1. Add check for valid world pose cmd values + * [Pull request #2640](https://github.com/gazebosim/gz-sim/pull/2640) + +1. Enabling Global Illumination (GI VCT) for sensors in SDF + * [Pull request #2550](https://github.com/gazebosim/gz-sim/pull/2550) + +1. Fixed typos in three places + * [Pull request #2599](https://github.com/gazebosim/gz-sim/pull/2599) + +1. Add point cloud data size sanity check to fix crash + * [Pull request #2549](https://github.com/gazebosim/gz-sim/pull/2549) + +1. Corrected command to move the kth_freeflyer spacecraft testbed + * [Pull request #2565](https://github.com/gazebosim/gz-sim/pull/2565) + +1. Backport nonbreaking changes from ionic + * [Pull request #2552](https://github.com/gazebosim/gz-sim/pull/2552) + +1. clang-tidy fixes: use empty(), fix includes + * [Pull request #2548](https://github.com/gazebosim/gz-sim/pull/2548) + +1. Check ranges before access + * [Pull request #2540](https://github.com/gazebosim/gz-sim/pull/2540) + +1. Fixed Odometry Publisher Angular Velocity Singularity in 3D + * [Pull request #2348](https://github.com/gazebosim/gz-sim/pull/2348) + +1. Revert behavior change introduced in #2452 + * [Pull request #2527](https://github.com/gazebosim/gz-sim/pull/2527) + +1. Specify System::PreUpdate, Update execution order + * [Pull request #2487](https://github.com/gazebosim/gz-sim/pull/2487) + +1. Force Qt to use xcb plugin on Wayland + * [Pull request #2526](https://github.com/gazebosim/gz-sim/pull/2526) + +1. Remove unused variable in thruster system + * [Pull request #2524](https://github.com/gazebosim/gz-sim/pull/2524) + +1. Make sure steering joints exist before updating velocity / odometry in AckermannSteering plugin + * [Pull request #2521](https://github.com/gazebosim/gz-sim/pull/2521) + +1. Fix ResourceSpawner + * [Pull request #2490](https://github.com/gazebosim/gz-sim/pull/2490) + +1. gui_system_plugin: clarify description in README + * [Pull request #2253](https://github.com/gazebosim/gz-sim/pull/2253) + +1. Fix adding system to non-existent entity + * [Pull request #2516](https://github.com/gazebosim/gz-sim/pull/2516) + +1. Checking linkEnity is empty + * [Pull request #2509](https://github.com/gazebosim/gz-sim/pull/2509) + +1. Initialize threadsNeedCleanUp + * [Pull request #2503](https://github.com/gazebosim/gz-sim/pull/2503) + +1. Added support for spacecraft thrusters + * [Pull request #2431](https://github.com/gazebosim/gz-sim/pull/2431) + +1. Disable rendering tests that are failing on github actions + * [Pull request #2480](https://github.com/gazebosim/gz-sim/pull/2480) + +1. Consolidate entity creation + * [Pull request #2452](https://github.com/gazebosim/gz-sim/pull/2452) + ### Gazebo Sim 8.6.0 (2024-07-25) 1. Fix error resolving gazebo classic material when loading world diff --git a/examples/worlds/global_illumination.sdf b/examples/worlds/global_illumination.sdf new file mode 100644 index 0000000000..1c2f0b0bac --- /dev/null +++ b/examples/worlds/global_illumination.sdf @@ -0,0 +1,328 @@ + + + + + + + + + ogre2 + + true + 16 16 16 + 1 1 1 + 6 + true + true + 1.0 + false + none + + + + + + + + + + 0.1 0.1 0.1 + 0 0 0 + + + + + + + Cornell box GI demo + true + docked + + ogre2 + -12 0 4 0 0 0 + + + + camera + + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 0 + 50 + 250 + 50 + docked + true + #777777 + + + + + false + 5 + 5 + floating + false + + + + + + + 0 0 7.5 0 0 0 + 1 1 1 1 + 0.0 0.0 0.0 0 + + 50 + 0 + 0 + 0.02 + + true + false + 1.0 + + + + + 0 5 4 0 0 0 + true + + + + + 10 1 10 + + + + + + + 10 1 10 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 0 -5 4 0 0 0 + true + + + + + 10 1 10 + + + + + + + 10 1 10 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 0 0 -0.5 0 0 0 + true + + + + + 10 10 1 + + + + + + + 10 10 1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 8.5 0 0 0 + true + + + + + 10 10 1 + + + + + + + 10 10 1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 4.5 0 4 0 0 0 + true + + + + + 1 10 10 + + + + + + + 1 10 10 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + -1.5 0 0 0 0 0 + + + + + https://fuel.gazebosim.org/1.0/openrobotics/models/pump/1/files/meshes/pump.dae + 3 3 3 + + + + + + + + https://fuel.gazebosim.org/1.0/openrobotics/models/pump/1/files/meshes/pump.dae + 3 3 3 + + + + 1.0 1.0 1.0 + 1.0 1.0 1.0 + + + https://fuel.gazebosim.org/1.0/openrobotics/models/pump/1/files/materials/textures/pump_albedo.png + https://fuel.gazebosim.org/1.0/openrobotics/models/pump/1/files/materials/textures/pump_roughness.png + https://fuel.gazebosim.org/1.0/openrobotics/models/pump/1/files/materials/textures/pump_metallic.png + https://fuel.gazebosim.org/1.0/openrobotics/models/pump/1/files/materials/textures/pump_normal.png + + + + + + + + + + -15 0 4 0 0.0 0 + true + + 0.05 0.05 0.05 0 0 0 + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1.047 + + 2560 + 1920 + + + 0.1 + 100 + + + 1 + 30 + true + camera + + + + + diff --git a/examples/worlds/ground_spacecraft_testbed.sdf b/examples/worlds/ground_spacecraft_testbed.sdf index 9d061f2fdd..abad03dc2b 100644 --- a/examples/worlds/ground_spacecraft_testbed.sdf +++ b/examples/worlds/ground_spacecraft_testbed.sdf @@ -2,9 +2,9 @@ diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index 18037896a7..24a85d0463 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -136,6 +136,14 @@ namespace gz public: sim::Entity CollisionByName(const EntityComponentManager &_ecm, const std::string &_name) const; + /// \brief Get the ID of a sensor entity which is an immediate child of + /// this link. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _name Sensor name. + /// \return Sensor entity. + public: sim::Entity SensorByName(const EntityComponentManager &_ecm, + const std::string &_name) const; + /// \brief Get the ID of a visual entity which is an immediate child of /// this link. /// \param[in] _ecm Entity-component manager. @@ -150,6 +158,12 @@ namespace gz public: std::vector Collisions( const EntityComponentManager &_ecm) const; + /// \brief Get all sensors which are immediate children of this link. + /// \param[in] _ecm Entity-component manager. + /// \return All sensors in this link. + public: std::vector Sensors( + const EntityComponentManager &_ecm) const; + /// \brief Get all visuals which are immediate children of this link. /// \param[in] _ecm Entity-component manager. /// \return All visuals in this link. @@ -162,6 +176,12 @@ namespace gz /// \return Number of collisions in this link. public: uint64_t CollisionCount(const EntityComponentManager &_ecm) const; + /// \brief Get the number of sensors which are immediate children of this + /// link. + /// \param[in] _ecm Entity-component manager. + /// \return Number of sensors in this link. + public: uint64_t SensorCount(const EntityComponentManager &_ecm) const; + /// \brief Get the number of visuals which are immediate children of this /// link. /// \param[in] _ecm Entity-component manager. diff --git a/include/gz/sim/Model.hh b/include/gz/sim/Model.hh index 9de49ea0a1..ddb0e8130d 100644 --- a/include/gz/sim/Model.hh +++ b/include/gz/sim/Model.hh @@ -137,6 +137,14 @@ namespace gz public: sim::Entity LinkByName(const EntityComponentManager &_ecm, const std::string &_name) const; + /// \brief Get the ID of a nested model entity which is an immediate + /// child of this model. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _name Nested model name. + /// \return Nested model entity. + public: sim::Entity ModelByName(const EntityComponentManager &_ecm, + const std::string &_name) const; + /// \brief Get all joints which are immediate children of this model. /// \param[in] _ecm Entity-component manager. /// \return All joints in this model. @@ -167,6 +175,12 @@ namespace gz /// \return Number of links in this model. public: uint64_t LinkCount(const EntityComponentManager &_ecm) const; + /// \brief Get the number of nested models which are immediate children + /// of this model. + /// \param[in] _ecm Entity-component manager. + /// \return Number of nested models in this model. + public: uint64_t ModelCount(const EntityComponentManager &_ecm) const; + /// \brief Set a command to change the model's pose. /// \param[in] _ecm Entity-component manager. /// \param[in] _pose New model pose. diff --git a/include/gz/sim/System.hh b/include/gz/sim/System.hh index f531c1d023..229cb6bae3 100644 --- a/include/gz/sim/System.hh +++ b/include/gz/sim/System.hh @@ -124,6 +124,20 @@ namespace gz EventManager &_eventMgr) = 0; }; + /// \class ISystemConfigure ISystem.hh gz/sim/System.hh + /// \brief Interface for a system that implements optional configuration + /// of the default priority value. + /// + /// ConfigurePriority is called before the system is instantiated to + /// override System::kDefaultPriority. It can still be overridden by the + /// XML priority element. + class ISystemConfigurePriority { + /// \brief Configure the default priority of the system, which can still + /// be overridden by the XML priority element. + /// \return The default priority for the system. + public: virtual System::PriorityType ConfigurePriority() = 0; + }; + /// \class ISystemConfigureParameters ISystem.hh gz/sim/System.hh /// \brief Interface for a system that declares parameters. /// diff --git a/include/gz/sim/components/Model.hh b/include/gz/sim/components/Model.hh index decd6776ed..524581f6c0 100644 --- a/include/gz/sim/components/Model.hh +++ b/include/gz/sim/components/Model.hh @@ -75,10 +75,18 @@ namespace serializers } } - _out << "" - << "" - << (skip ? std::string() : modelElem->ToString("")) - << ""; + if (!skip) + { + _out << "" + << "" + << modelElem->ToString("") + << ""; + + } + else + { + _out << ""; + } return _out; } @@ -89,13 +97,18 @@ namespace serializers public: static std::istream &Deserialize(std::istream &_in, sdf::Model &_model) { - sdf::Root root; std::string sdf(std::istreambuf_iterator(_in), {}); + if (sdf.empty()) + { + return _in; + } + // Its super expensive to create an SDFElement for some reason + sdf::Root root; sdf::Errors errors = root.LoadSdfString(sdf); if (!root.Model()) { - gzwarn << "Unable to deserialize sdf::Model" << std::endl; + gzwarn << "Unable to deserialize sdf::Model " << sdf<< std::endl; return _in; } diff --git a/include/gz/sim/components/WrenchMeasured.hh b/include/gz/sim/components/WrenchMeasured.hh new file mode 100644 index 0000000000..74c350514b --- /dev/null +++ b/include/gz/sim/components/WrenchMeasured.hh @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef GZ_SIM_COMPONENTS_WRENCHMEASURED_HH_ +#define GZ_SIM_COMPONENTS_WRENCHMEASURED_HH_ + +#include + +#include +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { + +namespace components +{ +/// \brief Wrench measured by a ForceTorqueSensor in SI units (Nm for torque, +/// N for force). +/// The wrench is expressed in the Sensor frame and the force component is +/// applied at the sensor origin. +/// \note The term Wrench is used here to mean a pair of 3D vectors representing +/// torque and force quantities expressed in a given frame and where the force +/// is applied at the origin of the frame. This is different from the Wrench +/// used in screw theory. +/// \note The value of force_offset in msgs::Wrench is ignored for this +/// component. The force is assumed to be applied at the origin of the sensor +/// frame. +using WrenchMeasured = + Component; +GZ_SIM_REGISTER_COMPONENT("gz_sim_components.WrenchMeasured", + WrenchMeasured) +} // namespace components +} +} +} + +#endif diff --git a/package.xml b/package.xml index 8be58c01b4..3085d34153 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ gz-sim8 - 8.6.0 + 8.7.0 Gazebo Sim : A Robotic Simulator Michael Carroll Apache License 2.0 diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 3069e98752..550d17e778 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -122,7 +122,7 @@ if (BUILD_TESTING) endif() set(_env_vars) - list(APPEND _env_vars "PYTHONPATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}/python/") + list(APPEND _env_vars "PYTHONPATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}/python/:$ENV{PYTHONPATH}") list(APPEND _env_vars "LD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}:$ENV{LD_LIBRARY_PATH}") set_tests_properties(${test} PROPERTIES ENVIRONMENT "${_env_vars}") diff --git a/python/src/gz/sim/Link.cc b/python/src/gz/sim/Link.cc index 1c8bb8381d..f6c425fcb4 100644 --- a/python/src/gz/sim/Link.cc +++ b/python/src/gz/sim/Link.cc @@ -57,6 +57,11 @@ void defineSimLink(py::object module) py::arg("name"), "Get the ID of a collision entity which is an immediate child of " "this link.") + .def("sensor_by_name", &gz::sim::Link::SensorByName, + py::arg("ecm"), + py::arg("name"), + "Get the ID of a sensor entity which is an immediate child of " + "this link.") .def("visual_by_name", &gz::sim::Link::VisualByName, py::arg("ecm"), py::arg("name"), @@ -65,6 +70,9 @@ void defineSimLink(py::object module) .def("collisions", &gz::sim::Link::Collisions, py::arg("ecm"), "Get all collisions which are immediate children of this link.") + .def("sensors", &gz::sim::Link::Sensors, + py::arg("ecm"), + "Get all sensors which are immediate children of this link.") .def("visuals", &gz::sim::Link::Visuals, py::arg("ecm"), "Get all visuals which are immediate children of this link.") @@ -72,6 +80,10 @@ void defineSimLink(py::object module) py::arg("ecm"), "Get the number of collisions which are immediate children of " "this link.") + .def("sensor_count", &gz::sim::Link::SensorCount, + py::arg("ecm"), + "Get the number of sensors which are immediate children of this " + "link.") .def("visual_count", &gz::sim::Link::VisualCount, py::arg("ecm"), "Get the number of visuals which are immediate children of this " diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py index 4d6be57631..d28c48d9e6 100755 --- a/python/test/link_TEST.py +++ b/python/test/link_TEST.py @@ -56,6 +56,9 @@ def on_pre_udpate_cb(_info, _ecm): # Collisions Test self.assertNotEqual(K_NULL_ENTITY, link.collision_by_name(_ecm, 'collision_test')) self.assertEqual(1, link.collision_count(_ecm)) + # Sensors Test + self.assertNotEqual(K_NULL_ENTITY, link.sensor_by_name(_ecm, 'my_sensor')) + self.assertEqual(1, link.sensor_count(_ecm)) # Visuals Test self.assertNotEqual(K_NULL_ENTITY, link.visual_by_name(_ecm, 'visual_test')) self.assertEqual(1, link.visual_count(_ecm)) diff --git a/python/test/sensor_TEST.py b/python/test/sensor_TEST.py index aba4c61ca9..61068c3fa3 100755 --- a/python/test/sensor_TEST.py +++ b/python/test/sensor_TEST.py @@ -33,11 +33,11 @@ def test_model(self): file_path = os.path.dirname(os.path.realpath(__file__)) fixture = TestFixture(os.path.join(file_path, 'joint_test.sdf')) - def on_post_udpate_cb(_info, _ecm): + def on_post_update_cb(_info, _ecm): self.post_iterations += 1 - def on_pre_udpate_cb(_info, _ecm): - self.pre_iterations += 1 + def on_update_cb(_info, _ecm): + self.iterations += 1 world_e = world_entity(_ecm) self.assertNotEqual(K_NULL_ENTITY, world_e) w = World(world_e) @@ -53,19 +53,19 @@ def on_pre_udpate_cb(_info, _ecm): # Pose Test self.assertEqual(Pose3d(0, 1, 0, 0, 0, 0), sensor.pose(_ecm)) # Topic Test - if self.pre_iterations <= 1: + if self.iterations <= 1: self.assertEqual(None, sensor.topic(_ecm)) else: self.assertEqual('sensor_topic_test', sensor.topic(_ecm)) # Parent Test self.assertEqual(j.entity(), sensor.parent(_ecm)) - def on_udpate_cb(_info, _ecm): - self.iterations += 1 + def on_pre_update_cb(_info, _ecm): + self.pre_iterations += 1 - fixture.on_post_update(on_post_udpate_cb) - fixture.on_update(on_udpate_cb) - fixture.on_pre_update(on_pre_udpate_cb) + fixture.on_post_update(on_post_update_cb) + fixture.on_update(on_update_cb) + fixture.on_pre_update(on_pre_update_cb) fixture.finalize() server = fixture.server() diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 2fd4e03c6e..155e22793e 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -20,6 +20,7 @@ #include #include +#include #include #include #include diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index f7a6c11e7e..603d5752b7 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -15,6 +15,7 @@ * */ +#include #include #include diff --git a/src/LevelManager.cc b/src/LevelManager.cc index a41a56d335..73a81ee5a1 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -18,18 +18,23 @@ #include "LevelManager.hh" #include +#include #include #include +#include #include #include #include +#include #include #include +#include #include #include "gz/sim/Events.hh" +#include "gz/sim/Entity.hh" #include "gz/sim/EntityComponentManager.hh" #include "gz/sim/components/Actor.hh" @@ -838,7 +843,7 @@ void LevelManager::UnloadLevel(const Entity &_entity, } } - if (entityNamesToUnload.size() > 0) + if (!entityNamesToUnload.empty()) { this->UnloadInactiveEntities(entityNamesToUnload); } diff --git a/src/Link.cc b/src/Link.cc index 348923a44a..715e541766 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -37,6 +37,7 @@ #include "gz/sim/components/Name.hh" #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Sensor.hh" #include "gz/sim/components/Visual.hh" #include "gz/sim/components/WindMode.hh" #include "gz/sim/Util.hh" @@ -126,6 +127,16 @@ Entity Link::CollisionByName(const EntityComponentManager &_ecm, components::Collision()); } +////////////////////////////////////////////////// +Entity Link::SensorByName(const EntityComponentManager &_ecm, + const std::string &_name) const +{ + return _ecm.EntityByComponents( + components::ParentEntity(this->dataPtr->id), + components::Name(_name), + components::Sensor()); +} + ////////////////////////////////////////////////// Entity Link::VisualByName(const EntityComponentManager &_ecm, const std::string &_name) const @@ -144,6 +155,14 @@ std::vector Link::Collisions(const EntityComponentManager &_ecm) const components::Collision()); } +////////////////////////////////////////////////// +std::vector Link::Sensors(const EntityComponentManager &_ecm) const +{ + return _ecm.EntitiesByComponents( + components::ParentEntity(this->dataPtr->id), + components::Sensor()); +} + ////////////////////////////////////////////////// std::vector Link::Visuals(const EntityComponentManager &_ecm) const { @@ -158,6 +177,12 @@ uint64_t Link::CollisionCount(const EntityComponentManager &_ecm) const return this->Collisions(_ecm).size(); } +////////////////////////////////////////////////// +uint64_t Link::SensorCount(const EntityComponentManager &_ecm) const +{ + return this->Sensors(_ecm).size(); +} + ////////////////////////////////////////////////// uint64_t Link::VisualCount(const EntityComponentManager &_ecm) const { diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index f5da1459f0..b2653ca2d9 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -17,7 +17,12 @@ #include +#include "gz/sim/EntityComponentManager.hh" #include "gz/sim/Link.hh" +#include "gz/sim/components/Link.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Sensor.hh" ///////////////////////////////////////////////// TEST(LinkTest, Constructor) @@ -74,3 +79,63 @@ TEST(LinkTest, MoveAssignmentOperator) linkMoved = std::move(link); EXPECT_EQ(id, linkMoved.Entity()); } + +///////////////////////////////////////////////// +TEST(LinkTest, Sensors) +{ + // linkA + // - sensorAA + // - sensorAB + // + // linkC + + gz::sim::EntityComponentManager ecm; + + // Link A + auto linkAEntity = ecm.CreateEntity(); + ecm.CreateComponent(linkAEntity, gz::sim::components::Link()); + ecm.CreateComponent(linkAEntity, + gz::sim::components::Name("linkA_name")); + + // Sensor AA - Child of Link A + auto sensorAAEntity = ecm.CreateEntity(); + ecm.CreateComponent(sensorAAEntity, gz::sim::components::Sensor()); + ecm.CreateComponent(sensorAAEntity, + gz::sim::components::Name("sensorAA_name")); + ecm.CreateComponent(sensorAAEntity, + gz::sim::components::ParentEntity(linkAEntity)); + + // Sensor AB - Child of Link A + auto sensorABEntity = ecm.CreateEntity(); + ecm.CreateComponent(sensorABEntity, gz::sim::components::Sensor()); + ecm.CreateComponent(sensorABEntity, + gz::sim::components::Name("sensorAB_name")); + ecm.CreateComponent(sensorABEntity, + gz::sim::components::ParentEntity(linkAEntity)); + + // Link C + auto linkCEntity = ecm.CreateEntity(); + ecm.CreateComponent(linkCEntity, gz::sim::components::Link()); + ecm.CreateComponent(linkCEntity, + gz::sim::components::Name("linkC_name")); + + std::size_t foundSensors = 0; + + gz::sim::Link linkA(linkAEntity); + auto sensors = linkA.Sensors(ecm); + EXPECT_EQ(2u, sensors.size()); + for (const auto &sensor : sensors) + { + if (sensor == sensorAAEntity || sensor == sensorABEntity) + foundSensors++; + } + EXPECT_EQ(foundSensors, sensors.size()); + + EXPECT_EQ(sensorAAEntity, linkA.SensorByName(ecm, "sensorAA_name")); + EXPECT_EQ(sensorABEntity, linkA.SensorByName(ecm, "sensorAB_name")); + EXPECT_EQ(gz::sim::kNullEntity, linkA.SensorByName(ecm, "invalid")); + + gz::sim::Link linkC(linkCEntity); + EXPECT_EQ(0u, linkC.Sensors(ecm).size()); + EXPECT_EQ(gz::sim::kNullEntity, linkC.SensorByName(ecm, "invalid")); +} diff --git a/src/Model.cc b/src/Model.cc index a23779981f..01f4538fb7 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -148,6 +148,16 @@ Entity Model::LinkByName(const EntityComponentManager &_ecm, components::Link()); } +////////////////////////////////////////////////// +Entity Model::ModelByName(const EntityComponentManager &_ecm, + const std::string &_name) const +{ + return _ecm.EntityByComponents( + components::ParentEntity(this->dataPtr->id), + components::Name(_name), + components::Model()); +} + ////////////////////////////////////////////////// std::vector Model::Joints(const EntityComponentManager &_ecm) const { @@ -184,6 +194,12 @@ uint64_t Model::LinkCount(const EntityComponentManager &_ecm) const return this->Links(_ecm).size(); } +////////////////////////////////////////////////// +uint64_t Model::ModelCount(const EntityComponentManager &_ecm) const +{ + return this->Models(_ecm).size(); +} + ////////////////////////////////////////////////// void Model::SetWorldPoseCmd(EntityComponentManager &_ecm, const math::Pose3d &_pose) diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index cd24a61cc3..2486241032 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -15,8 +15,13 @@ * */ +#include + #include #include +#include +#include +#include #include #include "gz/sim/Events.hh" @@ -68,7 +73,6 @@ #include #include "gz/sim/components/Performer.hh" #include "gz/sim/components/Physics.hh" -#include "gz/sim/components/PhysicsEnginePlugin.hh" #include "gz/sim/components/Pose.hh" #include #include "gz/sim/components/RgbdCamera.hh" diff --git a/src/Server.cc b/src/Server.cc index 86b8b7d1cc..f2ce3cdc59 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -33,31 +33,12 @@ #include "gz/sim/Server.hh" #include "gz/sim/Util.hh" -#include "MeshInertiaCalculator.hh" #include "ServerPrivate.hh" #include "SimulationRunner.hh" using namespace gz; using namespace sim; -/// \brief This struct provides access to the default world. -struct DefaultWorld -{ - /// \brief Get the default world as a string. - /// Plugins will be loaded from the server.config file. - /// \return An SDF string that contains the default world. - public: static std::string &World() - { - static std::string world = std::string("" - "" - "") + - "" - ""; - - return world; - } -}; - ///////////////////////////////////////////////// Server::Server(const ServerConfig &_config) : dataPtr(new ServerPrivate) @@ -96,107 +77,8 @@ Server::Server(const ServerConfig &_config) addResourcePaths(); - sdf::Errors errors; - - switch (_config.Source()) - { - // Load a world if specified. Check SDF string first, then SDF file - case ServerConfig::SourceType::kSdfRoot: - { - this->dataPtr->sdfRoot = _config.SdfRoot()->Clone(); - gzmsg << "Loading SDF world from SDF DOM.\n"; - break; - } - - case ServerConfig::SourceType::kSdfString: - { - std::string msg = "Loading SDF string. "; - if (_config.SdfFile().empty()) - { - msg += "File path not available.\n"; - } - else - { - msg += "File path [" + _config.SdfFile() + "].\n"; - } - gzmsg << msg; - sdf::ParserConfig sdfParserConfig = sdf::ParserConfig::GlobalConfig(); - sdfParserConfig.SetStoreResolvedURIs(true); - sdfParserConfig.SetCalculateInertialConfiguration( - sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD); - errors = this->dataPtr->sdfRoot.LoadSdfString( - _config.SdfString(), sdfParserConfig); - this->dataPtr->sdfRoot.ResolveAutoInertials(errors, sdfParserConfig); - break; - } - - case ServerConfig::SourceType::kSdfFile: - { - std::string filePath = resolveSdfWorldFile(_config.SdfFile(), - _config.ResourceCache()); - - if (filePath.empty()) - { - gzerr << "Failed to find world [" << _config.SdfFile() << "]" - << std::endl; - return; - } - - gzmsg << "Loading SDF world file[" << filePath << "].\n"; - - sdf::Root sdfRoot; - sdf::ParserConfig sdfParserConfig = sdf::ParserConfig::GlobalConfig(); - sdfParserConfig.SetStoreResolvedURIs(true); - sdfParserConfig.SetCalculateInertialConfiguration( - sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD); - - MeshInertiaCalculator meshInertiaCalculator; - sdfParserConfig.RegisterCustomInertiaCalc(meshInertiaCalculator); - // \todo(nkoenig) Async resource download. - // This call can block for a long period of time while - // resources are downloaded. Blocking here causes the GUI to block with - // a black screen (search for "Async resource download" in - // 'src/gui_main.cc'. - errors = sdfRoot.Load(filePath, sdfParserConfig); - if (errors.empty() || _config.BehaviorOnSdfErrors() != - ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY) - { - if (sdfRoot.Model() == nullptr) { - this->dataPtr->sdfRoot = std::move(sdfRoot); - } - else - { - // If the specified file only contains a model, load the default - // world and add the model to it. - errors = this->dataPtr->sdfRoot.LoadSdfString( - DefaultWorld::World(), sdfParserConfig); - sdf::World *world = this->dataPtr->sdfRoot.WorldByIndex(0); - if (world == nullptr) { - return; - } - world->AddModel(*sdfRoot.Model()); - if (errors.empty() || _config.BehaviorOnSdfErrors() != - ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY) - { - errors = this->dataPtr->sdfRoot.UpdateGraphs(); - } - } - } - - this->dataPtr->sdfRoot.ResolveAutoInertials(errors, sdfParserConfig); - break; - } - - case ServerConfig::SourceType::kNone: - default: - { - gzmsg << "Loading default world.\n"; - // Load an empty world. - /// \todo(nkoenig) Add a "AddWorld" function to sdf::Root. - errors = this->dataPtr->sdfRoot.LoadSdfString(DefaultWorld::World()); - break; - } - } + // Loads the SDF root object based on values in a ServerConfig object. + sdf::Errors errors = this->dataPtr->LoadSdfRootHelper(_config); if (!errors.empty()) { diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 1d23f81586..f93d4bbab8 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -14,10 +14,14 @@ * limitations under the License. * */ +#include "MeshInertiaCalculator.hh" #include "ServerPrivate.hh" #include +#include +#include + #include #include #include @@ -574,3 +578,120 @@ std::string ServerPrivate::FetchResourceUri(const common::URI &_uri) { return this->FetchResource(_uri.Str()); } + +////////////////////////////////////////////////// +sdf::Errors ServerPrivate::LoadSdfRootHelper(const ServerConfig &_config) +{ + sdf::Errors errors; + + switch (_config.Source()) + { + // Load a world if specified. Check SDF string first, then SDF file + case ServerConfig::SourceType::kSdfRoot: + { + this->sdfRoot = _config.SdfRoot()->Clone(); + gzmsg << "Loading SDF world from SDF DOM.\n"; + break; + } + + case ServerConfig::SourceType::kSdfString: + { + std::string msg = "Loading SDF string. "; + if (_config.SdfFile().empty()) + { + msg += "File path not available.\n"; + } + else + { + msg += "File path [" + _config.SdfFile() + "].\n"; + } + gzmsg << msg; + sdf::ParserConfig sdfParserConfig = sdf::ParserConfig::GlobalConfig(); + sdfParserConfig.SetStoreResolvedURIs(true); + sdfParserConfig.SetCalculateInertialConfiguration( + sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD); + errors = this->sdfRoot.LoadSdfString( + _config.SdfString(), sdfParserConfig); + this->sdfRoot.ResolveAutoInertials(errors, sdfParserConfig); + break; + } + + case ServerConfig::SourceType::kSdfFile: + { + std::string filePath = resolveSdfWorldFile(_config.SdfFile(), + _config.ResourceCache()); + + if (filePath.empty()) + { + std::string errStr = "Failed to find world [" + + _config.SdfFile() + "]"; + gzerr << errStr << std::endl; + errors.push_back({sdf::ErrorCode::FILE_READ, errStr}); + return errors; + } + + gzmsg << "Loading SDF world file[" << filePath << "].\n"; + + sdf::Root sdfRootLocal; + sdf::ParserConfig sdfParserConfig = sdf::ParserConfig::GlobalConfig(); + sdfParserConfig.SetStoreResolvedURIs(true); + sdfParserConfig.SetCalculateInertialConfiguration( + sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD); + + MeshInertiaCalculator meshInertiaCalculator; + sdfParserConfig.RegisterCustomInertiaCalc(meshInertiaCalculator); + // \todo(nkoenig) Async resource download. + // This call can block for a long period of time while + // resources are downloaded. Blocking here causes the GUI to block with + // a black screen (search for "Async resource download" in + // 'src/gui_main.cc'. + errors = sdfRootLocal.Load(filePath, sdfParserConfig); + if (errors.empty() || _config.BehaviorOnSdfErrors() != + ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY) + { + if (sdfRootLocal.Model() == nullptr) { + this->sdfRoot = std::move(sdfRootLocal); + } + else + { + sdf::World defaultWorld; + defaultWorld.SetName("default"); + + // If the specified file only contains a model, load the default + // world and add the model to it. + errors = this->sdfRoot.AddWorld(defaultWorld); + sdf::World *world = this->sdfRoot.WorldByIndex(0); + if (world == nullptr) { + errors.push_back({sdf::ErrorCode::FATAL_ERROR, + "sdf::World pointer is null"}); + return errors; + } + world->AddModel(*sdfRootLocal.Model()); + if (errors.empty() || _config.BehaviorOnSdfErrors() != + ServerConfig::SdfErrorBehavior::EXIT_IMMEDIATELY) + { + errors = this->sdfRoot.UpdateGraphs(); + } + } + } + + this->sdfRoot.ResolveAutoInertials(errors, sdfParserConfig); + break; + } + + case ServerConfig::SourceType::kNone: + default: + { + gzmsg << "Loading default world.\n"; + + sdf::World defaultWorld; + defaultWorld.SetName("default"); + + // Load an empty world. + errors = this->sdfRoot.AddWorld(defaultWorld); + break; + } + } + + return errors; +} diff --git a/src/ServerPrivate.hh b/src/ServerPrivate.hh index cbb298db3f..4b81144feb 100644 --- a/src/ServerPrivate.hh +++ b/src/ServerPrivate.hh @@ -99,6 +99,12 @@ namespace gz /// \return Path to the downloaded resource, empty on error. public: std::string FetchResourceUri(const common::URI &_uri); + /// \brief Helper function that loads an SDF root object based on + /// values in a ServerConfig object. + /// \param[in] _config Server config to read from. + /// \return Set of SDF errors. + public: sdf::Errors LoadSdfRootHelper(const ServerConfig &_config); + /// \brief Signal handler callback /// \param[in] _sig The signal number private: void OnSignal(int _sig); diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index ceea22ad49..5ce443ff19 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -18,6 +18,8 @@ #include "SimulationRunner.hh" #include +#include +#include #ifdef HAVE_PYBIND11 #include #endif @@ -32,6 +34,7 @@ #include #include +#include #include #include @@ -49,11 +52,14 @@ #include "gz/sim/components/RenderEngineGuiPlugin.hh" #include "gz/sim/components/RenderEngineServerHeadless.hh" #include "gz/sim/components/RenderEngineServerPlugin.hh" +#include "gz/sim/Conversions.hh" #include "gz/sim/Events.hh" +#include "gz/sim/ServerConfig.hh" #include "gz/sim/SdfEntityCreator.hh" #include "gz/sim/Util.hh" #include "gz/transport/TopicUtils.hh" #include "network/NetworkManagerPrimary.hh" +#include "LevelManager.hh" #include "SdfGenerator.hh" using namespace gz; diff --git a/src/SystemInternal.hh b/src/SystemInternal.hh index 0f4d442e4d..17b32b6758 100644 --- a/src/SystemInternal.hh +++ b/src/SystemInternal.hh @@ -48,6 +48,8 @@ namespace gz configure(systemPlugin->QueryInterface()), configureParameters( systemPlugin->QueryInterface()), + configurePriority( + systemPlugin->QueryInterface()), reset(systemPlugin->QueryInterface()), preupdate(systemPlugin->QueryInterface()), update(systemPlugin->QueryInterface()), @@ -66,6 +68,8 @@ namespace gz configure(dynamic_cast(_system.get())), configureParameters( dynamic_cast(_system.get())), + configurePriority( + dynamic_cast(_system.get())), reset(dynamic_cast(_system.get())), preupdate(dynamic_cast(_system.get())), update(dynamic_cast(_system.get())), @@ -95,6 +99,11 @@ namespace gz /// Will be nullptr if the System doesn't implement this interface. public: ISystemConfigureParameters *configureParameters = nullptr; + /// \brief Access this system via the ISystemConfigurePriority + /// interface. + /// Will be nullptr if the System doesn't implement this interface. + public: ISystemConfigurePriority *configurePriority = nullptr; + /// \brief Access this system via the ISystemReset interface /// Will be nullptr if the System doesn't implement this interface. public: ISystemReset *reset = nullptr; diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 9d86af1241..dcb24902c1 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -113,6 +113,10 @@ size_t SystemManager::ActivatePendingSystems() this->systems.push_back(system); PriorityType p {System::kDefaultPriority}; + if (system.configurePriority) + { + p = system.configurePriority->ConfigurePriority(); + } const std::string kPriorityElementName {gz::sim::System::kPriorityElementName}; if (system.configureSdf && @@ -120,6 +124,9 @@ size_t SystemManager::ActivatePendingSystems() { PriorityType newPriority = system.configureSdf->Get(kPriorityElementName); + gzdbg << "Changing priority for system [" << system.name + << "] from {" << p + << "} to {" << newPriority << "}\n"; p = newPriority; } @@ -307,6 +314,7 @@ const std::vector& SystemManager::SystemsConfigure() return this->systemsConfigure; } +////////////////////////////////////////////////// const std::vector& SystemManager::SystemsConfigureParameters() { diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc index 38ad82d726..956cb06810 100644 --- a/src/SystemManager_TEST.cc +++ b/src/SystemManager_TEST.cc @@ -28,6 +28,8 @@ using namespace gz::sim; +constexpr System::PriorityType kPriority = 64; + ///////////////////////////////////////////////// class SystemWithConfigure: public System, @@ -69,6 +71,16 @@ class SystemWithUpdates: const EntityComponentManager &) override {}; }; +///////////////////////////////////////////////// +class SystemWithPrioritizedUpdates: + public SystemWithUpdates, + public ISystemConfigurePriority +{ + // Documentation inherited + public: System::PriorityType ConfigurePriority() override + { return kPriority; } +}; + ///////////////////////////////////////////////// TEST(SystemManager, Constructor) { @@ -127,32 +139,40 @@ TEST(SystemManager, AddSystemNoEcm) EXPECT_EQ(1u, systemMgr.TotalByEntity(configEntity).size()); auto updateSystem = std::make_shared(); + auto prioritizedSystem = + std::make_shared(); Entity updateEntity{456u}; systemMgr.AddSystem(updateSystem, updateEntity, nullptr); + systemMgr.AddSystem(prioritizedSystem, updateEntity, nullptr); EXPECT_EQ(1u, systemMgr.ActiveCount()); - EXPECT_EQ(1u, systemMgr.PendingCount()); - EXPECT_EQ(2u, systemMgr.TotalCount()); + EXPECT_EQ(2u, systemMgr.PendingCount()); + EXPECT_EQ(3u, systemMgr.TotalCount()); EXPECT_EQ(1u, systemMgr.SystemsConfigure().size()); EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size()); EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); - EXPECT_EQ(1u, systemMgr.TotalByEntity(updateEntity).size()); + EXPECT_EQ(2u, systemMgr.TotalByEntity(updateEntity).size()); systemMgr.ActivatePendingSystems(); - EXPECT_EQ(2u, systemMgr.ActiveCount()); + EXPECT_EQ(3u, systemMgr.ActiveCount()); EXPECT_EQ(0u, systemMgr.PendingCount()); - EXPECT_EQ(2u, systemMgr.TotalCount()); + EXPECT_EQ(3u, systemMgr.TotalCount()); EXPECT_EQ(1u, systemMgr.SystemsConfigure().size()); - // Expect PreUpdate and Update to contain one map entry with Priority 0 and - // a vector of length 1. - EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().size()); + // Expect PreUpdate and Update to contain two map entries: + // 1. Priority {0} and a vector of length 1. + // 2. Priority {kPriority} and a vector of length 1. + EXPECT_EQ(2u, systemMgr.SystemsPreUpdate().size()); EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().count(0)); + EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().count(kPriority)); EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().at(0).size()); - EXPECT_EQ(1u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().at(kPriority).size()); + EXPECT_EQ(2u, systemMgr.SystemsUpdate().size()); EXPECT_EQ(1u, systemMgr.SystemsUpdate().count(0)); + EXPECT_EQ(1u, systemMgr.SystemsUpdate().count(kPriority)); EXPECT_EQ(1u, systemMgr.SystemsUpdate().at(0).size()); - EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size()); - EXPECT_EQ(1u, systemMgr.TotalByEntity(updateEntity).size()); + EXPECT_EQ(1u, systemMgr.SystemsUpdate().at(kPriority).size()); + EXPECT_EQ(2u, systemMgr.SystemsPostUpdate().size()); + EXPECT_EQ(2u, systemMgr.TotalByEntity(updateEntity).size()); } ///////////////////////////////////////////////// @@ -201,23 +221,37 @@ TEST(SystemManager, AddSystemEcm) EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); auto updateSystem = std::make_shared(); + auto prioritizedSystem = + std::make_shared(); systemMgr.AddSystem(updateSystem, kNullEntity, nullptr); + systemMgr.AddSystem(prioritizedSystem, kNullEntity, nullptr); EXPECT_EQ(1u, systemMgr.ActiveCount()); - EXPECT_EQ(1u, systemMgr.PendingCount()); - EXPECT_EQ(2u, systemMgr.TotalCount()); + EXPECT_EQ(2u, systemMgr.PendingCount()); + EXPECT_EQ(3u, systemMgr.TotalCount()); EXPECT_EQ(1u, systemMgr.SystemsConfigure().size()); EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size()); EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); systemMgr.ActivatePendingSystems(); - EXPECT_EQ(2u, systemMgr.ActiveCount()); + EXPECT_EQ(3u, systemMgr.ActiveCount()); EXPECT_EQ(0u, systemMgr.PendingCount()); - EXPECT_EQ(2u, systemMgr.TotalCount()); + EXPECT_EQ(3u, systemMgr.TotalCount()); EXPECT_EQ(1u, systemMgr.SystemsConfigure().size()); - EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().size()); - EXPECT_EQ(1u, systemMgr.SystemsUpdate().size()); - EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size()); + // Expect PreUpdate and Update to contain two map entries: + // 1. Priority {0} and a vector of length 1. + // 2. Priority {kPriority} and a vector of length 1. + EXPECT_EQ(2u, systemMgr.SystemsPreUpdate().size()); + EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().count(0)); + EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().count(kPriority)); + EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().at(0).size()); + EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().at(kPriority).size()); + EXPECT_EQ(2u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(1u, systemMgr.SystemsUpdate().count(0)); + EXPECT_EQ(1u, systemMgr.SystemsUpdate().count(kPriority)); + EXPECT_EQ(1u, systemMgr.SystemsUpdate().at(0).size()); + EXPECT_EQ(1u, systemMgr.SystemsUpdate().at(kPriority).size()); + EXPECT_EQ(2u, systemMgr.SystemsPostUpdate().size()); } ///////////////////////////////////////////////// @@ -247,28 +281,42 @@ TEST(SystemManager, AddAndRemoveSystemEcm) auto entity = ecm.CreateEntity(); auto updateSystemWithChild = std::make_shared(); + auto prioritizedSystemWithChild = + std::make_shared(); systemMgr.AddSystem(updateSystemWithChild, entity, nullptr); + systemMgr.AddSystem(prioritizedSystemWithChild, entity, nullptr); // Configure called during AddSystem EXPECT_EQ(1, configSystem->configured); EXPECT_EQ(1, configSystem->configuredParameters); EXPECT_EQ(0u, systemMgr.ActiveCount()); - EXPECT_EQ(2u, systemMgr.PendingCount()); - EXPECT_EQ(2u, systemMgr.TotalCount()); + EXPECT_EQ(3u, systemMgr.PendingCount()); + EXPECT_EQ(3u, systemMgr.TotalCount()); EXPECT_EQ(0u, systemMgr.SystemsConfigure().size()); EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size()); EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); systemMgr.ActivatePendingSystems(); - EXPECT_EQ(2u, systemMgr.ActiveCount()); + EXPECT_EQ(3u, systemMgr.ActiveCount()); EXPECT_EQ(0u, systemMgr.PendingCount()); - EXPECT_EQ(2u, systemMgr.TotalCount()); + EXPECT_EQ(3u, systemMgr.TotalCount()); EXPECT_EQ(1u, systemMgr.SystemsConfigure().size()); - EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().size()); - EXPECT_EQ(1u, systemMgr.SystemsUpdate().size()); - EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size()); + // Expect PreUpdate and Update to contain two map entries: + // 1. Priority {0} and a vector of length 1. + // 2. Priority {kPriority} and a vector of length 1. + EXPECT_EQ(2u, systemMgr.SystemsPreUpdate().size()); + EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().count(0)); + EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().count(kPriority)); + EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().at(0).size()); + EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().at(kPriority).size()); + EXPECT_EQ(2u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(1u, systemMgr.SystemsUpdate().count(0)); + EXPECT_EQ(1u, systemMgr.SystemsUpdate().count(kPriority)); + EXPECT_EQ(1u, systemMgr.SystemsUpdate().at(0).size()); + EXPECT_EQ(1u, systemMgr.SystemsUpdate().at(kPriority).size()); + EXPECT_EQ(2u, systemMgr.SystemsPostUpdate().size()); // Remove the entity ecm.RequestRemoveEntity(entity); diff --git a/src/gui/gui.config b/src/gui/gui.config index 4ea3b1be53..98214b4e33 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -171,7 +171,7 @@ false 0 0 - 250 + 300 50 floating false @@ -183,7 +183,7 @@ false - 250 + 300 0 150 50 diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc index 0f84e1423a..d4adcbd9f7 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.cc +++ b/src/systems/battery_plugin/LinearBatteryPlugin.cc @@ -178,6 +178,9 @@ class gz::sim::systems::LinearBatteryPluginPrivate /// \brief Initial power load set trough config public: double initialPowerLoad = 0.0; + + /// \brief Flag to invert the current sign + public: bool invertCurrentSign{false}; }; ///////////////////////////////////////////////// @@ -273,6 +276,10 @@ void LinearBatteryPlugin::Configure(const Entity &_entity, if (_sdf->HasElement("fix_issue_225")) this->dataPtr->fixIssue225 = _sdf->Get("fix_issue_225"); + if (_sdf->HasElement("invert_current_sign")) + this->dataPtr->invertCurrentSign = + _sdf->Get("invert_current_sign"); + if (_sdf->HasElement("battery_name") && _sdf->HasElement("voltage")) { this->dataPtr->batteryName = _sdf->Get("battery_name"); @@ -624,7 +631,10 @@ void LinearBatteryPlugin::PostUpdate(const UpdateInfo &_info, msg.mutable_header()->mutable_stamp()->CopyFrom( convert(_info.simTime)); msg.set_voltage(this->dataPtr->battery->Voltage()); - msg.set_current(this->dataPtr->ismooth); + if (this->dataPtr->invertCurrentSign) + msg.set_current(-this->dataPtr->ismooth); + else + msg.set_current(this->dataPtr->ismooth); msg.set_charge(this->dataPtr->q); msg.set_capacity(this->dataPtr->c); diff --git a/src/systems/elevator/Elevator.cc b/src/systems/elevator/Elevator.cc index 53c172dcef..12fd5287f4 100644 --- a/src/systems/elevator/Elevator.cc +++ b/src/systems/elevator/Elevator.cc @@ -421,6 +421,9 @@ void ElevatorPrivate::UpdateState(const gz::sim::UpdateInfo &_info, void ElevatorPrivate::OnLidarMsg(size_t _floorLevel, const msgs::LaserScan &_msg) { + if (_msg.ranges_size() <= 0) + return; + bool isDoorwayBlocked = _msg.ranges(0) < _msg.range_max() - 0.005; if (isDoorwayBlocked == this->isDoorwayBlockedStates[_floorLevel]) return; std::lock_guard lock(this->mutex); diff --git a/src/systems/force_torque/ForceTorque.cc b/src/systems/force_torque/ForceTorque.cc index 8792f514d5..4fa376433d 100644 --- a/src/systems/force_torque/ForceTorque.cc +++ b/src/systems/force_torque/ForceTorque.cc @@ -45,6 +45,7 @@ #include "gz/sim/components/Sensor.hh" #include "gz/sim/components/World.hh" #include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/System.hh" #include "gz/sim/Util.hh" using namespace gz; diff --git a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc index 2bfe779776..a3cda4b082 100644 --- a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc +++ b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc @@ -411,7 +411,8 @@ void MulticopterMotorModel::PreUpdate(const UpdateInfo &_info, const auto parentLinkName = _ecm.Component( this->dataPtr->jointEntity); - this->dataPtr->parentLinkName = parentLinkName->Data(); + if (parentLinkName) + this->dataPtr->parentLinkName = parentLinkName->Data(); } if (this->dataPtr->linkEntity == kNullEntity) diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index fe5270ebe2..bc1e234d3e 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -55,6 +55,15 @@ class gz::sim::systems::OdometryPublisherPrivate public: void UpdateOdometry(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm); + /// \brief Calculates angular velocity in body frame from world frame poses. + /// \param[in] _lastPose Pose at last timestep in world frame. + /// \param[in] _currentPose Pose at current timestep in world frame. + /// \param[in] _dt Time elapsed from last to current timestep. + /// \return Angular velocity computed in body frame at current timestep. + public: static math::Vector3d CalculateAngularVelocity( + const math::Pose3d &_lastPose, const math::Pose3d &_currentPose, + std::chrono::duration _dt); + /// \brief Gazebo communication node. public: transport::Node node; @@ -119,18 +128,12 @@ OdometryPublisher::OdometryPublisher() std::get<0>(this->dataPtr->linearMean).SetWindowSize(10); std::get<1>(this->dataPtr->linearMean).SetWindowSize(10); std::get<2>(this->dataPtr->angularMean).SetWindowSize(10); - std::get<0>(this->dataPtr->linearMean).Clear(); - std::get<1>(this->dataPtr->linearMean).Clear(); - std::get<2>(this->dataPtr->angularMean).Clear(); if (this->dataPtr->dimensions == 3) { std::get<2>(this->dataPtr->linearMean).SetWindowSize(10); std::get<0>(this->dataPtr->angularMean).SetWindowSize(10); std::get<1>(this->dataPtr->angularMean).SetWindowSize(10); - std::get<2>(this->dataPtr->linearMean).Clear(); - std::get<0>(this->dataPtr->angularMean).Clear(); - std::get<1>(this->dataPtr->angularMean).Clear(); } } @@ -329,6 +332,26 @@ void OdometryPublisher::PostUpdate(const UpdateInfo &_info, this->dataPtr->UpdateOdometry(_info, _ecm); } +////////////////////////////////////////////////// +math::Vector3d OdometryPublisherPrivate::CalculateAngularVelocity( + const math::Pose3d &_lastPose, const math::Pose3d &_currentPose, + std::chrono::duration _dt) +{ + // Compute the first order finite difference between current and previous + // rotation as quaternion. + const math::Quaterniond rotationDiff = + _currentPose.Rot() * _lastPose.Rot().Inverse(); + + math::Vector3d rotationAxis; + double rotationAngle; + rotationDiff.AxisAngle(rotationAxis, rotationAngle); + + const math::Vector3d angularVelocity = + (rotationAngle / _dt.count()) * rotationAxis; + + return _currentPose.Rot().RotateVectorReverse(angularVelocity); +} + ////////////////////////////////////////////////// void OdometryPublisherPrivate::UpdateOdometry( const gz::sim::UpdateInfo &_info, @@ -375,10 +398,8 @@ void OdometryPublisherPrivate::UpdateOdometry( double linearDisplacementY = pose.Pos().Y() - this->lastUpdatePose.Pos().Y(); double currentYaw = pose.Rot().Yaw(); - const double lastYaw = this->lastUpdatePose.Rot().Yaw(); - while (currentYaw < lastYaw - GZ_PI) currentYaw += 2 * GZ_PI; - while (currentYaw > lastYaw + GZ_PI) currentYaw -= 2 * GZ_PI; - const float yawDiff = currentYaw - lastYaw; + const math::Vector3d angularVelocityBody = CalculateAngularVelocity( + this->lastUpdatePose, pose, dt); // Get velocities assuming 2D if (this->dimensions == 2) @@ -406,18 +427,6 @@ void OdometryPublisherPrivate::UpdateOdometry( // Get velocities and roll/pitch rates assuming 3D else if (this->dimensions == 3) { - double currentRoll = pose.Rot().Roll(); - const double lastRoll = this->lastUpdatePose.Rot().Roll(); - while (currentRoll < lastRoll - GZ_PI) currentRoll += 2 * GZ_PI; - while (currentRoll > lastRoll + GZ_PI) currentRoll -= 2 * GZ_PI; - const float rollDiff = currentRoll - lastRoll; - - double currentPitch = pose.Rot().Pitch(); - const double lastPitch = this->lastUpdatePose.Rot().Pitch(); - while (currentPitch < lastPitch - GZ_PI) currentPitch += 2 * GZ_PI; - while (currentPitch > lastPitch + GZ_PI) currentPitch -= 2 * GZ_PI; - const float pitchDiff = currentPitch - lastPitch; - double linearDisplacementZ = pose.Pos().Z() - this->lastUpdatePose.Pos().Z(); math::Vector3 linearDisplacement(linearDisplacementX, linearDisplacementY, @@ -427,8 +436,8 @@ void OdometryPublisherPrivate::UpdateOdometry( std::get<0>(this->linearMean).Push(linearVelocity.X()); std::get<1>(this->linearMean).Push(linearVelocity.Y()); std::get<2>(this->linearMean).Push(linearVelocity.Z()); - std::get<0>(this->angularMean).Push(rollDiff / dt.count()); - std::get<1>(this->angularMean).Push(pitchDiff / dt.count()); + std::get<0>(this->angularMean).Push(angularVelocityBody.X()); + std::get<1>(this->angularMean).Push(angularVelocityBody.Y()); msg.mutable_twist()->mutable_linear()->set_x( std::get<0>(this->linearMean).Mean() + gz::math::Rand::DblNormal(0, this->gaussianNoise)); @@ -447,7 +456,7 @@ void OdometryPublisherPrivate::UpdateOdometry( } // Set yaw rate - std::get<2>(this->angularMean).Push(yawDiff / dt.count()); + std::get<2>(this->angularMean).Push(angularVelocityBody.Z()); msg.mutable_twist()->mutable_angular()->set_z( std::get<2>(this->angularMean).Mean() + gz::math::Rand::DblNormal(0, this->gaussianNoise)); diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index f1d6d618fa..f374e04218 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -630,7 +630,7 @@ void OpticalTactilePluginPrivate::Enable(const gz::msgs::Boolean &_req) this->enabled = _req.data(); - if (!_req.data()) + if (!_req.data() && this->visualizePtr) { this->visualizePtr->RemoveNormalForcesAndContactsMarkers(); } @@ -753,6 +753,15 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( if (!this->initialized) return; + // sanity check to make sure point cloud data size matches other fields + if (_msg.data().size() != _msg.row_step() * _msg.height()) + { + gzerr << "Invalid point cloud message. " + << "Point cloud data size != row_step * height." + << std::endl; + return; + } + // Get data from the message const char *msgBuffer = _msg.data().data(); diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 28f78e9032..682d71554a 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -93,6 +93,7 @@ #include "gz/sim/EntityComponentManager.hh" #include "gz/sim/Model.hh" +#include "gz/sim/System.hh" #include "gz/sim/Util.hh" // Components @@ -432,7 +433,7 @@ class gz::sim::systems::PhysicsPrivate } return true; }}; - /// \brief msgs::Contacts equality comparison function. + /// \brief msgs::Wrench equality comparison function. public: std::function wrenchEql{ [](const msgs::Wrench &_a, const msgs::Wrench &_b) @@ -2403,6 +2404,14 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) << std::endl; return true; } + math::Pose3d worldPoseCmd = _poseCmd->Data(); + if (!worldPoseCmd.Pos().IsFinite() || !worldPoseCmd.Rot().IsFinite() || + worldPoseCmd.Rot() == math::Quaterniond::Zero) + { + gzerr << "Unable to set world pose. Invalid pose value: " + << worldPoseCmd << std::endl; + return true; + } // TODO(addisu) Store the free group instead of searching for it at // every iteration @@ -2422,7 +2431,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) math::Pose3d linkPose = this->RelativePose(_entity, linkEntity, _ecm); - freeGroup->SetWorldPose(math::eigen3::convert(_poseCmd->Data() * + freeGroup->SetWorldPose(math::eigen3::convert(worldPoseCmd * linkPose)); // Process pose commands for static models here, as one-time changes @@ -2431,7 +2440,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) auto worldPoseComp = _ecm.Component(_entity); if (worldPoseComp) { - auto state = worldPoseComp->SetData(_poseCmd->Data(), + auto state = worldPoseComp->SetData(worldPoseCmd, this->pose3Eql) ? ComponentState::OneTimeChange : ComponentState::NoChange; diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 601a4b3c9d..defd28c3cf 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -64,11 +64,71 @@ #include "gz/sim/rendering/Events.hh" #include "gz/sim/rendering/RenderUtil.hh" +#include "gz/rendering/GlobalIlluminationVct.hh" using namespace gz; using namespace sim; using namespace systems; +/// \brief GI parameters holding default data +struct GiDefaultData +{ + /// \brief See rendering::GlobalIlluminationVct::SetResolution + math::Vector3d resolution{16, 16, 16}; + + /// \brief See rendering::GlobalIlluminationVct::SetOctantCount + math::Vector3d octantCount{1, 1, 1}; + + /// \brief See rendering::GlobalIlluminationVct::SetBounceCount + uint32_t bounceCount = 6; + + /// \brief See rendering::GlobalIlluminationVct::SetHighQuality + bool highQuality = true; + + /// \brief See rendering::GlobalIlluminationVct::SetAnisotropic + bool anisotropic = true; + + /// \brief See rendering::GlobalIlluminationVct::SetThinWallCounter + float thinWallCounter = 1.0f; + + /// \brief See rendering::GlobalIlluminationVct::SetConserveMemory + bool conserveMemory = false; + + /// \brief See rendering::GlobalIlluminationVct::DebugVisualizationMode + uint32_t debugVisMode = rendering::GlobalIlluminationVct::DVM_None; +}; + +/// \brief GI VCT flag and parameters +struct GiVctParameters +{ + /// \brief VCT enabled flag + bool enabled = false; + + /// \brief See rendering::GlobalIlluminationVct::SetResolution + uint32_t resolution[3]; + + /// \brief See rendering::GlobalIlluminationVct::SetOctantCount + uint32_t octantCount[3]; + + /// \brief See rendering::GlobalIlluminationVct::SetBounceCount + uint32_t bounceCount; + + /// \brief See rendering::GlobalIlluminationVct::SetHighQuality + bool highQuality; + + /// \brief See rendering::GlobalIlluminationVct::SetAnisotropic + bool anisotropic; + + /// \brief See rendering::GlobalIlluminationVct::SetThinWallCounter + float thinWallCounter; + + /// \brief See rendering::GlobalIlluminationVct::SetConserveMemory + bool conserveMemory; + + /// \brief See rendering::GlobalIlluminationVct::DebugVisualizationMode + uint32_t debugVisMode; +}; + // Private data class. class gz::sim::systems::SensorsPrivate { @@ -89,6 +149,18 @@ class gz::sim::systems::SensorsPrivate /// generate sensor data public: rendering::ScenePtr scene; + /// \brief Pointer to GlobalIlluminationVct + public: rendering::GlobalIlluminationVctPtr giVct; + + /// \brief GI VCT parameters passed to giVct + public: GiVctParameters giVctParameters; + + /// \brief Default GI data + public: GiDefaultData giDefaultData; + + /// \brief GI built flag + public: bool giBuilt = false; + /// \brief Temperature used by thermal camera. Defaults to temperature at /// sea level public: double ambientTemperature = 288.15; @@ -288,6 +360,28 @@ void SensorsPrivate::WaitForInit() #endif this->scene = this->renderUtil.Scene(); this->scene->SetCameraPassCountPerGpuFlush(6u); + + if (this->giVctParameters.enabled) + { + this->giVct = this->scene->CreateGlobalIlluminationVct(); + this->giVct->SetParticipatingVisuals( + rendering::GlobalIlluminationBase::DYNAMIC_VISUALS | + rendering::GlobalIlluminationBase::STATIC_VISUALS); + + this->giVct->SetResolution(this->giVctParameters.resolution); + this->giVct->SetOctantCount(this->giVctParameters.octantCount); + this->giVct->SetBounceCount(this->giVctParameters.bounceCount); + this->giVct->SetAnisotropic(this->giVctParameters.anisotropic); + this->giVct->SetHighQuality(this->giVctParameters.highQuality); + this->giVct->SetConserveMemory(this->giVctParameters.conserveMemory); + this->giVct->SetThinWallCounter(this->giVctParameters.thinWallCounter); + + this->giVct->SetDebugVisualization( + rendering::GlobalIlluminationVct::DVM_None); + + this->scene->SetActiveGlobalIllumination(this->giVct); + } + this->initialized = true; } @@ -359,6 +453,19 @@ void SensorsPrivate::RunOnce() // We only need to do this once per frame It is important to call // sensors::RenderingSensor::SetManualSceneUpdate and set it to true // so we don't waste cycles doing one scene graph update per sensor + + if (!this->giBuilt) + { + if (this->giVctParameters.enabled) + { + this->giVct->Build(); + this->giVct->SetDebugVisualization(static_cast< + rendering::GlobalIlluminationVct::DebugVisualizationMode> + (this->giVctParameters.debugVisMode)); + this->giBuilt = true; + } + } + this->scene->PreRender(); } @@ -438,6 +545,7 @@ void SensorsPrivate::RenderThread() for (const auto id : this->sensorIds) this->sensorManager.Remove(id); + this->giVct.reset(); this->scene.reset(); this->renderUtil.Destroy(); gzdbg << "SensorsPrivate::RenderThread stopped" << std::endl; @@ -521,6 +629,65 @@ Sensors::~Sensors() this->dataPtr->Stop(); } +/// \brief Helper to convert math::Vector3d to uint32_t array +/// \param[in] _valueToSet Array values to set +/// \param[in] _vecValues Vector values to convert +static void convertVector3dToUInt32Array(uint32_t _valueToSet[3], + const math::Vector3d &_vecValues) +{ + _valueToSet[0] = static_cast(_vecValues[0]); + _valueToSet[1] = static_cast(_vecValues[1]); + _valueToSet[2] = static_cast(_vecValues[2]); +} + +/// \brief Helper to parse math::Vector3d as uint32_t array +/// \param[in] _parentElem Parent element to look through +/// \param[in] _childName Child element name to look for +/// \param[in] _valueToSet Array values to set +/// \param[in] _defaultValue Default vector values to use +static void parseVector3dAsUInt32Array(sdf::ElementConstPtr _parentElem, + const char *_childName, uint32_t _valueToSet[3], + const math::Vector3d &_defaultValue) +{ + math::Vector3d parsedValues = (_parentElem == nullptr) ? _defaultValue : + _parentElem->Get(_childName, _defaultValue).first; + + convertVector3dToUInt32Array(_valueToSet, parsedValues); +} + +/// \brief Helper to set debug visualization mode (DVM) +/// \param[in] _text String text to parse +/// \param[in] _modeToSet DVM to set +/// \param[in] _defaultMode Default DVM to use +static void SetDebugVisMode(const std::string &_text, + uint32_t &_modeToSet, uint32_t _defaultMode) +{ + if (_text == "albedo") + { + _modeToSet = rendering::GlobalIlluminationVct::DVM_Albedo; + } + else if (_text == "normal") + { + _modeToSet = rendering::GlobalIlluminationVct::DVM_Normal; + } + else if (_text == "emissive") + { + _modeToSet = rendering::GlobalIlluminationVct::DVM_Emissive; + } + else if (_text == "lighting") + { + _modeToSet = rendering::GlobalIlluminationVct::DVM_Lighting; + } + else if (_text == "none") + { + _modeToSet = rendering::GlobalIlluminationVct::DVM_None; + } + else + { + _modeToSet = _defaultMode; + } +} + ////////////////////////////////////////////////// void Sensors::Configure(const Entity &/*_id*/, const std::shared_ptr &_sdf, @@ -549,6 +716,89 @@ void Sensors::Configure(const Entity &/*_id*/, if (_sdf->HasElement("ambient_light")) this->dataPtr->ambientLight = _sdf->Get("ambient_light"); + // Get the global illumination technique and its parameters, if specified + if (_sdf->HasElement("global_illumination")) + { + if (engineName != "ogre2") + { + gzerr << "Global illumination is only supported by the ogre2 " + << "render engine" << std::endl; + } + else + { + auto giElem = _sdf->FindElement("global_illumination"); + std::string giType = giElem->GetAttribute("type")->GetAsString(); + if (giType == "vct") + { + this->dataPtr->giVctParameters.enabled = giElem->Get( + "enabled", this->dataPtr->giVctParameters.enabled).first; + + // Use helper functions to parse the inputted set of values + // as a uint32_t array + if (giElem->HasElement("resolution")) + { + parseVector3dAsUInt32Array(giElem, "resolution", + this->dataPtr->giVctParameters.resolution, + this->dataPtr->giDefaultData.resolution); + } + else + { + convertVector3dToUInt32Array( + this->dataPtr->giVctParameters.resolution, + this->dataPtr->giDefaultData.resolution); + } + + if (giElem->HasElement("octant_count")) + { + parseVector3dAsUInt32Array(giElem, "octant_count", + this->dataPtr->giVctParameters.octantCount, + this->dataPtr->giDefaultData.octantCount); + } + else + { + convertVector3dToUInt32Array( + this->dataPtr->giVctParameters.octantCount, + this->dataPtr->giDefaultData.octantCount); + } + + this->dataPtr->giVctParameters.bounceCount = + giElem->Get("bounce_count", + this->dataPtr->giVctParameters.bounceCount).first; + this->dataPtr->giVctParameters.highQuality = + giElem->Get("high_quality", + this->dataPtr->giVctParameters.highQuality).first; + this->dataPtr->giVctParameters.anisotropic = + giElem->Get("anisotropic", + this->dataPtr->giVctParameters.anisotropic).first; + this->dataPtr->giVctParameters.thinWallCounter = + giElem->Get("thin_wall_counter", + this->dataPtr->giVctParameters.thinWallCounter).first; + this->dataPtr->giVctParameters.conserveMemory = + giElem->Get("conserve_memory", + this->dataPtr->giVctParameters.conserveMemory).first; + + if (giElem->HasElement("debug_vis_mode")) + { + const std::string text = giElem->Get( + "debug_vis_mode", "none").first; + SetDebugVisMode(text, this->dataPtr->giVctParameters.debugVisMode, + this->dataPtr->giDefaultData.debugVisMode); + } + } + else if (giType == "civct") + { + // todo: add CIVCT here. should also check if apiBackend is vulkan + // can use SetDebugVisMode when parsing DVM + gzerr << "GI CI VCT is not supported" << std::endl; + } + else + { + gzerr << "GI method type [" << giType << "] is not supported." + << std::endl; + } + } + } + this->dataPtr->renderUtil.SetEngineName(engineName); #ifdef __APPLE__ if (apiBackend.empty()) diff --git a/src/systems/spacecraft_thruster_model/SpacecraftThrusterModel.cc b/src/systems/spacecraft_thruster_model/SpacecraftThrusterModel.cc index 7f65212ae1..eacb9e93be 100644 --- a/src/systems/spacecraft_thruster_model/SpacecraftThrusterModel.cc +++ b/src/systems/spacecraft_thruster_model/SpacecraftThrusterModel.cc @@ -25,13 +25,15 @@ #include "SpacecraftThrusterModel.hh" +#include +#include #include -#include #include -#include +#include #include +#include #include #include @@ -47,8 +49,11 @@ #include "gz/sim/components/Actuators.hh" #include "gz/sim/components/ExternalWorldWrenchCmd.hh" #include "gz/sim/components/Pose.hh" +#include "gz/sim/Entity.hh" +#include "gz/sim/EntityComponentManager.hh" #include "gz/sim/Link.hh" #include "gz/sim/Model.hh" +#include "gz/sim/System.hh" #include "gz/sim/Util.hh" using namespace gz; diff --git a/src/systems/tracked_vehicle/TrackedVehicle.cc b/src/systems/tracked_vehicle/TrackedVehicle.cc index 848c672f43..2ed98c585f 100644 --- a/src/systems/tracked_vehicle/TrackedVehicle.cc +++ b/src/systems/tracked_vehicle/TrackedVehicle.cc @@ -692,11 +692,11 @@ void TrackedVehiclePrivate::UpdateVelocity( // radius of the turn the robot is doing this->desiredRotationRadiusSigned = - (fabs(angVel) < 0.1) ? + (fabs(angVel) < 1e-6) ? // is driving straight math::INF_D : ( - (fabs(linVel) < 0.1) ? + (fabs(linVel) < 1e-6) ? // is rotating about a single point 0 : // general movement diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 8f9ba3102c..02e5d7dee8 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -80,6 +80,7 @@ #include "gz/sim/EntityComponentManager.hh" #include "gz/sim/Model.hh" #include "gz/sim/SdfEntityCreator.hh" +#include "gz/sim/System.hh" #include "gz/sim/Util.hh" #include "gz/sim/World.hh" #include "gz/sim/components/ContactSensorData.hh" diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index da422d2a89..6304e97999 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -108,6 +108,7 @@ set(tests_needing_display dvl_system.cc dvl_system_bottom_tracking.cc dvl_system_water_mass_tracking.cc + camera_sensor_global_illumination.cc gpu_lidar.cc markers.cc mesh_uri.cc @@ -246,5 +247,5 @@ endif() if (TARGET INTEGRATION_python_system_loader) set_tests_properties(INTEGRATION_python_system_loader PROPERTIES - ENVIRONMENT "PYTHONPATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}/python/") + ENVIRONMENT "PYTHONPATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}/python/:$ENV{PYTHONPATH}") endif() diff --git a/test/integration/camera_sensor_global_illumination.cc b/test/integration/camera_sensor_global_illumination.cc new file mode 100644 index 0000000000..5a7e94677e --- /dev/null +++ b/test/integration/camera_sensor_global_illumination.cc @@ -0,0 +1,170 @@ +/* + * Copyright (C) 2024 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 "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "gz/sim/Util.hh" +#include "test_config.hh" + +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace std::chrono_literals; + +std::mutex mutex; +int cbValue = 0; +int giEnabled = false; + +////////////////////////////////////////////////// +/// Note: This test is almost identical to the test in +/// camera_sensor_scene_background.cc, and the `cameraCb` could have been +/// reused, but loading the world twice in a single processes causes errors with +/// Ogre. +class CameraSensorGlobalIlluminationTest : + public InternalFixture> +{ +}; + +///////////////////////////////////////////////// +void cameraCb(const msgs::Image & _msg) +{ + ASSERT_EQ(msgs::PixelFormatType::RGB_INT8, + _msg.pixel_format_type()); + + for (unsigned int y = 0; y < _msg.height(); ++y) + { + for (unsigned int x = 0; x < _msg.width(); ++x) + { + unsigned char r = _msg.data()[y * _msg.step() + x*3]; + unsigned char g = _msg.data()[y * _msg.step() + x*3+1]; + unsigned char b = _msg.data()[y * _msg.step() + x*3+2]; + + if (!giEnabled) + { + ASSERT_LT(static_cast(r), 2); + ASSERT_LT(static_cast(g), 2); + ASSERT_LT(static_cast(b), 2); + } + else + { + ASSERT_GT(static_cast(r), 50); + ASSERT_LT(static_cast(g), 25); + ASSERT_LT(static_cast(g), 25); + } + } + } + std::lock_guard lock(mutex); + if (!giEnabled) + cbValue = 1; + else + cbValue = 2; +} + +///////////////////////////////////////////////// +// Check that sensor reads a very dark value when GI is not enabled +TEST_F(CameraSensorGlobalIlluminationTest, + GlobalIlluminationNotEnabled) +{ + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "camera_sensor_gi_enabled_false.sdf"); + // Start server + sim::ServerConfig serverConfig; + serverConfig.SetSdfFile(sdfFile); + + sim::Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // subscribe to the camera topic + transport::Node node; + cbValue = 0; + giEnabled = false; + node.Subscribe("/camera", &cameraCb); + + // Run server and verify that we are receiving a message + // from the depth camera + server.Run(true, 100, false); + + int i = 0; + while (i < 100 && cbValue == 0) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + i++; + } + + std::lock_guard lock(mutex); + EXPECT_EQ(cbValue, 1); +} + +///////////////////////////////////////////////// +// Check that sensor reads less dark value when GI is enabled +TEST_F(CameraSensorGlobalIlluminationTest, + GZ_UTILS_TEST_DISABLED_ON_MAC(GlobalIlluminationEnabled)) +{ + // \todo(anyone) test fails on github action but pass on other + // ubuntu jenkins CI. Need to investigate further. + // Github action sets the MESA_GL_VERSION_OVERRIDE variable + // so check for this variable and disable test if it is set. +#ifdef __linux__ + std::string value; + bool result = common::env("MESA_GL_VERSION_OVERRIDE", value, true); + if (result && value == "3.3") + { + GTEST_SKIP() << "Test is run on machine with software rendering or mesa " + << "driver. Skipping test. " << std::endl; + } +#endif + + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "camera_sensor_gi_enabled_true.sdf"); + // Start server + sim::ServerConfig serverConfig; + serverConfig.SetSdfFile(sdfFile); + + sim::Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // subscribe to the camera topic + transport::Node node; + cbValue = 0; + giEnabled = true; + node.Subscribe("/camera", &cameraCb); + + // Run server and verify that we are receiving a message + // from the depth camera + server.Run(true, 100, false); + + int i = 0; + while (i < 100 && cbValue == 0) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + i++; + } + + std::lock_guard lock(mutex); + EXPECT_EQ(cbValue, 2); +} diff --git a/test/integration/force_torque_system.cc b/test/integration/force_torque_system.cc index dd4684e627..23ff6cbbb4 100644 --- a/test/integration/force_torque_system.cc +++ b/test/integration/force_torque_system.cc @@ -43,7 +43,7 @@ class ForceTorqueTest : public InternalFixture<::testing::Test> }; ///////////////////////////////////////////////// -TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeight)) +TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeightTopic)) { using namespace std::chrono_literals; // Start server @@ -59,8 +59,8 @@ TEST_F(ForceTorqueTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MeasureWeight)) // 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; + const size_t iters = 999u; + const size_t updates = 100u; std::vector wrenches; wrenches.reserve(updates); diff --git a/test/integration/link.cc b/test/integration/link.cc index 9e140a9499..c706c1135a 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -170,6 +171,30 @@ TEST_F(LinkIntegrationTest, VisualByName) EXPECT_EQ(1u, link.VisualCount(ecm)); } +////////////////////////////////////////////////// +TEST_F(LinkIntegrationTest, SensorByName) +{ + EntityComponentManager ecm; + + // Link + auto eLink = ecm.CreateEntity(); + Link link(eLink); + EXPECT_EQ(eLink, link.Entity()); + EXPECT_EQ(0u, link.SensorCount(ecm)); + + // Sensor + auto eSensor = ecm.CreateEntity(); + ecm.CreateComponent(eSensor, components::Sensor()); + ecm.CreateComponent(eSensor, + components::ParentEntity(eLink)); + ecm.CreateComponent(eSensor, + components::Name("sensor_name")); + + // Check link + EXPECT_EQ(eSensor, link.SensorByName(ecm, "sensor_name")); + EXPECT_EQ(1u, link.SensorCount(ecm)); +} + ////////////////////////////////////////////////// TEST_F(LinkIntegrationTest, CollisionByName) { diff --git a/test/integration/model.cc b/test/integration/model.cc index f878088631..f4a0f00b2c 100644 --- a/test/integration/model.cc +++ b/test/integration/model.cc @@ -164,6 +164,30 @@ TEST_F(ModelIntegrationTest, SourceFilePath) EXPECT_EQ("/tmp/path", model.SourceFilePath(ecm)); } +////////////////////////////////////////////////// +TEST_F(ModelIntegrationTest, ModelByName) +{ + EntityComponentManager ecm; + + // Model + auto eModel = ecm.CreateEntity(); + Model model(eModel); + EXPECT_EQ(eModel, model.Entity()); + EXPECT_EQ(0u, model.ModelCount(ecm)); + + // Nested Model + auto eNestedModel = ecm.CreateEntity(); + ecm.CreateComponent(eNestedModel, components::Model()); + ecm.CreateComponent(eNestedModel, + components::ParentEntity(eModel)); + ecm.CreateComponent(eNestedModel, + components::Name("nested_model_name")); + + // Check model + EXPECT_EQ(eNestedModel, model.ModelByName(ecm, "nested_model_name")); + EXPECT_EQ(1u, model.ModelCount(ecm)); +} + ////////////////////////////////////////////////// TEST_F(ModelIntegrationTest, LinkByName) { diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc index bf9cd2a66e..67e80d65f7 100644 --- a/test/integration/odometry_publisher.cc +++ b/test/integration/odometry_publisher.cc @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -380,6 +381,86 @@ class OdometryPublisherTest EXPECT_NEAR(poses.back().Rot().Z(), tfPoses.back().Rot().Z(), 1e-2); } + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _odomTopic Odometry topic. + protected: void TestMovement3dAtSingularity(const std::string &_sdfFile, + const std::string &_odomTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the body poses + test::Relay testSystem; + + std::vector poses; + testSystem.OnPostUpdate([&poses](const sim::UpdateInfo &, + const sim::EntityComponentManager &_ecm) + { + auto id = _ecm.EntityByComponents( + components::Model(), + components::Name("test_body")); + EXPECT_NE(kNullEntity, id); + + auto poseComp = _ecm.Component(id); + ASSERT_NE(nullptr, poseComp); + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); + + std::vector odomAngVels; + // Create function to store data from odometry messages + std::function odomCb = + [&](const msgs::Odometry &_msg) + { + odomAngVels.push_back(msgs::Convert(_msg.twist().angular())); + }; + + // Create node for publishing twist messages + transport::Node node; + auto cmdVel = node.Advertise("/model/test_body/cmd_vel"); + node.Subscribe(_odomTopic, odomCb); + + // Set an angular velocity command that would cause pitch to update from 0 + // to PI in 1 second, crossing the singularity when pitch is PI/2. + const math::Vector3d angVelCmd(0.0, GZ_PI, 0); + msgs::Twist msg; + msgs::Set(msg.mutable_linear(), math::Vector3d::Zero); + msgs::Set(msg.mutable_angular(), angVelCmd); + cmdVel.Publish(msg); + + // Run server while the model moves with the velocities set earlier + server.Run(true, 1000, false); + + // Poses for 1s + ASSERT_EQ(1000u, poses.size()); + + int sleep = 0; + int maxSleep = 30; + // Default publishing frequency for odometryPublisher is 50Hz. + for (; (odomAngVels.size() < 50) && + sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_NE(maxSleep, sleep); + + // Odom for 1s + ASSERT_FALSE(odomAngVels.empty()); + EXPECT_EQ(50u, odomAngVels.size()); + + // Check accuracy of velocities published in the odometry message + for (size_t i = 1; i < odomAngVels.size(); ++i) { + EXPECT_NEAR(odomAngVels[i].X(), angVelCmd[0], 1e-1); + EXPECT_NEAR(odomAngVels[i].Y(), angVelCmd[1], 1e-1); + EXPECT_NEAR(odomAngVels[i].Z(), angVelCmd[2], 1e-1); + } + } + /// \param[in] _sdfFile SDF file to load. /// \param[in] _odomTopic Odometry topic. /// \param[in] _frameId Name of the world-fixed coordinate frame @@ -624,6 +705,16 @@ TEST_P(OdometryPublisherTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Movement3d)) "/model/X3/odometry", "/model/X3/pose", "X3/odom", "X3/base_footprint"); } +///////////////////////////////////////////////// +TEST_P(OdometryPublisherTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(Movement3dAtSingularity)) +{ + TestMovement3dAtSingularity( + gz::common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "odometry_publisher_3d_singularity.sdf"), + "/model/test_body/odometry"); +} + ///////////////////////////////////////////////// TEST_P(OdometryPublisherTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(OdomFrameId)) { diff --git a/test/worlds/camera_sensor_gi_enabled_false.sdf b/test/worlds/camera_sensor_gi_enabled_false.sdf new file mode 100644 index 0000000000..b8700cf1f9 --- /dev/null +++ b/test/worlds/camera_sensor_gi_enabled_false.sdf @@ -0,0 +1,251 @@ + + + + + + + ogre2 + + + false + 16 16 16 + 1 1 1 + 6 + true + true + 1.0 + true + none + + + + + + + 0 0 0 + 0.1 0.1 0.1 + + + + + 0 -0.5 2.5 0 0 0 + 1 1 1 1 + 0 0 0 0 + + 50 + 0 + 0 + 0.02 + + true + true + 1.0 + + + + + 0 5 4 0 0 0 + true + + + + + 10 1 10 + + + + + + + 10 1 10 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 -5 4 0 0 0 + true + + + + + 10 1 10 + + + + + + + 10 1 10 + + + + 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 + true + + + + + 10 10 1 + + + + + + + 10 10 1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 8.5 0 0 0 + true + + + + + 10 10 1 + + + + + + + 10 10 1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 4.5 0 4 0 0 0 + true + + + + + 1 10 10 + + + + + + + 1 10 10 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + 0 -0.5 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 1 1 1 + + + + + + + + true + 0 0.5 0.5 0 0 -1.57 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + 0 0 1 1 + 0 0 1 1 + 1 1 1 1 + + + + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 30 + camera + + + + + diff --git a/test/worlds/camera_sensor_gi_enabled_true.sdf b/test/worlds/camera_sensor_gi_enabled_true.sdf new file mode 100644 index 0000000000..067466da8f --- /dev/null +++ b/test/worlds/camera_sensor_gi_enabled_true.sdf @@ -0,0 +1,251 @@ + + + + + + + ogre2 + + + true + 16 16 16 + 1 1 1 + 6 + true + true + 1.0 + true + none + + + + + + + 0 0 0 + 0.1 0.1 0.1 + + + + + 0 -0.5 2.5 0 0 0 + 1 1 1 1 + 0 0 0 0 + + 50 + 0 + 0 + 0.02 + + true + true + 1.0 + + + + + 0 5 4 0 0 0 + true + + + + + 10 1 10 + + + + + + + 10 1 10 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 -5 4 0 0 0 + true + + + + + 10 1 10 + + + + + + + 10 1 10 + + + + 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 + true + + + + + 10 10 1 + + + + + + + 10 10 1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 8.5 0 0 0 + true + + + + + 10 10 1 + + + + + + + 10 10 1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 4.5 0 4 0 0 0 + true + + + + + 1 10 10 + + + + + + + 1 10 10 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + 0 -0.5 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 1 1 1 + + + + + + + + true + 0 0.5 0.5 0 0 -1.57 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + 0 0 1 1 + 0 0 1 1 + 1 1 1 1 + + + + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 30 + camera + + + + + diff --git a/test/worlds/odometry_publisher_3d_singularity.sdf b/test/worlds/odometry_publisher_3d_singularity.sdf new file mode 100644 index 0000000000..d74288d69f --- /dev/null +++ b/test/worlds/odometry_publisher_3d_singularity.sdf @@ -0,0 +1,97 @@ + + + + + + 0.001 + 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 + + + + 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 + + + + + + + 0 0 1 0 0 0 + + + 1.5 + + 0.0347563 + 0 + 0 + 0.07 + 0 + 0.0977 + + + + + + 0.30 0.42 0.11 + + + + + + + 0.30 0.42 0.11 + + + + + + 0 0 0 + 0 3.1415 0 + + + 3 + + + + + diff --git a/tutorials.md.in b/tutorials.md.in index b75967e1ad..b3d734544e 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -24,6 +24,7 @@ Gazebo @GZ_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage headless_rendering "Headless rendering": Access the GPU on a remote machine to produce sensor data without an X server. * \subpage apply_force_torque "Apply Force and Torque": Applying forces and/or torques to models during simulation through the GUI. * \subpage mouse_drag "Mouse Drag": Move models by dragging them in the scene using forces and torques. +* \subpage global_illumination "Global illumination": Enable global illumination for the GUI and for the sensor view. ### Migration from Gazebo classic diff --git a/tutorials/files/global_illumination/global_illumination.gif b/tutorials/files/global_illumination/global_illumination.gif new file mode 100644 index 0000000000..fbc1435e28 Binary files /dev/null and b/tutorials/files/global_illumination/global_illumination.gif differ diff --git a/tutorials/files/global_illumination/gui_demo_default.png b/tutorials/files/global_illumination/gui_demo_default.png new file mode 100644 index 0000000000..516d32dea6 Binary files /dev/null and b/tutorials/files/global_illumination/gui_demo_default.png differ diff --git a/tutorials/files/global_illumination/gui_demo_with_vct.png b/tutorials/files/global_illumination/gui_demo_with_vct.png new file mode 100644 index 0000000000..823d23b3dc Binary files /dev/null and b/tutorials/files/global_illumination/gui_demo_with_vct.png differ diff --git a/tutorials/frame_reference.md b/tutorials/frame_reference.md index 3d904fa19e..eae0401ae5 100644 --- a/tutorials/frame_reference.md +++ b/tutorials/frame_reference.md @@ -40,7 +40,7 @@ being in the positive z direction and so forth. If you are creating a model, besides designing its kinematic structure (links and joints) and physical properties, you need to decide where to place your model's reference frame. Next are a few examples of our turtle model with three -differente choices for its reference frame. +different choices for its reference frame. @image html files/frame_reference/turtle_frames.png @@ -57,14 +57,14 @@ left, and `Z` pointing up. 3. Try to match the `Z` value of your reference frame with the point of your model that contacts the ground or the water. That way, when you spawn your model -at any point in the world, if you use `z` value of `0`, you know it will +at any point in the world, if you use `Z` value of `0`, you know it will smoothly sit on a stable place. # How to set your model reference frame There are a few ways to change the reference frame of your model: -1. Adjust your mesh reference frame. When desigining your mesh you'll be able to +1. Adjust your mesh reference frame. When designing your mesh you'll be able to set its reference frame. It's recommended to match the mesh reference frame with the one you will want in Gazebo for your model. diff --git a/tutorials/global_illumination.md b/tutorials/global_illumination.md new file mode 100644 index 0000000000..13ff5bc3a7 --- /dev/null +++ b/tutorials/global_illumination.md @@ -0,0 +1,419 @@ +\page global_illumination Global Illumination + +This tutorial showcases how to enable real-time global illumination in Gazebo. + +
+ \image html files/global_illumination/global_illumination.gif width=60% +
+ +## About global illumination + +Global illumination (GI) techniques account for illumination from indirect light reflections. When GI is enabled, objects not only receive direct light from light sources, but also from indirect light bouncing off of other surfaces, ensuring nice reflections and a more natural appearance. + +Gazebo supports two GI methods through the Ogre2 rendering engine, Voxel Cone Tracing (VCT) and Cascaded Image Voxel Cone Tracing (CI VCT). + +### Voxel Cone Tracing + +A scene rendered with VCT is reliable and good quality, and thus is the best choice for most scenes. + +VCT parameters include: +* **Resolution** +* **Octant count** +* **Bounce count** +* **High quality** +* **Anisotropic** +* **Thin wall counter** +* **Conserve memory** +* **Debug visualization mode** + +### Cascaded Image Voxel Cone Tracing + +Compared to VCT, a scene rendered with CI VCT is slightly lower quality, but the scene can be re-voxelized every frame, so it’s much faster when dealing with very large scenes. However, it is more memory-intensive than VCT. + +CI VCT parameters include: +* **Bounce count** +* **High quality** +* **Anisotropic** +* **Debug visualization mode** +* **Cascade** + * **Resolution** + * **Octant count** + * **Thin wall counter** + * **Area half size** + +## Enabling global illumination in Gazebo + +During simulation, the GUI and the camera sensor view are two different scenes. The GUI scene is rendered on the frontend client process, and the camera sensor scene is rendered on the backend server process. Thus, GI can be enabled on either or both of these processes. + +> **Note:** GI solutions require hardware that uses OpenGL4.
+> **Note:** CI VCT must be run with Vulkan as the render engine API backend. + +### How to enable global illumination for the GUI + +GI can be enabled for the GUI through a GUI plugin. Both VCT and CI VCT are supported for the GUI. + +#### Example usage for VCT + +1) Open the [global_illumination.sdf]( +https://github.com/gazebosim/gz-sim/tree/gz-sim8/examples/worlds/global_illumination.sdf) world with + +```bash +gz sim global_illumination.sdf +``` +
+ \image html files/global_illumination/gui_demo_default.png width=60% +
+
+ +2) From the plugin dropdown, select the Global Illumination Vct plugin. + +3) On the card, check the `Enabled` button to enable GI and change the parameter values as desired. + +
+ \image html files/global_illumination/gui_demo_with_vct.png width=60% +
+ +#### Example usage for CI VCT + +1) Open the [global_illumination.sdf]( +https://github.com/gazebosim/gz-sim/tree/gz-sim8/examples/worlds/global_illumination.sdf) world using Vulkan with + +```bash +gz sim global_illumination.sdf --render-engine-api-backend vulkan +``` + +2) From the plugin dropdown, select the Global Illumination Ci Vct plugin. + +3) On the card, check the `Enabled` button to enable GI and change the parameter values as desired. + +### How to enable global illumination for the sensor + +GI can be enabled for sensors through the `` element in the Sensors System plugin. VCT is supported for the sensor. + +#### Example usage with VCT + +We will demonstrate how to enable VCT for the sensor with the SDF file below. (The finished SDF file can be viewed [here]( +https://github.com/gazebosim/gz-sim/tree/gz-sim8/examples/worlds/global_illumination.sdf).) + +1) Save the below in an SDF file named `gi_demo.sdf`: + +```xml + + + + + + + + ogre2 + + + + + + + + + 0.4 0.4 0.4 + 0 0 0 + + + + + + + Cornell box GI demo + true + docked + + ogre2 + -12 0 4 0 0 0 + + + + camera + + + + + + + + false + 0 + 50 + 250 + 50 + docked + true + #777777 + + + + + false + 5 + 5 + floating + false + + + + + + + 0 0 7.5 0 0 0 + 1 1 1 1 + 0.0 0.0 0.0 0 + + 50 + 0 + 0 + 0.02 + + true + false + 1.0 + + + + + 0 5 4 0 0 0 + true + + + + + 10 1 10 + + + + + + + 10 1 10 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 0 -5 4 0 0 0 + true + + + + + 10 1 10 + + + + + + + 10 1 10 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 0 0 -0.5 0 0 0 + true + + + + + 10 10 1 + + + + + + + 10 10 1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 8.5 0 0 0 + true + + + + + 10 10 1 + + + + + + + 10 10 1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 4.5 0 4 0 0 0 + true + + + + + 1 10 10 + + + + + + + 1 10 10 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + -1.5 0 0 0 0 0 + + + + + https://fuel.gazebosim.org/1.0/openrobotics/models/pump/1/files/meshes/pump.dae + 3 3 3 + + + + + + + + https://fuel.gazebosim.org/1.0/openrobotics/models/pump/1/files/meshes/pump.dae + 3 3 3 + + + + 1.0 1.0 1.0 + 1.0 1.0 1.0 + + + https://fuel.gazebosim.org/1.0/openrobotics/models/pump/1/files/materials/textures/pump_albedo.png + https://fuel.gazebosim.org/1.0/openrobotics/models/pump/1/files/materials/textures/pump_roughness.png + https://fuel.gazebosim.org/1.0/openrobotics/models/pump/1/files/materials/textures/pump_metallic.png + https://fuel.gazebosim.org/1.0/openrobotics/models/pump/1/files/materials/textures/pump_normal.png + + + + + + + + + + -15 0 4 0 0.0 0 + true + + 0.05 0.05 0.05 0 0 0 + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1.047 + + 2560 + 1920 + + + 0.1 + 100 + + + 1 + 30 + true + camera + + + + + +``` + +2) Add the `` element to the Sensors System plugin. + +```xml + + true + 16 16 16 + 1 1 1 + 6 + true + true + 1.0 + false + none + +``` + +The parameters can be changed from the default values. + +3) Open the `gi_demo.sdf` world with + +```bash +gz sim gi_demo.sdf +```