Skip to content

Commit 4d8c5be

Browse files
bjv-caprabjv-capra
committed
feat: possibility to force update when no changes have occured
Signed-off-by: Bart Jimenez Vera <bjv@capra.ooo>
1 parent 0e62066 commit 4d8c5be

4 files changed

Lines changed: 93 additions & 12 deletions

File tree

‎micro_ros_diagnostic_updater/include/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.h‎

Lines changed: 14 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,10 @@
1717
#define MICRO_ROS_DIAGNOSTIC_UPDATER__MICRO_ROS_DIAGNOSTIC_UPDATER_H_
1818

1919
#include <rcl/types.h>
20+
#include <std_msgs/msg/empty.h>
2021
#include <rclc/publisher.h>
22+
#include <rclc/subscription.h>
23+
#include <rclc/executor.h>
2124
#include <micro_ros_diagnostic_msgs/msg/micro_ros_diagnostic_status.h>
2225
#include "micro_ros_diagnostic_updater/config.h"
2326

@@ -53,6 +56,9 @@ typedef struct diagnostic_updater_t
5356
diagnostic_task_t * tasks[MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_TASKS_PER_UPDATER];
5457
rcl_publisher_t diag_pub;
5558
micro_ros_diagnostic_msgs__msg__MicroROSDiagnosticStatus diag_status;
59+
rcl_subscription_t force_update_subscriber;
60+
bool force_update;
61+
std_msgs__msg__Empty empty_msg;
5662
} diagnostic_updater_t;
5763

5864
void rclc_diagnostic_value_set_int(
@@ -84,15 +90,21 @@ rclc_diagnostic_task_init(
8490
diagnostic_value_t[MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK],
8591
uint8_t * number_of_values));
8692

93+
// Added to work with force update, it's very important to call spin
94+
// or spin_some before updater_update
95+
void force_update_callback(const void *, void * updater_ptr);
96+
8797
rcl_ret_t
8898
rclc_diagnostic_updater_init(
8999
diagnostic_updater_t * updater,
90-
const rcl_node_t * node);
100+
rcl_node_t * node,
101+
rclc_executor_t * executor);
91102

92103
rcl_ret_t
93104
rclc_diagnostic_updater_fini(
94105
diagnostic_updater_t * updater,
95-
rcl_node_t * node);
106+
rcl_node_t * node,
107+
rclc_executor_t * executor);
96108

