Merge pull request #38 from bjv-capra/feature/key-value-array

Feature key value array

Author: bjv-capra

micro_ros_diagnostic_bridge/include/micro_ros_diagnostic_bridge/micro_ros_diagnostic_bridge.hpp

@@ -23,7 +23,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "diagnostic_msgs/msg/diagnostic_status.hpp"
+#include "diagnostic_msgs/msg/diagnostic_array.hpp"
 #include "micro_ros_diagnostic_msgs/msg/micro_ros_diagnostic_status.hpp"
 
 
@@ -33,6 +33,8 @@ static const char UROS_DIAGNOSTICS_BRIDGE_TOPIC_OUT[] = "diagnostics";
 namespace uros_diagnostic_msg = micro_ros_diagnostic_msgs::msg;
 namespace diagnostic_msg = diagnostic_msgs::msg;
 
+constexpr int UNIQUE_POLYNOM = 4567;
+
 struct MicroROSDiagnosticUpdater
 {
   std::string name;
@@ -46,7 +48,7 @@ struct MicroROSDiagnosticKey
 
   bool operator<(const MicroROSDiagnosticKey & rhs) const
   {
-    return (updater_id * UINT_MAX + key_id) < (rhs.updater_id * UINT_MAX + rhs.key_id);
+    return (updater_id * UNIQUE_POLYNOM + key_id) < (rhs.updater_id * UNIQUE_POLYNOM + rhs.key_id);
   }
 };
 
@@ -57,8 +59,10 @@ struct MicroROSDiagnosticValue
 
   bool operator<(const MicroROSDiagnosticValue & rhs) const
   {
-    return (task.updater_id * UINT_MAX * UINT_MAX + task.key_id * UINT_MAX + value_id) <
-           (rhs.task.updater_id * UINT_MAX * UINT_MAX + rhs.task.key_id * UINT_MAX + rhs.value_id);
+    return (task.updater_id * UNIQUE_POLYNOM * UNIQUE_POLYNOM + task.key_id * UNIQUE_POLYNOM +
+           value_id) <
+           (rhs.task.updater_id * UNIQUE_POLYNOM * UNIQUE_POLYNOM + rhs.task.key_id *
+           UNIQUE_POLYNOM + rhs.value_id);
   }
 };
 
@@ -72,25 +76,25 @@ class MicroROSDiagnosticBridge : public rclcpp::Node
 public:
   explicit MicroROSDiagnosticBridge(const std::string & path = "");
 
