Allow zero or more key/value pairs to be added to detection header in…

…formation

Signed-off-by: Nate Koenig <[email protected]>

src/systems/performer_detector/PerformerDetector.cc

@@ -82,6 +82,22 @@ void PerformerDetector::Configure(const Entity &_entity,
     this->poseOffset = sdfClone->Get<math::Pose3d>("pose");
   }
 
+  // Load optional header data.
+  if (sdfClone->HasElement("header_data"))
+  {
+    auto headerElem = sdfClone->GetElement("header_data");
+    while (headerElem)
+    {
+      std::string key = headerElem->Get<std::string>("key", "").first;
+      std::string value = headerElem->Get<std::string>("value", "").first;
+      if (!key.empty() || !value.empty())
+      {
+        this->extraHeaderData[key] = value;
+      }
+      headerElem = headerElem->GetNextElement("header_data");
+    }
+  }
+
   std::string defaultTopic{"/model/" + this->model.Name(_ecm) +
                              "/performer_detector/status"};
   auto topic = _sdf->Get<std::string>("topic", defaultTopic).first;
@@ -201,6 +217,14 @@ void PerformerDetector::Publish(
     headerData->add_value(std::to_string(_state));
   }
 
+  // Include the optional extra header data.
+  for (const auto &data : this->extraHeaderData)
+  {
+    auto *headerData = msg.mutable_header()->add_data();
+    headerData->set_key(data.first);
+    headerData->add_value(data.second);
+  }
+
   this->pub.Publish(msg);
 }
 

src/systems/performer_detector/PerformerDetector.hh

@@ -18,6 +18,7 @@
 #ifndef IGNITION_GAZEBO_SYSTEMS_PERFORMERDETECTOR_HH_
 #define IGNITION_GAZEBO_SYSTEMS_PERFORMERDETECTOR_HH_
 
+#include <map>
 #include <memory>
 #include <string>
 #include <unordered_set>
@@ -70,6 +71,11 @@ namespace systems
   /// `<pose>`: Additional pose offset relative to the parent model's pose.
   /// This pose is added to the parent model pose when computing the
   /// detection region. Only the position component of the `<pose>` is used.
+  /// `<header_data>`: Zero or more key-value pairs that will be
+  /// included in the header of the detection messages. A `<header_data>`
+  /// element should have child `<key>` and/or `<value>` elements whose
+  /// contents are interpreted as strings. Keys value pairs are stored in a
+  /// map, which means the keys are unique.
 
   class IGNITION_GAZEBO_VISIBLE PerformerDetector
       : public System,
@@ -134,6 +140,9 @@ namespace systems
 
     /// \brief Additional pose offset for the plugin.
     private: math::Pose3d poseOffset;
+
+    /// \brief Optional extra header data.
+    private: std::map<std::string, std::string> extraHeaderData;
   };
 
   }