Allow editing values_ in status_item and small refactor

diagnostic_aggregator/CMakeLists.txt

@@ -78,9 +78,13 @@ if(BUILD_TESTING)
   set(ament_cmake_copyright_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
 
+  find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_cmake_pytest REQUIRED)
   find_package(launch_testing_ament_cmake REQUIRED)
 
+  ament_add_gtest(test_status_item test/test_status_item.cpp)
+  target_link_libraries(test_status_item ${PROJECT_NAME})
+
   file(TO_CMAKE_PATH "${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_NAME}/aggregator_node" AGGREGATOR_NODE)
   file(TO_CMAKE_PATH "${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_NAME}/add_analyzer" ADD_ANALYZER)
   file(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/test_listener.py" TEST_LISTENER)

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_;
 

diagnostic_aggregator/src/status_item.cpp

@@ -36,8 +36,11 @@
 
 #include "diagnostic_aggregator/status_item.hpp"
 
+#include <algorithm>
+#include <iterator>
 #include <memory>
 #include <string>
+#include <utility>
 
 namespace diagnostic_aggregator
 {
@@ -74,6 +77,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 +137,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

diagnostic_aggregator/test/test_status_item.cpp

@@ -0,0 +1,109 @@
+// Copyright 2026 Mahmoud Almasri
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the copyright holder nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <vector>
+
+#include "diagnostic_aggregator/status_item.hpp"
+#include "diagnostic_msgs/msg/key_value.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+using diagnostic_aggregator::StatusItem;
+using diagnostic_aggregator::Level_OK;
+using diagnostic_aggregator::Level_Stale;
+using diagnostic_msgs::msg::KeyValue;
+
+namespace
+{
+KeyValue kv(const std::string & k, const std::string & v)
+{
+  KeyValue out;
+  out.key = k;
+  out.value = v;
+  return out;
+}
+}  // namespace
+
+TEST(StatusItem, constructorWithValuesInitializesAllFields)
+{
+  std::vector<KeyValue> values{kv("a", "1"), kv("b", "2")};
+  StatusItem item("sensor", values, "ok-msg", Level_OK);
+
+  EXPECT_EQ(item.getName(), "sensor");
+  EXPECT_EQ(item.getMessage(), "ok-msg");
+  EXPECT_EQ(item.getLevel(), Level_OK);
+  EXPECT_EQ(item.getValue("a"), "1");
+  EXPECT_EQ(item.getValue("b"), "2");
+}
+
+TEST(StatusItem, addValueNewKeyAppendsEntry)
+{
+  StatusItem item("sensor");
+  ASSERT_FALSE(item.hasKey("k"));
+
+  item.addValue("k", "v");
+
+  EXPECT_TRUE(item.hasKey("k"));
+  EXPECT_EQ(item.getValue("k"), "v");
+}
+
+TEST(StatusItem, addValueExistingKeyUpdatesInPlace)
+{
+  std::vector<KeyValue> values{kv("k", "old")};
+  StatusItem item("sensor", values);
+
+  item.addValue("k", "new");
+
+  EXPECT_EQ(item.getValue("k"), "new");
+
+  // values_ is private; check via the serialized message that no duplicate was added.
+  auto msg = item.toStatusMsg("path");
+  ASSERT_EQ(msg->values.size(), 1u);
+  EXPECT_EQ(msg->values[0].key, "k");
+  EXPECT_EQ(msg->values[0].value, "new");
+}
+
+TEST(StatusItem, hasKey)
+{
+  std::vector<KeyValue> values{kv("a", "1"), kv("b", "2")};
+  StatusItem item("sensor", values);
+
+  EXPECT_FALSE(item.hasKey("nope"));
+  EXPECT_TRUE(item.hasKey("a"));
+  EXPECT_TRUE(item.hasKey("b"));
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+
+  return RUN_ALL_TESTS();
+}