-  virtual std::string lookup_hardware(
+  std::string lookup_hardware(
     int hardware_id);
-  virtual const MicroROSDiagnosticUpdater lookup_updater(
+  const MicroROSDiagnosticUpdater lookup_updater(
     int updater_id);
-  virtual std::string lookup_key(
+  std::string lookup_key(
     int updater_id,
     int key);
-  virtual std::string lookup_value(
+  std::string lookup_value(
     int updater_id,
     int key,
     int value_id);
 
-protected:
-  virtual void read_lookup_table(const std::string & path);
+private:
+  void read_lookup_table(const std::string & path);
 
   rclcpp::Logger logger_;
-  rclcpp::Subscription<uros_diagnostic_msg::MicroROSDiagnosticStatus>::SharedPtr uros_sub_;
-  rclcpp::Publisher<diagnostic_msg::DiagnosticStatus>::SharedPtr ros2_pub_;
-  std::unique_ptr<diagnostic_msg::DiagnosticStatus> msg_out_;
+  rclcpp::Subscription<uros_diagnostic_msg::MicroROSDiagnosticStatus>::SharedPtr
+    uros_diagnostics_sub_;
+  rclcpp::Publisher<diagnostic_msg::DiagnosticArray>::SharedPtr ros2_diagnostics_pub_;
 
   HardwareMap hardware_map_;
   UpdaterMap updater_map_;

micro_ros_diagnostic_bridge/src/micro_ros_diagnostic_bridge/micro_ros_diagnostic_bridge.cpp

@@ -26,7 +26,9 @@
 #include <rcl_yaml_param_parser/parser.h>
 
 using micro_ros_diagnostic_msgs::msg::MicroROSDiagnosticStatus;
-using diagnostic_msgs::msg::DiagnosticStatus;
+using diagnostic_msgs::msg::DiagnosticArray;
+
+static inline std::string VALUE_NOT_FOUND = "NOTFOUND";
 
 MicroROSDiagnosticBridge::MicroROSDiagnosticBridge(const std::string & path)
 : Node("micro_ros_diagnostic_bridge"),
@@ -40,54 +42,64 @@ MicroROSDiagnosticBridge::MicroROSDiagnosticBridge(const std::string & path)
   }
   read_lookup_table(lookup_table_path);
 
+  rclcpp::QoS qos{rclcpp::KeepLast{10}};
+  qos.reliable();
+
+  ros2_diagnostics_pub_ = create_publisher<DiagnosticArray>(
+    UROS_DIAGNOSTICS_BRIDGE_TOPIC_OUT,
+    qos);
+
   auto callback =
     [this](const MicroROSDiagnosticStatus::SharedPtr msg_in) -> void
     {
       RCLCPP_DEBUG(
         get_logger(),
         "Bridging message from hardware %d, updater %d", msg_in->hardware_id,
         msg_in->updater_id);
-      msg_out_ = std::make_unique<diagnostic_msgs::msg::DiagnosticStatus>();
+      auto msg_out = std::make_unique<DiagnosticArray>();
+      diagnostic_msgs::msg::DiagnosticStatus status_msg{};
 
       auto updater = lookup_updater(msg_in->updater_id);
       auto hardware = lookup_hardware(msg_in->hardware_id);
-      msg_out_->hardware_id = hardware;
-      msg_out_->name = updater.name;
-      msg_out_->message = updater.description;
-      msg_out_->level = msg_in->level;
-
-      diagnostic_msgs::msg::KeyValue keyvalue;
-      RCLCPP_DEBUG(get_logger(), "Bridging updater %d, task %d", msg_in->updater_id, msg_in->key);
-      keyvalue.key = lookup_key(msg_in->updater_id, msg_in->key);
-      switch (msg_in->value_type) {
-        case micro_ros_diagnostic_msgs::msg::MicroROSDiagnosticStatus::VALUE_BOOL:
-          keyvalue.value = std::to_string(msg_in->bool_value);
-          break;
-        case micro_ros_diagnostic_msgs::msg::MicroROSDiagnosticStatus::VALUE_INT:
-          keyvalue.value = std::to_string(msg_in->int_value);
-          break;
-        case micro_ros_diagnostic_msgs::msg::MicroROSDiagnosticStatus::VALUE_DOUBLE:
-          keyvalue.value = std::to_string(msg_in->double_value);
-          break;
-        case micro_ros_diagnostic_msgs::msg::MicroROSDiagnosticStatus::VALUE_LOOKUP:
-          keyvalue.value = lookup_value(msg_in->updater_id, msg_in->key, msg_in->value_id);
-          break;
+      status_msg.hardware_id = hardware;
+      status_msg.name = updater.name;
+      status_msg.message = updater.description;
+      status_msg.level = msg_in->level;
+      RCLCPP_DEBUG(get_logger(), "Updater %s HW %s", updater.name.c_str(), hardware.c_str());
+      for (size_t value_index = 0; value_index < msg_in->number_of_values; value_index++) {
+        diagnostic_msgs::msg::KeyValue keyvalue;
+        RCLCPP_DEBUG(
+          get_logger(), "Bridging updater %d, key %d", msg_in->updater_id,
+          msg_in->values[value_index].key);
+        keyvalue.key = lookup_key(msg_in->updater_id, msg_in->values[value_index].key);
+        switch (msg_in->values[value_index].value_type) {
+          case MicroROSDiagnosticStatus::VALUE_BOOL:
+            keyvalue.value = std::to_string(msg_in->values[value_index].bool_value);
+            break;
+          case MicroROSDiagnosticStatus::VALUE_INT:
+            keyvalue.value = std::to_string(msg_in->values[value_index].int_value);
+            break;
+          case MicroROSDiagnosticStatus::VALUE_DOUBLE:
+            keyvalue.value = std::to_string(msg_in->values[value_index].double_value);
+            break;
+          case MicroROSDiagnosticStatus::VALUE_LOOKUP:
+            keyvalue.value = lookup_value(
+              msg_in->updater_id, msg_in->values[value_index].key,
+              msg_in->values[value_index].value_id);
+            break;
+        }
+        status_msg.values.push_back(keyvalue);
+        RCLCPP_DEBUG(get_logger(), "key %s value %s", keyvalue.key.c_str(), keyvalue.value.c_str());
       }
-      msg_out_->values.push_back(keyvalue);
-
-      ros2_pub_->publish(std::move(msg_out_));
+      msg_out->status.push_back(status_msg);
+      msg_out->header.stamp = rclcpp::Clock().now();
+      ros2_diagnostics_pub_->publish(std::move(msg_out));
     };
 
-  rclcpp::QoS qos{rclcpp::KeepLast{10}};
-  qos.reliable();
-
-  uros_sub_ = create_subscription<MicroROSDiagnosticStatus>(
+  uros_diagnostics_sub_ = create_subscription<MicroROSDiagnosticStatus>(
     UROS_DIAGNOSTICS_BRIDGE_TOPIC_IN,
     qos,
     callback);
-  ros2_pub_ = create_publisher<DiagnosticStatus>(
-    UROS_DIAGNOSTICS_BRIDGE_TOPIC_OUT,
-    qos);
 }
 
 std::string
@@ -102,7 +114,7 @@ MicroROSDiagnosticBridge::lookup_key(
       get_logger(),
       "Updater %d and key %d, not found in lookup table.",
       updater_id, key);
-    return "NOTFOUND";
+    return VALUE_NOT_FOUND;
   }
 }
 
@@ -119,7 +131,7 @@ MicroROSDiagnosticBridge::lookup_value(
       get_logger(),
       "Updater %d, key %d, and value id %d, not found in lookup table.",
       updater_id, key, value_id);
-    return "NOTFOUND";
+    return VALUE_NOT_FOUND;
   }
 }
 
