Skip to content

Commit 19a8dd0

Browse files
pablogs9jamoralp
pablogs9
and
jamoralp
authored
Static mem pools (#113)
* Static incoming mempools Update Parametrize lenght * Simplify xml/ref buffer * Uncrustified * Remove unused * Indent * Update * Update rmw_microxrcedds_c/test/test_sizes.cpp Co-authored-by: Jose Antonio Moral <joseantoniomoralparras@gmail.com> * Update default history value * Apply suggestions from code review Co-authored-by: Jose Antonio Moral <joseantoniomoralparras@gmail.com> * Update rmw_microxrcedds_c/src/memory.h Co-authored-by: GitHub Actions Bot <> Co-authored-by: Jose Antonio Moral <joseantoniomoralparras@gmail.com>
1 parent e575776 commit 19a8dd0

19 files changed

Lines changed: 228 additions & 200 deletions

‎rmw_microxrcedds_c/CMakeLists.txt‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,7 @@ endif()
4242
set(RMW_UXRCE_TRANSPORT "udp" CACHE STRING "Sets Micro XRCE-DDS transport to use. (udp | serial | custom)")
4343
set(RMW_UXRCE_IPV "ipv4" CACHE STRING "Sets Micro XRCE-DDS IP version to use. (ipv4 | ipv6)")
4444
set(RMW_UXRCE_CREATION_MODE "xml" CACHE STRING "Sets creation mode in Micro XRCE-DDS. (xml | refs)")
45-
set(RMW_UXRCE_MAX_HISTORY "4" CACHE STRING "This value sets the number of history slots available for RMW subscriptions, requests and replies")
45+
set(RMW_UXRCE_MAX_HISTORY "8" CACHE STRING "This value sets the number of history slots available for RMW subscriptions, requests and replies")
4646
set(RMW_UXRCE_MAX_SESSIONS "1" CACHE STRING "This value sets the maximum number of Micro XRCE-DDS sessions.")
4747
set(RMW_UXRCE_MAX_NODES "4" CACHE STRING "This value sets the maximum number of nodes.")
4848
set(RMW_UXRCE_MAX_PUBLISHERS "4" CACHE STRING "This value sets the maximum number of publishers for an application.")
@@ -96,12 +96,12 @@ else()
9696
endif()
9797

9898
# Create entities type define macros.
99-
set(RMW_UXRCE_TRANSPORT_USE_REFS OFF)
100-
set(RMW_UXRCE_TRANSPORT_USE_XML OFF)
99+
set(RMW_UXRCE_USE_REFS OFF)
100+
set(RMW_UXRCE_USE_XML OFF)
101101
if(${RMW_UXRCE_CREATION_MODE} STREQUAL "refs")
102-
set(RMW_UXRCE_TRANSPORT_USE_REFS ON)
102+
set(RMW_UXRCE_USE_REFS ON)
103103
elseif(${RMW_UXRCE_CREATION_MODE} STREQUAL "xml")
104-
set(RMW_UXRCE_TRANSPORT_USE_XML ON)
104+
set(RMW_UXRCE_USE_XML ON)
105105
else()
106106
message(FATAL_ERROR "Creation mode not supported. Use \"refs\" or \"xml\"")
107107
endif()

‎rmw_microxrcedds_c/src/callbacks.c‎

Lines changed: 56 additions & 43 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,8 @@
1414

1515
#include "./callbacks.h"
1616

17+
#include <rmw/error_handling.h>
18+
1719
void on_status(
1820
struct uxrSession* session,
1921
uxrObjectId object_id,
@@ -58,30 +60,35 @@ void on_topic(
5860
(void)args;
5961
#endif // RMW_UXRCE_GRAPH
6062

63+
// Iterate along the allocated subscriptions
6164
rmw_uxrce_mempool_item_t* subscription_item = subscription_memory.allocateditems;
6265
while (subscription_item != NULL)
6366
{
6467
rmw_uxrce_subscription_t* custom_subscription =
6568
(rmw_uxrce_subscription_t*)subscription_item->data;
69+
70+
// Check if topic is related to the subscription
6671
if ((custom_subscription->datareader_id.id == object_id.id) &&
6772
(custom_subscription->datareader_id.type == object_id.type))
6873
{
69-
custom_subscription->micro_buffer_lenght[custom_subscription->history_write_index] = length;
70-
ucdr_deserialize_array_uint8_t(
71-
ub,
72-
custom_subscription->micro_buffer[custom_subscription->history_write_index], length);
73-
74-
// TODO(pablogs9): Circular overlapping buffer implemented: use qos
75-
if (custom_subscription->micro_buffer_in_use &&
76-
custom_subscription->history_write_index == custom_subscription->history_read_index)
74+
rmw_uxrce_mempool_item_t* memory_node = get_memory(&static_buffer_memory);
75+
if (!memory_node)
7776
{
78-
custom_subscription->history_read_index = (custom_subscription->history_read_index + 1) %
79-
RMW_UXRCE_MAX_HISTORY;
77+
RMW_SET_ERROR_MSG("Not available static buffer memory node");
78+
return;
8079
}
8180

82-
custom_subscription->history_write_index = (custom_subscription->history_write_index + 1) %
83-
RMW_UXRCE_MAX_HISTORY;
84-
custom_subscription->micro_buffer_in_use = true;
81+
rmw_uxrce_static_input_buffer_t* static_buffer = (rmw_uxrce_static_input_buffer_t*)memory_node->data;
82+
static_buffer->owner = (void*) custom_subscription;
83+
static_buffer->length = length;
84+
85+
if (!ucdr_deserialize_array_uint8_t(
86+
ub,
87+
static_buffer->buffer,
88+
length))
89+
{
90+
put_memory(&static_buffer_memory, memory_node);
91+
}
8592

8693
break;
8794
}
@@ -102,31 +109,33 @@ void on_request(
102109
(void)object_id;
103110
(void)args;
104111

112+
// Iterate along the allocated services
105113
rmw_uxrce_mempool_item_t* service_item = service_memory.allocateditems;
106114
while (service_item != NULL)
107115
{
116+
// Check if request is related to the service
108117
rmw_uxrce_service_t* custom_service = (rmw_uxrce_service_t*)service_item->data;
109118
if (custom_service->service_data_resquest == request_id)
110119
{
111-
custom_service->micro_buffer_lenght[custom_service->history_write_index] = length;
112-
ucdr_deserialize_array_uint8_t(
113-
ub,
114-
custom_service->micro_buffer[custom_service->history_write_index], length);
115-
memcpy(
116-
&custom_service->sample_id[custom_service->history_write_index],
117-
sample_id, sizeof(SampleIdentity));
118-
119-
// TODO(pablogs9): Circular overlapping buffer implemented: use qos
120-
if (custom_service->micro_buffer_in_use &&
121-
custom_service->history_write_index == custom_service->history_read_index)
120+
rmw_uxrce_mempool_item_t* memory_node = get_memory(&static_buffer_memory);
121+
if (!memory_node)
122122
{
123-
custom_service->history_read_index = (custom_service->history_read_index + 1) %
124-
RMW_UXRCE_MAX_HISTORY;
123+
RMW_SET_ERROR_MSG("Not available static buffer memory node");
124+
return;
125125
}
126126

127-
custom_service->history_write_index = (custom_service->history_write_index + 1) %
128-
RMW_UXRCE_MAX_HISTORY;
129-
custom_service->micro_buffer_in_use = true;
127+
rmw_uxrce_static_input_buffer_t* static_buffer = (rmw_uxrce_static_input_buffer_t*)memory_node->data;
128+
static_buffer->owner = (void*) custom_service;
129+
static_buffer->length = length;
130+
static_buffer->related.sample_id = *sample_id;
131+
132+
if (!ucdr_deserialize_array_uint8_t(
133+
ub,
134+
static_buffer->buffer,
135+
length))
136+
{
137+
put_memory(&static_buffer_memory, memory_node);
138+
}
130139

131140
break;
132141
}
@@ -147,29 +156,33 @@ void on_reply(
147156
(void)object_id;
148157
(void)args;
149158

159+
// Iterate along the allocated clients
150160
rmw_uxrce_mempool_item_t* client_item = client_memory.allocateditems;
151161
while (client_item != NULL)
152162
{
163+
// Check if reply is related to the client
153164
rmw_uxrce_client_t* custom_client = (rmw_uxrce_client_t*)client_item->data;
154165
if (custom_client->client_data_request == request_id)
155166
{
156-
custom_client->micro_buffer_lenght[custom_client->history_write_index] = length;
157-
ucdr_deserialize_array_uint8_t(
158-
ub,
159-
custom_client->micro_buffer[custom_client->history_write_index], length);
160-
custom_client->reply_id[custom_client->history_write_index] = reply_id;
161-
162-
// TODO(pablogs9): Circular overlapping buffer implemented: use qos
163-
if (custom_client->micro_buffer_in_use &&
164-
custom_client->history_write_index == custom_client->history_read_index)
167+
rmw_uxrce_mempool_item_t* memory_node = get_memory(&static_buffer_memory);
168+
if (!memory_node)
165169
{
166-
custom_client->history_read_index = (custom_client->history_read_index + 1) %
167-
RMW_UXRCE_MAX_HISTORY;
170+
RMW_SET_ERROR_MSG("Not available static buffer memory node");
171+
return;
168172
}
169173

170-
custom_client->history_write_index = (custom_client->history_write_index + 1) %
171-
RMW_UXRCE_MAX_HISTORY;
172-
custom_client->micro_buffer_in_use = true;
174+
rmw_uxrce_static_input_buffer_t* static_buffer = (rmw_uxrce_static_input_buffer_t*)memory_node->data;
175+
static_buffer->owner = (void*) custom_client;
176+
static_buffer->length = length;
177+
static_buffer->related.reply_id = reply_id;
178+
179+
if (!ucdr_deserialize_array_uint8_t(
180+
ub,
181+
static_buffer->buffer,
182+
length))
183+
{
184+
put_memory(&static_buffer_memory, memory_node);
185+
}
173186

174187
break;
175188
}

‎rmw_microxrcedds_c/src/config.h.in‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,8 @@
88
#cmakedefine RMW_UXRCE_TRANSPORT_CUSTOM
99
#cmakedefine RMW_UXRCE_TRANSPORT_IPV4
1010
#cmakedefine RMW_UXRCE_TRANSPORT_IPV6
11-
#cmakedefine RMW_UXRCE_TRANSPORT_USE_REFS
12-
#cmakedefine RMW_UXRCE_TRANSPORT_USE_XML
11+
#cmakedefine RMW_UXRCE_USE_REFS
12+
#cmakedefine RMW_UXRCE_USE_XML
1313
#cmakedefine RMW_UXRCE_ALLOW_DYNAMIC_ALLOCATIONS
1414
#cmakedefine RMW_UXRCE_GRAPH
1515

@@ -60,7 +60,11 @@
6060
#define RMW_UXRCE_NODE_NAME_MAX_LENGTH @RMW_UXRCE_NODE_NAME_MAX_LENGTH@
6161
#define RMW_UXRCE_TOPIC_NAME_MAX_LENGTH @RMW_UXRCE_TOPIC_NAME_MAX_LENGTH@
6262
#define RMW_UXRCE_TYPE_NAME_MAX_LENGTH @RMW_UXRCE_TYPE_NAME_MAX_LENGTH@
63-
#define RMW_UXRCE_XML_BUFFER_LENGTH @RMW_UXRCE_XML_BUFFER_LENGTH@
64-
#define RMW_UXRCE_REF_BUFFER_LENGTH @RMW_UXRCE_REF_BUFFER_LENGTH@
63+
64+
#if defined(RMW_UXRCE_USE_XML)
65+
#define RMW_UXRCE_ENTITY_NAMING_BUFFER_LENGTH @RMW_UXRCE_XML_BUFFER_LENGTH@
66+
#elif defined(RMW_UXRCE_USE_REFS)
67+
#define RMW_UXRCE_ENTITY_NAMING_BUFFER_LENGTH @RMW_UXRCE_REF_BUFFER_LENGTH@
68+
#endif
6569

6670
#endif // RMW_MICROXRCEDDS_CONFIG_H

‎rmw_microxrcedds_c/src/memory.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,13 +26,13 @@ typedef struct rmw_uxrce_mempool_item_t
2626
void* data;
2727
} rmw_uxrce_mempool_item_t;
2828

29+
2930
typedef struct rmw_uxrce_mempool_t
3031
{
3132
struct rmw_uxrce_mempool_item_t* allocateditems;
3233
struct rmw_uxrce_mempool_item_t* freeitems;
3334

3435
bool is_initialized;
35-
size_t size;
3636
size_t element_size;
3737
} rmw_uxrce_mempool_t;
3838

‎rmw_microxrcedds_c/src/rmw_client.c‎

Lines changed: 6 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -87,8 +87,6 @@ rmw_create_client(
8787
custom_client->owner_node = custom_node;
8888
custom_client->client_gid.implementation_identifier =
8989
rmw_get_implementation_identifier();
90-
custom_client->history_write_index = 0;
91-
custom_client->history_read_index = 0;
9290

9391
const rosidl_service_type_support_t* type_support_xrce = NULL;
9492
#ifdef ROSIDL_TYPESUPPORT_MICROXRCEDDS_C__IDENTIFIER_VALUE
@@ -133,13 +131,13 @@ rmw_create_client(
133131

134132
uint16_t client_req = UXR_INVALID_REQUEST_ID;
135133

136-
#ifdef RMW_UXRCE_TRANSPORT_USE_XML
134+
#ifdef RMW_UXRCE_USE_XML
137135
char service_name_id[20];
138136
generate_name(&custom_client->client_id, service_name_id, sizeof(service_name_id));
139137
if (!build_service_xml(
140138
service_name_id, service_name, true,
141-
custom_client->type_support_callbacks, qos_policies, rmw_uxrce_xml_buffer,
142-
sizeof(rmw_uxrce_xml_buffer)))
139+
custom_client->type_support_callbacks, qos_policies, rmw_uxrce_entity_naming_buffer,
140+
sizeof(rmw_uxrce_entity_naming_buffer)))
143141
{
144142
RMW_SET_ERROR_MSG("failed to generate xml request for client creation");
145143
goto fail;
@@ -148,13 +146,13 @@ rmw_create_client(
148146
&custom_node->context->session,
149147
*custom_node->context->creation_destroy_stream,
150148
custom_client->client_id,
151-
custom_node->participant_id, rmw_uxrce_xml_buffer, UXR_REPLACE);
152-
#elif defined(RMW_UXRCE_TRANSPORT_USE_REFS)
149+
custom_node->participant_id, rmw_uxrce_entity_naming_buffer, UXR_REPLACE);
150+
#elif defined(RMW_UXRCE_USE_REFS)
153151
// TODO(pablogs9): Is possible to instantiate a replier by ref?
154152
// client_req = uxr_buffer_create_replier_ref(&custom_node->context->session,
155153
// *custom_node->context->creation_destroy_stream, custom_service->subscriber_id,
156154
// custom_node->participant_id, "", UXR_REPLACE);
157-
#endif /* ifdef RMW_UXRCE_TRANSPORT_USE_XML */
155+
#endif /* ifdef RMW_UXRCE_USE_XML */
158156

159157
rmw_client->data = custom_client;
160158

‎rmw_microxrcedds_c/src/rmw_graph.c‎

Lines changed: 10 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@ rmw_ret_t rmw_graph_init(
4545
const char* graph_participant_name = "microros_graph";
4646

4747
if (!build_participant_xml(microros_domain_id, graph_participant_name,
48-
rmw_uxrce_xml_buffer, sizeof(rmw_uxrce_xml_buffer)))
48+
rmw_uxrce_entity_naming_buffer, sizeof(rmw_uxrce_entity_naming_buffer)))
4949
{
5050
RMW_SET_ERROR_MSG("Failed to generate xml request for graph participant creation");
5151
return RMW_RET_ERROR;
@@ -55,7 +55,7 @@ rmw_ret_t rmw_graph_init(
5555
&context->session,
5656
*context->creation_destroy_stream,
5757
graph_info->participant_id, (int16_t)microros_domain_id,
58-
rmw_uxrce_xml_buffer, UXR_REPLACE);
58+
rmw_uxrce_entity_naming_buffer, UXR_REPLACE);
5959

6060
// Set graph subscription QoS policies
6161
// TODO (jamoralp): most of these QoS are not even being used.
@@ -76,7 +76,7 @@ rmw_ret_t rmw_graph_init(
7676
graph_info->subscriber_id = uxr_object_id(context->id_subscriber++, UXR_SUBSCRIBER_ID);
7777
char subscriber_name[20];
7878
generate_name(&graph_info->subscriber_id, subscriber_name, sizeof(subscriber_name));
79-
if (!build_subscriber_xml(subscriber_name, rmw_uxrce_xml_buffer, sizeof(rmw_uxrce_xml_buffer)))
79+
if (!build_subscriber_xml(subscriber_name, rmw_uxrce_entity_naming_buffer, sizeof(rmw_uxrce_entity_naming_buffer)))
8080
{
8181
RMW_SET_ERROR_MSG("Failed to generate xml request for graph subscriber creation");
8282
ret = RMW_RET_ERROR;
@@ -85,7 +85,7 @@ rmw_ret_t rmw_graph_init(
8585

8686
uint16_t subscriber_req = uxr_buffer_create_subscriber_xml(
8787
&context->session, *context->creation_destroy_stream, graph_info->subscriber_id,
88-
graph_info->participant_id, rmw_uxrce_xml_buffer, UXR_REPLACE);
88+
graph_info->participant_id, rmw_uxrce_entity_naming_buffer, UXR_REPLACE);
8989

9090
graph_info->datareader_id = uxr_object_id(context->id_datareader++, UXR_DATAREADER_ID);
9191
const char* graph_topic_name = "ros_to_microros_graph";
@@ -97,7 +97,8 @@ rmw_ret_t rmw_graph_init(
9797
if (!build_topic_xml(
9898
graph_topic_name,
9999
(message_type_support_callbacks_t*)(graph_info->graph_type_support->data),
100-
&graph_subscription_qos_policies, rmw_uxrce_xml_buffer, sizeof(rmw_uxrce_xml_buffer)))
100+
&graph_subscription_qos_policies, rmw_uxrce_entity_naming_buffer,
101+
sizeof(rmw_uxrce_entity_naming_buffer)))
101102
{
102103
RMW_SET_ERROR_MSG("Failed to generate xml request for graph topic creation");
103104
ret = RMW_RET_ERROR;
@@ -106,13 +107,14 @@ rmw_ret_t rmw_graph_init(
106107

107108
uint16_t topic_req = uxr_buffer_create_topic_xml(
108109
&context->session, *context->creation_destroy_stream, graph_info->topic_id,
109-
graph_info->participant_id, rmw_uxrce_xml_buffer, UXR_REPLACE);
110+
graph_info->participant_id, rmw_uxrce_entity_naming_buffer, UXR_REPLACE);
110111

111112
// Create graph datareader request
112113
if (!build_datareader_xml(
113114
graph_topic_name,
114115
(message_type_support_callbacks_t*)(graph_info->graph_type_support->data),
115-
&graph_subscription_qos_policies, rmw_uxrce_xml_buffer, sizeof(rmw_uxrce_xml_buffer)))
116+
&graph_subscription_qos_policies, rmw_uxrce_entity_naming_buffer,
117+
sizeof(rmw_uxrce_entity_naming_buffer)))
116118
{
117119
RMW_SET_ERROR_MSG("Failed to generate xml request for graph datareader creation");
118120
ret = RMW_RET_ERROR;
@@ -121,7 +123,7 @@ rmw_ret_t rmw_graph_init(
121123

122124
uint16_t datareader_req = uxr_buffer_create_datareader_xml(
123125
&context->session, *context->creation_destroy_stream, graph_info->datareader_id,
124-
graph_info->subscriber_id, rmw_uxrce_xml_buffer, UXR_REPLACE);
126+
graph_info->subscriber_id, rmw_uxrce_entity_naming_buffer, UXR_REPLACE);
125127

126128
// Run session
127129
uint16_t requests[] = {

‎rmw_microxrcedds_c/src/rmw_init.c‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -166,6 +166,7 @@ rmw_init(
166166
context->implementation_identifier = eprosima_microxrcedds_identifier;
167167

168168
rmw_uxrce_init_session_memory(&session_memory, custom_sessions, RMW_UXRCE_MAX_SESSIONS);
169+
rmw_uxrce_init_static_input_buffer_memory(&static_buffer_memory, custom_static_buffers, RMW_UXRCE_MAX_HISTORY);
169170

170171
rmw_uxrce_mempool_item_t* memory_node = get_memory(&session_memory);
171172
if (!memory_node)

‎rmw_microxrcedds_c/src/rmw_microxrcedds_topic.c‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -52,10 +52,10 @@ create_topic(
5252

5353
// Generate request
5454
uint16_t topic_req = 0;
55-
#ifdef RMW_UXRCE_TRANSPORT_USE_XML
55+
#ifdef RMW_UXRCE_USE_XML
5656
if (!build_topic_xml(
5757
topic_name, message_type_support_callbacks,
58-
qos_policies, rmw_uxrce_xml_buffer, sizeof(rmw_uxrce_xml_buffer)))
58+
qos_policies, rmw_uxrce_entity_naming_buffer, sizeof(rmw_uxrce_entity_naming_buffer)))
5959
{
6060
RMW_SET_ERROR_MSG("failed to generate xml request for subscriber creation");
6161
rmw_uxrce_fini_topic_memory(custom_topic);
@@ -66,10 +66,10 @@ create_topic(
6666
topic_req = uxr_buffer_create_topic_xml(
6767
&custom_node->context->session,
6868
*custom_node->context->creation_destroy_stream, custom_topic->topic_id,
69-
custom_node->participant_id, rmw_uxrce_xml_buffer, UXR_REPLACE);
70-
#elif defined(RMW_UXRCE_TRANSPORT_USE_REFS)
69+
custom_node->participant_id, rmw_uxrce_entity_naming_buffer, UXR_REPLACE);
70+
#elif defined(RMW_UXRCE_USE_REFS)
7171
(void)qos_policies;
72-
if (!build_topic_profile(topic_name, rmw_uxrce_profile_name, sizeof(rmw_uxrce_profile_name)))
72+
if (!build_topic_profile(topic_name, rmw_uxrce_entity_naming_buffer, sizeof(rmw_uxrce_entity_naming_buffer)))
7373
{
7474
RMW_SET_ERROR_MSG("failed to generate xml request for node creation");
7575
rmw_uxrce_fini_topic_memory(custom_topic);
@@ -80,8 +80,8 @@ create_topic(
8080
topic_req = uxr_buffer_create_topic_ref(
8181
&custom_node->context->session,
8282
*custom_node->context->creation_destroy_stream, custom_topic->topic_id,
83-
custom_node->participant_id, rmw_uxrce_profile_name, UXR_REPLACE);
84-
#endif /* ifdef RMW_UXRCE_TRANSPORT_USE_XML */
83+
custom_node->participant_id, rmw_uxrce_entity_naming_buffer, UXR_REPLACE);
84+
#endif /* ifdef RMW_UXRCE_USE_XML */
8585

8686
// Send the request and wait for response
8787
custom_topic->sync_with_agent = run_xrce_session(custom_node->context, topic_req);

0 commit comments

Comments
 (0)