Skip to content

Commit 9f69a26

Browse files
pablogs9pablogs9
authored
Add static type handler (#68)
* Add micro_ros_utilities repo * Add example * Add control msg * Update
1 parent 1d46c23 commit 9f69a26

9 files changed

Lines changed: 254 additions & 5 deletions

File tree

‎.github/workflows/ci.yml‎

Lines changed: 11 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@ name: CI
22

33
on:
44
pull_request:
5-
branches:
5+
branches:
66
- '**'
77

88
jobs:
@@ -47,7 +47,7 @@ jobs:
4747
cd micro_ros_espidf_component/examples/int32_publisher
4848
idf.py set-target ${{ matrix.idf_target }}
4949
idf.py build
50-
50+
5151
- name: Build sample - low_consumption
5252
shell: bash
5353
if: matrix.idf_target != 'esp32c3'
@@ -56,7 +56,15 @@ jobs:
5656
cd micro_ros_espidf_component/examples/low_consumption
5757
idf.py set-target ${{ matrix.idf_target }}
5858
idf.py build
59-
59+
60+
- name: Build sample - handle_static_types
61+
shell: bash
62+
run: |
63+
. $IDF_PATH/export.sh
64+
cd micro_ros_espidf_component/examples/handle_static_types
65+
idf.py set-target ${{ matrix.idf_target }}
66+
idf.py build
67+
6068
- name: Build sample - int32_publisher_custom_transport
6169
shell: bash
6270
run: |

‎examples/handle_static_types/.gitignore‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
build
2+
sdkconfig
3+
sdkconfig.old

‎examples/handle_static_types/CMakeLists.txt‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
3+
set (EXTRA_COMPONENT_DIRS "./../../.")
4+
5+
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
6+
project(handle_static_types)
7+

‎examples/handle_static_types/main/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
idf_component_register(SRCS main.c
2+
INCLUDE_DIRS ""
3+
)

‎examples/handle_static_types/main/Kconfig.projbuild‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
menu "micro-ROS example-app settings"
2+
3+
config MICRO_ROS_APP_STACK
4+
int "Stack the micro-ROS app (Bytes)"
5+
default 5000
6+
help
7+
Stack size in Bytes of the micro-ROS app
8+
9+
config MICRO_ROS_APP_TASK_PRIO
10+
int "Priority of the micro-ROS app"
11+
default 5
12+
help
13+
Priority of micro-ros task higher value means higher priority
14+
15+
endmenu

‎examples/handle_static_types/main/component.mk‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
#
2+
# "main" pseudo-component makefile.
3+
#
4+
# (Uses default behaviour of compiling all source files in directory, adding 'include' to include path.)
5+

‎examples/handle_static_types/main/main.c‎

Lines changed: 204 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,204 @@
1+
#include <string.h>
2+
#include <stdio.h>
3+
#include <unistd.h>
4+
5+
#include "freertos/FreeRTOS.h"
6+
#include "freertos/task.h"
7+
#include "esp_log.h"
8+
#include "esp_system.h"
9+
10+
#include <uros_network_interfaces.h>
11+
#include <rcl/rcl.h>
12+
#include <rcl/error_handling.h>
13+
#include <std_msgs/msg/int32.h>
14+
#include <rclc/rclc.h>
15+
#include <rclc/executor.h>
16+
#include <rmw_microros/rmw_microros.h>
17+
#include "uxr/client/config.h"
18+
19+
#include <micro_ros_utilities/type_utilities.h>
20+
#include <micro_ros_utilities/string_utilities.h>
21+
22+
#include <sensor_msgs/msg/image.h>
23+
24+
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc);vTaskDelete(NULL);}}
25+
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}
26+
27+
rcl_publisher_t publisher;
28+
sensor_msgs__msg__Image msg;
29+
sensor_msgs__msg__Image msg_static;
30+
31+
uint8_t my_buffer[1000];
32+
33+
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
34+
{
35+
RCLC_UNUSED(last_call_time);
36+
if (timer != NULL) {
37+
RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
38+
RCSOFTCHECK(rcl_publish(&publisher, &msg_static, NULL));
39+
}
40+
}
41+
42+
void micro_ros_task(void * arg)
43+
{
44+
rcl_allocator_t allocator = rcl_get_default_allocator();
45+
rclc_support_t support;
46+
47+
rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
48+
RCCHECK(rcl_init_options_init(&init_options, allocator));
49+
rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options);
50+
51+
// Static Agent IP and port can be used instead of autodisvery.
52+
RCCHECK(rmw_uros_options_set_udp_address(CONFIG_MICRO_ROS_AGENT_IP, CONFIG_MICRO_ROS_AGENT_PORT, rmw_options));
53+
//RCCHECK(rmw_uros_discover_agent(rmw_options));
54+
55+
// create init_options
56+
RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));
57+
58+
// create node
59+
rcl_node_t node;
60+
RCCHECK(rclc_node_init_default(&node, "esp32_node", "", &support));
61+
62+
// create publisher
63+
RCCHECK(rclc_publisher_init_default(
64+
&publisher,
65+
&node,
66+
ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Image),
67+
"esp32_publisher"));
68+
69+
// create timer,
70+
rcl_timer_t timer;
71+
const unsigned int timer_timeout = 1000;
72+
RCCHECK(rclc_timer_init_default(
73+
&timer,
74+
&support,
75+
RCL_MS_TO_NS(timer_timeout),
76+
timer_callback));
77+
78+
// create executor
79+
rclc_executor_t executor;
80+
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
81+
RCCHECK(rclc_executor_add_timer(&executor, &timer));
82+
83+
// INIT MESSAGE MEMORY
84+
85+
// --- Configuration ---
86+
87+
// micro-ROS utilities allows to configure the dynamic memory initialization using a micro_ros_utilities_memory_conf_t structure
88+
// If some member of this struct is set to zero, the library will use the default value.
89+
// Check `micro_ros_utilities_memory_conf_default` in `<micro_ros_utilities/type_utilities.h>` for those defaults.
90+
91+
static micro_ros_utilities_memory_conf_t conf = {0};
92+
93+
// OPTIONALLY this struct can configure the default size of strings, basic sequences and composed sequences
94+
95+
conf.max_string_capacity = 50;
96+
conf.max_ros2_type_sequence_capacity = 5;
97+
conf.max_basic_type_sequence_capacity = 5;
98+
99+
// OPTIONALLY this struct can store rules for specific members
100+
// !! Using the API with rules will use dynamic memory allocations for handling strings !!
101+
102+
micro_ros_utilities_memory_rule_t rules[] = {
103+
{"header.frame_id", 30},
104+
{"encoding", 3},
105+
{"data", 400}
106+
};
107+
conf.rules = rules;
108+
conf.n_rules = sizeof(rules) / sizeof(rules[0]);
109+
110+
// --- API ---
111+
// It can be calculated the size that will be needed by a msg with a certain configuration
112+
113+
size_t dynamic_size = micro_ros_utilities_get_dynamic_size(
114+
ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Image),
115+
conf
116+
);
117+
118+
// The total (stack, static & dynamic) memory usage of a packet will be:
119+
120+
size_t message_total_size = dynamic_size + sizeof(sensor_msgs__msg__Image);
121+
122+
// The message dynamic memory can be allocated using the following call.
123+
// This will use rcutils default allocators for getting memory.
124+
125+
bool success = micro_ros_utilities_create_message_memory(
126+
ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Image),
127+
&msg,
128+
conf
129+
);
130+
131+
// The message dynamic memory can also be allocated using a buffer.
132+
// This will NOT use dynamic memory for the allocation.
133+
// If no rules set in the conf, no dynamic allocation is guaranteed.
134+
// This method will use contiguos memory and will not take into account aligment
135+
// so handling statically allocated msg can be less efficient that dynamic ones
136+
137+
size_t static_size = micro_ros_utilities_get_static_size(
138+
ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Image),
139+
conf
140+
);
141+
142+
message_total_size = static_size + sizeof(sensor_msgs__msg__Image);
143+
144+
// my_buffer should have at least static_size Bytes
145+
success &= micro_ros_utilities_create_static_message_memory(
146+
ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Image),
147+
&msg_static,
148+
conf,
149+
my_buffer,
150+
sizeof(my_buffer)
151+
);
152+
153+
// Dynamically allocated messages can be destroyed using:
154+
155+
// success &= micro_ros_utilities_destroy_message_memory(
156+
// ROSIDL_GET_MSG_TYPE_SUPPORT(control_msgs, msg, JointJog),
157+
// &msg,
158+
// conf
159+
// );
160+
161+
// Fill the message
162+
msg.header.frame_id = micro_ros_string_utilities_set(msg.header.frame_id, "myframe");
163+
msg_static.header.frame_id = micro_ros_string_utilities_set(msg_static.header.frame_id, "myframestatic");
164+
165+
msg.height = msg_static.height = 10;
166+
msg.width = msg_static.width = 40;
167+
168+
msg.encoding = micro_ros_string_utilities_set(msg.encoding, "RGB");
169+
msg_static.encoding = micro_ros_string_utilities_set(msg_static.encoding, "RGB");
170+
171+
msg.data.size = msg_static.data.size = 400;
172+
for (size_t i = 0; i < msg.data.size; i++)
173+
{
174+
msg.data.data[i] = i;
175+
msg_static.data.data[i] = i;
176+
}
177+
// spin executor
178+
while(1){
179+
rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
180+
usleep(10000);
181+
}
182+
183+
// free resources
184+
RCCHECK(rcl_publisher_fini(&publisher, &node));
185+
RCCHECK(rcl_node_fini(&node));
186+
187+
vTaskDelete(NULL);
188+
}
189+
190+
void app_main(void)
191+
{
192+
#ifdef UCLIENT_PROFILE_UDP
193+
// Start the networking if required
194+
ESP_ERROR_CHECK(uros_network_interface_initialize());
195+
#endif // UCLIENT_PROFILE_UDP
196+
197+
//pin micro-ros task in APP_CPU to make PRO_CPU to deal with wifi:
198+
xTaskCreate(micro_ros_task,
199+
"uros_task",
200+
CONFIG_MICRO_ROS_APP_STACK,
201+
NULL,
202+
CONFIG_MICRO_ROS_APP_TASK_PRIO,
203+
NULL);
204+
}

