From cf9f0ba44ac91d7d923778120f039f278f0ca51c Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 25 Jul 2025 23:42:57 -0500 Subject: [PATCH 01/13] Add EntitySemantics system that enables setting categories and tags Signed-off-by: Addisu Z. Taddese --- include/gz/sim/components/SemanticCategory.hh | 44 +++ include/gz/sim/components/SemanticTag.hh | 83 +++++ src/systems/CMakeLists.txt | 1 + src/systems/entity_semantics/CMakeLists.txt | 4 + .../entity_semantics/EntitySemantics.cc | 140 ++++++++ .../entity_semantics/EntitySemantics.hh | 92 +++++ test/integration/CMakeLists.txt | 1 + test/integration/entity_semantics_system.cc | 82 +++++ test/worlds/entity_semantics.sdf | 333 ++++++++++++++++++ 9 files changed, 780 insertions(+) create mode 100644 include/gz/sim/components/SemanticCategory.hh create mode 100644 include/gz/sim/components/SemanticTag.hh create mode 100644 src/systems/entity_semantics/CMakeLists.txt create mode 100644 src/systems/entity_semantics/EntitySemantics.cc create mode 100644 src/systems/entity_semantics/EntitySemantics.hh create mode 100644 test/integration/entity_semantics_system.cc create mode 100644 test/worlds/entity_semantics.sdf diff --git a/include/gz/sim/components/SemanticCategory.hh b/include/gz/sim/components/SemanticCategory.hh new file mode 100644 index 0000000000..feee3dbf94 --- /dev/null +++ b/include/gz/sim/components/SemanticCategory.hh @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2025 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_CATEGORY_HH_ +#define GZ_SIM_COMPONENTS_CATEGORY_HH_ + +#include "gz/sim/components/Component.hh" +#include "gz/sim/components/Factory.hh" +#include "gz/sim/config.hh" + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace components +{ +/// \brief A component that holds the semantic category of an entity. This is +/// meant to be used by systems such as EntitySemantics to assign categories to +/// entities. +/// See +/// https://github.com/ros-simulation/simulation_interfaces/blob/1.0.0/msg/EntityCategory.msg +using SemanticCategory = Component; +GZ_SIM_REGISTER_COMPONENT("gz_sim_components.SemanticCategory", + SemanticCategory) +} // namespace components +} // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace sim +} // namespace gz +#endif diff --git a/include/gz/sim/components/SemanticTag.hh b/include/gz/sim/components/SemanticTag.hh new file mode 100644 index 0000000000..21379b2896 --- /dev/null +++ b/include/gz/sim/components/SemanticTag.hh @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2025 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_SEMANTIC_TAG_HH_ +#define GZ_SIM_COMPONENTS_SEMANTIC_TAG_HH_ + +#include + +#include +#include + +#include "gz/sim/components/Component.hh" +#include "gz/sim/components/Factory.hh" +#include "gz/sim/components/Serialization.hh" +#include "gz/sim/config.hh" + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE +{ +namespace serializers +{ +class SemanticTagSerializer +{ + public: + static std::ostream &Serialize(std::ostream &_out, + const std::set &_tags) + { + msgs::StringMsg_V msg; + for (const auto &tag : _tags) + { + msg.add_data(tag); + } + msg.SerializeToOstream(&_out); + return _out; + } + + public: + static std::istream &Deserialize(std::istream &_in, + std::set &_tags) + { + msgs::StringMsg_V msg; + msg.ParsePartialFromIstream(&_in); + for (const auto &item : msg.data()) + { + _tags.insert(item); + } + return _in; + } +}; +} // namespace serializers +// +namespace components +{ + +/// \brief A component that holds the semantic tag of an entity. This is +/// meant to be used by systems such as EntitySemantics to assign tags to +/// entities. See +/// https://github.com/ros-simulation/simulation_interfaces/blob/1.0.0/msg/TagsFilter.msg +using SemanticTag = Component, class SemanticTagTag, + serializers::SemanticTagSerializer>; +GZ_SIM_REGISTER_COMPONENT("gz_sim_components.SemanticTag", SemanticTag) +} // namespace components +} // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace sim +} // namespace gz +#endif diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 16eae8326b..f85b4117ea 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -99,6 +99,7 @@ add_subdirectory(dvl) if (NOT WIN32) add_subdirectory(elevator) endif() +add_subdirectory(entity_semantics) add_subdirectory(environment_preload) add_subdirectory(environmental_sensor_system) add_subdirectory(follow_actor) diff --git a/src/systems/entity_semantics/CMakeLists.txt b/src/systems/entity_semantics/CMakeLists.txt new file mode 100644 index 0000000000..0536905e6c --- /dev/null +++ b/src/systems/entity_semantics/CMakeLists.txt @@ -0,0 +1,4 @@ +gz_add_system(entity-semantics + SOURCES + EntitySemantics.cc +) diff --git a/src/systems/entity_semantics/EntitySemantics.cc b/src/systems/entity_semantics/EntitySemantics.cc new file mode 100644 index 0000000000..cbf4110b47 --- /dev/null +++ b/src/systems/entity_semantics/EntitySemantics.cc @@ -0,0 +1,140 @@ +/* + * Copyright (C) 2025 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 "EntitySemantics.hh" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/SemanticCategory.hh" +#include "gz/sim/components/SemanticTag.hh" + +using namespace gz; +using namespace sim; +using namespace systems; + +namespace +{ + +// clang-format off +std::array kCategoryStrToInt = { + std::pair{"object", 0u}, + std::pair{"robot", 1u}, + std::pair{"human", 2u}, + std::pair{"dynamic_object", 4u}, + std::pair{"static_object", 5u}, +}; +// clang-format on + +std::optional ConvertCategoryStrToInt(const std::string &_category) +{ + auto it = + std::find_if(kCategoryStrToInt.begin(), kCategoryStrToInt.end(), + [&](const auto &_item) { return _category == _item.first; }); + + if (it != kCategoryStrToInt.end()) + { + return it->second; + } + return std::nullopt; +} +} // namespace + +////////////////////////////////////////////////// +void EntitySemantics::PreUpdate(const UpdateInfo &, + EntityComponentManager &_ecm) +{ + GZ_PROFILE("EntitySemantics::PreUpdate"); + + std::map entitiesToCategorize; + std::map> entitiesToTag; + _ecm.EachNew( + [&](const Entity &_entity, const components::Name *_name, + const components::ModelSdf *_modelSdf) + { + auto elem = _modelSdf->Data().Element(); + auto semanticsElem = elem->FindElement("gz:semantics"); + if (semanticsElem) + { + auto categoryElem = semanticsElem->FindElement("category"); + if (categoryElem) + { + auto categoryStr = categoryElem->Get(); + auto category = + ConvertCategoryStrToInt(common::lowercase(categoryStr)); + if (category) + { + entitiesToCategorize.emplace(_entity, *category); + } + else + { + gzerr << "Invalid category specified [" << categoryStr + << "] in model [" << _name->Data() << "]\n"; + } + + if (categoryElem->GetNextElement("category")) + { + // TODO(azeey) Include file and line number in error message. + gzerr << "There should be only one element for an " + "entity in model [" + << _name->Data() << "]\n"; + } + } + + for (auto tagElem = semanticsElem->FindElement("tag"); tagElem; + tagElem = tagElem->GetNextElement("tag")) + { + auto tagStr = tagElem->Get(); + if (tagStr.empty()) + { + gzerr << " cannot be an empty string in model [" + << _name->Data() << "]\n"; + } + else + { + entitiesToTag[_entity].insert(tagStr); + } + } + } + return true; + }); + + for (const auto &[entity, category] : entitiesToCategorize) + { + _ecm.SetComponentData(entity, category); + } + + for (const auto &[entity, tags] : entitiesToTag) + { + _ecm.SetComponentData(entity, tags); + } +} + +GZ_ADD_PLUGIN(EntitySemantics, System, EntitySemantics::ISystemPreUpdate) + +GZ_ADD_PLUGIN_ALIAS(EntitySemantics, "gz::sim::systems::EntitySemantics") diff --git a/src/systems/entity_semantics/EntitySemantics.hh b/src/systems/entity_semantics/EntitySemantics.hh new file mode 100644 index 0000000000..d0070cf746 --- /dev/null +++ b/src/systems/entity_semantics/EntitySemantics.hh @@ -0,0 +1,92 @@ +/* + * Copyright (C) 2025 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_SYSTEMS_ENTITYSEMANTICS_HH_ +#define GZ_SIM_SYSTEMS_ENTITYSEMANTICS_HH_ + +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE +{ +namespace systems +{ +// Forward declaration +class EntitySemanticsPrivate; + +/// \brief System for assigning semantics to entities, such as models, +/// by classifying them into categories and assigning them tags. +/// Reference: +/// https://github.com/ros-simulation/simulation_interfaces/blob/1.0.0/msg/EntityCategory.msg, +/// https://github.com/ros-simulation/simulation_interfaces/blob/1.0.0/msg/TagsFilter.msg +/// +/// ## System Parameters +/// +/// This system is meant to be attached to a ``, but it parses parameters +/// found in entities such as models. +/// +/// ## Entity Parameters +/// +/// - ``: Root element to be specified inside entities, e.g. +/// . It has the following child parameters: +/// +/// - ``: string representation of the value as defined in +/// https://github.com/ros-simulation/simulation_interfaces/blob/1.0.0/msg/EntityCategory.msg. +/// The string is formed by removing the prefix `CATEGORY_` from the +/// categories listed in the EntityCategory message. Example: for +/// `CATEGORY_ROBOT`, it would be `ROBOT`. Lowercase +/// values will also be accepted. There can only be one instance of the +/// `` element for an entity. +/// +/// - ``: string labels that can be assigned according to the user's +/// classification scheme. They serve as a secondary layer of classification +/// in on top of categories. Multiple instances of the `` elements are +/// allowed for an entity. +/// +/// +/// Example: +/// +/// ```xml +/// +/// +/// ROBOT +/// mobile +/// diff_drive +/// +/// +/// +/// +/// ``` +/// + +class EntitySemantics : public System, + public ISystemPreUpdate +{ + // Documentation inherited + public: void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; +}; +} // namespace systems +} // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace sim +} // namespace gz + +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 1519f96bf0..195f4d06fd 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -23,6 +23,7 @@ set(tests each_new_removed.cc entity_erase.cc entity_system.cc + entity_semantics_system.cc environment_preload_system.cc environmental_sensor_system.cc events.cc diff --git a/test/integration/entity_semantics_system.cc b/test/integration/entity_semantics_system.cc new file mode 100644 index 0000000000..c105f2605f --- /dev/null +++ b/test/integration/entity_semantics_system.cc @@ -0,0 +1,82 @@ +/* + * Copyright (C) 2025 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 "../helpers/EnvTestFixture.hh" +#include "gz/sim/TestFixture.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/SemanticCategory.hh" +#include "gz/sim/components/SemanticTag.hh" +#include "test_config.hh" + +using namespace gz; +using namespace sim; +using namespace std::chrono_literals; + +/// \brief Test EntitySemantics system +class EntitySemanticsTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +TEST_F(EntitySemanticsTest, CanSetCategoriesAndTags) +{ + TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", + "entity_semantics.sdf")); + + bool testsExecuted = false; + + // Put the checks in an extra lambda as opposed to doing it OnPostUpdate so we + // can call ASSERT_* + auto runChecks = [](const sim::EntityComponentManager &_ecm) + { + auto robots = _ecm.EntitiesByComponents(components::SemanticCategory(1)); + ASSERT_EQ(1u, robots.size()); + + auto robotTags = _ecm.ComponentData(robots[0]); + ASSERT_TRUE(robotTags.has_value()); + ASSERT_EQ(2u, robotTags->size()); + EXPECT_EQ(1u, robotTags->count("mobile")); + EXPECT_EQ(1u, robotTags->count("diff_drive")); + + auto dynamicObjects = + _ecm.EntitiesByComponents(components::SemanticCategory(4)); + EXPECT_EQ(2u, dynamicObjects.size()); + + auto staticObjects = + _ecm.EntitiesByComponents(components::SemanticCategory(5)); + EXPECT_EQ(1u, staticObjects.size()); + }; + + fixture.OnPostUpdate( + [&](const sim::UpdateInfo &, const sim::EntityComponentManager &_ecm) + { + runChecks(_ecm); + testsExecuted = true; + return true; + }); + + fixture.Finalize(); + fixture.Server()->Run(true, 1, false); + EXPECT_TRUE(testsExecuted); +} diff --git a/test/worlds/entity_semantics.sdf b/test/worlds/entity_semantics.sdf new file mode 100644 index 0000000000..f1729614df --- /dev/null +++ b/test/worlds/entity_semantics.sdf @@ -0,0 +1,333 @@ + + + + + + + + + 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 + + + + + STATIC_OBJECT + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + ROBOT + mobile + diff_drive + + + 0 0 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + left_wheel_joint + right_wheel_joint + 1.25 + 0.3 + 1 + -1 + 2 + -2 + 0.5 + -0.5 + 1 + -1 + + + + + + DYNAMIC_OBJECT + + 5 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + + 0 5 0 0 0 1 + + DYNAMIC_OBJECT + + + 0.3 0.3 0.3 0 0 0 + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + 0.31 0.31 0.31 0 0 0 + + + 23.4 + + + + + + 50 + 0.32 0.32 0.32 0 0 0 + 0.5 + false + + + 1.2 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + From e16485384391a9d9225bd0211e15eac77a6df053 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 31 Jul 2025 13:19:03 -0500 Subject: [PATCH 02/13] Add description field Signed-off-by: Addisu Z. Taddese --- include/gz/sim/components/SemanticCategory.hh | 4 +- .../gz/sim/components/SemanticDescription.hh | 46 +++++++++++++++++++ include/gz/sim/components/SemanticTag.hh | 1 - src/SimulationRunner.cc | 14 +++--- .../entity_semantics/EntitySemantics.cc | 32 +++++++++++++ .../entity_semantics/EntitySemantics.hh | 9 ++-- test/integration/entity_semantics_system.cc | 7 ++- test/worlds/entity_semantics.sdf | 1 + 8 files changed, 100 insertions(+), 14 deletions(-) create mode 100644 include/gz/sim/components/SemanticDescription.hh diff --git a/include/gz/sim/components/SemanticCategory.hh b/include/gz/sim/components/SemanticCategory.hh index feee3dbf94..165e9ed943 100644 --- a/include/gz/sim/components/SemanticCategory.hh +++ b/include/gz/sim/components/SemanticCategory.hh @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef GZ_SIM_COMPONENTS_CATEGORY_HH_ -#define GZ_SIM_COMPONENTS_CATEGORY_HH_ +#ifndef GZ_SIM_COMPONENTS_SEMANTIC_CATEGORY_HH_ +#define GZ_SIM_COMPONENTS_SEMANTIC_CATEGORY_HH_ #include "gz/sim/components/Component.hh" #include "gz/sim/components/Factory.hh" diff --git a/include/gz/sim/components/SemanticDescription.hh b/include/gz/sim/components/SemanticDescription.hh new file mode 100644 index 0000000000..9b5b388084 --- /dev/null +++ b/include/gz/sim/components/SemanticDescription.hh @@ -0,0 +1,46 @@ +/* + * Copyright (C) 2025 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_SEMANTIC_DESCRIPTION_HH_ +#define GZ_SIM_COMPONENTS_SEMANTIC_DESCRIPTION_HH_ + +#include + +#include "gz/sim/components/Component.hh" +#include "gz/sim/components/Factory.hh" +#include "gz/sim/config.hh" + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace components +{ +/// \brief A component that holds the semantic description of an entity. This is +/// meant to be used by systems such as EntitySemantics to assign descriptions +/// to entities. See +/// https://github.com/ros-simulation/simulation_interfaces/blob/1.0.0/msg/EntityInfo.msg +using SemanticDescription = + Component; +GZ_SIM_REGISTER_COMPONENT("gz_sim_components.SemanticDescription", + SemanticDescription) +} // namespace components +} // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace sim +} // namespace gz +#endif diff --git a/include/gz/sim/components/SemanticTag.hh b/include/gz/sim/components/SemanticTag.hh index 21379b2896..f178e3c977 100644 --- a/include/gz/sim/components/SemanticTag.hh +++ b/include/gz/sim/components/SemanticTag.hh @@ -24,7 +24,6 @@ #include "gz/sim/components/Component.hh" #include "gz/sim/components/Factory.hh" -#include "gz/sim/components/Serialization.hh" #include "gz/sim/config.hh" namespace gz diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index dc21ec2de7..944bc6e452 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -192,13 +192,13 @@ SimulationRunner::SimulationRunner(const sdf::World &_world, this->node = std::make_unique(opts); - std::vector pubs; - std::vector subs; - this->node->TopicInfo("/world/" + this->worldName + "/stats", pubs, subs); - if (!pubs.empty()) - { - gzerr << "Another world of the same name is running" << std::endl; - } + // std::vector pubs; + // std::vector subs; + // this->node->TopicInfo("/world/" + this->worldName + "/stats", pubs, subs); + // if (!pubs.empty()) + // { + // gzerr << "Another world of the same name is running" << std::endl; + // } // Create the system manager this->systemMgr = std::make_unique( diff --git a/src/systems/entity_semantics/EntitySemantics.cc b/src/systems/entity_semantics/EntitySemantics.cc index cbf4110b47..03853c628b 100644 --- a/src/systems/entity_semantics/EntitySemantics.cc +++ b/src/systems/entity_semantics/EntitySemantics.cc @@ -32,6 +32,7 @@ #include "gz/sim/components/Model.hh" #include "gz/sim/components/Name.hh" #include "gz/sim/components/SemanticCategory.hh" +#include "gz/sim/components/SemanticDescription.hh" #include "gz/sim/components/SemanticTag.hh" using namespace gz; @@ -71,7 +72,10 @@ void EntitySemantics::PreUpdate(const UpdateInfo &, { GZ_PROFILE("EntitySemantics::PreUpdate"); + // Store entities temporarily since it's not safe to add components in an + // `EachNew` loop. std::map entitiesToCategorize; + std::map entitiesToDescribe; std::map> entitiesToTag; _ecm.EachNew( [&](const Entity &_entity, const components::Name *_name, @@ -106,6 +110,29 @@ void EntitySemantics::PreUpdate(const UpdateInfo &, } } + auto descriptionElem = semanticsElem->FindElement("description"); + if (descriptionElem) + { + auto description = descriptionElem->Get(); + if (!description.empty()) + { + entitiesToDescribe.emplace(_entity, description); + } + else + { + gzerr << " cannot be an empty string" + << "] in model [" << _name->Data() << "]\n"; + } + + if (descriptionElem->GetNextElement("description")) + { + // TODO(azeey) Include file and line number in error message. + gzerr << "There should be only one element for an " + "entity in model [" + << _name->Data() << "]\n"; + } + } + for (auto tagElem = semanticsElem->FindElement("tag"); tagElem; tagElem = tagElem->GetNextElement("tag")) { @@ -129,6 +156,11 @@ void EntitySemantics::PreUpdate(const UpdateInfo &, _ecm.SetComponentData(entity, category); } + for (const auto &[entity, description] : entitiesToDescribe) + { + _ecm.SetComponentData(entity, description); + } + for (const auto &[entity, tags] : entitiesToTag) { _ecm.SetComponentData(entity, tags); diff --git a/src/systems/entity_semantics/EntitySemantics.hh b/src/systems/entity_semantics/EntitySemantics.hh index d0070cf746..85a11a5a28 100644 --- a/src/systems/entity_semantics/EntitySemantics.hh +++ b/src/systems/entity_semantics/EntitySemantics.hh @@ -18,7 +18,6 @@ #define GZ_SIM_SYSTEMS_ENTITYSEMANTICS_HH_ #include -#include namespace gz { @@ -35,8 +34,9 @@ class EntitySemanticsPrivate; /// \brief System for assigning semantics to entities, such as models, /// by classifying them into categories and assigning them tags. /// Reference: -/// https://github.com/ros-simulation/simulation_interfaces/blob/1.0.0/msg/EntityCategory.msg, -/// https://github.com/ros-simulation/simulation_interfaces/blob/1.0.0/msg/TagsFilter.msg +/// https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/EntityInfo.msg, +/// https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/EntityCategory.msg, +/// https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/TagsFilter.msg /// /// ## System Parameters /// @@ -61,6 +61,8 @@ class EntitySemanticsPrivate; /// in on top of categories. Multiple instances of the `` elements are /// allowed for an entity. /// +/// - ``: freeform string that describes the entity. There can +/// only be one instance of the `` element for an entity. /// /// Example: /// @@ -70,6 +72,7 @@ class EntitySemanticsPrivate; /// ROBOT /// mobile /// diff_drive +/// Food delivery mobile robot /// /// /// diff --git a/test/integration/entity_semantics_system.cc b/test/integration/entity_semantics_system.cc index c105f2605f..37f1d5afb9 100644 --- a/test/integration/entity_semantics_system.cc +++ b/test/integration/entity_semantics_system.cc @@ -23,8 +23,8 @@ #include "../helpers/EnvTestFixture.hh" #include "gz/sim/TestFixture.hh" -#include "gz/sim/components/Name.hh" #include "gz/sim/components/SemanticCategory.hh" +#include "gz/sim/components/SemanticDescription.hh" #include "gz/sim/components/SemanticTag.hh" #include "test_config.hh" @@ -53,6 +53,11 @@ TEST_F(EntitySemanticsTest, CanSetCategoriesAndTags) auto robots = _ecm.EntitiesByComponents(components::SemanticCategory(1)); ASSERT_EQ(1u, robots.size()); + auto robotDescription = + _ecm.ComponentData(robots[0]); + ASSERT_TRUE(robotDescription .has_value()); + EXPECT_EQ("Food delivery mobile robot", *robotDescription); + auto robotTags = _ecm.ComponentData(robots[0]); ASSERT_TRUE(robotTags.has_value()); ASSERT_EQ(2u, robotTags->size()); diff --git a/test/worlds/entity_semantics.sdf b/test/worlds/entity_semantics.sdf index f1729614df..c68b7e7a04 100644 --- a/test/worlds/entity_semantics.sdf +++ b/test/worlds/entity_semantics.sdf @@ -54,6 +54,7 @@ ROBOT + Food delivery mobile robot mobile diff_drive From eb8880bebac8ea0b58df166e9d98730eb52c17b1 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 31 Jul 2025 17:09:46 -0500 Subject: [PATCH 03/13] Convert type for tags to std::vector to match ROS definition Signed-off-by: Addisu Z. Taddese --- include/gz/sim/components/SemanticTag.hh | 10 +++++----- src/systems/entity_semantics/EntitySemantics.cc | 4 ++-- test/integration/entity_semantics_system.cc | 4 ++-- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/include/gz/sim/components/SemanticTag.hh b/include/gz/sim/components/SemanticTag.hh index f178e3c977..9213115040 100644 --- a/include/gz/sim/components/SemanticTag.hh +++ b/include/gz/sim/components/SemanticTag.hh @@ -19,8 +19,8 @@ #include -#include #include +#include #include "gz/sim/components/Component.hh" #include "gz/sim/components/Factory.hh" @@ -39,7 +39,7 @@ class SemanticTagSerializer { public: static std::ostream &Serialize(std::ostream &_out, - const std::set &_tags) + const std::vector &_tags) { msgs::StringMsg_V msg; for (const auto &tag : _tags) @@ -52,13 +52,13 @@ class SemanticTagSerializer public: static std::istream &Deserialize(std::istream &_in, - std::set &_tags) + std::vector &_tags) { msgs::StringMsg_V msg; msg.ParsePartialFromIstream(&_in); for (const auto &item : msg.data()) { - _tags.insert(item); + _tags.push_back(item); } return _in; } @@ -72,7 +72,7 @@ namespace components /// meant to be used by systems such as EntitySemantics to assign tags to /// entities. See /// https://github.com/ros-simulation/simulation_interfaces/blob/1.0.0/msg/TagsFilter.msg -using SemanticTag = Component, class SemanticTagTag, +using SemanticTag = Component, class SemanticTagTag, serializers::SemanticTagSerializer>; GZ_SIM_REGISTER_COMPONENT("gz_sim_components.SemanticTag", SemanticTag) } // namespace components diff --git a/src/systems/entity_semantics/EntitySemantics.cc b/src/systems/entity_semantics/EntitySemantics.cc index 03853c628b..0220056762 100644 --- a/src/systems/entity_semantics/EntitySemantics.cc +++ b/src/systems/entity_semantics/EntitySemantics.cc @@ -76,7 +76,7 @@ void EntitySemantics::PreUpdate(const UpdateInfo &, // `EachNew` loop. std::map entitiesToCategorize; std::map entitiesToDescribe; - std::map> entitiesToTag; + std::map> entitiesToTag; _ecm.EachNew( [&](const Entity &_entity, const components::Name *_name, const components::ModelSdf *_modelSdf) @@ -144,7 +144,7 @@ void EntitySemantics::PreUpdate(const UpdateInfo &, } else { - entitiesToTag[_entity].insert(tagStr); + entitiesToTag[_entity].push_back(tagStr); } } } diff --git a/test/integration/entity_semantics_system.cc b/test/integration/entity_semantics_system.cc index 37f1d5afb9..8e2aeb989b 100644 --- a/test/integration/entity_semantics_system.cc +++ b/test/integration/entity_semantics_system.cc @@ -61,8 +61,8 @@ TEST_F(EntitySemanticsTest, CanSetCategoriesAndTags) auto robotTags = _ecm.ComponentData(robots[0]); ASSERT_TRUE(robotTags.has_value()); ASSERT_EQ(2u, robotTags->size()); - EXPECT_EQ(1u, robotTags->count("mobile")); - EXPECT_EQ(1u, robotTags->count("diff_drive")); + EXPECT_EQ("mobile", (*robotTags)[0]); + EXPECT_EQ("diff_drive", (*robotTags)[1]); auto dynamicObjects = _ecm.EntitiesByComponents(components::SemanticCategory(4)); From 282cd953b73f0046951f8cb7b7060486eb2dabb3 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 31 Jul 2025 19:43:37 -0500 Subject: [PATCH 04/13] Undo unintended change Signed-off-by: Addisu Z. Taddese --- src/SimulationRunner.cc | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index e51d97c379..34dac922d2 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -197,13 +197,13 @@ SimulationRunner::SimulationRunner(const sdf::World &_world, this->node = std::make_unique(opts); - // std::vector pubs; - // std::vector subs; - // this->node->TopicInfo("/world/" + this->worldName + "/stats", pubs, subs); - // if (!pubs.empty()) - // { - // gzerr << "Another world of the same name is running" << std::endl; - // } + std::vector pubs; + std::vector subs; + this->node->TopicInfo("/world/" + this->worldName + "/stats", pubs, subs); + if (!pubs.empty()) + { + gzerr << "Another world of the same name is running" << std::endl; + } // Create the system manager this->systemMgr = std::make_unique( From 175b916a0900ab43a6dfbaebd3b5ced7797f6486 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 31 Jul 2025 23:10:33 -0500 Subject: [PATCH 05/13] Fix serialization Signed-off-by: Addisu Z. Taddese --- .../gz/sim/components/SemanticDescription.hh | 30 +++++++++++++++++-- 1 file changed, 28 insertions(+), 2 deletions(-) diff --git a/include/gz/sim/components/SemanticDescription.hh b/include/gz/sim/components/SemanticDescription.hh index 9b5b388084..3abd46c994 100644 --- a/include/gz/sim/components/SemanticDescription.hh +++ b/include/gz/sim/components/SemanticDescription.hh @@ -17,10 +17,13 @@ #ifndef GZ_SIM_COMPONENTS_SEMANTIC_DESCRIPTION_HH_ #define GZ_SIM_COMPONENTS_SEMANTIC_DESCRIPTION_HH_ +#include + #include #include "gz/sim/components/Component.hh" #include "gz/sim/components/Factory.hh" +#include "gz/sim/components/Serialization.hh" #include "gz/sim/config.hh" namespace gz @@ -28,7 +31,29 @@ namespace gz namespace sim { // Inline bracket to help doxygen filtering. -inline namespace GZ_SIM_VERSION_NAMESPACE { +inline namespace GZ_SIM_VERSION_NAMESPACE +{ +namespace serializers +{ +class SemanticDescriptionSerializer +{ + public: static std::ostream &Serialize(std::ostream &_out, const std::string &_desc) + { + msgs::StringMsg msg; + msg.set_data(_desc); + msg.SerializeToOstream(&_out); + return _out; + } + + public: static std::istream &Deserialize(std::istream &_in, std::string &_desc) + { + msgs::StringMsg msg; + msg.ParsePartialFromIstream(&_in); + _desc = msg.data(); + return _in; + } +}; +} // namespace serializers namespace components { /// \brief A component that holds the semantic description of an entity. This is @@ -36,7 +61,8 @@ namespace components /// to entities. See /// https://github.com/ros-simulation/simulation_interfaces/blob/1.0.0/msg/EntityInfo.msg using SemanticDescription = - Component; + Component; GZ_SIM_REGISTER_COMPONENT("gz_sim_components.SemanticDescription", SemanticDescription) } // namespace components From c31da6a7cc11da9b0cfa519e16ea6ef64ed14448 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 31 Jul 2025 23:10:57 -0500 Subject: [PATCH 06/13] Rename SemanticTag to SemanticTags Signed-off-by: Addisu Z. Taddese --- .../components/{SemanticTag.hh => SemanticTags.hh} | 13 +++++++------ src/systems/entity_semantics/EntitySemantics.cc | 4 ++-- test/integration/entity_semantics_system.cc | 4 ++-- 3 files changed, 11 insertions(+), 10 deletions(-) rename include/gz/sim/components/{SemanticTag.hh => SemanticTags.hh} (84%) diff --git a/include/gz/sim/components/SemanticTag.hh b/include/gz/sim/components/SemanticTags.hh similarity index 84% rename from include/gz/sim/components/SemanticTag.hh rename to include/gz/sim/components/SemanticTags.hh index 9213115040..e0a070822e 100644 --- a/include/gz/sim/components/SemanticTag.hh +++ b/include/gz/sim/components/SemanticTags.hh @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef GZ_SIM_COMPONENTS_SEMANTIC_TAG_HH_ -#define GZ_SIM_COMPONENTS_SEMANTIC_TAG_HH_ +#ifndef GZ_SIM_COMPONENTS_SEMANTIC_TAGS_HH_ +#define GZ_SIM_COMPONENTS_SEMANTIC_TAGS_HH_ #include @@ -35,7 +35,7 @@ inline namespace GZ_SIM_VERSION_NAMESPACE { namespace serializers { -class SemanticTagSerializer +class SemanticTagsSerializer { public: static std::ostream &Serialize(std::ostream &_out, @@ -56,6 +56,7 @@ class SemanticTagSerializer { msgs::StringMsg_V msg; msg.ParsePartialFromIstream(&_in); + _tags.clear(); for (const auto &item : msg.data()) { _tags.push_back(item); @@ -72,9 +73,9 @@ namespace components /// meant to be used by systems such as EntitySemantics to assign tags to /// entities. See /// https://github.com/ros-simulation/simulation_interfaces/blob/1.0.0/msg/TagsFilter.msg -using SemanticTag = Component, class SemanticTagTag, - serializers::SemanticTagSerializer>; -GZ_SIM_REGISTER_COMPONENT("gz_sim_components.SemanticTag", SemanticTag) +using SemanticTags = Component, class SemanticTagsTag, + serializers::SemanticTagsSerializer>; +GZ_SIM_REGISTER_COMPONENT("gz_sim_components.SemanticTags", SemanticTags) } // namespace components } // namespace GZ_SIM_VERSION_NAMESPACE } // namespace sim diff --git a/src/systems/entity_semantics/EntitySemantics.cc b/src/systems/entity_semantics/EntitySemantics.cc index 0220056762..cabb3ee9ee 100644 --- a/src/systems/entity_semantics/EntitySemantics.cc +++ b/src/systems/entity_semantics/EntitySemantics.cc @@ -33,7 +33,7 @@ #include "gz/sim/components/Name.hh" #include "gz/sim/components/SemanticCategory.hh" #include "gz/sim/components/SemanticDescription.hh" -#include "gz/sim/components/SemanticTag.hh" +#include "gz/sim/components/SemanticTags.hh" using namespace gz; using namespace sim; @@ -163,7 +163,7 @@ void EntitySemantics::PreUpdate(const UpdateInfo &, for (const auto &[entity, tags] : entitiesToTag) { - _ecm.SetComponentData(entity, tags); + _ecm.SetComponentData(entity, tags); } } diff --git a/test/integration/entity_semantics_system.cc b/test/integration/entity_semantics_system.cc index 8e2aeb989b..f0b841a2a0 100644 --- a/test/integration/entity_semantics_system.cc +++ b/test/integration/entity_semantics_system.cc @@ -25,7 +25,7 @@ #include "gz/sim/TestFixture.hh" #include "gz/sim/components/SemanticCategory.hh" #include "gz/sim/components/SemanticDescription.hh" -#include "gz/sim/components/SemanticTag.hh" +#include "gz/sim/components/SemanticTags.hh" #include "test_config.hh" using namespace gz; @@ -58,7 +58,7 @@ TEST_F(EntitySemanticsTest, CanSetCategoriesAndTags) ASSERT_TRUE(robotDescription .has_value()); EXPECT_EQ("Food delivery mobile robot", *robotDescription); - auto robotTags = _ecm.ComponentData(robots[0]); + auto robotTags = _ecm.ComponentData(robots[0]); ASSERT_TRUE(robotTags.has_value()); ASSERT_EQ(2u, robotTags->size()); EXPECT_EQ("mobile", (*robotTags)[0]); From a25a73077c9d3c705eefc5835950134f9c520ca5 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 31 Jul 2025 23:12:31 -0500 Subject: [PATCH 07/13] Fix linter issues Signed-off-by: Addisu Z. Taddese --- include/gz/sim/components/SemanticDescription.hh | 11 ++++++----- src/systems/entity_semantics/EntitySemantics.cc | 2 +- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/include/gz/sim/components/SemanticDescription.hh b/include/gz/sim/components/SemanticDescription.hh index 3abd46c994..1499511e5d 100644 --- a/include/gz/sim/components/SemanticDescription.hh +++ b/include/gz/sim/components/SemanticDescription.hh @@ -37,7 +37,8 @@ namespace serializers { class SemanticDescriptionSerializer { - public: static std::ostream &Serialize(std::ostream &_out, const std::string &_desc) + public: + static std::ostream &Serialize(std::ostream &_out, const std::string &_desc) { msgs::StringMsg msg; msg.set_data(_desc); @@ -45,7 +46,8 @@ class SemanticDescriptionSerializer return _out; } - public: static std::istream &Deserialize(std::istream &_in, std::string &_desc) + public: + static std::istream &Deserialize(std::istream &_in, std::string &_desc) { msgs::StringMsg msg; msg.ParsePartialFromIstream(&_in); @@ -60,9 +62,8 @@ namespace components /// meant to be used by systems such as EntitySemantics to assign descriptions /// to entities. See /// https://github.com/ros-simulation/simulation_interfaces/blob/1.0.0/msg/EntityInfo.msg -using SemanticDescription = - Component; +using SemanticDescription = Component; GZ_SIM_REGISTER_COMPONENT("gz_sim_components.SemanticDescription", SemanticDescription) } // namespace components diff --git a/src/systems/entity_semantics/EntitySemantics.cc b/src/systems/entity_semantics/EntitySemantics.cc index cabb3ee9ee..16d9609511 100644 --- a/src/systems/entity_semantics/EntitySemantics.cc +++ b/src/systems/entity_semantics/EntitySemantics.cc @@ -26,8 +26,8 @@ #include #include #include -#include #include +#include #include "gz/sim/components/Model.hh" #include "gz/sim/components/Name.hh" From a883b75b518b82841c8995b9b4b8ce1287e74a54 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 18 Aug 2025 15:17:26 -0500 Subject: [PATCH 08/13] Address reviewer comments Signed-off-by: Addisu Z. Taddese --- include/gz/sim/components/SemanticCategory.hh | 2 ++ include/gz/sim/components/SemanticDescription.hh | 2 ++ include/gz/sim/components/SemanticTags.hh | 2 ++ src/systems/entity_semantics/EntitySemantics.cc | 15 ++++++++------- test/integration/entity_semantics_system.cc | 11 +++++++---- 5 files changed, 21 insertions(+), 11 deletions(-) diff --git a/include/gz/sim/components/SemanticCategory.hh b/include/gz/sim/components/SemanticCategory.hh index 165e9ed943..235854dfc1 100644 --- a/include/gz/sim/components/SemanticCategory.hh +++ b/include/gz/sim/components/SemanticCategory.hh @@ -17,6 +17,8 @@ #ifndef GZ_SIM_COMPONENTS_SEMANTIC_CATEGORY_HH_ #define GZ_SIM_COMPONENTS_SEMANTIC_CATEGORY_HH_ +#include + #include "gz/sim/components/Component.hh" #include "gz/sim/components/Factory.hh" #include "gz/sim/config.hh" diff --git a/include/gz/sim/components/SemanticDescription.hh b/include/gz/sim/components/SemanticDescription.hh index 1499511e5d..b16a5e6743 100644 --- a/include/gz/sim/components/SemanticDescription.hh +++ b/include/gz/sim/components/SemanticDescription.hh @@ -19,6 +19,8 @@ #include +#include +#include #include #include "gz/sim/components/Component.hh" diff --git a/include/gz/sim/components/SemanticTags.hh b/include/gz/sim/components/SemanticTags.hh index e0a070822e..24effb3d8f 100644 --- a/include/gz/sim/components/SemanticTags.hh +++ b/include/gz/sim/components/SemanticTags.hh @@ -19,6 +19,8 @@ #include +#include +#include #include #include diff --git a/src/systems/entity_semantics/EntitySemantics.cc b/src/systems/entity_semantics/EntitySemantics.cc index 16d9609511..08a5821b2b 100644 --- a/src/systems/entity_semantics/EntitySemantics.cc +++ b/src/systems/entity_semantics/EntitySemantics.cc @@ -21,24 +21,22 @@ #include #include #include -#include -#include -#include #include #include #include +#include #include +#include +#include +#include + #include "gz/sim/components/Model.hh" #include "gz/sim/components/Name.hh" #include "gz/sim/components/SemanticCategory.hh" #include "gz/sim/components/SemanticDescription.hh" #include "gz/sim/components/SemanticTags.hh" -using namespace gz; -using namespace sim; -using namespace systems; - namespace { @@ -66,6 +64,8 @@ std::optional ConvertCategoryStrToInt(const std::string &_category) } } // namespace +namespace gz::sim::systems +{ ////////////////////////////////////////////////// void EntitySemantics::PreUpdate(const UpdateInfo &, EntityComponentManager &_ecm) @@ -170,3 +170,4 @@ void EntitySemantics::PreUpdate(const UpdateInfo &, GZ_ADD_PLUGIN(EntitySemantics, System, EntitySemantics::ISystemPreUpdate) GZ_ADD_PLUGIN_ALIAS(EntitySemantics, "gz::sim::systems::EntitySemantics") +} // namespace gz::sim::systems diff --git a/test/integration/entity_semantics_system.cc b/test/integration/entity_semantics_system.cc index f0b841a2a0..4ba7c8cb8a 100644 --- a/test/integration/entity_semantics_system.cc +++ b/test/integration/entity_semantics_system.cc @@ -17,6 +17,8 @@ #include +#include + #include #include #include @@ -28,9 +30,10 @@ #include "gz/sim/components/SemanticTags.hh" #include "test_config.hh" -using namespace gz; -using namespace sim; -using namespace std::chrono_literals; +namespace common = gz::common; +namespace sim = gz::sim; +namespace components = gz::sim::components; +using gz::sim::TestFixture; /// \brief Test EntitySemantics system class EntitySemanticsTest : public InternalFixture<::testing::Test> @@ -48,7 +51,7 @@ TEST_F(EntitySemanticsTest, CanSetCategoriesAndTags) // Put the checks in an extra lambda as opposed to doing it OnPostUpdate so we // can call ASSERT_* - auto runChecks = [](const sim::EntityComponentManager &_ecm) + auto runChecks = [](const gz::sim::EntityComponentManager &_ecm) { auto robots = _ecm.EntitiesByComponents(components::SemanticCategory(1)); ASSERT_EQ(1u, robots.size()); From fec4f3c2008724a800612589102707d3e32281ce Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 18 Aug 2025 15:01:02 -0500 Subject: [PATCH 09/13] Expose APIs for clearing the ECMs internal state tracking addition and removal (#3002) This is necessary for use with the `SetState` API. This API is commonly used to create a local version of the servers ECM that is periodically synchronized. However, without exposing this function into our public API additions and removals of entities or components are not reflected in the local ECM. Currently, we use `friend` relationships for the `GuiRunner` class to allow it to run these functions, but this is not feasible for external code that wants to use `SetState`. https://github.com/gazebosim/gz-sim/blob/5ce62a8826b930078fae940288ac9677aae9ec7b/include/gz/sim/EntityComponentManager.hh#L834, https://github.com/gazebosim/gz-sim/blob/5ce62a8826b930078fae940288ac9677aae9ec7b/src/gui/GuiRunner.cc#L316-L318 Signed-off-by: Addisu Z. Taddese --- include/gz/sim/EntityComponentManager.hh | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/include/gz/sim/EntityComponentManager.hh b/include/gz/sim/EntityComponentManager.hh index 61e420cb48..12b8a68f67 100644 --- a/include/gz/sim/EntityComponentManager.hh +++ b/include/gz/sim/EntityComponentManager.hh @@ -684,21 +684,19 @@ namespace gz const std::string &_name) const; /// \brief Clear the list of newly added entities so that a call to - /// EachAdded after this will have no entities to iterate. This function - /// is protected to facilitate testing. - protected: void ClearNewlyCreatedEntities(); + /// EachAdded after this will have no entities to iterate. + public: void ClearNewlyCreatedEntities(); /// \brief Clear the list of removed components so that a call to /// RemoveComponent doesn't make the list grow indefinitely. - protected: void ClearRemovedComponents(); + public: void ClearRemovedComponents(); /// \brief Process all entity remove requests. This will remove - /// entities and their components. This function is protected to - /// facilitate testing. - protected: void ProcessRemoveEntityRequests(); + /// entities and their components. + public: void ProcessRemoveEntityRequests(); /// \brief Mark all components as not changed. - protected: void SetAllComponentsUnchanged(); + public: void SetAllComponentsUnchanged(); /// Compute the diff between this EntityComponentManager and _other at the /// entity level. This does not compute the diff between components of an From 6cc2ac6a2d60f6b56137801a98dbdf487f82a78f Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 20 Aug 2025 22:54:46 -0500 Subject: [PATCH 10/13] Fix windows warning Signed-off-by: Addisu Z. Taddese --- src/systems/entity_semantics/EntitySemantics.cc | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/systems/entity_semantics/EntitySemantics.cc b/src/systems/entity_semantics/EntitySemantics.cc index 08a5821b2b..555076126d 100644 --- a/src/systems/entity_semantics/EntitySemantics.cc +++ b/src/systems/entity_semantics/EntitySemantics.cc @@ -64,7 +64,11 @@ std::optional ConvertCategoryStrToInt(const std::string &_category) } } // namespace -namespace gz::sim::systems +namespace gz +{ +namespace sim +{ +namespace systems { ////////////////////////////////////////////////// void EntitySemantics::PreUpdate(const UpdateInfo &, @@ -170,4 +174,6 @@ void EntitySemantics::PreUpdate(const UpdateInfo &, GZ_ADD_PLUGIN(EntitySemantics, System, EntitySemantics::ISystemPreUpdate) GZ_ADD_PLUGIN_ALIAS(EntitySemantics, "gz::sim::systems::EntitySemantics") -} // namespace gz::sim::systems +} // namespace systems +} // namespace sim +} // namespace gz From 9097a0003d476b8f97eea06ac527131e881148f6 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 20 Aug 2025 22:54:55 -0500 Subject: [PATCH 11/13] Add bazel rule for plugin Signed-off-by: Addisu Z. Taddese Co-authored-by: Ian Chen --- BUILD.bazel | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/BUILD.bazel b/BUILD.bazel index 5fbd1714df..a09fe2810b 100644 --- a/BUILD.bazel +++ b/BUILD.bazel @@ -701,6 +701,26 @@ gz_sim_system_libraries( # ], # ) +gz_sim_system_libraries( + srcs = glob( + [ + "src/systems/entity_semantics/**/*.cc", + "src/systems/entity_semantics/**/*.hh", + ], + ), + so_lib_name = "gz-sim-entity-semantics-system.so", + static_lib_name = "gz-sim-entity-semantics-system-static", + visibility = ["//visibility:public"], + deps = [ + ":gz-sim", + "@gz-common", + "@gz-common//profiler", + "@gz-plugin//:register", + "@gz-transport", + "@sdformat", + ], +) + gz_sim_system_libraries( srcs = glob( [ From bccf3d3c3944d86e1a9e3b0c9acd81d171eacd29 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 21 Aug 2025 12:28:26 -0500 Subject: [PATCH 12/13] Fix naemspace issue on windows Signed-off-by: Addisu Z. Taddese --- src/systems/entity_semantics/EntitySemantics.cc | 11 +++++------ src/systems/entity_semantics/EntitySemantics.hh | 3 +++ 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/systems/entity_semantics/EntitySemantics.cc b/src/systems/entity_semantics/EntitySemantics.cc index 555076126d..1786444fc7 100644 --- a/src/systems/entity_semantics/EntitySemantics.cc +++ b/src/systems/entity_semantics/EntitySemantics.cc @@ -64,10 +64,9 @@ std::optional ConvertCategoryStrToInt(const std::string &_category) } } // namespace -namespace gz -{ -namespace sim +namespace gz::sim { +inline namespace GZ_SIM_VERSION_NAMESPACE { namespace systems { ////////////////////////////////////////////////// @@ -174,6 +173,6 @@ void EntitySemantics::PreUpdate(const UpdateInfo &, GZ_ADD_PLUGIN(EntitySemantics, System, EntitySemantics::ISystemPreUpdate) GZ_ADD_PLUGIN_ALIAS(EntitySemantics, "gz::sim::systems::EntitySemantics") -} // namespace systems -} // namespace sim -} // namespace gz +} // namespace systems +} // inline namespace +} // namespace gz::sim diff --git a/src/systems/entity_semantics/EntitySemantics.hh b/src/systems/entity_semantics/EntitySemantics.hh index 85a11a5a28..99f0f98d81 100644 --- a/src/systems/entity_semantics/EntitySemantics.hh +++ b/src/systems/entity_semantics/EntitySemantics.hh @@ -19,6 +19,9 @@ #include +#include +#include + namespace gz { namespace sim From 8fcbd8b13cb1d506d27af1fbd162df08fe76b4cc Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 21 Aug 2025 15:50:57 -0500 Subject: [PATCH 13/13] Fix cpplint Signed-off-by: Addisu Z. Taddese --- src/systems/entity_semantics/EntitySemantics.hh | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/systems/entity_semantics/EntitySemantics.hh b/src/systems/entity_semantics/EntitySemantics.hh index 99f0f98d81..4c56ea16f2 100644 --- a/src/systems/entity_semantics/EntitySemantics.hh +++ b/src/systems/entity_semantics/EntitySemantics.hh @@ -18,9 +18,7 @@ #define GZ_SIM_SYSTEMS_ENTITYSEMANTICS_HH_ #include - #include -#include namespace gz {