MicroROS on Espressif

.gitignore

@@ -9,6 +9,7 @@
 *.o
 *.a
 !atmel-samd/asf/**/*.a
+!ports/espressif/microros-lib/**/*.a
 *.elf
 *.bin
 !*.toml.bin

.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

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"

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.

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.

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)

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

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);

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

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() ?