Add context-aware diagnostic tasks

NOTICE

@@ -24,5 +24,7 @@
 Capra Robotics ApS
     Bartolome Jimenez Vera <bjv@capra.ooo>
 
+Daniil Mordanov <153565951+Daniiiil1@users.noreply.github.com>
+
 Robert Bosch GmbH
     Arne Nordmann <arne.nordmann@de.bosch.com>

micro_ros_diagnostic_updater/README.md

@@ -67,6 +67,33 @@ colcon build --packages-select micro_ros_diagnostic_updater --cmake-args -DMICRO
 
 The updater won't publish statuses of task who's data is unchanged, this is to reduce the traffic and processing needed by the updater on each iteration. However, due to different reasons, one may want to force the updater to publish everything. This is done with a subscription that is added to the executor passed on the initialization of the updater. The subscriber will be listening for a message of type `std_msgs/msg/Empty`, and the topic is always `<namespace>/diagnostics_uros/force_update`. Keep in mind, the `<namespace>` can be modified as indicated above.
 
+### Pass context to a diagnostic task
+
+Use `rclc_diagnostic_task_init_with_context` when a diagnostic callback needs
+instance-specific state. The context pointer is stored in the task and passed
+as the final callback argument whenever the updater calls it:
+
+```c
+rcl_ret_t update_temperature(
+  diagnostic_value_t * values,
+  uint8_t * number_of_values,
+  void * context)
+{
+  temperature_sensor_t * sensor = (temperature_sensor_t *) context;
+  *number_of_values = 1;
+  rclc_diagnostic_value_set_float(&values[0], sensor->temperature);
+  return RCL_RET_OK;
+}
+
+diagnostic_task_t task;
+temperature_sensor_t sensor;
+rclc_diagnostic_task_init_with_context(
+  &task, hardware_id, updater_id, update_temperature, &sensor);
+```
+
+The original `rclc_diagnostic_task_init` remains available for callbacks that
+do not need a context.
+
 ## License
 
 The micro-ROS diagnostics framework packages are open-sourced under the Apache-2.0 license. See the [../LICENSE](LICENSE) file for details.

micro_ros_diagnostic_updater/example/example_website_checker.c

@@ -20,8 +20,11 @@
 #include <micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.h>
 #include <micro_ros_diagnostic_msgs/msg/micro_ros_diagnostic_status.h>
 
-static int my_diagnostic_status = 0;
-static int my_website_status = 0;
+typedef struct website_diagnostic_context_t
+{
+  int diagnostic_status;
+  int website_status;
+} website_diagnostic_context_t;
 // Hardware ID
 static const int16_t WEBSITE_SERIAL = 998;
 // Updater ID
