diff --git a/.gitignore b/.gitignore index 6725b901b92bf..7f798c415c784 100644 --- a/.gitignore +++ b/.gitignore @@ -9,6 +9,7 @@ *.o *.a !atmel-samd/asf/**/*.a +!ports/espressif/microros-lib/**/*.a *.elf *.bin !*.toml.bin diff --git a/.gitmodules b/.gitmodules index a5226ffafdd52..067dd612b8038 100644 --- a/.gitmodules +++ b/.gitmodules @@ -412,3 +412,6 @@ [submodule "ports/analog/msdk"] path = ports/analog/msdk url = https://github.com/analogdevicesinc/msdk.git +[submodule "ports/espressif/microros-lib"] + path = ports/espressif/microros-lib + url = https://github.com/hierophect/microros-lib.git diff --git a/locale/circuitpython.pot b/locale/circuitpython.pot index 1c6fcce1d417a..c43e3a945f478 100644 --- a/locale/circuitpython.pot +++ b/locale/circuitpython.pot @@ -848,6 +848,10 @@ msgstr "" msgid "Coordinate arrays types have different sizes" msgstr "" +#: ports/espressif/common-hal/rclcpy/Publisher.c +msgid "Could not publish to ROS topic" +msgstr "" + #: shared-bindings/_bleio/Adapter.c msgid "Could not set address" msgstr "" @@ -860,6 +864,11 @@ msgstr "" msgid "Couldn't allocate decoder" msgstr "" +#: ports/espressif/common-hal/rclcpy/__init__.c +#, c-format +msgid "Critical ROS failure during soft reboot, reset required: %d" +msgstr "" + #: ports/stm/common-hal/analogio/AnalogOut.c msgid "DAC Channel Init Error" msgstr "" @@ -1287,6 +1296,10 @@ msgstr "" msgid "Invalid MAC address" msgstr "" +#: ports/espressif/common-hal/rclcpy/__init__.c +msgid "Invalid ROS domain ID" +msgstr "" + #: ports/espressif/common-hal/espidf/__init__.c py/moderrno.c msgid "Invalid argument" msgstr "" @@ -1841,6 +1854,10 @@ msgstr "" msgid "Program too long" msgstr "" +#: shared-bindings/rclcpy/Publisher.c +msgid "Publishers can only be created from a parent node" +msgstr "" + #: shared-bindings/digitalio/DigitalInOut.c msgid "Pull not used when direction is output." msgstr "" @@ -1861,6 +1878,26 @@ msgstr "" msgid "RNG Init Error" msgstr "" +#: ports/espressif/common-hal/rclcpy/__init__.c +msgid "ROS failed to initialize. Is agent connected?" +msgstr "" + +#: ports/espressif/common-hal/rclcpy/__init__.c +msgid "ROS internal setup failure" +msgstr "" + +#: ports/espressif/common-hal/rclcpy/__init__.c +msgid "ROS memory allocator failure" +msgstr "" + +#: ports/espressif/common-hal/rclcpy/Node.c +msgid "ROS node failed to initialize" +msgstr "" + +#: ports/espressif/common-hal/rclcpy/Publisher.c +msgid "ROS topic failed to initialize" +msgstr "" + #: ports/atmel-samd/common-hal/busio/UART.c ports/cxd56/common-hal/busio/UART.c #: ports/nordic/common-hal/busio/UART.c ports/stm/common-hal/busio/UART.c msgid "RS485" diff --git a/ports/espressif/Makefile b/ports/espressif/Makefile index 09522ea795050..bbd39852bf2bd 100644 --- a/ports/espressif/Makefile +++ b/ports/espressif/Makefile @@ -440,6 +440,55 @@ CFLAGS += -isystem esp-camera/driver/include CFLAGS += -isystem esp-camera/conversions/include endif +ifneq ($(CIRCUITPY_RCLCPY),0) +CFLAGS += -isystem microros-lib/include +CFLAGS += -isystem microros-lib/include/rcl +CFLAGS += -isystem microros-lib/include/statistics_msgs +CFLAGS += -isystem microros-lib/include/composition_interfaces +CFLAGS += -isystem microros-lib/include/example_interfaces +CFLAGS += -isystem microros-lib/include/rmw_microxrcedds_c +CFLAGS += -isystem microros-lib/include/test_msgs +CFLAGS += -isystem microros-lib/include/std_msgs +CFLAGS += -isystem microros-lib/include/rcutils +CFLAGS += -isystem microros-lib/include/lifecycle_msgs +CFLAGS += -isystem microros-lib/include/rosidl_typesupport_interface +CFLAGS += -isystem microros-lib/include/service_msgs +CFLAGS += -isystem microros-lib/include/visualization_msgs +CFLAGS += -isystem microros-lib/include/rosidl_dynamic_typesupport +CFLAGS += -isystem microros-lib/include/stereo_msgs +CFLAGS += -isystem microros-lib/include/ucdr +CFLAGS += -isystem microros-lib/include/rosidl_typesupport_c +CFLAGS += -isystem microros-lib/include/std_srvs +CFLAGS += -isystem microros-lib/include/rcl_lifecycle +CFLAGS += -isystem microros-lib/include/action_msgs +CFLAGS += -isystem microros-lib/include/micro_ros_utilities +CFLAGS += -isystem microros-lib/include/rcl_action +CFLAGS += -isystem microros-lib/include/rcl_logging_interface +CFLAGS += -isystem microros-lib/include/type_description_interfaces +CFLAGS += -isystem microros-lib/include/nav_msgs +CFLAGS += -isystem microros-lib/include/actionlib_msgs +CFLAGS += -isystem microros-lib/include/rmw +CFLAGS += -isystem microros-lib/include/rclc_parameter +CFLAGS += -isystem microros-lib/include/geometry_msgs +CFLAGS += -isystem microros-lib/include/sensor_msgs +CFLAGS += -isystem microros-lib/include/trajectory_msgs +CFLAGS += -isystem microros-lib/include/shape_msgs +CFLAGS += -isystem microros-lib/include/rosidl_runtime_c +CFLAGS += -isystem microros-lib/include/rclc +CFLAGS += -isystem microros-lib/include/rosgraph_msgs +CFLAGS += -isystem microros-lib/include/rclc_lifecycle +CFLAGS += -isystem microros-lib/include/rcl_interfaces +CFLAGS += -isystem microros-lib/include/diagnostic_msgs +CFLAGS += -isystem microros-lib/include/micro_ros_msgs +CFLAGS += -isystem microros-lib/include/rosidl_typesupport_introspection_c +CFLAGS += -isystem microros-lib/include/uxr +CFLAGS += -isystem microros-lib/include/unique_identifier_msgs +CFLAGS += -isystem microros-lib/include/rosidl_typesupport_microxrcedds_c +CFLAGS += -isystem microros-lib/include/builtin_interfaces +CFLAGS += -isystem microros-lib/include/tracetools +CFLAGS += -isystem microros-lib/include/rmw_microros +endif + ifneq ($(CIRCUITPY_ESPIDF),0) SRC_ESPIDF := \ $(wildcard common-hal/espidf/*.c) \ @@ -685,6 +734,18 @@ ESP_IDF_COMPONENTS_EXPANDED += $(BUILD)/esp-idf/esp-idf/esp-camera/libesp-camera #$(error $(ESP_IDF_COMPONENTS_EXPANDED)) endif +ifneq ($(CIRCUITPY_RCLCPY),0) + ifeq ($(IDF_TARGET),esp32) + BINARY_BLOBS += microros-lib/esp32/libmicroros.a + else ifeq ($(IDF_TARGET),esp32s2) + BINARY_BLOBS+= microros-lib/esp32s2/libmicroros.a + else ifeq ($(IDF_TARGET),esp32s3) + BINARY_BLOBS += microros-lib/esp32s3/libmicroros.a + else + $(error Unsupported Espressif target for Micro-ROS: $(IDF_TARGET).) + endif +endif + ifneq ($(VALID_BOARD),) # From esp-idf/components/bootloader/Kconfig.projbuild # BOOTLOADER_OFFSET is determined by chip type, based on the ROM bootloader, and is not changeable. diff --git a/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/board.c b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/board.c new file mode 100644 index 0000000000000..a3a9eec047145 --- /dev/null +++ b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/board.c @@ -0,0 +1,9 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2020 Scott Shawcroft for Adafruit Industries +// +// SPDX-License-Identifier: MIT + +#include "supervisor/board.h" + +// Use the MP_WEAK supervisor/shared/board.c versions of routines not defined here. diff --git a/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/mpconfigboard.h b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/mpconfigboard.h new file mode 100644 index 0000000000000..02fe72fe4a709 --- /dev/null +++ b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/mpconfigboard.h @@ -0,0 +1,16 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2019 Scott Shawcroft for Adafruit Industries +// +// SPDX-License-Identifier: MIT + +#pragma once + +// Micropython setup +#define MICROPY_HW_BOARD_NAME "ESP32-S3-DevKitC-1-N8R2 (ROS version)" +#define MICROPY_HW_MCU_NAME "ESP32S3" + +#define MICROPY_HW_NEOPIXEL (&pin_GPIO48) + +#define DEFAULT_UART_BUS_RX (&pin_GPIO44) +#define DEFAULT_UART_BUS_TX (&pin_GPIO43) diff --git a/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/mpconfigboard.mk b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/mpconfigboard.mk new file mode 100644 index 0000000000000..336b9f4dd8926 --- /dev/null +++ b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/mpconfigboard.mk @@ -0,0 +1,16 @@ +USB_VID = 0x303A +USB_PID = 0x7003 +USB_PRODUCT = "ESP32-S3-DevKitC-1-N8R2 CPY/ROS" +USB_MANUFACTURER = "Espressif" + +IDF_TARGET = esp32s3 + +CIRCUITPY_ESP_FLASH_SIZE = 8MB +CIRCUITPY_ESP_FLASH_MODE = qio +CIRCUITPY_ESP_FLASH_FREQ = 80m + +CIRCUITPY_ESP_PSRAM_SIZE = 2MB +CIRCUITPY_ESP_PSRAM_MODE = qio +CIRCUITPY_ESP_PSRAM_FREQ = 80m + +CIRCUITPY_RCLCPY = 1 diff --git a/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/pins.c b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/pins.c new file mode 100644 index 0000000000000..3bb64f434d02f --- /dev/null +++ b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/pins.c @@ -0,0 +1,55 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2020 Scott Shawcroft for Adafruit Industries +// +// SPDX-License-Identifier: MIT + +#include "shared-bindings/board/__init__.h" + +static const mp_rom_map_elem_t board_module_globals_table[] = { + CIRCUITPYTHON_BOARD_DICT_STANDARD_ITEMS + + { MP_ROM_QSTR(MP_QSTR_IO0), MP_ROM_PTR(&pin_GPIO0) }, + { MP_ROM_QSTR(MP_QSTR_IO1), MP_ROM_PTR(&pin_GPIO1) }, + { MP_ROM_QSTR(MP_QSTR_IO2), MP_ROM_PTR(&pin_GPIO2) }, + { MP_ROM_QSTR(MP_QSTR_IO3), MP_ROM_PTR(&pin_GPIO3) }, + { MP_ROM_QSTR(MP_QSTR_IO4), MP_ROM_PTR(&pin_GPIO4) }, + { MP_ROM_QSTR(MP_QSTR_IO5), MP_ROM_PTR(&pin_GPIO5) }, + { MP_ROM_QSTR(MP_QSTR_IO6), MP_ROM_PTR(&pin_GPIO6) }, + { MP_ROM_QSTR(MP_QSTR_IO7), MP_ROM_PTR(&pin_GPIO7) }, + { MP_ROM_QSTR(MP_QSTR_IO8), MP_ROM_PTR(&pin_GPIO8) }, + { MP_ROM_QSTR(MP_QSTR_IO9), MP_ROM_PTR(&pin_GPIO9) }, + { MP_ROM_QSTR(MP_QSTR_IO10), MP_ROM_PTR(&pin_GPIO10) }, + { MP_ROM_QSTR(MP_QSTR_IO11), MP_ROM_PTR(&pin_GPIO11) }, + { MP_ROM_QSTR(MP_QSTR_IO12), MP_ROM_PTR(&pin_GPIO12) }, + { MP_ROM_QSTR(MP_QSTR_IO13), MP_ROM_PTR(&pin_GPIO13) }, + { MP_ROM_QSTR(MP_QSTR_IO14), MP_ROM_PTR(&pin_GPIO14) }, + { MP_ROM_QSTR(MP_QSTR_IO15), MP_ROM_PTR(&pin_GPIO15) }, + { MP_ROM_QSTR(MP_QSTR_IO16), MP_ROM_PTR(&pin_GPIO16) }, + { MP_ROM_QSTR(MP_QSTR_IO17), MP_ROM_PTR(&pin_GPIO17) }, + { MP_ROM_QSTR(MP_QSTR_IO18), MP_ROM_PTR(&pin_GPIO18) }, + { MP_ROM_QSTR(MP_QSTR_IO19), MP_ROM_PTR(&pin_GPIO19) }, + { MP_ROM_QSTR(MP_QSTR_IO20), MP_ROM_PTR(&pin_GPIO20) }, + { MP_ROM_QSTR(MP_QSTR_IO21), MP_ROM_PTR(&pin_GPIO21) }, + { MP_ROM_QSTR(MP_QSTR_IO35), MP_ROM_PTR(&pin_GPIO35) }, + { MP_ROM_QSTR(MP_QSTR_IO36), MP_ROM_PTR(&pin_GPIO36) }, + { MP_ROM_QSTR(MP_QSTR_IO37), MP_ROM_PTR(&pin_GPIO37) }, + { MP_ROM_QSTR(MP_QSTR_IO38), MP_ROM_PTR(&pin_GPIO38) }, + { MP_ROM_QSTR(MP_QSTR_IO39), MP_ROM_PTR(&pin_GPIO39) }, + { MP_ROM_QSTR(MP_QSTR_IO40), MP_ROM_PTR(&pin_GPIO40) }, + { MP_ROM_QSTR(MP_QSTR_IO41), MP_ROM_PTR(&pin_GPIO41) }, + { MP_ROM_QSTR(MP_QSTR_IO42), MP_ROM_PTR(&pin_GPIO42) }, + { MP_ROM_QSTR(MP_QSTR_IO43), MP_ROM_PTR(&pin_GPIO43) }, + { MP_ROM_QSTR(MP_QSTR_IO44), MP_ROM_PTR(&pin_GPIO44) }, + { MP_ROM_QSTR(MP_QSTR_IO45), MP_ROM_PTR(&pin_GPIO45) }, + { MP_ROM_QSTR(MP_QSTR_IO46), MP_ROM_PTR(&pin_GPIO46) }, + { MP_ROM_QSTR(MP_QSTR_IO47), MP_ROM_PTR(&pin_GPIO47) }, + { MP_ROM_QSTR(MP_QSTR_IO48), MP_ROM_PTR(&pin_GPIO48) }, + { MP_ROM_QSTR(MP_QSTR_NEOPIXEL), MP_ROM_PTR(&pin_GPIO48) }, + + { MP_ROM_QSTR(MP_QSTR_TX), MP_ROM_PTR(&pin_GPIO43) }, + { MP_ROM_QSTR(MP_QSTR_RX), MP_ROM_PTR(&pin_GPIO44) }, + + { MP_ROM_QSTR(MP_QSTR_UART), MP_ROM_PTR(&board_uart_obj) }, +}; +MP_DEFINE_CONST_DICT(board_module_globals, board_module_globals_table); diff --git a/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/sdkconfig b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/sdkconfig new file mode 100644 index 0000000000000..e962866216039 --- /dev/null +++ b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/sdkconfig @@ -0,0 +1,14 @@ +# +# Espressif IoT Development Framework Configuration +# +# +# Component config +# +# +# LWIP +# +# end of LWIP + +# end of Component config + +# end of Espressif IoT Development Framework Configuration diff --git a/ports/espressif/boards/m5stack_cardputer_ros/board.c b/ports/espressif/boards/m5stack_cardputer_ros/board.c new file mode 100644 index 0000000000000..6fdc2f8ea5c46 --- /dev/null +++ b/ports/espressif/boards/m5stack_cardputer_ros/board.c @@ -0,0 +1,95 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2020 Scott Shawcroft for Adafruit Industries +// +// SPDX-License-Identifier: MIT + +#include "mpconfigboard.h" +#include "supervisor/board.h" +#include "supervisor/shared/serial.h" +#include "shared-bindings/busio/SPI.h" +#include "shared-bindings/fourwire/FourWire.h" +#include "shared-bindings/microcontroller/Pin.h" +#include "shared-module/displayio/__init__.h" +#include "shared-module/displayio/mipi_constants.h" +#include "shared-bindings/board/__init__.h" +#include "py/runtime.h" +#include "py/ringbuf.h" +#include "shared/runtime/interrupt_char.h" + + +#define DELAY 0x80 + +uint8_t display_init_sequence[] = { + // SWRESET and Delay 140ms + 0x01, 0 | DELAY, 140, + // SLPOUT and Delay 10ms + 0x11, 0 | DELAY, 10, + // COLMOD 65k colors and 16 bit 5-6-5 + 0x3A, 1, 0x55, + // INVON Iiversion on + 0x21, 0, + // NORON normal operation (full update) + 0x13, 0, + // MADCTL columns RTL, page/column reverse order + 0x36, 1, 0x60, + // RAMCTRL color word little endian + 0xB0, 2, 0x00, 0xF8, + // DIPON display on + 0x29, 0, +}; + + +// Overrides the weakly linked function from supervisor/shared/board.c +void board_init(void) { + busio_spi_obj_t *spi = common_hal_board_create_spi(0); + fourwire_fourwire_obj_t *bus = &allocate_display_bus()->fourwire_bus; + bus->base.type = &fourwire_fourwire_type; + + // see here for inspiration: https://github.com/m5stack/M5GFX/blob/33d7d3135e816a86a008fae8ab3757938cee95d2/src/M5GFX.cpp#L1350 + common_hal_fourwire_fourwire_construct( + bus, + spi, + &pin_GPIO34, // DC + &pin_GPIO37, // CS + &pin_GPIO33, // RST + 40000000, // baudrate + 0, // polarity + 0 // phase + ); + busdisplay_busdisplay_obj_t *display = &allocate_display()->display; + display->base.type = &busdisplay_busdisplay_type; + + common_hal_busdisplay_busdisplay_construct( + display, + bus, + 240, // width (after rotation) + 135, // height (after rotation) + 40, // column start + 53, // row start + 0, // rotation + 16, // color depth + false, // grayscale + false, // pixels in a byte share a row. Only valid for depths < 8 + 1, // bytes per cell. Only valid for depths < 8 + false, // reverse_pixels_in_byte. Only valid for depths < 8 + false, // reverse_pixels_in_word + MIPI_COMMAND_SET_COLUMN_ADDRESS, // set column command + MIPI_COMMAND_SET_PAGE_ADDRESS, // set row command + MIPI_COMMAND_WRITE_MEMORY_START, // write memory command + display_init_sequence, + sizeof(display_init_sequence), + &pin_GPIO38, // backlight pin + NO_BRIGHTNESS_COMMAND, + 1.0f, // brightness + false, // single_byte_bounds + false, // data_as_commands + true, // auto_refresh + 60, // native_frames_per_second + true, // backlight_on_high + false, // SH1107_addressing + 350 // backlight pwm frequency + ); +} + +// TODO: Should we turn off the display when asleep, in board_deinit() ? diff --git a/ports/espressif/boards/m5stack_cardputer_ros/cardputer_keyboard.c b/ports/espressif/boards/m5stack_cardputer_ros/cardputer_keyboard.c new file mode 100644 index 0000000000000..73880f66e19d9 --- /dev/null +++ b/ports/espressif/boards/m5stack_cardputer_ros/cardputer_keyboard.c @@ -0,0 +1,238 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2016 Scott Shawcroft +// +// SPDX-License-Identifier: MIT + +#include "py/obj.h" +#include "py/objstr.h" +#include "py/runtime.h" + +#include "supervisor/shared/serial.h" +#include "shared-bindings/keypad/EventQueue.h" +#include "shared-bindings/keypad_demux/DemuxKeyMatrix.h" +#include "shared-bindings/microcontroller/Pin.h" +#include "shared-module/keypad/EventQueue.h" +#include "shared-module/keypad_demux/DemuxKeyMatrix.h" +#include "supervisor/shared/reload.h" + +#include "keymap.h" + +//| """M5Stack Cardputer keyboard integration. +//| """ +//| +//| """The KEYBOARD object is an instance of DemuxKeyMatrix, configured with correct pins. +//| The pins cannot be used for any other purposes (even though exposed in the board module). +//| By default all keyboard events are consumed and routed to the standard input - there is +//| not much use of the KEYBOARD object in this configuration - just read the input via sys.stdin. +//| +//| If you need to manually process individual key up / key down events via KEYBOARD.events, +//| call `detach_serial()`. +//| """" +//| KEYBOARD: keypad_demux.DemuxKeymatrix +//| +keypad_demux_demuxkeymatrix_obj_t cardputer_keyboard_obj; +bool cardputer_keyboard_serial_attached = false; + +void cardputer_keyboard_init(void); +void keyboard_seq(const char *seq); +void update_keyboard(keypad_eventqueue_obj_t *queue); + +//| def detach_serial() -> None: +//| """Stops consuming keyboard events and routing them to sys.stdin.""" +//| ... +//| +static mp_obj_t detach_serial(void) { + cardputer_keyboard_serial_attached = false; + common_hal_keypad_eventqueue_set_event_handler(cardputer_keyboard_obj.events, NULL); + return mp_const_none; +} +static MP_DEFINE_CONST_FUN_OBJ_0(detach_serial_obj, detach_serial); + +//| def attach_serial() -> None: +//| """Starts consuming keyboard events and routing them to sys.stdin.""" +//| ... +//| +static mp_obj_t attach_serial(void) { + common_hal_keypad_eventqueue_set_event_handler(cardputer_keyboard_obj.events, update_keyboard); + cardputer_keyboard_serial_attached = true; + return mp_const_none; +} +static MP_DEFINE_CONST_FUN_OBJ_0(attach_serial_obj, attach_serial); + +//| def key_to_char(key: int, shifted: bool) -> str | None: +//| """Converts a key index to the respective key (with or without shift modifier). +//| Returns None for functional & modifier keys or whenever not 0 <= key < 56. +//| """ +//| ... +//| +static mp_obj_t key_to_char(mp_obj_t key_obj, mp_obj_t shifted_obj) { + mp_int_t key = mp_obj_get_int(key_obj); + if (key < 0 || key > (mp_int_t)(sizeof keymap / sizeof *keymap) || keymap[key] == 0) { + return mp_const_none; + } else if (shifted_obj == mp_const_true) { + return mp_obj_new_str(&keymap_shifted[key], 1); + } else { + return mp_obj_new_str(&keymap[key], 1); + } +} +static MP_DEFINE_CONST_FUN_OBJ_2(key_to_char_obj, key_to_char); + +// Ring buffer of characters consumed from keyboard events (when serial attached) +ringbuf_t keyqueue; +char keybuf[32]; + +keypad_event_obj_t event; +char keystate[56]; + +// Keyboard pins +const mcu_pin_obj_t *row_addr_pins[] = { + &pin_GPIO8, + &pin_GPIO9, + &pin_GPIO11, +}; + +const mcu_pin_obj_t *column_pins[] = { + &pin_GPIO13, + &pin_GPIO15, + &pin_GPIO3, + &pin_GPIO4, + &pin_GPIO5, + &pin_GPIO6, + &pin_GPIO7 +}; + +void cardputer_keyboard_init(void) { + cardputer_keyboard_obj.base.type = &keypad_demux_demuxkeymatrix_type; + common_hal_keypad_demux_demuxkeymatrix_construct( + &cardputer_keyboard_obj, // self + 3, // num_row_addr_pins + row_addr_pins, // row_addr_pins + 7, // num_column_pins + column_pins, // column_pins + true, // columns_to_anodes + false, // transpose + 0.01f, // interval + 20, // max_events + 2 // debounce_threshold + ); + demuxkeymatrix_never_reset(&cardputer_keyboard_obj); + + ringbuf_init(&keyqueue, (uint8_t *)keybuf, sizeof(keybuf)); + attach_serial(); +} + +// Overrides the weakly linked function from supervisor/shared/serial.c +void board_serial_init(void) { + cardputer_keyboard_init(); +} + +// Overrides the weakly linked function from supervisor/shared/serial.c +bool board_serial_connected(void) { + return cardputer_keyboard_serial_attached; +} + +// Overrides the weakly linked function from supervisor/shared/serial.c +uint32_t board_serial_bytes_available(void) { + if (cardputer_keyboard_serial_attached) { + return ringbuf_num_filled(&keyqueue); + } else { + return 0; + } +} + +// Overrides the weakly linked function from supervisor/shared/serial.c +char board_serial_read(void) { + if (cardputer_keyboard_serial_attached) { + return ringbuf_get(&keyqueue); + } else { + return 0; + } +} + +void keyboard_seq(const char *seq) { + while (*seq) { + ringbuf_put(&keyqueue, *seq++); + } +} + +void update_keyboard(keypad_eventqueue_obj_t *queue) { + uint8_t ascii = 0; + + if (common_hal_keypad_eventqueue_get_length(queue) == 0) { + return; + } + + while (common_hal_keypad_eventqueue_get_into(queue, &event)) { + if (event.pressed) { + keystate[event.key_number] = 1; + + if (keystate[KEY_CTRL]) { + if (keystate[KEY_ALT] && keystate[KEY_BACKSPACE]) { + reload_initiate(RUN_REASON_REPL_RELOAD); + } + ascii = keymap[event.key_number]; + if (ascii >= 'a' && ascii <= 'z') { + ascii -= 'a' - 1; + } + + if (ascii == mp_interrupt_char) { + mp_sched_keyboard_interrupt(); + } + } else if (keystate[KEY_SHIFT]) { + ascii = keymap_shifted[event.key_number]; + } else if (keystate[KEY_FN] && event.key_number != KEY_FN) { + switch (event.key_number | FN_MOD) + { + case KEY_DOWN: + keyboard_seq("\e[B"); + break; + case KEY_UP: + keyboard_seq("\e[A"); + break; + case KEY_DELETE: + keyboard_seq("\e[3~"); + break; + case KEY_LEFT: + keyboard_seq("\e[D"); + break; + case KEY_RIGHT: + keyboard_seq("\e[C"); + break; + case KEY_ESC: + ringbuf_put(&keyqueue, '\e'); + break; + } + } else { + ascii = keymap[event.key_number]; + } + + if (ascii > 0) { + if (keystate[KEY_ALT]) { + ringbuf_put(&keyqueue, '\e'); + } else if (keystate[KEY_OPT]) { + ringbuf_put(&keyqueue, '\x10'); + } + ringbuf_put(&keyqueue, ascii); + } + } else { + keystate[event.key_number] = 0; + } + } +} + +static const mp_rom_map_elem_t cardputer_keyboard_module_globals_table[] = { + {MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_cardputer_keyboard)}, + {MP_ROM_QSTR(MP_QSTR_KEYBOARD), MP_ROM_PTR(&cardputer_keyboard_obj)}, + {MP_ROM_QSTR(MP_QSTR_attach_serial), MP_ROM_PTR(&attach_serial_obj)}, + {MP_ROM_QSTR(MP_QSTR_detach_serial), MP_ROM_PTR(&detach_serial_obj)}, + {MP_ROM_QSTR(MP_QSTR_key_to_char), MP_ROM_PTR(&key_to_char_obj)}, +}; +MP_DEFINE_CONST_DICT(cardputer_keyboard_module_globals, cardputer_keyboard_module_globals_table); + +const mp_obj_module_t cardputer_keyboard_module = { + .base = {&mp_type_module}, + .globals = (mp_obj_dict_t *)&cardputer_keyboard_module_globals, +}; + +MP_REGISTER_MODULE(MP_QSTR_cardputer_keyboard, cardputer_keyboard_module); diff --git a/ports/espressif/boards/m5stack_cardputer_ros/keymap.h b/ports/espressif/boards/m5stack_cardputer_ros/keymap.h new file mode 100644 index 0000000000000..0256fafaa0f57 --- /dev/null +++ b/ports/espressif/boards/m5stack_cardputer_ros/keymap.h @@ -0,0 +1,214 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2020 Scott Shawcroft for Adafruit Industries +// +// SPDX-License-Identifier: MIT + +#pragma once + +#define SHIFT_MOD 0x40 +#define FN_MOD 0x80 + +#define KEY_OPT 0 +#define KEY_Z 1 +#define KEY_C 2 +#define KEY_B 3 +#define KEY_M 4 +#define KEY_DOT 5 +#define KEY_SPACE 6 +#define KEY_SHIFT 7 +#define KEY_S 8 +#define KEY_F 9 +#define KEY_H 10 +#define KEY_K 11 +#define KEY_SEMICOLON 12 +#define KEY_ENTER 13 +#define KEY_Q 14 +#define KEY_E 15 +#define KEY_T 16 +#define KEY_U 17 +#define KEY_O 18 +#define KEY_LEFT_BRACKET 19 +#define KEY_BACKSLASH 20 +#define KEY_1 21 +#define KEY_3 22 +#define KEY_5 23 +#define KEY_7 24 +#define KEY_9 25 +#define KEY_UNDERSCORE 26 +#define KEY_BACKSPACE 27 +#define KEY_CTRL 28 +#define KEY_ALT 29 +#define KEY_X 30 +#define KEY_V 31 +#define KEY_N 32 +#define KEY_COMMA 33 +#define KEY_SLASH 34 +#define KEY_FN 35 +#define KEY_A 36 +#define KEY_D 37 +#define KEY_G 38 +#define KEY_J 39 +#define KEY_L 40 +#define KEY_APOSTROPHE 41 +#define KEY_TAB 42 +#define KEY_W 43 +#define KEY_R 44 +#define KEY_Y 45 +#define KEY_I 46 +#define KEY_P 47 +#define KEY_RIGHT_BRACKET 48 +#define KEY_GRAVE 49 +#define KEY_2 50 +#define KEY_4 51 +#define KEY_6 52 +#define KEY_8 53 +#define KEY_0 54 +#define KEY_EQUALS 55 + +#define KEY_GREATER (5 | SHIFT_MOD) +#define KEY_COLON (12 | SHIFT_MOD) +#define KEY_LEFT_CURLY_BRACKET (19 | SHIFT_MOD) +#define KEY_PIPE (20 | SHIFT_MOD) +#define KEY_EXCLAMATION (21 | SHIFT_MOD) +#define KEY_HASH (22 | SHIFT_MOD) +#define KEY_PERCENT (23 | SHIFT_MOD) +#define KEY_AMPERSAND (24 | SHIFT_MOD) +#define KEY_OPEN_PARENTHESIS (25 | SHIFT_MOD) +#define KEY_MINUS (26 | SHIFT_MOD) +#define KEY_LESS (33 | SHIFT_MOD) +#define KEY_QUESTION (34 | SHIFT_MOD) +#define KEY_DOUBLE_QUOTE (41 | SHIFT_MOD) +#define KEY_RIGHT_CURLY_BRACKET (48 | SHIFT_MOD) +#define KEY_TILDE (49 | SHIFT_MOD) +#define KEY_AT (50 | SHIFT_MOD) +#define KEY_DOLLAR (51 | SHIFT_MOD) +#define KEY_CARET (52 | SHIFT_MOD) +#define KEY_ASTERISK (53 | SHIFT_MOD) +#define KEY_CLOSE_PARENTHESIS (54 | SHIFT_MOD) +#define KEY_PLUS (55 | SHIFT_MOD) + +#define KEY_DOWN (5 | FN_MOD) +#define KEY_UP (12 | FN_MOD) +#define KEY_DELETE (27 | FN_MOD) +#define KEY_LEFT (33 | FN_MOD) +#define KEY_RIGHT (34 | FN_MOD) +#define KEY_ESC (49 | FN_MOD) + +const char keymap[56] = { + 0, // KEY_OPT + 'z', // KEY_Z + 'c', // KEY_C + 'b', // KEY_B + 'm', // KEY_M + '.', // KEY_DOT + ' ', // KEY_SPACE + 0, // KEY_SHIFT + 's', // KEY_S + 'f', // KEY_F + 'h', // KEY_H + 'k', // KEY_K + ';', // KEY_SEMICOLON + '\r',// KEY_ENTER + 'q', // KEY_Q + 'e', // KEY_E + 't', // KEY_T + 'u', // KEY_U + 'o', // KEY_O + '[', // KEY_LEFT_BRACKET + '\\',// KEY_BACKSLASH + '1', // KEY_1 + '3', // KEY_3 + '5', // KEY_5 + '7', // KEY_7 + '9', // KEY_9 + '_', // KEY_UNDERSCORE + '\b',// KEY_BACKSPACE + 0, // KEY_CTRL + 0, // KEY_ALT + 'x', // KEY_X + 'v', // KEY_V + 'n', // KEY_N + ',', // KEY_COMMA + '/', // KEY_SLASH + 0, // KEY_FN + 'a', // KEY_A + 'd', // KEY_D + 'g', // KEY_G + 'j', // KEY_J + 'l', // KEY_L + '\'',// KEY_APOSTROPHE + '\t',// KEY_TAB + 'w', // KEY_W + 'r', // KEY_R + 'y', // KEY_Y + 'i', // KEY_I + 'p', // KEY_P + ']', // KEY_RIGHT_BRACKET + '`', // KEY_GRAVE + '2', // KEY_2 + '4', // KEY_4 + '6', // KEY_6 + '8', // KEY_8 + '0', // KEY_0 + '=' // KEY_EQUALS +}; + +const char keymap_shifted[56] = { + 0, // KEY_OPT + 'Z', // KEY_Z + 'C', // KEY_C + 'B', // KEY_B + 'M', // KEY_M + '>', // KEY_DOT -> '>' + ' ', // KEY_SPACE + 0, // KEY_SHIFT + 'S', // KEY_S + 'F', // KEY_F + 'H', // KEY_H + 'K', // KEY_K + ':', // KEY_SEMICOLON -> ':' + '\r',// KEY_ENTER + 'Q', // KEY_Q + 'E', // KEY_E + 'T', // KEY_T + 'U', // KEY_U + 'O', // KEY_O + '{', // KEY_LEFT_BRACKET -> '{' + '|', // KEY_BACKSLASH -> '|' + '!', // KEY_1 -> '!' + '#', // KEY_3 -> '#' + '%', // KEY_5 -> '%' + '&', // KEY_7 -> '&' + '(', // KEY_9 -> '(' + '-', // KEY_UNDERSCORE -> '-' + '\b',// KEY_BACKSPACE + 0, // KEY_CTRL + 0, // KEY_ALT + 'X', // KEY_X + 'V', // KEY_V + 'N', // KEY_N + '<', // KEY_COMMA -> '<' + '?', // KEY_SLASH -> '?' + 0, // KEY_FN + 'A', // KEY_A + 'D', // KEY_D + 'G', // KEY_G + 'J', // KEY_J + 'L', // KEY_L + '"', // KEY_APOSTROPHE -> '"' + '\t',// KEY_TAB + 'W', // KEY_W + 'R', // KEY_R + 'Y', // KEY_Y + 'I', // KEY_I + 'P', // KEY_P + '}', // KEY_RIGHT_BRACKET -> '}' + '~', // KEY_GRAVE -> '~' + '@', // KEY_2 -> '@' + '$', // KEY_4 -> '$' + '^', // KEY_6 -> '^' + '*', // KEY_8 -> '*' + ')', // KEY_0 -> ')' + '+' // KEY_EQUALS -> '+' +}; diff --git a/ports/espressif/boards/m5stack_cardputer_ros/mpconfigboard.h b/ports/espressif/boards/m5stack_cardputer_ros/mpconfigboard.h new file mode 100644 index 0000000000000..4b62c7d76e6a1 --- /dev/null +++ b/ports/espressif/boards/m5stack_cardputer_ros/mpconfigboard.h @@ -0,0 +1,19 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2019 Scott Shawcroft for Adafruit Industries +// +// SPDX-License-Identifier: MIT + +#pragma once + +// Micropython setup + +#define MICROPY_HW_BOARD_NAME "M5Stack Cardputer (ROS version)" +#define MICROPY_HW_MCU_NAME "ESP32S3" + +#define MICROPY_HW_NEOPIXEL (&pin_GPIO21) +#define DEFAULT_I2C_BUS_SCL (&pin_GPIO1) +#define DEFAULT_I2C_BUS_SDA (&pin_GPIO2) +#define CIRCUITPY_BOARD_SPI (2) +#define CIRCUITPY_BOARD_SPI_PIN {{.clock = &pin_GPIO36, .mosi = &pin_GPIO35}, \ + {.clock = &pin_GPIO40, .mosi = &pin_GPIO14, .miso = &pin_GPIO39}} diff --git a/ports/espressif/boards/m5stack_cardputer_ros/mpconfigboard.mk b/ports/espressif/boards/m5stack_cardputer_ros/mpconfigboard.mk new file mode 100644 index 0000000000000..8d296939a1a7b --- /dev/null +++ b/ports/espressif/boards/m5stack_cardputer_ros/mpconfigboard.mk @@ -0,0 +1,17 @@ +USB_VID = 0x303A +USB_PID = 0x81DA +USB_PRODUCT = "M5Stack Cardputer - Circuitpython/ROS" +USB_MANUFACTURER = "M5STACK" + +IDF_TARGET = esp32s3 + +CIRCUITPY_ESP_FLASH_MODE = qio +CIRCUITPY_ESP_FLASH_FREQ = 80m +CIRCUITPY_ESP_FLASH_SIZE = 8MB +CIRCUITPY_ESPCAMERA = 0 +CIRCUITPY_RCLCPY = 1 + +CIRCUITPY_GIFIO = 1 +CIRCUITPY_MAX3421E = 0 + +SRC_C += boards/$(BOARD)/cardputer_keyboard.c diff --git a/ports/espressif/boards/m5stack_cardputer_ros/pins.c b/ports/espressif/boards/m5stack_cardputer_ros/pins.c new file mode 100644 index 0000000000000..ba309e0dcbff1 --- /dev/null +++ b/ports/espressif/boards/m5stack_cardputer_ros/pins.c @@ -0,0 +1,86 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2020 Scott Shawcroft for Adafruit Industries +// +// SPDX-License-Identifier: MIT + +#include "shared-bindings/board/__init__.h" + +#include "shared-module/displayio/__init__.h" +CIRCUITPY_BOARD_BUS_SINGLETON(sd_spi, spi, 1) +static const mp_rom_map_elem_t board_module_globals_table[] = { + CIRCUITPYTHON_BOARD_DICT_STANDARD_ITEMS + + // Port A + { MP_ROM_QSTR(MP_QSTR_TX), MP_ROM_PTR(&pin_GPIO1) }, + { MP_ROM_QSTR(MP_QSTR_PORTA1), MP_ROM_PTR(&pin_GPIO1) }, + { MP_ROM_QSTR(MP_QSTR_D1), MP_ROM_PTR(&pin_GPIO1) }, + { MP_ROM_QSTR(MP_QSTR_G1), MP_ROM_PTR(&pin_GPIO1) }, + { MP_ROM_QSTR(MP_QSTR_RX), MP_ROM_PTR(&pin_GPIO2) }, + { MP_ROM_QSTR(MP_QSTR_PORTA2), MP_ROM_PTR(&pin_GPIO2) }, + { MP_ROM_QSTR(MP_QSTR_D2), MP_ROM_PTR(&pin_GPIO2) }, + { MP_ROM_QSTR(MP_QSTR_G2), MP_ROM_PTR(&pin_GPIO2) }, + + // Keyboard + { MP_ROM_QSTR(MP_QSTR_KB_COL_0), MP_ROM_PTR(&pin_GPIO13) }, + { MP_ROM_QSTR(MP_QSTR_KB_COL_1), MP_ROM_PTR(&pin_GPIO15) }, + { MP_ROM_QSTR(MP_QSTR_KB_COL_2), MP_ROM_PTR(&pin_GPIO3) }, + { MP_ROM_QSTR(MP_QSTR_KB_COL_3), MP_ROM_PTR(&pin_GPIO4) }, + { MP_ROM_QSTR(MP_QSTR_KB_COL_4), MP_ROM_PTR(&pin_GPIO5) }, + { MP_ROM_QSTR(MP_QSTR_KB_COL_5), MP_ROM_PTR(&pin_GPIO6) }, + { MP_ROM_QSTR(MP_QSTR_KB_COL_6), MP_ROM_PTR(&pin_GPIO7) }, + { MP_ROM_QSTR(MP_QSTR_KB_A_0), MP_ROM_PTR(&pin_GPIO8) }, + { MP_ROM_QSTR(MP_QSTR_KB_A_1), MP_ROM_PTR(&pin_GPIO9) }, + { MP_ROM_QSTR(MP_QSTR_KB_A_2), MP_ROM_PTR(&pin_GPIO11) }, + + // Neopixel + { MP_ROM_QSTR(MP_QSTR_NEOPIXEL), MP_ROM_PTR(&pin_GPIO21) }, + + // Speaker + { MP_ROM_QSTR(MP_QSTR_I2S_BIT_CLOCK), MP_ROM_PTR(&pin_GPIO41) }, + { MP_ROM_QSTR(MP_QSTR_I2S_DATA), MP_ROM_PTR(&pin_GPIO42) }, + { MP_ROM_QSTR(MP_QSTR_I2S_WORD_SELECT), MP_ROM_PTR(&pin_GPIO43) }, + + // Mic + { MP_ROM_QSTR(MP_QSTR_MIC_DATA), MP_ROM_PTR(&pin_GPIO46) }, + { MP_ROM_QSTR(MP_QSTR_MIC_CLK), MP_ROM_PTR(&pin_GPIO43) }, + + // IR + { MP_ROM_QSTR(MP_QSTR_IR_TX), MP_ROM_PTR(&pin_GPIO44) }, + + // SD + { MP_ROM_QSTR(MP_QSTR_SD_MOSI), MP_ROM_PTR(&pin_GPIO14) }, + { MP_ROM_QSTR(MP_QSTR_SD_SCK), MP_ROM_PTR(&pin_GPIO40) }, + { MP_ROM_QSTR(MP_QSTR_SD_MISO), MP_ROM_PTR(&pin_GPIO39) }, + { MP_ROM_QSTR(MP_QSTR_SD_CS), MP_ROM_PTR(&pin_GPIO12) }, + + + // Display + { MP_ROM_QSTR(MP_QSTR_TFT_RST), MP_ROM_PTR(&pin_GPIO33) }, + { MP_ROM_QSTR(MP_QSTR_TFT_RESET), MP_ROM_PTR(&pin_GPIO33) }, + { MP_ROM_QSTR(MP_QSTR_TFT_DC), MP_ROM_PTR(&pin_GPIO34) }, + { MP_ROM_QSTR(MP_QSTR_TFT_RS), MP_ROM_PTR(&pin_GPIO34) }, + { MP_ROM_QSTR(MP_QSTR_TFT_MOSI), MP_ROM_PTR(&pin_GPIO35) }, + { MP_ROM_QSTR(MP_QSTR_TFT_DATA), MP_ROM_PTR(&pin_GPIO35) }, + { MP_ROM_QSTR(MP_QSTR_TFT_SCK), MP_ROM_PTR(&pin_GPIO36) }, + { MP_ROM_QSTR(MP_QSTR_TFT_CS), MP_ROM_PTR(&pin_GPIO37) }, + { MP_ROM_QSTR(MP_QSTR_TFT_BACKLIGHT), MP_ROM_PTR(&pin_GPIO38) }, + { MP_ROM_QSTR(MP_QSTR_TFT_BL), MP_ROM_PTR(&pin_GPIO38) }, + + // Button + { MP_ROM_QSTR(MP_QSTR_BUTTON), MP_ROM_PTR(&pin_GPIO0) }, + { MP_ROM_QSTR(MP_QSTR_BOOT0), MP_ROM_PTR(&pin_GPIO0) }, + + // Battery + { MP_ROM_QSTR(MP_QSTR_BAT_ADC), MP_ROM_PTR(&pin_GPIO10) }, + + // Other + { MP_ROM_QSTR(MP_QSTR_I2C), MP_ROM_PTR(&board_i2c_obj) }, + { MP_ROM_QSTR(MP_QSTR_PORTA_I2C), MP_ROM_PTR(&board_i2c_obj) }, + { MP_ROM_QSTR(MP_QSTR_TFT_SPI), MP_ROM_PTR(&board_spi_obj) }, + { MP_ROM_QSTR(MP_QSTR_SD_SPI), MP_ROM_PTR(&board_sd_spi_obj) }, + + // Display object + { MP_ROM_QSTR(MP_QSTR_DISPLAY), MP_ROM_PTR(&displays[0].display)}, +}; +MP_DEFINE_CONST_DICT(board_module_globals, board_module_globals_table); diff --git a/ports/espressif/boards/m5stack_cardputer_ros/sdkconfig b/ports/espressif/boards/m5stack_cardputer_ros/sdkconfig new file mode 100644 index 0000000000000..e962866216039 --- /dev/null +++ b/ports/espressif/boards/m5stack_cardputer_ros/sdkconfig @@ -0,0 +1,14 @@ +# +# Espressif IoT Development Framework Configuration +# +# +# Component config +# +# +# LWIP +# +# end of LWIP + +# end of Component config + +# end of Espressif IoT Development Framework Configuration diff --git a/ports/espressif/common-hal/rclcpy/Node.c b/ports/espressif/common-hal/rclcpy/Node.c new file mode 100644 index 0000000000000..f68daa44fc34d --- /dev/null +++ b/ports/espressif/common-hal/rclcpy/Node.c @@ -0,0 +1,47 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#include "shared-bindings/rclcpy/Node.h" +#include "shared-bindings/rclcpy/__init__.h" + +#include "esp_log.h" + +void common_hal_rclcpy_node_construct(rclcpy_node_obj_t *self, + const char *node_name, const char *node_namespace) { + + rclc_node_init_default(&self->rcl_node, node_name, node_namespace, &rclcpy_default_context.rcl_support); + if (!rcl_node_is_valid(&self->rcl_node)) { + mp_raise_RuntimeError(MP_ERROR_TEXT("ROS node failed to initialize")); + } + self->deinited = false; +} + +bool common_hal_rclcpy_node_deinited(rclcpy_node_obj_t *self) { + return self->deinited; +} + +void common_hal_rclcpy_node_deinit(rclcpy_node_obj_t *self) { + if (common_hal_rclcpy_node_deinited(self)) { + return; + } + // Clean up Micro-ROS object + rcl_ret_t ret = rcl_node_fini(&self->rcl_node); + if (ret != RCL_RET_OK) { + // TODO: node_fini returns a fail here, but doesn't impede microros + // from restarting. Debug left for future investigation. + ESP_LOGW("RCLCPY", "Node cleanup error: %d", ret); + // rclcpy_default_context.critical_fail=RCLCPY_NODE_FAIL; + } + self->deinited = true; +} + +const char *common_hal_rclcpy_node_get_name(rclcpy_node_obj_t *self) { + return rcl_node_get_name(&self->rcl_node); +} + +const char *common_hal_rclcpy_node_get_namespace(rclcpy_node_obj_t *self) { + return rcl_node_get_namespace(&self->rcl_node); +} diff --git a/ports/espressif/common-hal/rclcpy/Node.h b/ports/espressif/common-hal/rclcpy/Node.h new file mode 100644 index 0000000000000..c019a654e5add --- /dev/null +++ b/ports/espressif/common-hal/rclcpy/Node.h @@ -0,0 +1,23 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#pragma once + +#include "py/obj.h" +#include "py/runtime.h" + +#include +#include +#include +#include +#include + + +typedef struct { + mp_obj_base_t base; + bool deinited; + rcl_node_t rcl_node; +} rclcpy_node_obj_t; diff --git a/ports/espressif/common-hal/rclcpy/Publisher.c b/ports/espressif/common-hal/rclcpy/Publisher.c new file mode 100644 index 0000000000000..81df154687ace --- /dev/null +++ b/ports/espressif/common-hal/rclcpy/Publisher.c @@ -0,0 +1,64 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#include "shared-bindings/rclcpy/Publisher.h" + +#include "esp_log.h" + +void common_hal_rclcpy_publisher_construct(rclcpy_publisher_obj_t *self, rclcpy_node_obj_t *node, + const char *topic_name) { + + // Create Int32 type object + // TODO: support other message types through class imports + const rosidl_message_type_support_t *type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32); + + // Creates a reliable Int32 publisher + rcl_ret_t rc = rclc_publisher_init_default( + &self->rcl_publisher, &node->rcl_node, + type_support, topic_name); + if (RCL_RET_OK != rc) { + mp_raise_RuntimeError(MP_ERROR_TEXT("ROS topic failed to initialize")); + } + + self->node = node; +} + +bool common_hal_rclcpy_publisher_deinited(rclcpy_publisher_obj_t *self) { + return self->node == NULL; +} + +void common_hal_rclcpy_publisher_deinit(rclcpy_publisher_obj_t *self) { + if (common_hal_rclcpy_publisher_deinited(self)) { + return; + } + // Clean up Micro-ROS object + rcl_ret_t ret = rcl_publisher_fini(&self->rcl_publisher, &self->node->rcl_node); + if (ret != RCL_RET_OK) { + // TODO: publisher_fini returns a fail here, but doesn't impede microros + // from restarting. Debug left for future investigation. + ESP_LOGW("RCLCPY", "Publisher cleanup error: %d", ret); + // rclcpy_default_context.critical_fail=RCLCPY_PUB_FAIL; + } + self->node = NULL; +} + +void common_hal_rclcpy_publisher_publish_int32(rclcpy_publisher_obj_t *self, int32_t data) { + // Int32 message object + std_msgs__msg__Int32 msg; + + // Set message value + msg.data = data; + + // Publish message + rcl_ret_t rc = rcl_publish(&self->rcl_publisher, &msg, NULL); + if (RCL_RET_OK != rc) { + mp_raise_RuntimeError(MP_ERROR_TEXT("Could not publish to ROS topic")); + } +} + +const char *common_hal_rclcpy_publisher_get_topic_name(rclcpy_publisher_obj_t *self) { + return rcl_publisher_get_topic_name(&self->rcl_publisher); +} diff --git a/ports/espressif/common-hal/rclcpy/Publisher.h b/ports/espressif/common-hal/rclcpy/Publisher.h new file mode 100644 index 0000000000000..fdc45293cd270 --- /dev/null +++ b/ports/espressif/common-hal/rclcpy/Publisher.h @@ -0,0 +1,26 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#pragma once + +#include "py/obj.h" + +#include "common-hal/rclcpy/Node.h" +#include "common-hal/rclcpy/__init__.h" + +#include +#include +#include +#include +#include +#include + + +typedef struct { + mp_obj_base_t base; + rclcpy_node_obj_t *node; + rcl_publisher_t rcl_publisher; +} rclcpy_publisher_obj_t; diff --git a/ports/espressif/common-hal/rclcpy/__init__.c b/ports/espressif/common-hal/rclcpy/__init__.c new file mode 100644 index 0000000000000..e0e6fd8b4f4fc --- /dev/null +++ b/ports/espressif/common-hal/rclcpy/__init__.c @@ -0,0 +1,122 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#include "shared-bindings/rclcpy/__init__.h" + +#include "esp_log.h" + +#define RCLCPY_SUPPORT_FAIL 1 +#define RCLCPY_OPTIONS_FAIL 2 + +rclcpy_context_t rclcpy_default_context = { + .initialized = false, + .critical_fail = 0, +}; + +static void *microros_allocate(size_t size, void *state) { + (void)state; + return m_malloc(size); +} + +static void microros_deallocate(void *pointer, void *state) { + (void)state; + m_free(pointer); +} + +static void *microros_reallocate(void *pointer, size_t size, void *state) { + (void)state; + return m_realloc(pointer, size); +} + +static void *microros_zero_allocate(size_t number_of_elements, size_t size_of_element, void *state) { + (void)state; + size_t total_size = number_of_elements * size_of_element; + void *ptr = m_malloc(total_size); + if (ptr != NULL) { + memset(ptr, 0, total_size); + } + return ptr; +} + +void rclcpy_reset(void) { + if (rclcpy_default_context.initialized) { + // Clean up micro-ROS objects + rcl_ret_t ret = rclc_support_fini(&rclcpy_default_context.rcl_support); + if (ret != RCL_RET_OK) { + // ESP_LOGW("RCLCPY", "Support cleanup error: %d", ret); + rclcpy_default_context.critical_fail = RCLCPY_SUPPORT_FAIL; + } + ret = rcl_init_options_fini(&rclcpy_default_context.init_options); + if (ret != RCL_RET_OK) { + // ESP_LOGW("RCLCPY", "Init options cleanup error: %d", ret); + rclcpy_default_context.critical_fail = RCLCPY_OPTIONS_FAIL; + } + + // Reset context state + memset(&rclcpy_default_context, 0, sizeof(rclcpy_default_context)); + rclcpy_default_context.initialized = false; + } +} + +void common_hal_rclcpy_init(const char *agent_ip, const char *agent_port, int16_t domain_id) { + if (rclcpy_default_context.critical_fail != 0) { + mp_raise_RuntimeError_varg(MP_ERROR_TEXT("Critical ROS failure during soft reboot, reset required: %d"), rclcpy_default_context.critical_fail); + } + + // Set up circuitpython-friendly allocator + rcl_allocator_t custom_allocator = rcutils_get_zero_initialized_allocator(); + custom_allocator.allocate = microros_allocate; + custom_allocator.deallocate = microros_deallocate; + custom_allocator.reallocate = microros_reallocate; + custom_allocator.zero_allocate = microros_zero_allocate; + if (!rcutils_set_default_allocator(&custom_allocator)) { + mp_raise_RuntimeError(MP_ERROR_TEXT("ROS memory allocator failure")); + } + rclcpy_default_context.rcl_allocator = custom_allocator; + + rcl_ret_t ret; + + // Micro-ROS options initialization + rclcpy_default_context.init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&rclcpy_default_context.init_options, rclcpy_default_context.rcl_allocator); + if (ret != RCL_RET_OK) { + mp_raise_RuntimeError(MP_ERROR_TEXT("ROS internal setup failure")); + } + if (domain_id < 0) { + mp_raise_RuntimeError(MP_ERROR_TEXT("Invalid ROS domain ID")); + } + ret = rcl_init_options_set_domain_id(&rclcpy_default_context.init_options, domain_id); + if (ret != RCL_RET_OK) { + mp_raise_RuntimeError(MP_ERROR_TEXT("ROS internal setup failure")); + } + + // Set up Micro-ROS Agent + rclcpy_default_context.rmw_options = rcl_init_options_get_rmw_init_options(&rclcpy_default_context.init_options); + ret = rmw_uros_options_set_udp_address(agent_ip, agent_port, rclcpy_default_context.rmw_options); + if (ret != RCL_RET_OK) { + mp_raise_RuntimeError(MP_ERROR_TEXT("ROS internal setup failure")); + } + + // Final support object init requires a connected agent + ret = rclc_support_init_with_options(&rclcpy_default_context.rcl_support, + 0, + NULL, + &rclcpy_default_context.init_options, + &rclcpy_default_context.rcl_allocator); + if (ret != RCL_RET_OK) { + mp_raise_RuntimeError(MP_ERROR_TEXT("ROS failed to initialize. Is agent connected?")); + } else { + rclcpy_default_context.initialized = true; + } +} + +rclcpy_context_t *common_hal_rclcpy_get_default_context(void) { + return &rclcpy_default_context; +} + +bool common_hal_rclcpy_default_context_is_initialized(void) { + return rclcpy_default_context.initialized; +} diff --git a/ports/espressif/common-hal/rclcpy/__init__.h b/ports/espressif/common-hal/rclcpy/__init__.h new file mode 100644 index 0000000000000..f3ebd4787e757 --- /dev/null +++ b/ports/espressif/common-hal/rclcpy/__init__.h @@ -0,0 +1,30 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#pragma once + +#include "py/obj.h" +#include "py/runtime.h" + +#include +#include +#include +#include +#include +#include + + +typedef struct { + bool initialized; + uint8_t critical_fail; + rcl_allocator_t rcl_allocator; + rclc_support_t rcl_support; + rcl_init_options_t init_options; + rmw_init_options_t *rmw_options; +} rclcpy_context_t; + +extern rclcpy_context_t rclcpy_default_context; +void rclcpy_reset(void); diff --git a/ports/espressif/microros-lib b/ports/espressif/microros-lib new file mode 160000 index 0000000000000..4b2a696dec7f0 --- /dev/null +++ b/ports/espressif/microros-lib @@ -0,0 +1 @@ +Subproject commit 4b2a696dec7f0f865e014ea4b83f2332df7e9560 diff --git a/ports/espressif/mpconfigport.mk b/ports/espressif/mpconfigport.mk index ccc305761c4dc..516111e5a8586 100644 --- a/ports/espressif/mpconfigport.mk +++ b/ports/espressif/mpconfigport.mk @@ -64,6 +64,7 @@ CIRCUITPY_HASHLIB ?= 1 CIRCUITPY_I2CTARGET ?= 0 CIRCUITPY_MAX3421E ?= 1 CIRCUITPY_MEMORYMAP ?= 1 +CIRCUITPY_RCLCPY ?= 0 CIRCUITPY_NVM ?= 1 CIRCUITPY_PARALLELDISPLAYBUS ?= 1 CIRCUITPY_PS2IO ?= 1 diff --git a/ports/espressif/supervisor/port.c b/ports/espressif/supervisor/port.c index 121335245dc33..ba25747476c5e 100644 --- a/ports/espressif/supervisor/port.c +++ b/ports/espressif/supervisor/port.c @@ -55,6 +55,10 @@ #include "esp_camera.h" #endif +#if CIRCUITPY_RCLCPY +#include "common-hal/rclcpy/__init__.h" +#endif + #include "soc/efuse_reg.h" #if defined(SOC_LP_AON_SUPPORTED) #include "soc/lp_aon_reg.h" @@ -378,6 +382,10 @@ void reset_port(void) { ps2_reset(); #endif + #if CIRCUITPY_RCLCPY + rclcpy_reset(); + #endif + #if CIRCUITPY_RTC rtc_reset(); #endif diff --git a/ports/zephyr-cp/boards/nordic/nrf5340dk/autogen_board_info.toml b/ports/zephyr-cp/boards/nordic/nrf5340dk/autogen_board_info.toml index aecccebb9c850..222a325e346b6 100644 --- a/ports/zephyr-cp/boards/nordic/nrf5340dk/autogen_board_info.toml +++ b/ports/zephyr-cp/boards/nordic/nrf5340dk/autogen_board_info.toml @@ -78,6 +78,7 @@ pwmio = false qrio = false rainbowio = false random = true +rclcpy = false rgbmatrix = false rotaryio = false rtc = false diff --git a/ports/zephyr-cp/boards/nordic/nrf54l15dk/autogen_board_info.toml b/ports/zephyr-cp/boards/nordic/nrf54l15dk/autogen_board_info.toml index 27e4f35f22366..41631c3d6c02c 100644 --- a/ports/zephyr-cp/boards/nordic/nrf54l15dk/autogen_board_info.toml +++ b/ports/zephyr-cp/boards/nordic/nrf54l15dk/autogen_board_info.toml @@ -78,6 +78,7 @@ pwmio = false qrio = false rainbowio = false random = true +rclcpy = false rgbmatrix = false rotaryio = false rtc = false diff --git a/ports/zephyr-cp/boards/nordic/nrf7002dk/autogen_board_info.toml b/ports/zephyr-cp/boards/nordic/nrf7002dk/autogen_board_info.toml index 35e3791507023..ea009aa36e1ca 100644 --- a/ports/zephyr-cp/boards/nordic/nrf7002dk/autogen_board_info.toml +++ b/ports/zephyr-cp/boards/nordic/nrf7002dk/autogen_board_info.toml @@ -78,6 +78,7 @@ pwmio = false qrio = false rainbowio = false random = true +rclcpy = false rgbmatrix = false rotaryio = false rtc = false diff --git a/ports/zephyr-cp/boards/renesas/ek_ra6m5/autogen_board_info.toml b/ports/zephyr-cp/boards/renesas/ek_ra6m5/autogen_board_info.toml index 4f4ee95bd0296..a96d1bad1d2a6 100644 --- a/ports/zephyr-cp/boards/renesas/ek_ra6m5/autogen_board_info.toml +++ b/ports/zephyr-cp/boards/renesas/ek_ra6m5/autogen_board_info.toml @@ -78,6 +78,7 @@ pwmio = false qrio = false rainbowio = false random = true +rclcpy = false rgbmatrix = false rotaryio = false rtc = false diff --git a/ports/zephyr-cp/boards/renesas/ek_ra8d1/autogen_board_info.toml b/ports/zephyr-cp/boards/renesas/ek_ra8d1/autogen_board_info.toml index 90bb28c341ef6..e4946decb190b 100644 --- a/ports/zephyr-cp/boards/renesas/ek_ra8d1/autogen_board_info.toml +++ b/ports/zephyr-cp/boards/renesas/ek_ra8d1/autogen_board_info.toml @@ -78,6 +78,7 @@ pwmio = false qrio = false rainbowio = false random = true +rclcpy = false rgbmatrix = false rotaryio = false rtc = false diff --git a/ports/zephyr-cp/boards/st/nucleo_u575zi_q/autogen_board_info.toml b/ports/zephyr-cp/boards/st/nucleo_u575zi_q/autogen_board_info.toml index c6fa66037a9ac..7519d8817687b 100644 --- a/ports/zephyr-cp/boards/st/nucleo_u575zi_q/autogen_board_info.toml +++ b/ports/zephyr-cp/boards/st/nucleo_u575zi_q/autogen_board_info.toml @@ -78,6 +78,7 @@ pwmio = false qrio = false rainbowio = false random = true +rclcpy = false rgbmatrix = false rotaryio = false rtc = false diff --git a/ports/zephyr-cp/boards/st/stm32h7b3i_dk/autogen_board_info.toml b/ports/zephyr-cp/boards/st/stm32h7b3i_dk/autogen_board_info.toml index 9e117262b3f91..e029020982d20 100644 --- a/ports/zephyr-cp/boards/st/stm32h7b3i_dk/autogen_board_info.toml +++ b/ports/zephyr-cp/boards/st/stm32h7b3i_dk/autogen_board_info.toml @@ -78,6 +78,7 @@ pwmio = false qrio = false rainbowio = false random = true +rclcpy = false rgbmatrix = false rotaryio = false rtc = false diff --git a/py/circuitpy_defns.mk b/py/circuitpy_defns.mk index f73d52a6c4b12..d6569bc39bd8e 100644 --- a/py/circuitpy_defns.mk +++ b/py/circuitpy_defns.mk @@ -201,6 +201,9 @@ endif ifeq ($(CIRCUITPY_DISPLAYIO),1) SRC_PATTERNS += displayio/% endif +ifeq ($(CIRCUITPY_DUALBANK),1) +SRC_PATTERNS += dualbank/% +endif ifeq ($(CIRCUITPY__EVE),1) SRC_PATTERNS += _eve/% endif @@ -279,6 +282,9 @@ endif ifeq ($(CIRCUITPY_MAX3421E),1) SRC_PATTERNS += max3421e/% endif +ifeq ($(CIRCUITPY_MDNS),1) +SRC_PATTERNS += mdns/% +endif ifeq ($(CIRCUITPY_MEMORYMAP),1) SRC_PATTERNS += memorymap/% endif @@ -288,9 +294,6 @@ endif ifeq ($(CIRCUITPY_MICROCONTROLLER),1) SRC_PATTERNS += microcontroller/% endif -ifeq ($(CIRCUITPY_MDNS),1) -SRC_PATTERNS += mdns/% -endif ifeq ($(CIRCUITPY_MSGPACK),1) SRC_PATTERNS += msgpack/% endif @@ -306,9 +309,6 @@ endif ifeq ($(CIRCUITPY_OS),1) SRC_PATTERNS += os/% endif -ifeq ($(CIRCUITPY_DUALBANK),1) -SRC_PATTERNS += dualbank/% -endif ifeq ($(CIRCUITPY_PARALLELDISPLAYBUS),1) SRC_PATTERNS += paralleldisplaybus/% endif @@ -342,6 +342,9 @@ endif ifeq ($(CIRCUITPY_RANDOM),1) SRC_PATTERNS += random/% endif +ifeq ($(CIRCUITPY_RCLCPY),1) +SRC_PATTERNS += rclcpy/% +endif ifeq ($(CIRCUITPY_RGBMATRIX),1) SRC_PATTERNS += rgbmatrix/% endif @@ -540,6 +543,9 @@ SRC_COMMON_HAL_ALL = \ pulseio/__init__.c \ pwmio/PWMOut.c \ pwmio/__init__.c \ + rclcpy/__init__.c \ + rclcpy/Node.c \ + rclcpy/Publisher.c \ rgbmatrix/RGBMatrix.c \ rgbmatrix/__init__.c \ rotaryio/IncrementalEncoder.c \ diff --git a/py/circuitpy_mpconfig.mk b/py/circuitpy_mpconfig.mk index 6ddac5eeccd8c..2009c4e177da6 100644 --- a/py/circuitpy_mpconfig.mk +++ b/py/circuitpy_mpconfig.mk @@ -379,6 +379,9 @@ CFLAGS += -DCIRCUITPY_MATH=$(CIRCUITPY_MATH) CIRCUITPY_MAX3421E ?= 0 CFLAGS += -DCIRCUITPY_MAX3421E=$(CIRCUITPY_MAX3421E) +CIRCUITPY_MDNS ?= $(CIRCUITPY_WIFI) +CFLAGS += -DCIRCUITPY_MDNS=$(CIRCUITPY_MDNS) + CIRCUITPY_MEMORYMAP ?= 0 CFLAGS += -DCIRCUITPY_MEMORYMAP=$(CIRCUITPY_MEMORYMAP) @@ -388,9 +391,6 @@ CFLAGS += -DCIRCUITPY_MEMORYMONITOR=$(CIRCUITPY_MEMORYMONITOR) CIRCUITPY_MICROCONTROLLER ?= 1 CFLAGS += -DCIRCUITPY_MICROCONTROLLER=$(CIRCUITPY_MICROCONTROLLER) -CIRCUITPY_MDNS ?= $(CIRCUITPY_WIFI) -CFLAGS += -DCIRCUITPY_MDNS=$(CIRCUITPY_MDNS) - CIRCUITPY_MSGPACK ?= $(CIRCUITPY_FULL_BUILD) CFLAGS += -DCIRCUITPY_MSGPACK=$(CIRCUITPY_MSGPACK) @@ -454,6 +454,9 @@ CFLAGS += -DCIRCUITPY_RAINBOWIO=$(CIRCUITPY_RAINBOWIO) CIRCUITPY_RANDOM ?= 1 CFLAGS += -DCIRCUITPY_RANDOM=$(CIRCUITPY_RANDOM) +CIRCUITPY_RCLCPY ?= 0 +CFLAGS += -DCIRCUITPY_RCLCPY=$(CIRCUITPY_RCLCPY) + CIRCUITPY_RE ?= $(CIRCUITPY_FULL_BUILD) CFLAGS += -DCIRCUITPY_RE=$(CIRCUITPY_RE) diff --git a/shared-bindings/rclcpy/Node.c b/shared-bindings/rclcpy/Node.c new file mode 100644 index 0000000000000..a2fa9bd36a656 --- /dev/null +++ b/shared-bindings/rclcpy/Node.c @@ -0,0 +1,145 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#include +#include "shared-bindings/rclcpy/Node.h" +#include "shared-bindings/rclcpy/Publisher.h" +#include "shared-bindings/util.h" +#include "py/objproperty.h" +#include "py/objtype.h" +#include "py/runtime.h" + + +//| class Node: +//| """A ROS2 Node""" +//| +//| def __init__( +//| self, +//| node_name: str, +//| *, +//| namespace: str | None = None, +//| ) -> None: +//| """Create a Node. +//| +//| Creates an instance of a ROS2 Node. Nodes can be used to create other ROS +//| entities like publishers or subscribers. Nodes must have a unique name, and +//| may also be constructed from their class. +//| +//| :param str node_name: The name of the node. Must be a valid ROS 2 node name +//| :param str namespace: The namespace for the node. If None, the node will be +//| created in the root namespace +//| """ +//| ... +//| +static mp_obj_t rclcpy_node_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { + enum { ARG_node_name, ARG_namespace }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_node_name, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_namespace, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_obj = mp_const_none} }, + }; + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + const char *node_name = mp_obj_str_get_str(args[ARG_node_name].u_obj); + const char *namespace = ""; + if (args[ARG_namespace].u_obj != mp_const_none) { + namespace = mp_obj_str_get_str(args[ARG_namespace].u_obj); + } + + rclcpy_node_obj_t *self = mp_obj_malloc_with_finaliser(rclcpy_node_obj_t, &rclcpy_node_type); + common_hal_rclcpy_node_construct(self, node_name, namespace); + return (mp_obj_t)self; +} + +//| def deinit(self) -> None: +//| """Deinitializes the node and frees any hardware or remote agent resources +//| used by it. Deinitialized nodes cannot be used again. +//| """ +//| ... +//| +static mp_obj_t rclcpy_node_obj_deinit(mp_obj_t self_in) { + rclcpy_node_obj_t *self = MP_OBJ_TO_PTR(self_in); + common_hal_rclcpy_node_deinit(self); + return mp_const_none; +} +MP_DEFINE_CONST_FUN_OBJ_1(rclcpy_node_deinit_obj, rclcpy_node_obj_deinit); + +static void check_for_deinit(rclcpy_node_obj_t *self) { + if (common_hal_rclcpy_node_deinited(self)) { + raise_deinited_error(); + } +} + +//| def create_publisher(self, topic: str) -> Publisher: +//| """Create a publisher for a given topic string. +//| +//| Creates an instance of a ROS2 Publisher. +//| +//| :param str topic: The name of the topic +//| :return: A new Publisher object for the specified topic +//| :rtype: Publisher +//| """ +//| ... +//| +static mp_obj_t rclcpy_node_create_publisher(mp_obj_t self_in, mp_obj_t topic) { + rclcpy_node_obj_t *self = MP_OBJ_TO_PTR(self_in); + check_for_deinit(self); + const char *topic_name = mp_obj_str_get_str(topic); + + rclcpy_publisher_obj_t *publisher = mp_obj_malloc_with_finaliser(rclcpy_publisher_obj_t, &rclcpy_publisher_type); + common_hal_rclcpy_publisher_construct(publisher, self, topic_name); + return (mp_obj_t)publisher; +} +static MP_DEFINE_CONST_FUN_OBJ_2(rclcpy_node_create_publisher_obj, rclcpy_node_create_publisher); + +//| def get_name(self) -> str: +//| """Get the name of the node. +//| +//| :return: The node's name +//| :rtype: str +//| """ +//| ... +//| +static mp_obj_t rclcpy_node_get_name(mp_obj_t self_in) { + rclcpy_node_obj_t *self = MP_OBJ_TO_PTR(self_in); + check_for_deinit(self); + const char *name_str = common_hal_rclcpy_node_get_name(self); + return mp_obj_new_str(name_str, strlen(name_str)); +} +static MP_DEFINE_CONST_FUN_OBJ_1(rclcpy_node_get_name_obj, rclcpy_node_get_name); + +//| def get_namespace(self) -> str: +//| """Get the namespace of the node. +//| +//| :return: The node's namespace +//| :rtype: str +//| """ +//| ... +//| +static mp_obj_t rclcpy_node_get_namespace(mp_obj_t self_in) { + rclcpy_node_obj_t *self = MP_OBJ_TO_PTR(self_in); + check_for_deinit(self); + const char *namespace_str = common_hal_rclcpy_node_get_namespace(self); + return mp_obj_new_str(namespace_str, strlen(namespace_str)); +} +static MP_DEFINE_CONST_FUN_OBJ_1(rclcpy_node_get_namespace_obj, rclcpy_node_get_namespace); + +static const mp_rom_map_elem_t rclcpy_node_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR_deinit), MP_ROM_PTR(&rclcpy_node_deinit_obj) }, + { MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&rclcpy_node_deinit_obj) }, + { MP_ROM_QSTR(MP_QSTR_create_publisher), MP_ROM_PTR(&rclcpy_node_create_publisher_obj) }, + { MP_ROM_QSTR(MP_QSTR_get_name), MP_ROM_PTR(&rclcpy_node_get_name_obj) }, + { MP_ROM_QSTR(MP_QSTR_get_namespace), MP_ROM_PTR(&rclcpy_node_get_namespace_obj) }, +}; +static MP_DEFINE_CONST_DICT(rclcpy_node_locals_dict, rclcpy_node_locals_dict_table); + + +MP_DEFINE_CONST_OBJ_TYPE( + rclcpy_node_type, + MP_QSTR_Node, + MP_TYPE_FLAG_HAS_SPECIAL_ACCESSORS, + make_new, rclcpy_node_make_new, + locals_dict, &rclcpy_node_locals_dict + ); diff --git a/shared-bindings/rclcpy/Node.h b/shared-bindings/rclcpy/Node.h new file mode 100644 index 0000000000000..be0230846f6bd --- /dev/null +++ b/shared-bindings/rclcpy/Node.h @@ -0,0 +1,18 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#pragma once +#include "common-hal/rclcpy/Node.h" + + +extern const mp_obj_type_t rclcpy_node_type; + +void common_hal_rclcpy_node_construct(rclcpy_node_obj_t *self, + const char *node_name, const char *namespace); +bool common_hal_rclcpy_node_deinited(rclcpy_node_obj_t *self); +void common_hal_rclcpy_node_deinit(rclcpy_node_obj_t *self); +const char *common_hal_rclcpy_node_get_name(rclcpy_node_obj_t *self); +const char *common_hal_rclcpy_node_get_namespace(rclcpy_node_obj_t *self); diff --git a/shared-bindings/rclcpy/Publisher.c b/shared-bindings/rclcpy/Publisher.c new file mode 100644 index 0000000000000..be1e229cce4f8 --- /dev/null +++ b/shared-bindings/rclcpy/Publisher.c @@ -0,0 +1,99 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#include +#include "shared-bindings/rclcpy/Publisher.h" +#include "shared-bindings/util.h" +#include "py/objproperty.h" +#include "py/objtype.h" +#include "py/runtime.h" + + +//| class Publisher: +//| """A ROS2 publisher""" +//| +//| def __init__(self) -> None: +//| """Publishers cannot be created directly. +//| +//| Use :meth:`Node.create_publisher` to create a publisher from a node. +//| +//| :raises NotImplementedError: Always, as direct instantiation is not supported +//| """ +//| ... +//| +static mp_obj_t rclcpy_publisher_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { + mp_raise_NotImplementedError(MP_ERROR_TEXT("Publishers can only be created from a parent node")); +} + +//| def deinit(self) -> None: +//| """Deinitializes the publisher and frees any hardware or remote agent resources +//| used by it. Deinitialized publishers cannot be used again. +//| """ +//| ... +//| +static mp_obj_t rclcpy_publisher_obj_deinit(mp_obj_t self_in) { + rclcpy_publisher_obj_t *self = MP_OBJ_TO_PTR(self_in); + common_hal_rclcpy_publisher_deinit(self); + return mp_const_none; +} +MP_DEFINE_CONST_FUN_OBJ_1(rclcpy_publisher_deinit_obj, rclcpy_publisher_obj_deinit); + +static void check_for_deinit(rclcpy_publisher_obj_t *self) { + if (common_hal_rclcpy_publisher_deinited(self)) { + raise_deinited_error(); + } +} + +//| def publish_int32(self, message: int) -> None: +//| """Publish a 32-bit signed integer message to the topic. +//| +//| :param int message: The integer value to publish. Must be within the range +//| of a 32-bit signed integer (-2,147,483,648 to 2,147,483,647) +//| """ +//| ... +//| +static mp_obj_t rclcpy_publisher_publish_int32(mp_obj_t self_in, mp_obj_t in_msg) { + rclcpy_publisher_obj_t *self = MP_OBJ_TO_PTR(self_in); + check_for_deinit(self); + int32_t msg = mp_obj_get_int(in_msg); + common_hal_rclcpy_publisher_publish_int32(self, msg); + return mp_const_none; +} +static MP_DEFINE_CONST_FUN_OBJ_2(rclcpy_publisher_publish_int32_obj, rclcpy_publisher_publish_int32); + +//| def get_topic_name(self) -> str: +//| """Get the name of the topic this publisher publishes to. +//| +//| :return: The topic name as specified when the publisher was created +//| :rtype: str +//| """ +//| ... +//| +static mp_obj_t rclcpy_publisher_get_topic_name(mp_obj_t self_in) { + rclcpy_publisher_obj_t *self = MP_OBJ_TO_PTR(self_in); + check_for_deinit(self); + const char *topic_str = common_hal_rclcpy_publisher_get_topic_name(self); + return mp_obj_new_str(topic_str, strlen(topic_str)); +} +static MP_DEFINE_CONST_FUN_OBJ_1(rclcpy_publisher_get_topic_name_obj, rclcpy_publisher_get_topic_name); + + +static const mp_rom_map_elem_t rclcpy_publisher_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR_deinit), MP_ROM_PTR(&rclcpy_publisher_deinit_obj) }, + { MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&rclcpy_publisher_deinit_obj) }, + { MP_ROM_QSTR(MP_QSTR_publish_int32), MP_ROM_PTR(&rclcpy_publisher_publish_int32_obj) }, + { MP_ROM_QSTR(MP_QSTR_get_topic_name), MP_ROM_PTR(&rclcpy_publisher_get_topic_name_obj) }, +}; +static MP_DEFINE_CONST_DICT(rclcpy_publisher_locals_dict, rclcpy_publisher_locals_dict_table); + + +MP_DEFINE_CONST_OBJ_TYPE( + rclcpy_publisher_type, + MP_QSTR_Publisher, + MP_TYPE_FLAG_HAS_SPECIAL_ACCESSORS, + make_new, rclcpy_publisher_make_new, + locals_dict, &rclcpy_publisher_locals_dict + ); diff --git a/shared-bindings/rclcpy/Publisher.h b/shared-bindings/rclcpy/Publisher.h new file mode 100644 index 0000000000000..21909fe12bed9 --- /dev/null +++ b/shared-bindings/rclcpy/Publisher.h @@ -0,0 +1,18 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#pragma once +#include "common-hal/rclcpy/Publisher.h" + + +extern const mp_obj_type_t rclcpy_publisher_type; + +void common_hal_rclcpy_publisher_construct(rclcpy_publisher_obj_t *self, rclcpy_node_obj_t *node, + const char *topic_name); +bool common_hal_rclcpy_publisher_deinited(rclcpy_publisher_obj_t *self); +void common_hal_rclcpy_publisher_deinit(rclcpy_publisher_obj_t *self); +void common_hal_rclcpy_publisher_publish_int32(rclcpy_publisher_obj_t *self, int32_t data); +const char *common_hal_rclcpy_publisher_get_topic_name(rclcpy_publisher_obj_t *self); diff --git a/shared-bindings/rclcpy/__init__.c b/shared-bindings/rclcpy/__init__.c new file mode 100644 index 0000000000000..a6631642ea7c0 --- /dev/null +++ b/shared-bindings/rclcpy/__init__.c @@ -0,0 +1,134 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#include "shared-bindings/rclcpy/__init__.h" +#include "shared-bindings/rclcpy/Node.h" +#include "shared-bindings/rclcpy/Publisher.h" + +#include "py/obj.h" +#include "py/objproperty.h" +#include "py/objstr.h" +#include "py/runtime.h" + +//| """Robot Operating System (ROS2) connectivity through micro-ROS +//| +//| The `rclcpy` module contains basic classes and connectivity options for +//| communicating with a ROS network running on a linux machine, using the +//| eProsima's `micro-ROS client API `_. +//| +//| The underlying micro-ROS system uses a resource-constrained middleware layer +//| (XRCE-DDS) that must be connected to an agent running within ROS2 on a host +//| Linux computer. The API exposed by Circuitpython aims to be close to the +//| standard Python API for ROS2, ``rclpy`` with minor additions to support +//| connecting to this agent. +//| +//| Wifi must be connected before calling any `rclcpy` functions. As with +//| ``rclpy``, the `rclcpy.init()` function must be run before creating any ROS +//| objects. Child objects, such as publishers, must be created by their parent +//| objects. For example:: +//| +//| import os, wifi, time +//| import rclcpy +//| wifi.radio.connect(ssid=os.getenv('CIRCUITPY_WIFI_SSID'), +//| password=os.getenv('CIRCUITPY_WIFI_PASSWORD')) +//| rclcpy.init("192.168.10.111","8888") +//| mynode = rclcpy.Node("foo") +//| mypub = mynode.create_publisher("bar") +//| mypub.publish_int32(42) +//| """ + +//| def init( +//| agent_ip: str, +//| agent_port: str, +//| *, +//| domain_id: int = 0, +//| ) -> None: +//| """Initialize micro-ROS and connect to a micro-ROS agent. +//| +//| This function starts ROS communications and connects to the micro-ROS agent +//| on a linux computer. It must be called before creating ROS objects. +//| +//| :param str agent_ip: The IP address of the micro-ROS agent +//| :param str agent_port: The port number of the micro-ROS agent as a string +//| :param int domain_id: The ROS 2 domain ID for network isolation and organization. +//| Devices with the same domain ID can communicate with each other. +//| """ +//| ... +//| +static mp_obj_t rclcpy_init(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_agent_ip, ARG_agent_port, ARG_domain_id}; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_agent_ip, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_agent_port, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_domain_id, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = 0} }, + }; + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + const char *agent_ip = mp_obj_str_get_str(args[ARG_agent_ip].u_obj); + const char *agent_port = mp_obj_str_get_str(args[ARG_agent_port].u_obj); + int16_t domain_id = args[ARG_domain_id].u_int; + + common_hal_rclcpy_init(agent_ip, agent_port, domain_id); + return mp_const_none; +} +static MP_DEFINE_CONST_FUN_OBJ_KW(rclcpy_init_obj, 2, rclcpy_init); + + +//| def create_node( +//| node_name: str, +//| *, +//| namespace: str | None = None +//| ) -> Node: +//| """Create a Node. +//| +//| Creates an instance of a ROS2 Node. Nodes can be used to create other ROS +//| entities like publishers or subscribers. Nodes must have a unique name, and +//| may also be constructed from their class. +//| +//| :param str node_name: The name of the node. Must be a valid ROS 2 node name. +//| :param str namespace: The namespace for the node. If None, the node will be +//| created in the root namespace. +//| :return: A new Node object +//| :rtype: Node +//| """ +//| ... +//| +static mp_obj_t rclcpy_create_node(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_node_name, ARG_namespace }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_node_name, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_namespace, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_obj = mp_const_none} }, + }; + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + const char *node_name = mp_obj_str_get_str(args[ARG_node_name].u_obj); + const char *namespace = ""; + if (args[ARG_namespace].u_obj != mp_const_none) { + namespace = mp_obj_str_get_str(args[ARG_namespace].u_obj); + } + + rclcpy_node_obj_t *self = mp_obj_malloc_with_finaliser(rclcpy_node_obj_t, &rclcpy_node_type); + common_hal_rclcpy_node_construct(self, node_name, namespace); + return (mp_obj_t)self; +} +static MP_DEFINE_CONST_FUN_OBJ_KW(rclcpy_create_node_obj, 2, rclcpy_create_node); + +static const mp_rom_map_elem_t rclcpy_module_globals_table[] = { + { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_rclcpy) }, + { MP_ROM_QSTR(MP_QSTR_Node), MP_ROM_PTR(&rclcpy_node_type) }, + { MP_ROM_QSTR(MP_QSTR_Publisher), MP_ROM_PTR(&rclcpy_publisher_type) }, + { MP_ROM_QSTR(MP_QSTR_init), MP_ROM_PTR(&rclcpy_init_obj) }, + { MP_ROM_QSTR(MP_QSTR_create_node), MP_ROM_PTR(&rclcpy_create_node_obj) }, +}; + +static MP_DEFINE_CONST_DICT(rclcpy_module_globals, rclcpy_module_globals_table); + +const mp_obj_module_t rclcpy_module = { + .base = { &mp_type_module }, + .globals = (mp_obj_dict_t *)&rclcpy_module_globals, +}; + +MP_REGISTER_MODULE(MP_QSTR_rclcpy, rclcpy_module); diff --git a/shared-bindings/rclcpy/__init__.h b/shared-bindings/rclcpy/__init__.h new file mode 100644 index 0000000000000..1b4734216889c --- /dev/null +++ b/shared-bindings/rclcpy/__init__.h @@ -0,0 +1,12 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#pragma once +#include "common-hal/rclcpy/__init__.h" + +void common_hal_rclcpy_init(const char *agent_ip, const char *agent_port, int16_t domain_id); +rclcpy_context_t *common_hal_rclcpy_get_default_context(void); +bool common_hal_rclcpy_default_context_is_initialized(void); diff --git a/tools/ci_check_duplicate_usb_vid_pid.py b/tools/ci_check_duplicate_usb_vid_pid.py index 233c7acdffc42..5361914af199b 100644 --- a/tools/ci_check_duplicate_usb_vid_pid.py +++ b/tools/ci_check_duplicate_usb_vid_pid.py @@ -53,6 +53,7 @@ "0x303A:0x7003": [ "espressif_esp32s3_devkitc_1_n8", "espressif_esp32s3_devkitc_1_n8r2", + "espressif_esp32s3_devkitc_1_n8r2_ros", "espressif_esp32s3_devkitc_1_n8r8", "espressif_esp32s3_devkitc_1_n8r8_hacktablet", "espressif_esp32s3_devkitc_1_n16", @@ -63,6 +64,10 @@ "espressif_esp32s2_devkitc_1_n4r2", "espressif_esp32s2_devkitc_1_n8r2", ], + "0x303A:0x81DA": [ + "m5stack_cardputer", + "m5stack_cardputer_ros", + ], "0x239A:0x102E": ["weact_studio_pico", "weact_studio_pico_16mb"], "0x303A:0x8166": ["yd_esp32_s3_n8r8", "yd_esp32_s3_n16r8"], "0x2341:0x056B": ["arduino_nano_esp32s3", "arduino_nano_esp32s3_inverted_statusled"],