@@ -169,6 +181,9 @@ MicroROSDiagnosticBridge::read_lookup_table(const std::string & path)
       for (auto & p : it->second) {
         try {
           hardware_map_[std::stoi(p.get_name())] = p.value_to_string();
+          RCLCPP_DEBUG(
+            get_logger(), "FOUND Parameter: %s HW_ID %s",
+            p.get_name().c_str(), p.value_to_string().c_str());
         } catch (const std::invalid_argument &) {
           throw std::runtime_error("Failed to parse hardware_id from lookup_table.");
         }
@@ -181,28 +196,37 @@ MicroROSDiagnosticBridge::read_lookup_table(const std::string & path)
         auto pos = p.get_name().find('.');
         if (pos != std::string::npos) {
           updater_key = p.get_name().substr(0, pos);
+          RCLCPP_DEBUG(get_logger(), "Updater key: %s", updater_key.c_str());
         } else {
           throw std::runtime_error("Failed to load updater key.");
         }
 
         // Updater
         if (p.get_name().compare(updater_key + ".name") == 0) {
           updater_name = p.value_to_string();
+          RCLCPP_DEBUG(
+            get_logger(), "Updater Name: %s, Description: %s",
+            updater_name.c_str(), updater_descr.c_str());
           updater_map_[std::stoi(updater_key)] = {updater_name, updater_descr};
         }
         if (p.get_name().compare(updater_key + ".description") == 0) {
           updater_descr = p.value_to_string();
           updater_map_[std::stoi(updater_key)] = {updater_name, updater_descr};
+          RCLCPP_DEBUG(
+            get_logger(), "Updater Name: %s, Description: %s",
+            updater_name.c_str(), updater_descr.c_str());
         }
 
         // Keys
         if (p.get_name().rfind(updater_key + ".keys", 0) == 0) {
           auto start = updater_key.length() + 6;
           pos = p.get_name().find('.', start);
           key = p.get_name().substr(start, pos - start);
+          RCLCPP_DEBUG(get_logger(), "Updater key position: %ld, key: %s", pos, key.c_str());
         }
         if (p.get_name().compare(updater_key + ".keys." + key + ".name") == 0) {
           key_name = p.value_to_string();
+          RCLCPP_DEBUG(get_logger(), "Key name: %s", key_name.c_str());
           key_map_[{std::stoi(updater_key), std::stoi(key)}] = key_name;
         }
 
@@ -211,8 +235,8 @@ MicroROSDiagnosticBridge::read_lookup_table(const std::string & path)
           auto start = updater_key.length() + key.length() + 14;
           pos = p.get_name().find('.', start);
           auto value_id = std::stoi(p.get_name().substr(start, pos - start));
-          value_map_[{{std::stoi(updater_key), std::stoi(key)}, value_id}] =
-            p.value_to_string();
+          value_map_[{{std::stoi(updater_key), std::stoi(key)}, value_id}] = p.value_to_string();
+          RCLCPP_DEBUG(get_logger(), "Value ID %d Value %s", value_id, p.value_to_string().c_str());
         }
       }
     }

