Add status_item::addValue and refactor

* Introduce the addValue() method to StatusItem.

* Refactor addValue and hasKey and move them to the cpp

Signed-off-by: Mahmoud Almasri <[email protected]>

Author: MahmoudAlmasri

diagnostic_aggregator/include/diagnostic_aggregator/status_item.hpp

@@ -253,16 +253,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 +272,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_;
 

diagnostic_aggregator/src/status_item.cpp

@@ -126,4 +126,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