‎examples/handle_static_types/sdkconfig.defaults‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
CONFIG_ESP_MAIN_TASK_STACK_SIZE=6000
2+
CONFIG_ESP_TASK_WDT=n
3+

‎libmicroros.mk‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -43,7 +43,7 @@ $(EXTENSIONS_DIR)/micro_ros_dev/install:
4343
git clone -b ros2 https://github.com/ament/googletest src/googletest; \
4444
git clone -b master https://github.com/ros2/ament_cmake_ros src/ament_cmake_ros; \
4545
git clone -b master https://github.com/ament/ament_index src/ament_index; \
46-
colcon build --cmake-args -DBUILD_TESTING=OFF;
46+
colcon build --cmake-args -DBUILD_TESTING=OFF;
4747

4848
$(EXTENSIONS_DIR)/micro_ros_src/src:
4949
rm -rf micro_ros_src; \
@@ -67,6 +67,7 @@ $(EXTENSIONS_DIR)/micro_ros_src/src:
6767
git clone -b master https://github.com/ros2/rmw_implementation src/rmw_implementation; \
6868
git clone -b master https://github.com/ros2/rcl_logging src/rcl_logging; \
6969
git clone -b master https://gitlab.com/micro-ROS/ros_tracing/ros2_tracing src/ros2_tracing; \
70+
git clone -b main https://github.com/micro-ROS/micro_ros_utilities; \
7071
touch src/rosidl/rosidl_typesupport_introspection_cpp/COLCON_IGNORE; \
7172
touch src/rcl_logging/rcl_logging_log4cxx/COLCON_IGNORE; \
7273
touch src/rcl_logging/rcl_logging_spdlog/COLCON_IGNORE; \
@@ -107,7 +108,7 @@ ifeq ($(IDF_TARGET),$(filter $(IDF_TARGET),esp32s2 esp32c3))
107108
$(X_AR) rc -s librcutils.a *.obj; \
108109
cp -rf librcutils.a $(UROS_DIR)/install/lib/librcutils.a; \
109110
rm -rf $(UROS_DIR)/atomic_workaround; \
110-
cd ..;
111+
cd ..;
111112
endif
112113

113114
$(EXTENSIONS_DIR)/libmicroros.a: $(EXTENSIONS_DIR)/micro_ros_src/install patch_atomic

0 commit comments

Comments
 (0)