97109
rcl_ret_t
98110
rclc_diagnostic_updater_add_task(

‎micro_ros_diagnostic_updater/src/config.h.in‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,5 +4,6 @@
44
#define MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_TASKS_PER_UPDATER @MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_TASKS_PER_UPDATER@
55
#define MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK @MICRO_ROS_DIAGNOSTIC_UPDATER_MAX_VALUES_PER_TASK@
66
static const char UROS_DIAGNOSTIC_UPDATER_TOPIC[] = "@MICRO_ROS_DIAGNOSTIC_UPDATER_DIAGNOSTICS_TOPIC_PREFIX@diagnostics_uros";
7+
static const char UROS_DIAGNOSTIC_UPDATER_FORCE_UPDATE_TOPIC[] = "@MICRO_ROS_DIAGNOSTIC_UPDATER_DIAGNOSTICS_TOPIC_PREFIX@diagnostics_uros/force_update";
78

89
#endif // MICRO_ROS_DIAGNOSTIC_UPDATER_CONFIG_H

‎micro_ros_diagnostic_updater/src/micro_ros_diagnostic_updater/micro_ros_diagnostic_updater.c‎

Lines changed: 54 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -102,10 +102,19 @@ rclc_diagnostic_task_init(
102102
return RCL_RET_OK;
103103
}
104104

105+
void force_update_callback(const void * msgin, void * updater_ptr)
106+
{
107+
//we ignore msgin as it's empty
108+
(void) msgin;
109+
diagnostic_updater_t * updater = (diagnostic_updater_t *) updater_ptr;
110+
updater->force_update = true;
111+
}
112+
105113
rcl_ret_t
106114
rclc_diagnostic_updater_init(
107115
diagnostic_updater_t * updater,
108-
const rcl_node_t * node)
116+
rcl_node_t * node,
117+
rclc_executor_t * executor)
109118
{
110119
RCL_CHECK_FOR_NULL_WITH_MSG(
111120
updater, "updater is a null pointer", return RCL_RET_INVALID_ARGUMENT);
@@ -139,13 +148,45 @@ rclc_diagnostic_updater_init(
139148
updater->diag_status.values.size = 0;
140149
updater->diag_status.values.capacity = sizeof(key_value_buffer);
141150

151+
updater->force_update = false;
152+
153+
const rosidl_message_type_support_t * empty_type_support =
154+
ROSIDL_GET_MSG_TYPE_SUPPORT(
155+
std_msgs,
156+
msg,
157+
Empty);
158+
rc = rclc_subscription_init_default(
159+
&updater->force_update_subscriber, node, empty_type_support,
160+
UROS_DIAGNOSTIC_UPDATER_FORCE_UPDATE_TOPIC);
161+
if (RCL_RET_OK != rc) {
162+
RCUTILS_LOG_ERROR(
163+
"Updater '%d' could not create subscriber /%s.",
164+
updater->id,
165+
UROS_DIAGNOSTIC_UPDATER_FORCE_UPDATE_TOPIC);
166+
return RCL_RET_ERROR;
167+
}
168+
std_msgs__msg__Empty__init(&updater->empty_msg);
169+
// Executor should already be initialized
170+
rc = rclc_executor_add_subscription_with_context(
171+
executor, &updater->force_update_subscriber,
172+
&updater->empty_msg, &force_update_callback,
173+
(void *)updater, ON_NEW_DATA);
174+
175+
if (RCL_RET_OK != rc) {
176+
RCUTILS_LOG_ERROR(
177+
"Updater '%d' could not add subscription to executor.",
178+
updater->id);
179+
return RCL_RET_ERROR;
180+
}
181+
142182
return RCL_RET_OK;
143183
}
144184

145185
rcl_ret_t
146186
rclc_diagnostic_updater_fini(
147187
diagnostic_updater_t * updater,
148-
rcl_node_t * node)
188+
rcl_node_t * node,
189+
rclc_executor_t * executor)
149190
{
150191
RCL_CHECK_FOR_NULL_WITH_MSG(
151192
updater, "updater is a null pointer", return RCL_RET_INVALID_ARGUMENT);
@@ -164,6 +205,13 @@ rclc_diagnostic_updater_fini(
164205
return RCL_RET_ERROR;
165206
}
166207

208+
rc = rclc_executor_remove_subscription(executor, &updater->force_update_subscriber);
209+
if (RCL_RET_OK != rc) {
210+
RCUTILS_LOG_ERROR(
211+
"Error when cleaning updater. Could not remove subscription.");
212+
return RCL_RET_ERROR;
213+
}
214+
167215
return RCL_RET_OK;
168216
}
169217

@@ -222,8 +270,8 @@ rclc_diagnostic_updater_update(
222270
for (uint8_t value_index = 0u; value_index < updater->tasks[i]->number_of_values;
223271
value_index++)
224272
{
225-
// Quickly go to the next value if it has not changed
226-
if (!updater->tasks[i]->values[value_index].value_has_changed) {
273+
// Quickly go to the next value if it has not changed or no force update
274+
if (!updater->tasks[i]->values[value_index].value_has_changed && !updater->force_update) {
227275
continue;
228276
}
229277
must_publish = true;
@@ -254,6 +302,8 @@ rclc_diagnostic_updater_update(
254302
"Updater %d could not update diagnostic task '%d'.", updater->id, i);
255303
}
256304
}
305+
// Reset regardless
306+
updater->force_update = false;
257307

258308
return RCL_RET_OK;
259309
}

