Add EntitySemantics system that enables setting categories and tags

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<uint8_t, class SemanticCategoryTag>;
+GZ_SIM_REGISTER_COMPONENT("gz_sim_components.SemanticCategory",
+                          SemanticCategory)
+}  // namespace components
+}  // namespace GZ_SIM_VERSION_NAMESPACE
+}  // namespace sim
+}  // namespace gz
+#endif

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 <gz/msgs/stringmsg_v.pb.h>
+
+#include <set>
+#include <string>
+
+#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<std::string> &_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<std::string> &_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<std::set<std::string>, 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

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)

src/systems/entity_semantics/CMakeLists.txt

@@ -0,0 +1,4 @@
+gz_add_system(entity-semantics
+  SOURCES
+    EntitySemantics.cc
+)

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 <algorithm>
+#include <array>
+#include <cctype>
+#include <cstdint>
+#include <gz/common/Profiler.hh>
+#include <gz/plugin/Register.hh>
+#include <gz/transport/Node.hh>
+#include <map>
+#include <optional>
+#include <set>
+#include <string>
+
+#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<uint8_t> 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<Entity, uint8_t> entitiesToCategorize;
+  std::map<Entity, std::set<std::string>> entitiesToTag;
+  _ecm.EachNew<components::Name, components::ModelSdf>(
+      [&](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<std::string>();
+            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 <category> element for an "
+                       "entity in model ["
+                    << _name->Data() << "]\n";
+            }
+          }
+
+          for (auto tagElem = semanticsElem->FindElement("tag"); tagElem;
+               tagElem = tagElem->GetNextElement("tag"))
+          {
+            auto tagStr = tagElem->Get<std::string>();
+            if (tagStr.empty())
+            {
+              gzerr << "<tag> 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<components::SemanticCategory>(entity, category);
+  }
+
+  for (const auto &[entity, tags] : entitiesToTag)
+  {
+    _ecm.SetComponentData<components::SemanticTag>(entity, tags);
+  }
+}
+
+GZ_ADD_PLUGIN(EntitySemantics, System, EntitySemantics::ISystemPreUpdate)
+
+GZ_ADD_PLUGIN_ALIAS(EntitySemantics, "gz::sim::systems::EntitySemantics")

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 <gz/sim/System.hh>
+#include <memory>
+
+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 `<world>`, but it parses parameters
+/// found in entities such as models.
+///
+/// ## Entity Parameters
+///
+/// - `<gz:semantics>`: Root element to be specified inside entities, e.g.
+/// <model>. It has the following child parameters:
+///
+///   - `<category>`: 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 `<category>ROBOT</category>`. Lowercase
+///   values will also be accepted. There can only be one instance of the
+///   `<category>` element for an entity.
+///
+///   - `<tag>`: 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 `<tag>` elements are
+///   allowed for an entity.
+///
+///
+/// Example:
+///
+/// ```xml
+/// <model name="robot1">
+///   <gz:semantics>
+///     <category>ROBOT</category>
+///     <tag>mobile</tag>
+///     <tag>diff_drive</tag>
+///   </gz:semantics>
+/// </model>
+///
+///
+/// ```
+///
+
+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

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