feat: only publish updates when there are changes

Signed-off-by: Bartolome Jimenez Vera <bjv@capra.ooo>

Author: bjv-capra

micro_ros_diagnostic_bridge/src/micro_ros_diagnostic_bridge/micro_ros_diagnostic_bridge.cpp

@@ -26,6 +26,7 @@
 #include <rcl_yaml_param_parser/parser.h>
 
 using micro_ros_diagnostic_msgs::msg::MicroROSDiagnosticStatus;
+using micro_ros_diagnostic_msgs::msg::MicroROSDiagnosticKeyValue;
 using diagnostic_msgs::msg::DiagnosticArray;
 
 static inline std::string VALUE_NOT_FOUND = "NOTFOUND";
@@ -73,16 +74,16 @@ MicroROSDiagnosticBridge::MicroROSDiagnosticBridge(const std::string & path)
           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:
+          case MicroROSDiagnosticKeyValue::VALUE_BOOL:
             keyvalue.value = std::to_string(msg_in->values[value_index].bool_value);
             break;
-          case MicroROSDiagnosticStatus::VALUE_INT:
+          case MicroROSDiagnosticKeyValue::VALUE_INT:
             keyvalue.value = std::to_string(msg_in->values[value_index].int_value);
             break;
-          case MicroROSDiagnosticStatus::VALUE_DOUBLE:
+          case MicroROSDiagnosticKeyValue::VALUE_FLOAT:
             keyvalue.value = std::to_string(msg_in->values[value_index].double_value);
             break;
-          case MicroROSDiagnosticStatus::VALUE_LOOKUP:
+          case MicroROSDiagnosticKeyValue::VALUE_LOOKUP:
             keyvalue.value = lookup_value(
               msg_in->updater_id, msg_in->values[value_index].key,
               msg_in->values[value_index].value_id);

micro_ros_diagnostic_msgs/msg/MicroROSDiagnosticKeyValue.msg

@@ -7,7 +7,7 @@ byte STALE=3
 # Possible value types
 byte VALUE_BOOL=1
 byte VALUE_INT=2
-byte VALUE_DOUBLE=3
+byte VALUE_FLOAT=3
 byte VALUE_LOOKUP=10
 
 # Level of operation enumerated above.

micro_ros_diagnostic_msgs/msg/MicroROSDiagnosticStatus.msg

@@ -12,12 +12,6 @@ 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
 

micro_ros_diagnostic_updater/example/example_processor_updater.c

@@ -62,7 +62,7 @@ my_diagnostic_load(diagnostic_value_t * values, uint8_t * number_of_values)
   // Set the key to get translation
   values[0].key = PROCESSOR_LOAD_KEY;
   // Fake a processor load
-  rclc_diagnostic_value_set_int(&values[0], my_diagnostic_temp / 2);
+  rclc_diagnostic_value_set_float(&values[0], (float)my_diagnostic_temp / 2.0f);
 
   // Calculate the diagnostic level
   if (my_diagnostic_temp > 46) {

micro_ros_diagnostic_updater/include/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.h

@@ -32,6 +32,7 @@ typedef struct diagnostic_value_t
   int16_t value_id;
 
   int8_t level;
+  bool value_has_changed;
 } diagnostic_value_t;
 
 typedef struct diagnostic_task_t
@@ -58,6 +59,14 @@ void rclc_diagnostic_value_set_int(
   diagnostic_value_t * kv,
   int32_t value);
 
+void rclc_diagnostic_value_set_float(
+  diagnostic_value_t * kv,
+  float value);
+
+void rclc_diagnostic_value_set_bool(
+  diagnostic_value_t * kv,
+  bool value);
+
 void rclc_diagnostic_value_lookup(
   diagnostic_value_t * kv,
   int16_t value_id);

micro_ros_diagnostic_updater/src/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.c

