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"], pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy