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_SEMANTIC_CATEGORY_HH_
+#define GZ_SIM_COMPONENTS_SEMANTIC_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/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 <string>
+
+#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<std::string, class SemanticDescriptionTag>;
+GZ_SIM_REGISTER_COMPONENT("gz_sim_components.SemanticDescription",
+                          SemanticDescription)
+}  // namespace components
+}  // namespace GZ_SIM_VERSION_NAMESPACE
+}  // namespace sim
+}  // namespace gz
+#endif

include/gz/sim/components/SemanticTag.hh

@@ -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.
+ *
+ */
+#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/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/SimulationRunner.cc

@@ -192,13 +192,13 @@ SimulationRunner::SimulationRunner(const sdf::World &_world,
 
   this->node = std::make_unique<transport::Node>(opts);
 
-  std::vector<transport::MessagePublisher> pubs;
-  std::vector<transport::MessagePublisher> 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<transport::MessagePublisher> pubs;
+  // std::vector<transport::MessagePublisher> 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<SystemManager>(

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,172 @@
+/*
+ * 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/SemanticDescription.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");
+
+  // Store entities temporarily since it's not safe to add components in an
+  // `EachNew` loop.
+  std::map<Entity, uint8_t> entitiesToCategorize;
+  std::map<Entity, std::string> entitiesToDescribe;
+  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";
+            }
+          }
+
+          auto descriptionElem = semanticsElem->FindElement("description");
+          if (descriptionElem)
+          {
+            auto description = descriptionElem->Get<std::string>();
+            if (!description.empty())
+            {
+              entitiesToDescribe.emplace(_entity, description);
+            }
+            else
+            {
+              gzerr << "<description> 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 <description> 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, description] : entitiesToDescribe)
+  {
+    _ecm.SetComponentData<components::SemanticDescription>(entity, description);
+  }
+
+  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")