@@ -27,16 +27,46 @@ rclc_diagnostic_value_set_int(
   diagnostic_value_t * kv,
   int32_t value)
 {
-  kv->value_type = micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticStatus__VALUE_INT;
+  kv->value_type = micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticKeyValue__VALUE_INT;
+  if (kv->int_value != value) {
+    kv->value_has_changed = true;
+  }
   kv->int_value = value;
 }
 
+void
+rclc_diagnostic_value_set_float(
+  diagnostic_value_t * kv,
+  float value)
+{
+  kv->value_type = micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticKeyValue__VALUE_FLOAT;
+  if (kv->double_value != value) {
+    kv->value_has_changed = true;
+  }
+  kv->double_value = value;
+}
+
+void
+rclc_diagnostic_value_set_bool(
+  diagnostic_value_t * kv,
+  bool value)
+{
+  kv->value_type = micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticKeyValue__VALUE_BOOL;
+  if (kv->bool_value != value) {
+    kv->value_has_changed = true;
+  }
+  kv->bool_value = value;
+}
+
 void
 rclc_diagnostic_value_lookup(
   diagnostic_value_t * kv,
   int16_t value_id)
 {
-  kv->value_type = micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticStatus__VALUE_LOOKUP;
+  kv->value_type = micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticKeyValue__VALUE_LOOKUP;
+  if (kv->value_id != value_id) {
+    kv->value_has_changed = true;
+  }
   kv->value_id = value_id;
 }
 
@@ -46,6 +76,9 @@ rclc_diagnostic_value_set_level(
   int8_t level)
 {
   kv->level = level;
+  if (kv->level != level) {
+    kv->value_has_changed = true;
+  }
 }
 
 rcl_ret_t
@@ -182,11 +215,21 @@ rclc_diagnostic_updater_update(
       updater->diag_status.hardware_id = updater->tasks[i]->hardware_id;
       updater->diag_status.number_of_values = updater->tasks[i]->number_of_values;
 
-      micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticKeyValue key_value;
+      updater->diag_status.values.size = updater->tasks[i]->number_of_values;
 
+      bool must_publish = false;
+      micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticKeyValue key_value;
       for (uint8_t value_index = 0u; value_index < updater->tasks[i]->number_of_values;
         value_index++)
       {
+        // Quickly go to the next value if it has not changed
+        if (!updater->tasks[i]->values[value_index].value_has_changed) {
+          continue;
+        }
+        must_publish = true;
+        // We reset the value_has_changed flag
+        updater->tasks[i]->values[value_index].value_has_changed = false;
+
         key_value.key = updater->tasks[i]->values[value_index].key;
         key_value.value_type = updater->tasks[i]->values[value_index].value_type;
         key_value.bool_value = updater->tasks[i]->values[value_index].bool_value;
@@ -197,8 +240,10 @@ rclc_diagnostic_updater_update(
 
         memcpy(&updater->diag_status.values.data[value_index], &key_value, sizeof(key_value));
       }
-      updater->diag_status.values.size = updater->tasks[i]->number_of_values;
-
+      // We only publish if at least one value has changed
+      if (!must_publish) {
+        continue;
+      }
       rcl_ret_t rc = rcl_publish(&updater->diag_pub, &updater->diag_status, NULL);
       if (rc == RCL_RET_OK) {
         RCUTILS_LOG_DEBUG(

micro_ros_diagnostic_updater/test/test_diagnostic_updater.cpp

@@ -73,13 +73,13 @@ TEST(TestDiagnosticUpdater, create_diagnostic_values) {
   EXPECT_EQ(value.int_value, 17);
   EXPECT_EQ(
     value.value_type,
-    micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticStatus__VALUE_INT);
+    micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticKeyValue__VALUE_INT);
 
   rclc_diagnostic_value_lookup(&value, 17);
   EXPECT_EQ(value.value_id, 17);
   EXPECT_EQ(
     value.value_type,
-    micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticStatus__VALUE_LOOKUP);
+    micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticKeyValue__VALUE_LOOKUP);
 
   rclc_diagnostic_value_set_level(
     &value,