Skip to content

Commit 4604c6e

Browse files
hierophecthierophect
committed
Initial shared-bindings implementation
Adds minimal implementations for the rclcpy module, including Node and Publisher, with a placeholder int-only publish function for testing.
1 parent 24e71f9 commit 4604c6e

File tree

17 files changed

+447
-22
lines changed

17 files changed

+447
-22
lines changed

‎ports/espressif/Makefile‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -423,6 +423,10 @@ CFLAGS += -isystem esp-camera/conversions/include
423423
endif
424424

425425
ifneq ($(CIRCUITPY_MICROROS),0)
426+
SRC_MICROROS := \
427+
$(wildcard common-hal/rclcpy/*.c) \
428+
$(wildcard shared-bindings/rclcpy/*.c)
429+
SRC_C += $(SRC_MICROROS)
426430
CFLAGS += -isystem microros-lib/include
427431
CFLAGS += -isystem microros-lib/include/rcl
428432
CFLAGS += -isystem microros-lib/include/statistics_msgs

‎ports/espressif/boards/m5stack_cardputer/board.c‎

Lines changed: 0 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -17,17 +17,6 @@
1717
#include "py/ringbuf.h"
1818
#include "shared/runtime/interrupt_char.h"
1919

20-
// ROS TESTING
21-
// #include <uros_network_interfaces.h>
22-
#include <rcl/rcl/rcl.h>
23-
#include <rcl/rcl/error_handling.h>
24-
#include <std_msgs/msg/int32.h>
25-
#include <rclc/rclc.h>
26-
#include <rclc/executor.h>
27-
// END ROS TESTING
28-
29-
30-
3120
#define DELAY 0x80
3221

3322
uint8_t display_init_sequence[] = {
@@ -53,16 +42,6 @@ uint8_t display_init_sequence[] = {
5342
// Overrides the weakly linked function from supervisor/shared/board.c
5443
void board_init(void) {
5544

56-
// ROS TESTING
57-
rcl_allocator_t allocator = rcl_get_default_allocator();
58-
rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
59-
rcl_ret_t temp_rc = rcl_init_options_init(&init_options, allocator);
60-
if ((temp_rc != RCL_RET_OK)) {
61-
common_hal_board_create_spi(0);
62-
}
63-
// END ROS TESTING
64-
65-
6645
busio_spi_obj_t *spi = common_hal_board_create_spi(0);
6746
fourwire_fourwire_obj_t *bus = &allocate_display_bus()->fourwire_bus;
6847
bus->base.type = &fourwire_fourwire_type;

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

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
// This file is part of the CircuitPython project: https://circuitpython.org
2+
//
3+
// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland for Adafruit
4+
// Industries
5+
//
6+
// SPDX-License-Identifier: MIT
7+
8+
#include "shared-bindings/rclcpy/Node.h"
9+
#include "shared-bindings/rclcpy/__init__.h"
10+
11+
void common_hal_rclcpy_node_construct(rclcpy_node_obj_t *self,
12+
const char *node_name, const char *node_namespace) {
13+
14+
rclc_node_init_default(&self->rcl_node, node_name, node_namespace, &rclcpy_default_context.rcl_support);
15+
if (!rcl_node_is_valid(&self->rcl_node)) {
16+
mp_raise_RuntimeError(MP_ERROR_TEXT("ROS node failed to initialize"));
17+
}
18+
}
19+
20+
const char * common_hal_rclcpy_node_get_name(rclcpy_node_obj_t *self) {
21+
return rcl_node_get_name(&self->rcl_node);
22+
}
23+
24+
const char * common_hal_rclcpy_node_get_namespace(rclcpy_node_obj_t *self) {
25+
return rcl_node_get_namespace(&self->rcl_node);
26+
}

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

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
// This file is part of the CircuitPython project: https://circuitpython.org
2+
//
3+
// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland
4+
//
5+
// SPDX-License-Identifier: MIT
6+
7+
#pragma once
8+
9+
#include "py/obj.h"
10+
#include "py/runtime.h"
11+
12+
#include <rcl/error_handling.h>
13+
#include <rcl/rcl.h>
14+
#include <rclc/executor.h>
15+
#include <rclc/rclc.h>
16+
#include <stdio.h>
17+
18+
typedef struct {
19+
mp_obj_base_t base;
20+
rcl_node_t rcl_node;
21+
} rclcpy_node_obj_t;
22+

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

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
// This file is part of the CircuitPython project: https://circuitpython.org
2+
//
3+
// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland for Adafruit
4+
// Industries
5+
//
6+
// SPDX-License-Identifier: MIT
7+
8+
#include "shared-bindings/rclcpy/Publisher.h"
9+
10+
11+
void common_hal_rclcpy_publisher_construct(rclcpy_publisher_obj_t *self, rclcpy_node_obj_t * node,
12+
const char *topic_name) {
13+
14+
// Create Int32 type object
15+
// TODO: support other message types through class imports
16+
const rosidl_message_type_support_t * type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32);
17+
18+
// Creates a reliable Int32 publisher
19+
rcl_ret_t rc = rclc_publisher_init_default(
20+
&self->rcl_publisher, &node->rcl_node,
21+
type_support, topic_name);
22+
23+
if (RCL_RET_OK != rc) {
24+
mp_raise_RuntimeError(MP_ERROR_TEXT("ROS topic failed to initialize"));
25+
}
26+
}
27+
28+
void common_hal_rclcpy_publisher_publish_int32(rclcpy_publisher_obj_t * self, int32_t data) {
29+
// Int32 message object
30+
std_msgs__msg__Int32 msg;
31+
32+
// Set message value
33+
msg.data = data;
34+
35+
// Publish message
36+
rcl_ret_t rc = rcl_publish(&self->rcl_publisher, &msg, NULL);
37+
if (RCL_RET_OK != rc) {
38+
mp_raise_RuntimeError(MP_ERROR_TEXT("Could not publish to ROS topic"));
39+
}
40+
}
41+
42+
const char * common_hal_rclcpy_publisher_get_topic_name(rclcpy_publisher_obj_t *self) {
43+
return rcl_publisher_get_topic_name(&self->rcl_publisher);
44+
}

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

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
// This file is part of the CircuitPython project: https://circuitpython.org
2+
//
3+
// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland
4+
//
5+
// SPDX-License-Identifier: MIT
6+
7+
#pragma once
8+
9+
#include "py/obj.h"
10+
11+
#include "common-hal/rclcpy/Node.h"
12+
#include "common-hal/rclcpy/__init__.h"
13+
14+
#include <rcl/error_handling.h>
15+
#include <rcl/rcl.h>
16+
#include <rclc/executor.h>
17+
#include <rclc/rclc.h>
18+
#include <stdio.h>
19+
20+
#include <std_msgs/msg/int32.h>
21+
22+
23+
typedef struct {
24+
mp_obj_base_t base;
25+
rclcpy_node_obj_t * node;
26+
rcl_publisher_t rcl_publisher;
27+
} rclcpy_publisher_obj_t;

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

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
// This file is part of the CircuitPython project: https://circuitpython.org
2+
//
3+
// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland for Adafruit Industries
4+
//
5+
// SPDX-License-Identifier: MIT
6+
7+
#include "shared-bindings/rclcpy/__init__.h"
8+
9+
rclcpy_context_t rclcpy_default_context = {
10+
.initialized = false,
11+
};
12+
13+
static void* microros_allocate(size_t size, void* state) {
14+
(void) state;
15+
return m_malloc(size);
16+
}
17+
18+
static void microros_deallocate(void* pointer, void* state) {
19+
(void) state;
20+
m_free(pointer);
21+
}
22+
23+
static void* microros_reallocate(void* pointer, size_t size, void* state) {
24+
(void) state;
25+
return m_realloc(pointer, size);
26+
}
27+
28+
static void* microros_zero_allocate(size_t number_of_elements, size_t size_of_element, void* state) {
29+
(void) state;
30+
size_t total_size = number_of_elements * size_of_element;
31+
void* ptr = m_malloc(total_size);
32+
if (ptr != NULL) {
33+
memset(ptr, 0, total_size);
34+
}
35+
return ptr;
36+
}
37+
38+
void common_hal_rclcpy_init(void) {
39+
// Get empty allocator
40+
rcl_allocator_t custom_allocator = rcutils_get_zero_initialized_allocator();
41+
42+
// Set custom allocation methods
43+
custom_allocator.allocate = microros_allocate;
44+
custom_allocator.deallocate = microros_deallocate;
45+
custom_allocator.reallocate = microros_reallocate;
46+
custom_allocator.zero_allocate = microros_zero_allocate;
47+
48+
// Set custom allocator as default
49+
if (!rcutils_set_default_allocator(&custom_allocator)) {
50+
mp_raise_RuntimeError(MP_ERROR_TEXT("ROS memory allocator failure"));
51+
}
52+
53+
rclcpy_default_context.rcl_allocator = custom_allocator;
54+
rcl_ret_t ret;
55+
ret = rclc_support_init(&rclcpy_default_context.rcl_support, 0, NULL, &rclcpy_default_context.rcl_allocator);
56+
if (ret != RCL_RET_OK) {
57+
mp_raise_RuntimeError(MP_ERROR_TEXT("ROS failed to initialize"));
58+
} else {
59+
rclcpy_default_context.initialized = true;
60+
}
61+
}
62+
63+
rclcpy_context_t * common_hal_rclcpy_get_default_context(void) {
64+
return &rclcpy_default_context;
65+
}
66+
67+
bool common_hal_rclcpy_default_context_is_initialized(void) {
68+
return rclcpy_default_context.initialized;
69+
}

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

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
// This file is part of the CircuitPython project: https://circuitpython.org
2+
//
3+
// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland
4+
//
5+
// SPDX-License-Identifier: MIT
6+
7+
#pragma once
8+
9+
#include "py/obj.h"
10+
#include "py/runtime.h"
11+
12+
#include <stdio.h>
13+
#include <rcl/rcl.h>
14+
#include <rcl/error_handling.h>
15+
#include <rclc/rclc.h>
16+
#include <rclc/executor.h>
17+
18+
typedef struct {
19+
bool initialized;
20+
rcl_allocator_t rcl_allocator;
21+
rclc_support_t rcl_support;
22+
} rclcpy_context_t;
23+
24+
extern rclcpy_context_t rclcpy_default_context;

‎ports/espressif/mpconfigport.mk‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,7 @@ CIRCUITPY_HASHLIB ?= 1
6666
CIRCUITPY_I2CTARGET ?= 0
6767
CIRCUITPY_MAX3421E ?= 1
6868
CIRCUITPY_MEMORYMAP ?= 1
69-
CIRCUITPY_MICROROS ?= 0
69+
CIRCUITPY_MICROROS ?= 1
7070
CIRCUITPY_NVM ?= 1
7171
CIRCUITPY_PARALLELDISPLAYBUS ?= 1
7272
CIRCUITPY_PS2IO ?= 1

‎py/circuitpy_defns.mk‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -285,6 +285,9 @@ endif
285285
ifeq ($(CIRCUITPY_MICROCONTROLLER),1)
286286
SRC_PATTERNS += microcontroller/%
287287
endif
288+
ifeq ($(CIRCUITPY_MICROROS),1)
289+
SRC_PATTERNS += rclcpy/%
290+
endif
288291
ifeq ($(CIRCUITPY_MDNS),1)
289292
SRC_PATTERNS += mdns/%
290293
endif

0 commit comments

Comments
 (0)