From 654cdbae861c629db38a66d7ab053de7f07e4a7d Mon Sep 17 00:00:00 2001
From: Mahmoud Almasri <mahm.al.masri@gmail.com>
Date: Wed, 19 Feb 2025 14:52:46 +0100
Subject: [PATCH] Allow editing the values in status_item

* Add new constructor that takes an extra vector of KeyValue

* Introduce the addValue() method to StatusItem.

* Refactor addValue and hasKey and move them to the cpp

Signed-off-by: Mahmoud Almasri <mahm.al.masri@gmail.com>
---
 .../diagnostic_aggregator/status_item.hpp     | 39 +++++++++++++-----
 diagnostic_aggregator/src/status_item.cpp     | 40 +++++++++++++++++++
 2 files changed, 69 insertions(+), 10 deletions(-)

diff --git a/diagnostic_aggregator/include/diagnostic_aggregator/status_item.hpp b/diagnostic_aggregator/include/diagnostic_aggregator/status_item.hpp
index e11dad5d6..19e5b25a5 100644
--- a/diagnostic_aggregator/include/diagnostic_aggregator/status_item.hpp
+++ b/diagnostic_aggregator/include/diagnostic_aggregator/status_item.hpp
@@ -197,6 +197,14 @@ class StatusItem
     const std::string item_name, const std::string message = "Missing",
     const DiagnosticLevel level = Level_Stale);
 
+  /*!
+  *\brief Constructed from string of item name and vector of key values
+  */
+  DIAGNOSTIC_AGGREGATOR_PUBLIC
+  StatusItem(
+    const std::string item_name, const std::vector<diagnostic_msgs::msg::KeyValue> & values,
+    const std::string message = "Missing", const DiagnosticLevel level = Level_Stale);
+
   DIAGNOSTIC_AGGREGATOR_PUBLIC
   ~StatusItem();
 
@@ -253,16 +261,8 @@ class StatusItem
    *
    *\return True if has key
    */
-  bool hasKey(const std::string & key) const
-  {
-    for (unsigned int i = 0; i < values_.size(); ++i) {
-      if (values_[i].key == key) {
-        return true;
-      }
-    }
-
-    return false;
-  }
+  DIAGNOSTIC_AGGREGATOR_PUBLIC
+  bool hasKey(const std::string & key) const;
 
   /*!
    *\brief Returns value for given key, "" if doens't exist
@@ -280,7 +280,26 @@ class StatusItem
     return std::string("");
   }
 
+  /*!
+  * \brief Adds key value pair to values_ vector
+  *
+  * If key already exists, updates value. Otherwise, adds new key value pair.
+  *
+  * \param key : Key to add
+  * \param value : Value to add
+  */
+  DIAGNOSTIC_AGGREGATOR_PUBLIC
+  void addValue(const std::string & key, const std::string & value);
+
 private:
+  /*!
+  * \brief Returns index of key in values_ vector, values_.size() if not found
+  *
+  * \param key : Key to search for
+  * \return Index of key in values_ vector if key present, values_.size() if not
+  */
+  std::size_t findKey(const std::string & key) const;
+
   rclcpp::Time update_time_;
   rclcpp::Clock::SharedPtr clock_;
 
diff --git a/diagnostic_aggregator/src/status_item.cpp b/diagnostic_aggregator/src/status_item.cpp
index 23921301c..eed706594 100644
--- a/diagnostic_aggregator/src/status_item.cpp
+++ b/diagnostic_aggregator/src/status_item.cpp
@@ -74,6 +74,14 @@ StatusItem::StatusItem(const string item_name, const string message, const Diagn
   RCLCPP_DEBUG(rclcpp::get_logger("StatusItem"), "StatusItem constructor from string");
 }
 
+StatusItem::StatusItem(
+  const std::string item_name, const std::vector<diagnostic_msgs::msg::KeyValue> & values,
+  const std::string message, const DiagnosticLevel level)
+{
+  StatusItem(item_name, message, level);
+  values_ = values;
+}
+
 StatusItem::~StatusItem() {}
 
 bool StatusItem::update(const diagnostic_msgs::msg::DiagnosticStatus * status)
@@ -126,4 +134,36 @@ std::shared_ptr<diagnostic_msgs::msg::DiagnosticStatus> StatusItem::toStatusMsg(
   return status;
 }
 
+bool StatusItem::hasKey(const std::string & key) const {return findKey(key) != values_.size();}
+
+void StatusItem::addValue(const std::string & key, const std::string & value)
+{
+  std::size_t index = findKey(key);
+  if (index != values_.size()) {
+    // the key already exists, update the value
+    values_[index].value = value;
+    return;
+  }
+
+  diagnostic_msgs::msg::KeyValue kv;
+  kv.key = key;
+  kv.value = value;
+  values_.push_back(kv);
+}
+
+std::size_t StatusItem::findKey(const std::string & key) const
+{
+  auto it = std::find_if(
+    values_.begin(), values_.end(),
+    [&key = std::as_const(key)](const diagnostic_msgs::msg::KeyValue & kv) {
+      return kv.key == key;
+    });
+
+  if (it != values_.end()) {
+    return std::distance(values_.begin(), it);
+  }
+
+  return values_.size();
+}
+
 }  // namespace diagnostic_aggregator