Skip to content

Commit d9df00b

Browse files
committed
Add continuos serialization
1 parent d53cc47 commit d9df00b

5 files changed

Lines changed: 63 additions & 14 deletions

File tree

_docs/tutorials/programming_rcl_rclc/executor/executor.md

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -53,7 +53,6 @@ void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
5353
5454
During the callback the timer can be canceled or have its period and/or callback modified using the passed pointer. Check [rcl/timer.h](https://github.com/ros2/rcl/blob/galactic/rcl/include/rcl/timer.h) for details.
5555
56-
5756
### Cleaning Up
5857
5958
To destroy an initialized timer:

_docs/tutorials/programming_rcl_rclc/micro-ROS/micro-ROS.md

Lines changed: 59 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -168,9 +168,62 @@ else
168168

169169
## Continous serialization
170170

171-
-```c
172-
void rmw_uros_set_continous_serialization_callbacks(
173-
rmw_publisher_t * publisher,
174-
rmw_uros_continous_serialization_size size_cb,
175-
rmw_uros_continous_serialization serialization_cb);
176-
```
171+
This utility allows the client to serialize and send data up to a customized size. The user can set the topic lenght and then serialize the data within the publish process. An example can be found on [`micro-ROS-demos/rclc/ping_uros_agent`](https://github.com/micro-ROS/micro-ROS-demos/blob/galactic/rclc/ping_uros_agent/main.c), where fragments from an image are requested and serialized on the spot.
172+
173+
The user needs to define two callbacks, then set them on the `rmw`. It is recommended to clean the callbacks after the publication, to avoid interferences with other topics on the same process:
174+
175+
```c
176+
// Set serialization callbacks
177+
rmw_uros_set_continous_serialization_callbacks(size_cb, serialization_cb);
178+
179+
// Publish message
180+
rcl_publish(...);
181+
182+
// Clean callbacks
183+
rmw_uros_set_continous_serialization_callbacks(NULL, NULL);
184+
```
185+
186+
- Size callback:
187+
188+
This callback will pass a pointer with the calculated message size. The user is responsible of increase this size to the expected value:
189+
190+
```c
191+
// Function prototype:
192+
void (* rmw_uros_continous_serialization_size)(uint32_t * topic_length);
193+
194+
// Implementation example:
195+
void size_cb(uint32_t * topic_length){
196+
// Increase message size
197+
*topic_length += ucdr_alignment(*topic_length, sizeof(uint32_t)) + sizeof(uint32_t);
198+
*topic_length += IMAGE_BYTES;
199+
}
200+
```
201+
202+
- Serialize callback:
203+
204+
This callback gives the user the message buffer to be completed. The user is responsible of serialize the data up to the lenght established on the size callback:
205+
206+
```c
207+
// Function prototype:
208+
void (* rmw_uros_continous_serialization)(ucdrBuffer * ucdr);
209+
210+
// Implementation example:
211+
void serialization_cb(ucdrBuffer * ucdr){
212+
size_t len = 0;
213+
micro_ros_fragment_t fragment;
214+
215+
// Serialize array size
216+
ucdr_serialize_uint32_t(ucdr, IMAGE_BYTES);
217+
218+
while(len < IMAGE_BYTES){
219+
// Wait for new image "fragment"
220+
...
221+
222+
// Serialize data fragment
223+
ucdr_serialize_array_uint8_t(ucdr, fragment.data, fragment.len);
224+
len += fragment.len;
225+
}
226+
}
227+
```
228+
229+
*Note: When the callback ends, the message will be published.*

_docs/tutorials/programming_rcl_rclc/pub_sub/pub_sub.md

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -199,7 +199,7 @@ The function will have the message as its only argument, containing the values s
199199
// Function prototype:
200200
void (* rclc_subscription_callback_t)(const void *);
201201
202-
// Example:
202+
// Implementation example:
203203
void subscription_callback(const void * msgin)
204204
{
205205
// Cast received message to used type
@@ -210,7 +210,6 @@ void subscription_callback(const void * msgin)
210210
}
211211
```
212212

213-
214213
Once the subscriber and the executor are initialized, the subscriber callback must be added to the executor to receive incoming publications once its spinning:
215214

216215
```c
@@ -225,6 +224,7 @@ rcl_ret_t rc = rclc_executor_add_subscription(
225224
if (RCL_RET_OK != rc) {
226225
... // Handle error
227226
return -1;
227+
}
228228

229229
// Spin executor to receive messages
230230
rclc_executor_spin(&executor);

_docs/tutorials/programming_rcl_rclc/qos/QoS.md

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -3,9 +3,6 @@ title: Quality of service
33
permalink: /docs/tutorials/programming_rcl_rclc/qos/
44
---
55

6-
// Add benchmark results for Throughput and RTT to compare both modes?
7-
// Explain custom QoS options
8-
96
- [Reliable QoS](#reliable-qos)
107
- [Best Effort](#best-effort)
118
- [Custom QoS configuration](#custom-qos-configuration)

_docs/tutorials/programming_rcl_rclc/service/services.md

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -113,7 +113,7 @@ The client request message will contain two integers `a` and `b`, and expects th
113113
// Function prototype:
114114
void (* rclc_service_callback_t)(const void *, void *);
115115

116-
// Example:
116+
// Implementation example:
117117
void service_callback(const void * request_msg, void * response_msg){
118118
// Cast messages to expected types
119119
example_interfaces__srv__AddTwoInts_Request * req_in =
@@ -233,7 +233,7 @@ It is necessary to cast the response message to the expected type. Example:
233233
// Function prototype:
234234
void (* rclc_client_callback_t)(const void *);
235235
236-
// Example:
236+
// Implementation example:
237237
void client_callback(const void * response_msg){
238238
// Cast response message to expected type
239239
example_interfaces__srv__AddTwoInts_Response * msgin =

0 commit comments

Comments
 (0)