From 7601a7aff382d65a669049757e0ce8bc3b12e886 Mon Sep 17 00:00:00 2001 From: Daniil Mordanov <153565951+Daniiiil1@users.noreply.github.com> Date: Fri, 7 Aug 2026 19:43:01 +0700 Subject: [PATCH] Add context-aware diagnostic tasks Signed-off-by: Daniil Mordanov <153565951+Daniiiil1@users.noreply.github.com> --- NOTICE | 2 + micro_ros_diagnostic_updater/README.md | 27 +++++++++ .../example/example_website_checker.c | 37 +++++++------ .../micro_ros_diagnostic_updater.h | 27 +++++++-- .../micro_ros_diagnostic_updater.c | 32 ++++++++++- .../test/test_diagnostic_updater.cpp | 55 +++++++++++++++++++ 6 files changed, 155 insertions(+), 25 deletions(-) diff --git a/NOTICE b/NOTICE index 0ab1cd3..42aa8e8 100644 --- a/NOTICE +++ b/NOTICE @@ -24,5 +24,7 @@ Capra Robotics ApS Bartolome Jimenez Vera +Daniil Mordanov <153565951+Daniiiil1@users.noreply.github.com> + Robert Bosch GmbH Arne Nordmann diff --git a/micro_ros_diagnostic_updater/README.md b/micro_ros_diagnostic_updater/README.md index 864192b..978e5b7 100644 --- a/micro_ros_diagnostic_updater/README.md +++ b/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 `/diagnostics_uros/force_update`. Keep in mind, the `` 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. diff --git a/micro_ros_diagnostic_updater/example/example_website_checker.c b/micro_ros_diagnostic_updater/example/example_website_checker.c index b56e299..4811080 100644 --- a/micro_ros_diagnostic_updater/example/example_website_checker.c +++ b/micro_ros_diagnostic_updater/example/example_website_checker.c @@ -20,8 +20,11 @@ #include #include -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; diff --git a/micro_ros_diagnostic_updater/include/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.h b/micro_ros_diagnostic_updater/include/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.h index ad73a12..40153e8 100644 --- a/micro_ros_diagnostic_updater/include/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.h +++ b/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 diff --git a/micro_ros_diagnostic_updater/src/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.c b/micro_ros_diagnostic_updater/src/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.c index 4a6c9eb..24c1439 100644 --- a/micro_ros_diagnostic_updater/src/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.c +++ b/micro_ros_diagnostic_updater/src/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.c @@ -86,9 +86,7 @@ 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); @@ -96,6 +94,30 @@ rclc_diagnostic_task_init( 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); } diff --git a/micro_ros_diagnostic_updater/test/test_diagnostic_updater.cpp b/micro_ros_diagnostic_updater/test/test_diagnostic_updater.cpp index fdfde4e..ac9cd46 100644 --- a/micro_ros_diagnostic_updater/test/test_diagnostic_updater.cpp +++ b/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(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) {