micro_ros_diagnostic_msgs/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
 
 set(msg_files
+  "msg/MicroROSDiagnosticKeyValue.msg"
   "msg/MicroROSDiagnosticStatus.msg"
 )
 set(srv_files

micro_ros_diagnostic_msgs/msg/MicroROSDiagnosticKeyValue.msg

@@ -0,0 +1,23 @@
+# Possible levels of operations.
+byte OK=0
+byte WARN=1
+byte ERROR=2
+byte STALE=3
+
+# Possible value types
+byte VALUE_BOOL=1
+byte VALUE_INT=2
+byte VALUE_DOUBLE=3
+byte VALUE_LOOKUP=10
+
+# Level of operation enumerated above.
+byte level
+
+# What key, value is transmitted.
+uint16 key
+
+byte value_type
+bool bool_value
+int32 int_value
+float32 double_value
+uint16 value_id

micro_ros_diagnostic_msgs/msg/MicroROSDiagnosticStatus.msg

@@ -26,11 +26,5 @@ uint16 updater_id
 # A hardware unique string.
 uint16 hardware_id
 
-# What key, value is transmitted.
-uint16 key
-
-byte value_type
-bool bool_value
-int32 int_value
-float32 double_value
-uint16 value_id
+uint8 number_of_values
+micro_ros_diagnostic_msgs/MicroROSDiagnosticKeyValue[] values

micro_ros_diagnostic_updater/CMakeLists.txt

@@ -30,6 +30,7 @@ option(MICRO_ROS_DIAGNOSTIC_UPDATER_EXAMPLES "Build example updaters for the mic
 # Configuration
 set(MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_TASKS_PER_UPDATER "5" CACHE STRING "Number of tasks preallocated per updater.")
 set(MICRO_ROS_DIAGNOSTIC_UPDATER_DIAGNOSTICS_TOPIC_PREFIX "" CACHE STRING "Prefix for micro-ROS diagnostic topic (append slash '/')")
+set(MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK "4" CACHE STRING "Number of pre allocated key value pairs.")
 
 # Create library
 add_library(${PROJECT_NAME}

micro_ros_diagnostic_updater/example/example_processor_updater.c

@@ -30,37 +30,39 @@ static const int16_t PROCESSOR_TEMPERATURE_TASK_ID = 0;
 static const int16_t PROCESSOR_LOAD_TASK_ID = 1;
 
 rcl_ret_t
-my_diagnostic_temperature(diagnostic_value_t * temp_kv)
+my_diagnostic_temperature(diagnostic_value_t * values, uint8_t * number_of_values)
 {
+  *number_of_values = 1;
   // Fake a temperature
   ++my_diagnostic_temp;
   if (my_diagnostic_temp > 99) {
     my_diagnostic_temp -= 0;
   }
-  rclc_diagnostic_value_set_int(temp_kv, my_diagnostic_temp);
+  rclc_diagnostic_value_set_int(&values[0], my_diagnostic_temp);
 
   // Calculate the diagnostic level
   if (my_diagnostic_temp > 85) {
-    rclc_diagnostic_value_set_level(temp_kv, 2);
+    rclc_diagnostic_value_set_level(&values[0], 2);
   } else if (my_diagnostic_temp > 75) {
-    rclc_diagnostic_value_set_level(temp_kv, 1);
+    rclc_diagnostic_value_set_level(&values[0], 1);
   } else {
-    rclc_diagnostic_value_set_level(temp_kv, 0);
+    rclc_diagnostic_value_set_level(&values[0], 0);
   }
   return RCL_RET_OK;
 }
 
 rcl_ret_t
-my_diagnostic_load(diagnostic_value_t * load_kv)
+my_diagnostic_load(diagnostic_value_t * values, uint8_t * number_of_values)
 {
+  *number_of_values = 1;
   // Fake a processor load
-  rclc_diagnostic_value_set_int(load_kv, my_diagnostic_temp / 2);
+  rclc_diagnostic_value_set_int(&values[0], my_diagnostic_temp / 2);
 
   // Calculate the diagnostic level
   if (my_diagnostic_temp > 46) {
-    rclc_diagnostic_value_set_level(load_kv, 1);
+    rclc_diagnostic_value_set_level(&values[0], 1);
   } else {
-    rclc_diagnostic_value_set_level(load_kv, 0);
+    rclc_diagnostic_value_set_level(&values[0], 0);
   }
   return RCL_RET_OK;
 }