@@ -33,35 +36,36 @@ static const uint16_t WEBSITE_STATUS_TASK_ID = 42;
 rcl_ret_t
 my_diagnostic_website_check(
   diagnostic_value_t values[MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK],
-  uint8_t * number_of_values)
+  uint8_t * number_of_values,
+  void * context)
 {
-  // Cast to avoid warnings
-  (void)number_of_values;
+  website_diagnostic_context_t * website_context =
+    (website_diagnostic_context_t *) context;
   *number_of_values = 1;
 
-  ++my_diagnostic_status;
+  ++website_context->diagnostic_status;
 
   values[0].key = WEBSITE_STATUS_TASK_ID;
-  if (my_diagnostic_status > 99) {
-    my_diagnostic_status = 0;
+  if (website_context->diagnostic_status > 99) {
+    website_context->diagnostic_status = 0;
   }
-  if (my_diagnostic_status % 13 == 0) {
-    my_website_status = 404;
+  if (website_context->diagnostic_status % 13 == 0) {
+    website_context->website_status = 404;
     rclc_diagnostic_value_set_level(
       &values[0],
       micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticStatus__WARN);
-  } else if (my_diagnostic_status % 17 == 0) {
-    my_website_status = 500;
+  } else if (website_context->diagnostic_status % 17 == 0) {
+    website_context->website_status = 500;
     rclc_diagnostic_value_set_level(
       &values[0],
       micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticStatus__ERROR);
   } else {
-    my_website_status = 200;
+    website_context->website_status = 200;
     rclc_diagnostic_value_set_level(
       &values[0],
       micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticStatus__OK);
   }
-  rclc_diagnostic_value_lookup(&values[0], my_website_status);
+  rclc_diagnostic_value_lookup(&values[0], website_context->website_status);
 
   return RCL_RET_OK;
 }
@@ -115,9 +119,10 @@ int main(int argc, const char * argv[])
     return -1;
   }
   diagnostic_task_t task;
-  rc = rclc_diagnostic_task_init(
+  website_diagnostic_context_t website_context = {0, 0};
+  rc = rclc_diagnostic_task_init_with_context(
     &task, WEBSITE_SERIAL, WEBSITE_ID,
-    &my_diagnostic_website_check);
+    &my_diagnostic_website_check, &website_context);
   if (rc != RCL_RET_OK) {
     printf("Error in creating diagnostic task\n");
     return -1;

micro_ros_diagnostic_updater/include/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.h

@@ -38,15 +38,24 @@ typedef struct diagnostic_value_t
   bool value_has_changed;
 } diagnostic_value_t;
 
+typedef rcl_ret_t (* diagnostic_task_function_t)(
+  diagnostic_value_t[MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK],
+  uint8_t * number_of_values);
+
+typedef rcl_ret_t (* diagnostic_task_function_with_context_t)(
+  diagnostic_value_t[MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK],
+  uint8_t * number_of_values,
+  void * context);
+
 typedef struct diagnostic_task_t
 {
   uint8_t number_of_values;
   diagnostic_value_t values[MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK];
   int16_t hardware_id;
   int16_t updater_id;
-  rcl_ret_t (* function)(
-    diagnostic_value_t[MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK],
-    uint8_t * number_of_values);
+  diagnostic_task_function_t function;
+  diagnostic_task_function_with_context_t function_with_context;
+  void * context;
 } diagnostic_task_t;
 
 typedef struct diagnostic_updater_t
@@ -86,9 +95,15 @@ rclc_diagnostic_task_init(
   diagnostic_task_t * task,
   int16_t hardware_id,
   int16_t updater_id,
-  rcl_ret_t (* function)(
-    diagnostic_value_t[MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK],
-    uint8_t * number_of_values));
+  diagnostic_task_function_t function);
+
+rcl_ret_t
+rclc_diagnostic_task_init_with_context(
+  diagnostic_task_t * task,
+  int16_t hardware_id,
+  int16_t updater_id,
+  diagnostic_task_function_with_context_t function,
+  void * context);
 
 // Added to work with force update, it's very important to call spin
 // or spin_some before updater_update

micro_ros_diagnostic_updater/src/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.c

