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/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..276a82c 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 @@ -56,6 +56,8 @@ typedef struct diagnostic_updater_t diagnostic_task_t * tasks[MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_TASKS_PER_UPDATER]; rcl_publisher_t diag_pub; micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticStatus diag_status; + micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticKeyValue key_value_buffer[ + MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK]; rcl_subscription_t force_update_subscriber; bool force_update; std_msgs__msg__Empty empty_msg; 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..2007bca 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 @@ -19,9 +19,6 @@ #include #include -static micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticKeyValue key_value_buffer[ - MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK]; - void rclc_diagnostic_value_set_int( diagnostic_value_t * kv, @@ -144,9 +141,9 @@ rclc_diagnostic_updater_init( // message micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticStatus__init(&updater->diag_status); - updater->diag_status.values.data = key_value_buffer; + updater->diag_status.values.data = updater->key_value_buffer; updater->diag_status.values.size = 0; - updater->diag_status.values.capacity = sizeof(key_value_buffer); + updater->diag_status.values.capacity = MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK; updater->force_update = false; diff --git a/micro_ros_diagnostic_updater/test/test_diagnostic_updater.cpp b/micro_ros_diagnostic_updater/test/test_diagnostic_updater.cpp index fdfde4e..326fe9d 100644 --- a/micro_ros_diagnostic_updater/test/test_diagnostic_updater.cpp +++ b/micro_ros_diagnostic_updater/test/test_diagnostic_updater.cpp @@ -123,6 +123,53 @@ TEST(TestDiagnosticUpdater, create_updater) { EXPECT_EQ(RCL_RET_OK, rc); } +TEST(TestDiagnosticUpdater, updaters_have_independent_value_buffers) { + rclc_support_t support; + rcl_ret_t rc; + + // node + rcl_allocator_t allocator = rcl_get_default_allocator(); + rc = rclc_support_init(&support, 0, nullptr, &allocator); + const char * my_name = "test_independent_updaters_node"; + const char * my_namespace = ""; + rcl_node_t node = rcl_get_zero_initialized_node(); + rc = rclc_node_init_default(&node, my_name, my_namespace, &support); + + // executor + rclc_executor_t executor; + executor = rclc_executor_get_zero_initialized_executor(); + unsigned int num_handles = 2; + rclc_executor_init(&executor, &support.context, num_handles, &allocator); + + // updaters + diagnostic_updater_t updater_0; + diagnostic_updater_t updater_1; + rc = rclc_diagnostic_updater_init(&updater_0, &node, &executor); + ASSERT_EQ(RCL_RET_OK, rc); + rc = rclc_diagnostic_updater_init(&updater_1, &node, &executor); + ASSERT_EQ(RCL_RET_OK, rc); + + EXPECT_EQ(updater_0.key_value_buffer, updater_0.diag_status.values.data); + EXPECT_EQ(updater_1.key_value_buffer, updater_1.diag_status.values.data); + EXPECT_NE(updater_0.diag_status.values.data, updater_1.diag_status.values.data); + EXPECT_EQ( + MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK, + updater_0.diag_status.values.capacity); + EXPECT_EQ( + MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK, + updater_1.diag_status.values.capacity); + + updater_0.key_value_buffer[0].key = 17; + updater_1.key_value_buffer[0].key = 42; + EXPECT_EQ(17, updater_0.diag_status.values.data[0].key); + EXPECT_EQ(42, updater_1.diag_status.values.data[0].key); + + rc = rclc_diagnostic_updater_fini(&updater_0, &node, &executor); + EXPECT_EQ(RCL_RET_OK, rc); + rc = rclc_diagnostic_updater_fini(&updater_1, &node, &executor); + EXPECT_EQ(RCL_RET_OK, rc); +} + TEST(TestDiagnosticUpdater, updater_add_tasks) { rclc_support_t support; rcl_ret_t rc;