Skip to content

Commit 1cccbc7

Browse files
hierophecthierophect
committed
Add reset support
Adds deinit and check for deinit functions across Node and Publisher, which call corresponding microros `fini` functions to deconstruct ros objects and release resources in the linux agent. Also adds rclcpy_reset, which deconstructs the ros support and options objects. Adds alternative Node creation function to align with rclpy.
1 parent 031ca37 commit 1cccbc7

File tree

11 files changed

+136
-33
lines changed

11 files changed

+136
-33
lines changed

‎ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/sdkconfig‎

Lines changed: 14 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,14 @@
1-
CONFIG_MICRO_ROS_ESP_XRCE_DDS_MIDDLEWARE=y
2-
CONFIG_MICRO_ROS_ESP_NETIF_WLAN=y
3-
CONFIG_MICRO_ROS_APP_STACK=16000
4-
CONFIG_MICRO_ROS_APP_TASK_PRIO=5
1+
#
2+
# Espressif IoT Development Framework Configuration
3+
#
4+
#
5+
# Component config
6+
#
7+
#
8+
# LWIP
9+
#
10+
# end of LWIP
11+
12+
# end of Component config
13+
14+
# end of Espressif IoT Development Framework Configuration

‎ports/espressif/common-hal/rclcpy/Node.c‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,13 +7,31 @@
77
#include "shared-bindings/rclcpy/Node.h"
88
#include "shared-bindings/rclcpy/__init__.h"
99

10+
#include "esp_log.h"
11+
1012
void common_hal_rclcpy_node_construct(rclcpy_node_obj_t *self,
1113
const char *node_name, const char *node_namespace) {
1214

1315
rclc_node_init_default(&self->rcl_node, node_name, node_namespace, &rclcpy_default_context.rcl_support);
1416
if (!rcl_node_is_valid(&self->rcl_node)) {
1517
mp_raise_RuntimeError(MP_ERROR_TEXT("ROS node failed to initialize"));
1618
}
19+
self->deinited = false;
20+
}
21+
22+
bool common_hal_rclcpy_node_deinited(rclcpy_node_obj_t * self) {
23+
return self->deinited;
24+
}
25+
26+
void common_hal_rclcpy_node_deinit(rclcpy_node_obj_t * self) {
27+
if (common_hal_rclcpy_node_deinited(self)) {
28+
return;
29+
}
30+
rcl_ret_t ret = rcl_node_fini(&self->rcl_node);
31+
if (ret != RCL_RET_OK || !rcl_node_is_valid(&self->rcl_node)) {
32+
ESP_LOGW("RCLCPY", "Node cleanup error: %d", ret);
33+
}
34+
self->deinited = true;
1735
}
1836