@@ -86,16 +86,38 @@ rclc_diagnostic_task_init(
   diagnostic_task_t * task,
   int16_t hardware_id,
   int16_t updater_id,
-  rcl_ret_t (* function)(
-    diagnostic_value_t[MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK],
-    uint8_t * number_of_values))
+  diagnostic_task_function_t function)
 {
   RCL_CHECK_FOR_NULL_WITH_MSG(
     task, "task is a null pointer", return RCL_RET_INVALID_ARGUMENT);
   RCL_CHECK_FOR_NULL_WITH_MSG(
     function, "function is a null pointer", return RCL_RET_INVALID_ARGUMENT);
 
   task->function = function;
+  task->function_with_context = NULL;
+  task->context = NULL;
+  task->updater_id = updater_id;
+  task->hardware_id = hardware_id;
+
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rclc_diagnostic_task_init_with_context(
+  diagnostic_task_t * task,
+  int16_t hardware_id,
+  int16_t updater_id,
+  diagnostic_task_function_with_context_t function,
+  void * context)
+{
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    task, "task is a null pointer", return RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    function, "function is a null pointer", return RCL_RET_INVALID_ARGUMENT);
+
+  task->function = NULL;
+  task->function_with_context = function;
+  task->context = context;
   task->updater_id = updater_id;
   task->hardware_id = hardware_id;
 
@@ -241,6 +263,10 @@ rcl_ret_t
 rclc_diagnostic_call_task(
   diagnostic_task_t * task)
 {
+  if (task->function_with_context != NULL) {
+    return (task->function_with_context)(
+      task->values, &task->number_of_values, task->context);
+  }
   return (task->function)(task->values, &task->number_of_values);
 }
 

micro_ros_diagnostic_updater/test/test_diagnostic_updater.cpp

@@ -27,6 +27,12 @@ extern "C"
 static int diagnostic_mockup_counter_0 = 0;
 static int diagnostic_mockup_counter_1 = 0;
 
+typedef struct diagnostic_mockup_context_t
+{
+  int value;
+  int calls;
+} diagnostic_mockup_context_t;
+
 rcl_ret_t
 update_function_mockup_0(
   diagnostic_value_t values[MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK],
@@ -60,10 +66,59 @@ update_function_mockup_1(
   return RCL_RET_OK;
 }
 
+rcl_ret_t
+update_function_mockup_with_context(
+  diagnostic_value_t values[MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK],
+  uint8_t * number_of_values,
+  void * context)
+{
+  diagnostic_mockup_context_t * task_context =
+    static_cast<diagnostic_mockup_context_t *>(context);
+  ++task_context->calls;
+
+  *number_of_values = 1;
+  rclc_diagnostic_value_set_int(&values[0], task_context->value);
+
+  return RCL_RET_OK;
+}
+
 TEST(TestDiagnosticUpdater, create_diagnostic_task) {
   diagnostic_task_t task;
   rcl_ret_t rc = rclc_diagnostic_task_init(&task, 0, 0, &update_function_mockup_0);
   EXPECT_EQ(RCL_RET_OK, rc);
+  EXPECT_EQ(&update_function_mockup_0, task.function);
+  EXPECT_EQ(nullptr, task.function_with_context);
+  EXPECT_EQ(nullptr, task.context);
+}
+
+TEST(TestDiagnosticUpdater, create_diagnostic_task_with_context) {
+  diagnostic_mockup_context_t context = {73, 0};
+  diagnostic_task_t task;
+  rcl_ret_t rc = rclc_diagnostic_task_init_with_context(
+    &task, 0, 0, &update_function_mockup_with_context, &context);
+
+  ASSERT_EQ(RCL_RET_OK, rc);
+  EXPECT_EQ(&context, task.context);
+  EXPECT_EQ(nullptr, task.function);
+  EXPECT_EQ(&update_function_mockup_with_context, task.function_with_context);
+
+  rc = rclc_diagnostic_call_task(&task);
+  EXPECT_EQ(RCL_RET_OK, rc);
+  EXPECT_EQ(1, context.calls);
+  EXPECT_EQ(1, task.number_of_values);
+  EXPECT_EQ(73, task.values[0].int_value);
+}
+
+TEST(TestDiagnosticUpdater, create_diagnostic_task_with_context_rejects_nulls) {
+  diagnostic_task_t task;
+
+  EXPECT_EQ(
+    RCL_RET_INVALID_ARGUMENT,
+    rclc_diagnostic_task_init_with_context(
+      nullptr, 0, 0, &update_function_mockup_with_context, nullptr));
+  EXPECT_EQ(
+    RCL_RET_INVALID_ARGUMENT,
+    rclc_diagnostic_task_init_with_context(&task, 0, 0, nullptr, nullptr));
 }
 
 TEST(TestDiagnosticUpdater, create_diagnostic_values) {