Skip to content

Commit 77aee92

Browse files
pablogs9pablogs9
authored
Add example on static type handling (#44)
* Add micro_ros_utilities example * Update * Update
1 parent ce93f69 commit 77aee92

File tree

4 files changed

+208
-0
lines changed

4 files changed

+208
-0
lines changed

‎rclc/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@ export_executable(timer)
5252
export_executable(string_publisher)
5353
export_executable(string_subscriber)
5454
export_executable(autodiscover_agent)
55+
export_executable(static_type_handling)
5556
# export_executable(configuration_example custom_transports)
5657

5758
if(EXISTS ${CMAKE_BINARY_DIR}/temp_install/)

‎rclc/package.xml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,8 +17,10 @@
1717
<depend>std_msgs</depend>
1818
<depend>example_interfaces</depend>
1919
<depend>complex_msgs</depend>
20+
<depend>sensor_msgs</depend>
2021
<depend>builtin_interfaces</depend>
2122
<depend>microxrcedds_client</depend>
23+
<depend>micro_ros_utilities</depend>
2224

2325
<!-- configured_publisher and configured_subscriber-->
2426
<depend>geometry_msgs</depend>

‎rclc/static_type_handling/CMakeLists.txt‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
3+
project(static_type_handling LANGUAGES C)
4+
5+
set (CMAKE_C_STANDARD 11)
6+
7+
find_package(ament_cmake REQUIRED)
8+
find_package(rcl REQUIRED)
9+
find_package(rclc REQUIRED)
10+
find_package(visualization_msgs REQUIRED)
11+
find_package(rmw_microxrcedds REQUIRED)
12+
find_package(micro_ros_utilities REQUIRED)
13+
find_package(sensor_msgs REQUIRED)
14+
15+
add_executable(${PROJECT_NAME} main.c)
16+
17+
ament_target_dependencies(${PROJECT_NAME}
18+
rcl
19+
rclc
20+
sensor_msgs
21+
visualization_msgs
22+
rmw_microxrcedds
23+
micro_ros_utilities
24+
)
25+
26+
install(TARGETS ${PROJECT_NAME}
27+
DESTINATION ${PROJECT_NAME}
28+
)

‎rclc/static_type_handling/main.c‎

Lines changed: 177 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,177 @@
1+
#include <unistd.h>
2+
#include <stdio.h>
3+
#include <rcl/rcl.h>
4+
#include <rcl/error_handling.h>
5+
#include <rclc/rclc.h>
6+
#include <rclc/executor.h>
7+
#include <micro_ros_utilities/type_utilities.h>
8+
#include <micro_ros_utilities/string_utilities.h>
9+
10+
#include <sensor_msgs/msg/image.h>
11+
12+
rcl_publisher_t publisher;
13+
sensor_msgs__msg__Image msg;
14+
sensor_msgs__msg__Image msg_static;
15+
rclc_executor_t executor;
16+
rclc_support_t support;
17+
rcl_allocator_t allocator;
18+
rcl_node_t node;
19+
rcl_timer_t timer;
20+
21+
uint8_t my_buffer[1000];
22+
23+
#define LED_PIN 13
24+
25+
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
26+
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
27+
28+
29+
void error_loop(){
30+
while(1){
31+
}
32+
}
33+
34+
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
35+
{
36+
RCLC_UNUSED(last_call_time);
37+
if (timer != NULL) {
38+
RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
39+
RCSOFTCHECK(rcl_publish(&publisher, &msg_static, NULL));
40+
}
41+
}
42+
43+
int main() {
44+
allocator = rcl_get_default_allocator();
45+
46+
//create init_options
47+
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
48+
49+
// create node
50+
RCCHECK(rclc_node_init_default(&node, "micro_ros_node", "", &support));
51+
52+
// create publisher
53+
RCCHECK(rclc_publisher_init_default(
54+
&publisher,
55+
&node,
56+
ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Image),
57+
"micro_ros_publisher"));
58+
59+
// create timer,
60+
const unsigned int timer_timeout = 1000;
61+
RCCHECK(rclc_timer_init_default(
62+
&timer,
63+
&support,
64+
RCL_MS_TO_NS(timer_timeout),
65+
timer_callback));
66+
67+
// create executor
68+
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
69+
RCCHECK(rclc_executor_add_timer(&executor, &timer));
70+
71+
// INIT MESSAGE MEMORY
72+
73+
// --- Configuration ---
74+
75+
// micro-ROS utilities allows to configure the dynamic memory initialization using a micro_ros_utilities_memory_conf_t structure
76+
// If some member of this struct is set to zero, the library will use the default value.
77+
// Check `micro_ros_utilities_memory_conf_default` in `<micro_ros_utilities/type_utilities.h>` for those defaults.
78+
79+
static micro_ros_utilities_memory_conf_t conf = {0};
80+
81+
// OPTIONALLY this struct can configure the default size of strings, basic sequences and composed sequences
82+
83+
conf.max_string_capacity = 50;
84+
conf.max_ros2_type_sequence_capacity = 5;
85+
conf.max_basic_type_sequence_capacity = 5;
86+
87+
// OPTIONALLY this struct can store rules for specific members
88+
// !! Using the API with rules will use dynamic memory allocations for handling strings !!
89+
90+
micro_ros_utilities_memory_rule_t rules[] = {
91+
{"header.frame_id", 30},
92+
{"encoding", 3},
93+
{"data", 400}
94+
};
95+
conf.rules = rules;
96+
conf.n_rules = sizeof(rules) / sizeof(rules[0]);
97+
98+
// --- API ---
99+
// Type can be instrospected using:
100+
101+
rosidl_runtime_c__String data = micro_ros_utilities_type_info(
102+
ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Image));
103+
printf("%s", data.data);
104+
105+
// It can be calculated the size that will be needed by a msg with a certain configuration
106+
107+
size_t dynamic_size = micro_ros_utilities_get_dynamic_size(
108+
ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Image),
109+
conf
110+
);
111+
112+
// The total (stack, static & dynamic) memory usage of a packet will be:
113+
114+
size_t message_total_size = dynamic_size + sizeof(sensor_msgs__msg__Image);
115+
116+
// The message dynamic memory can be allocated using the following call.
117+
// This will use rcutils default allocators for getting memory.
118+
119+
bool success = micro_ros_utilities_create_message_memory(
120+
ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Image),
121+
&msg,
122+
conf
123+
);
124+
125+
// The message dynamic memory can also be allocated using a buffer.
126+
// This will NOT use dynamic memory for the allocation.
127+
// If no rules set in the conf, no dynamic allocation is guaranteed.
128+
// This method will use contiguos memory and will not take into account aligment
129+
// so handling statically allocated msg can be less efficient that dynamic ones
130+
131+
size_t static_size = micro_ros_utilities_get_static_size(
132+
ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Image),
133+
conf
134+
);
135+
136+
message_total_size = static_size + sizeof(sensor_msgs__msg__Image);
137+
138+
// my_buffer should have at least static_size Bytes
139+
success &= micro_ros_utilities_create_static_message_memory(
140+
ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Image),
141+
&msg_static,
142+
conf,
143+
my_buffer,
144+
sizeof(my_buffer)
145+
);
146+
147+
// Dynamically allocated messages can be destroyed using:
148+
149+
// success &= micro_ros_utilities_destroy_message_memory(
150+
// ROSIDL_GET_MSG_TYPE_SUPPORT(control_msgs, msg, JointJog),
151+
// &msg,
152+
// conf
153+
// );
154+
155+
// Fill the message
156+
msg.header.frame_id = micro_ros_string_utilities_set(msg.header.frame_id, "myframe");
157+
msg_static.header.frame_id = micro_ros_string_utilities_set(msg_static.header.frame_id, "myframestatic");
158+
159+
msg.height = msg_static.height = 10;
160+
msg.width = msg_static.width = 40;
161+
162+
msg.encoding = micro_ros_string_utilities_set(msg.encoding, "RGB");
163+
msg_static.encoding = micro_ros_string_utilities_set(msg_static.encoding, "RGB");
164+
165+
msg.data.size = msg_static.data.size = 400;
166+
for (size_t i = 0; i < msg.data.size; i++)
167+
{
168+
msg.data.data[i] = i;
169+
msg_static.data.data[i] = i;
170+
}
171+
172+
while(1) {
173+
usleep(100000);
174+
RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
175+
}
176+
177+
}

0 commit comments

Comments
 (0)