8
8
9
9
#include "esp_log.h"
10
10
11
+
11
12
rclcpy_context_t rclcpy_default_context = {
12
13
.initialized = false,
13
14
};
14
15
15
- static void * microros_allocate (size_t size , void * state ) {
16
- (void ) state ;
16
+ static void * microros_allocate (size_t size , void * state ) {
17
+ (void )state ;
17
18
return m_malloc (size );
18
19
}
19
20
20
- static void microros_deallocate (void * pointer , void * state ) {
21
- (void ) state ;
21
+ static void microros_deallocate (void * pointer , void * state ) {
22
+ (void )state ;
22
23
m_free (pointer );
23
24
}
24
25
25
- static void * microros_reallocate (void * pointer , size_t size , void * state ) {
26
- (void ) state ;
26
+ static void * microros_reallocate (void * pointer , size_t size , void * state ) {
27
+ (void )state ;
27
28
return m_realloc (pointer , size );
28
29
}
29
30
30
- static void * microros_zero_allocate (size_t number_of_elements , size_t size_of_element , void * state ) {
31
- (void ) state ;
31
+ static void * microros_zero_allocate (size_t number_of_elements , size_t size_of_element , void * state ) {
32
+ (void )state ;
32
33
size_t total_size = number_of_elements * size_of_element ;
33
- void * ptr = m_malloc (total_size );
34
+ void * ptr = m_malloc (total_size );
34
35
if (ptr != NULL ) {
35
36
memset (ptr , 0 , total_size );
36
37
}
@@ -65,7 +66,7 @@ void common_hal_rclcpy_init(const char *agent_ip, const char *agent_port, int16_
65
66
custom_allocator .allocate = microros_allocate ;
66
67
custom_allocator .deallocate = microros_deallocate ;
67
68
custom_allocator .reallocate = microros_reallocate ;
68
- custom_allocator .zero_allocate = microros_zero_allocate ;
69
+ custom_allocator .zero_allocate = microros_zero_allocate ;
69
70
// Set custom allocator as default
70
71
if (!rcutils_set_default_allocator (& custom_allocator )) {
71
72
ESP_LOGW ("RCLCPY" , "allocator failure" );
@@ -77,31 +78,31 @@ void common_hal_rclcpy_init(const char *agent_ip, const char *agent_port, int16_
77
78
78
79
// Options Init
79
80
rclcpy_default_context .init_options = rcl_get_zero_initialized_init_options ();
80
- ret = rcl_init_options_init (& rclcpy_default_context .init_options , rclcpy_default_context .rcl_allocator );
81
+ ret = rcl_init_options_init (& rclcpy_default_context .init_options , rclcpy_default_context .rcl_allocator );
81
82
if (ret != RCL_RET_OK ) {
82
83
ESP_LOGW ("RCLCPY" , "Options init failure: %d" , ret );
83
84
}
84
85
if (domain_id < 0 ) {
85
86
mp_raise_RuntimeError (MP_ERROR_TEXT ("Invalid ROS domain ID" ));
86
87
}
87
- ret = rcl_init_options_set_domain_id (& rclcpy_default_context .init_options , domain_id );
88
+ ret = rcl_init_options_set_domain_id (& rclcpy_default_context .init_options , domain_id );
88
89
if (ret != RCL_RET_OK ) {
89
90
ESP_LOGW ("RCLCPY" , "Options domain failure: %d" , ret );
90
91
}
91
92
92
93
// Set up Agent
93
94
rclcpy_default_context .rmw_options = rcl_init_options_get_rmw_init_options (& rclcpy_default_context .init_options );
94
- ret = rmw_uros_options_set_udp_address (agent_ip , agent_port , rclcpy_default_context .rmw_options );
95
+ ret = rmw_uros_options_set_udp_address (agent_ip , agent_port , rclcpy_default_context .rmw_options );
95
96
if (ret != RCL_RET_OK ) {
96
97
ESP_LOGW ("RCLCPY" , "Agent options failure: %d" , ret );
97
98
}
98
99
99
100
// Support Init
100
101
ret = rclc_support_init_with_options (& rclcpy_default_context .rcl_support ,
101
- 0 ,
102
- NULL ,
103
- & rclcpy_default_context .init_options ,
104
- & rclcpy_default_context .rcl_allocator );
102
+ 0 ,
103
+ NULL ,
104
+ & rclcpy_default_context .init_options ,
105
+ & rclcpy_default_context .rcl_allocator );
105
106
if (ret != RCL_RET_OK ) {
106
107
ESP_LOGW ("RCLCPY" , "Initialization failure: %d" , ret );
107
108
mp_raise_RuntimeError (MP_ERROR_TEXT ("ROS failed to initialize. Is agent connected?" ));
0 commit comments