‎micro_ros_diagnostic_updater/test/test_diagnostic_updater.cpp‎

Lines changed: 24 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -107,13 +107,19 @@ TEST(TestDiagnosticUpdater, create_updater) {
107107
rcl_node_t node = rcl_get_zero_initialized_node();
108108
rc = rclc_node_init_default(&node, my_name, my_namespace, &support);
109109

110+
// executor
111+
rclc_executor_t executor;
112+
executor = rclc_executor_get_zero_initialized_executor();
113+
unsigned int num_handles = 1;
114+
rclc_executor_init(&executor, &support.context, num_handles, &allocator);
115+
110116
// updater
111117
diagnostic_updater_t updater;
112-
rc = rclc_diagnostic_updater_init(&updater, &node);
118+
rc = rclc_diagnostic_updater_init(&updater, &node, &executor);
113119
EXPECT_EQ(RCL_RET_OK, rc);
114120

115121
// updater
116-
rc = rclc_diagnostic_updater_fini(&updater, &node);
122+
rc = rclc_diagnostic_updater_fini(&updater, &node, &executor);
117123
EXPECT_EQ(RCL_RET_OK, rc);
118124
}
119125

@@ -129,9 +135,15 @@ TEST(TestDiagnosticUpdater, updater_add_tasks) {
129135
rcl_node_t node = rcl_get_zero_initialized_node();
130136
rc = rclc_node_init_default(&node, my_name, my_namespace, &support);
131137

138+
// executor
139+
rclc_executor_t executor;
140+
executor = rclc_executor_get_zero_initialized_executor();
141+
unsigned int num_handles = 1;
142+
rclc_executor_init(&executor, &support.context, num_handles, &allocator);
143+
132144
// updater
133145
diagnostic_updater_t updater;
134-
rc = rclc_diagnostic_updater_init(&updater, &node);
146+
rc = rclc_diagnostic_updater_init(&updater, &node, &executor);
135147
EXPECT_EQ(RCL_RET_OK, rc);
136148

137149
diagnostic_task_t task;
@@ -151,7 +163,7 @@ TEST(TestDiagnosticUpdater, updater_add_tasks) {
151163
EXPECT_EQ(RCL_RET_ERROR, rc);
152164

153165
// updater
154-
rc = rclc_diagnostic_updater_fini(&updater, &node);
166+
rc = rclc_diagnostic_updater_fini(&updater, &node, &executor);
155167
EXPECT_EQ(RCL_RET_OK, rc);
156168
}
157169

@@ -167,9 +179,15 @@ TEST(TestDiagnosticUpdater, updater_update) {
167179
rcl_node_t node = rcl_get_zero_initialized_node();
168180
rc = rclc_node_init_default(&node, my_name, my_namespace, &support);
169181

182+
// executor
183+
rclc_executor_t executor;
184+
executor = rclc_executor_get_zero_initialized_executor();
185+
unsigned int num_handles = 1;
186+
rclc_executor_init(&executor, &support.context, num_handles, &allocator);
187+
170188
// updater
171189
diagnostic_updater_t updater;
172-
rc = rclc_diagnostic_updater_init(&updater, &node);
190+
rc = rclc_diagnostic_updater_init(&updater, &node, &executor);
173191
EXPECT_EQ(RCL_RET_OK, rc);
174192

175193
diagnostic_task_t task0, task1;
@@ -194,6 +212,6 @@ TEST(TestDiagnosticUpdater, updater_update) {
194212
EXPECT_EQ(RCL_RET_OK, rc);
195213

196214
// updater
197-
rc = rclc_diagnostic_updater_fini(&updater, &node);
215+
rc = rclc_diagnostic_updater_fini(&updater, &node, &executor);
198216
EXPECT_EQ(RCL_RET_OK, rc);
199217
}

0 commit comments

Comments
 (0)