1937
const char * common_hal_rclcpy_node_get_name(rclcpy_node_obj_t *self) {

‎ports/espressif/common-hal/rclcpy/Node.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
typedef struct {
1919
mp_obj_base_t base;
20+
bool deinited;
2021
rcl_node_t rcl_node;
2122
} rclcpy_node_obj_t;
2223

‎ports/espressif/common-hal/rclcpy/Publisher.c‎

Lines changed: 18 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66

77
#include "shared-bindings/rclcpy/Publisher.h"
88

9+
#include "esp_log.h"
910

1011
void common_hal_rclcpy_publisher_construct(rclcpy_publisher_obj_t *self, rclcpy_node_obj_t * node,
1112
const char *topic_name) {
@@ -18,10 +19,26 @@ void common_hal_rclcpy_publisher_construct(rclcpy_publisher_obj_t *self, rclcpy_
1819
rcl_ret_t rc = rclc_publisher_init_default(
1920
&self->rcl_publisher, &node->rcl_node,
2021
type_support, topic_name);
21-
2222
if (RCL_RET_OK != rc) {
2323
mp_raise_RuntimeError(MP_ERROR_TEXT("ROS topic failed to initialize"));
2424
}
25+
26+
self->node = node;
27+
}
28+
29+
bool common_hal_rclcpy_publisher_deinited(rclcpy_publisher_obj_t *self) {
30+
return self->node == NULL;
31+
}
32+
33+
void common_hal_rclcpy_publisher_deinit(rclcpy_publisher_obj_t *self) {
34+
if (common_hal_rclcpy_publisher_deinited(self)) {
35+
return;
36+
}
37+
rcl_ret_t ret = rcl_publisher_fini(&self->rcl_publisher, &self->node->rcl_node);
38+
if (ret != RCL_RET_OK || !rcl_publisher_is_valid(&self->rcl_publisher)) {
39+
ESP_LOGW("RCLCPY", "Publisher cleanup warning: %d", ret);
40+
}
41+
self->node = NULL;
2542
}
2643

2744
void common_hal_rclcpy_publisher_publish_int32(rclcpy_publisher_obj_t * self, int32_t data) {

‎ports/espressif/common-hal/rclcpy/__init__.c‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,23 @@ static void* microros_zero_allocate(size_t number_of_elements, size_t size_of_el
3838
}
3939

4040
void rclcpy_reset(void) {
41+
if (rclcpy_default_context.initialized) {
42+
// Clean up support context
43+
rcl_ret_t ret = rclc_support_fini(&rclcpy_default_context.rcl_support);
44+
if (ret != RCL_RET_OK) {
45+
ESP_LOGW("RCLCPY", "Support cleanup error: %d", ret);
46+
}
47+
48+
// Clean up init options
49+
ret = rcl_init_options_fini(&rclcpy_default_context.init_options);
50+
if (ret != RCL_RET_OK) {
51+
ESP_LOGW("RCLCPY", "Init options cleanup error: %d", ret);
52+
}
53+
54+
// Reset context state
55+
memset(&rclcpy_default_context, 0, sizeof(rclcpy_default_context));
56+
rclcpy_default_context.initialized = false;
57+
}
4158
}
4259

4360
void common_hal_rclcpy_init(const char *agent_ip, const char *agent_port, int16_t domain_id) {

‎ports/espressif/supervisor/port.c‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,10 @@
5555
#include "esp_camera.h"
5656
#endif
5757

58+
#if CIRCUITPY_RCLCPY
59+
#include "common-hal/rclcpy/__init__.h"
60+
#endif
61+
5862
#include "soc/efuse_reg.h"
5963
#if defined(SOC_LP_AON_SUPPORTED)
6064
#include "soc/lp_aon_reg.h"
@@ -378,6 +382,10 @@ void reset_port(void) {
378382
ps2_reset();
379383
#endif
380384

385+
#if CIRCUITPY_RCLCPY
386+
rclcpy_reset();
387+
#endif
388+
381389
#if CIRCUITPY_RTC
382390
rtc_reset();
383391
#endif

‎shared-bindings/rclcpy/Node.c‎

Lines changed: 19 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77
#include <stdint.h>
88
#include "shared-bindings/rclcpy/Node.h"
99
#include "shared-bindings/rclcpy/Publisher.h"
10+
#include "shared-bindings/util.h"
1011
#include "py/objproperty.h"
1112
#include "py/objtype.h"
1213
#include "py/runtime.h"
@@ -30,8 +31,22 @@ static mp_obj_t rclcpy_node_make_new(const mp_obj_type_t *type, size_t n_args, s
3031
return (mp_obj_t)self;
3132
}
3233

34+
static mp_obj_t rclcpy_node_obj_deinit(mp_obj_t self_in) {
35+
rclcpy_node_obj_t *self = MP_OBJ_TO_PTR(self_in);
36+
common_hal_rclcpy_node_deinit(self);
37+
return mp_const_none;
38+
}
39+
MP_DEFINE_CONST_FUN_OBJ_1(rclcpy_node_deinit_obj, rclcpy_node_obj_deinit);
40+
41+
static void check_for_deinit(rclcpy_node_obj_t *self) {
42+
if (common_hal_rclcpy_node_deinited(self)) {
43+
raise_deinited_error();
44+
}
45+
}
46+
3347
static mp_obj_t rclcpy_node_create_publisher(mp_obj_t self_in, mp_obj_t topic) {
3448
rclcpy_node_obj_t *self = MP_OBJ_TO_PTR(self_in);
49+
check_for_deinit(self);
3550
const char *topic_name = mp_obj_str_get_str(topic);
3651

3752
rclcpy_publisher_obj_t *publisher = mp_obj_malloc_with_finaliser(rclcpy_publisher_obj_t, &rclcpy_publisher_type);
@@ -41,24 +56,24 @@ static mp_obj_t rclcpy_node_create_publisher(mp_obj_t self_in, mp_obj_t topic) {
4156
static MP_DEFINE_CONST_FUN_OBJ_2(rclcpy_node_create_publisher_obj, rclcpy_node_create_publisher);
4257

4358
static mp_obj_t rclcpy_node_get_name(mp_obj_t self_in) {
44-
// TODO: probably a good idea
45-
// check_for_deinit(self);
4659
rclcpy_node_obj_t *self = MP_OBJ_TO_PTR(self_in);
60+
check_for_deinit(self);
4761
const char * name_str = common_hal_rclcpy_node_get_name(self);
4862
return mp_obj_new_str(name_str,strlen(name_str));
4963
}
5064
static MP_DEFINE_CONST_FUN_OBJ_1(rclcpy_node_get_name_obj, rclcpy_node_get_name);
5165

5266
static mp_obj_t rclcpy_node_get_namespace(mp_obj_t self_in) {
53-
// TODO: probably a good idea
54-
// check_for_deinit(self);
5567
rclcpy_node_obj_t *self = MP_OBJ_TO_PTR(self_in);
68+
check_for_deinit(self);
5669
const char * namespace_str = common_hal_rclcpy_node_get_namespace(self);
5770
return mp_obj_new_str(namespace_str,strlen(namespace_str));
5871
}
5972
static MP_DEFINE_CONST_FUN_OBJ_1(rclcpy_node_get_namespace_obj, rclcpy_node_get_namespace);
6073

6174
static const mp_rom_map_elem_t rclcpy_node_locals_dict_table[] = {
75+
{ MP_ROM_QSTR(MP_QSTR_deinit), MP_ROM_PTR(&rclcpy_node_deinit_obj) },
76+
{ MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&rclcpy_node_deinit_obj) },
6277
{ MP_ROM_QSTR(MP_QSTR_create_publisher), MP_ROM_PTR(&rclcpy_node_create_publisher_obj) },
6378
{ MP_ROM_QSTR(MP_QSTR_get_name), MP_ROM_PTR(&rclcpy_node_get_name_obj) },
6479
{ MP_ROM_QSTR(MP_QSTR_get_namespace), MP_ROM_PTR(&rclcpy_node_get_namespace_obj) },
@@ -73,21 +88,3 @@ MP_DEFINE_CONST_OBJ_TYPE(
7388
make_new, rclcpy_node_make_new,
7489
locals_dict, &rclcpy_node_locals_dict
7590
);
76-
77-
// Examples:
78-
79-
// MP_DEFINE_CONST_OBJ_TYPE(
80-
// rtc_rtc_type,
81-
// MP_QSTR_RTC,
82-
// MP_TYPE_FLAG_HAS_SPECIAL_ACCESSORS,
83-
// make_new, rtc_rtc_make_new,
84-
// locals_dict, &rtc_rtc_locals_dict
85-
// );
86-
87-
// MP_DEFINE_CONST_OBJ_TYPE(
88-
// socketpool_socket_type,
89-
// MP_QSTR_Socket,
90-
// MP_TYPE_FLAG_HAS_SPECIAL_ACCESSORS,
91-
// locals_dict, &socketpool_socket_locals_dict,
92-
// protocol, &socket_stream_p
93-
// );

‎shared-bindings/rclcpy/Node.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,5 +11,7 @@ extern const mp_obj_type_t rclcpy_node_type;
1111

1212
void common_hal_rclcpy_node_construct(rclcpy_node_obj_t *self,
1313
const char *node_name, const char *namespace);
14+
bool common_hal_rclcpy_node_deinited(rclcpy_node_obj_t * self);
15+
void common_hal_rclcpy_node_deinit(rclcpy_node_obj_t * self);
1416
const char * common_hal_rclcpy_node_get_name(rclcpy_node_obj_t *self);
1517
const char * common_hal_rclcpy_node_get_namespace(rclcpy_node_obj_t *self);

‎shared-bindings/rclcpy/Publisher.c‎

Lines changed: 18 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66

77
#include <stdint.h>
88
#include "shared-bindings/rclcpy/Publisher.h"
9+
#include "shared-bindings/util.h"
910
#include "py/objproperty.h"
1011
#include "py/objtype.h"
1112
#include "py/runtime.h"
@@ -14,25 +15,40 @@ static mp_obj_t rclcpy_publisher_make_new(const mp_obj_type_t *type, size_t n_ar
1415
mp_raise_NotImplementedError(MP_ERROR_TEXT("Publishers can only be created from a parent node"));
1516
}
1617

18+
static mp_obj_t rclcpy_publisher_obj_deinit(mp_obj_t self_in) {
19+
rclcpy_publisher_obj_t *self = MP_OBJ_TO_PTR(self_in);
20+
common_hal_rclcpy_publisher_deinit(self);
21+
return mp_const_none;
22+
}
23+
MP_DEFINE_CONST_FUN_OBJ_1(rclcpy_publisher_deinit_obj, rclcpy_publisher_obj_deinit);
24+
25+
static void check_for_deinit(rclcpy_publisher_obj_t *self) {
26+
if (common_hal_rclcpy_publisher_deinited(self)) {
27+
raise_deinited_error();
28+
}
29+
}
30+
1731
static mp_obj_t rclcpy_publisher_publish_int32(mp_obj_t self_in, mp_obj_t in_msg) {
1832
rclcpy_publisher_obj_t *self = MP_OBJ_TO_PTR(self_in);
33+
check_for_deinit(self);
1934
int32_t msg = mp_obj_get_int(in_msg);
2035
common_hal_rclcpy_publisher_publish_int32(self,msg);
2136
return mp_const_none;
2237
}
2338
static MP_DEFINE_CONST_FUN_OBJ_2(rclcpy_publisher_publish_int32_obj, rclcpy_publisher_publish_int32);
2439

2540
static mp_obj_t rclcpy_publisher_get_topic_name(mp_obj_t self_in) {
26-
// TODO: probably a good idea
27-
// check_for_deinit(self);
2841
rclcpy_publisher_obj_t *self = MP_OBJ_TO_PTR(self_in);
42+
check_for_deinit(self);
2943
const char * topic_str = common_hal_rclcpy_publisher_get_topic_name(self);
3044
return mp_obj_new_str(topic_str,strlen(topic_str));
3145
}
3246
static MP_DEFINE_CONST_FUN_OBJ_1(rclcpy_publisher_get_topic_name_obj, rclcpy_publisher_get_topic_name);
3347

3448

3549
static const mp_rom_map_elem_t rclcpy_publisher_locals_dict_table[] = {
50+
{ MP_ROM_QSTR(MP_QSTR_deinit), MP_ROM_PTR(&rclcpy_publisher_deinit_obj) },
51+
{ MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&rclcpy_publisher_deinit_obj) },
3652
{ MP_ROM_QSTR(MP_QSTR_publish_int32), MP_ROM_PTR(&rclcpy_publisher_publish_int32_obj) },
3753
{ MP_ROM_QSTR(MP_QSTR_get_topic_name), MP_ROM_PTR(&rclcpy_publisher_get_topic_name_obj) },
3854
};

‎shared-bindings/rclcpy/Publisher.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,5 +11,7 @@ extern const mp_obj_type_t rclcpy_publisher_type;
1111

1212
void common_hal_rclcpy_publisher_construct(rclcpy_publisher_obj_t *self, rclcpy_node_obj_t * node,
1313
const char *topic_name);
14+
bool common_hal_rclcpy_publisher_deinited(rclcpy_publisher_obj_t * self);
15+
void common_hal_rclcpy_publisher_deinit(rclcpy_publisher_obj_t * self);
1416
void common_hal_rclcpy_publisher_publish_int32(rclcpy_publisher_obj_t * self, int32_t data);
1517
const char * common_hal_rclcpy_publisher_get_topic_name(rclcpy_publisher_obj_t *self);

0 commit comments

Comments
 (0)