Pull it together.

src/conversions.cpp

@@ -1,6 +1,6 @@
 #include <j7s_diagnostics_ros/conversions.hpp>
 
-std::string convertString(const rust::String& rust)
+std::string convertString(const rust::String & rust)
 {
     auto copy = rust;
     return std::string(copy.c_str());
@@ -8,7 +8,7 @@ std::string convertString(const rust::String& rust)
 
 j7s_diagnostics_cxx::DiagnosticLevel convertLevel(const uint8_t rosLevel)
 {
-    switch(rosLevel)
+    switch (rosLevel)
     {
         case diagnostic_msgs::msg::DiagnosticStatus::OK:
             return j7s_diagnostics_cxx::DiagnosticLevel::OK;
@@ -22,9 +22,9 @@ j7s_diagnostics_cxx::DiagnosticLevel convertLevel(const uint8_t rosLevel)
     }
 }
 
-uint8_t convertLevel(const j7s_diagnostics_cxx::DiagnosticLevel& level )
+uint8_t convertLevel(const j7s_diagnostics_cxx::DiagnosticLevel & level)
 {
-    switch(level)
+    switch (level)
     {
         case j7s_diagnostics_cxx::DiagnosticLevel::OK:
             return diagnostic_msgs::msg::DiagnosticStatus::OK;
@@ -39,17 +39,18 @@ uint8_t convertLevel(const j7s_diagnostics_cxx::DiagnosticLevel& level )
     }
 }
 
-std::vector<diagnostic_msgs::msg::KeyValue> convertValues(const rust::Box<j7s_diagnostics_cxx::StringMap>& map)
+std::vector<diagnostic_msgs::msg::KeyValue> convertValues(
+    const rust::Box<j7s_diagnostics_cxx::StringMap> & map)
 {
     std::vector<diagnostic_msgs::msg::KeyValue> toReturn;
 
     const rust::Vec<rust::String> keys = map->keys();
-    if(keys.empty())
+    if (keys.empty())
     {
         return toReturn;
     }
 
-    for(const auto& key : keys)
+    for (const auto & key : keys)
     {
         const auto value = map->get(key);
         diagnostic_msgs::msg::KeyValue msg;
@@ -59,14 +60,14 @@ std::vector<diagnostic_msgs::msg::KeyValue> convertValues(const rust::Box<j7s_di
     }
 
     return toReturn;
-
 }
 
-rust::Box<j7s_diagnostics_cxx::StringMap> convertValues(const std::vector<diagnostic_msgs::msg::KeyValue>& map)
+rust::Box<j7s_diagnostics_cxx::StringMap> convertValues(
+    const std::vector<diagnostic_msgs::msg::KeyValue> & map)
 {
     rust::Box<j7s_diagnostics_cxx::StringMap> toReturn = j7s_diagnostics_cxx::StringMap::build();
 
-    for(const auto& pair: map)
+    for (const auto & pair : map)
     {
         toReturn->insert(rust::String(pair.key), rust::String(pair.value));
     }
@@ -74,7 +75,7 @@ rust::Box<j7s_diagnostics_cxx::StringMap> convertValues(const std::vector<diagno
     return toReturn;
 }
 
-j7s_diagnostics_cxx::DiagnosticStatus fromMsg(const diagnostic_msgs::msg::DiagnosticStatus& msg)
+j7s_diagnostics_cxx::DiagnosticStatus fromMsg(const diagnostic_msgs::msg::DiagnosticStatus & msg)
 {
     auto toReturn = j7s_diagnostics_cxx::DiagnosticStatus::build();
     toReturn.level = convertLevel(msg.level);
@@ -86,7 +87,20 @@ j7s_diagnostics_cxx::DiagnosticStatus fromMsg(const diagnostic_msgs::msg::Diagno
     return toReturn;
 }
 
-diagnostic_msgs::msg::DiagnosticStatus toMsg(const j7s_diagnostics_cxx::DiagnosticStatus& status)
+rust::Vec<j7s_diagnostics_cxx::DiagnosticStatus> fromMsg(
+    const diagnostic_msgs::msg::DiagnosticArray & msg)
+{
+    rust::Vec<j7s_diagnostics_cxx::DiagnosticStatus> toReturn;
+    toReturn.reserve(msg.status.size());
+
+    for (const auto & rosMsg : msg.status)
+    {
+        toReturn.emplace_back(fromMsg(rosMsg));
+    }
+    return toReturn;
+}
+
+diagnostic_msgs::msg::DiagnosticStatus toMsg(const j7s_diagnostics_cxx::DiagnosticStatus & status)
 {
     diagnostic_msgs::msg::DiagnosticStatus toReturn;
     toReturn.level = convertLevel(status.level);
@@ -97,3 +111,16 @@ diagnostic_msgs::msg::DiagnosticStatus toMsg(const j7s_diagnostics_cxx::Diagnost
 
     return toReturn;
 }
+
+diagnostic_msgs::msg::DiagnosticArray toMsg(
+    const rust::Vec<j7s_diagnostics_cxx::DiagnosticStatus> & statuses)
+{
+    diagnostic_msgs::msg::DiagnosticArray msg;
+    msg.status.reserve(statuses.size());
+
+    for (const auto & status : statuses)
+    {
+        msg.status.emplace_back(toMsg(status));
+    }
+    return msg;
+}

src/test.cpp

@@ -1,17 +1,54 @@
+#include <fmt/core.h>
+
 #include <iostream>
 #include <j7s_diagnostics_ros/conversions.hpp>
 
-int main(int argc, char *argv[])
+diagnostic_msgs::msg::DiagnosticStatus make_test_status(const std::string & name, uint8_t level)
 {
-    const auto my_string = j7s_diagnostics_cxx::OptionalString::some(rust::String("test"));
-    if(my_string->is_some())
-    {
-        auto as_string = my_string->value();
-        std::cout << as_string.c_str() << std::endl;
-    }
+    diagnostic_msgs::msg::DiagnosticStatus status;
+    status.name = name;
+    status.level = level;
 
-    auto my_map = j7s_diagnostics_cxx::StringMap::build();
-    my_map->insert(rust::String("a"), rust::String("b"));
+    return status;
+}
+
+std::string levelToString(uint8_t level)
+{
+    switch (level)
+    {
+        case diagnostic_msgs::msg::DiagnosticStatus::OK:
+            return "OK";
+        case diagnostic_msgs::msg::DiagnosticStatus::WARN:
+            return "WARN";
+        case diagnostic_msgs::msg::DiagnosticStatus::ERROR:
+            return "ERROR";
+        case diagnostic_msgs::msg::DiagnosticStatus::STALE:
+            return "STALE";
+        default:
+            return "N/A";
+    }
+}
+
+int main(int argc, char * argv[])
+{
+    diagnostic_msgs::msg::DiagnosticArray diagArray;
+    diagArray.status.emplace_back(
+        make_test_status("a", diagnostic_msgs::msg::DiagnosticStatus::OK));
+    diagArray.status.emplace_back(
+        make_test_status("a/b", diagnostic_msgs::msg::DiagnosticStatus::WARN));
+    diagArray.status.emplace_back(
+        make_test_status("a/c", diagnostic_msgs::msg::DiagnosticStatus::OK));
+    diagArray.status.emplace_back(
+        make_test_status("a/c/d", diagnostic_msgs::msg::DiagnosticStatus::OK));
+
+    auto tree = j7s_diagnostics_cxx::DiagnosticTree::build();
+    tree->add_statuses(fromMsg(diagArray));
+    tree->reconcile();
+    const auto aggregate = toMsg(tree->aggregate());
+    for (const auto & status : aggregate.status)
+    {
+        fmt::println("Name: {} Level: {}", status.name, levelToString(status.level));
+    }
 
     return 0;
 }