Fix ros agent issue with RMW

Resolves an issue where an incomplete handshake with the ROS agent
caused an error failure code during microros initialization.

Author: hierophect

ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/mpconfigboard.mk

@@ -14,3 +14,30 @@ CIRCUITPY_ESP_PSRAM_MODE = qio
 CIRCUITPY_ESP_PSRAM_FREQ = 80m
 
 CIRCUITPY_RCLCPY = 1
+
+# turn off for debug flash space
+CIRCUITPY_ESPCAMERA = 0
+CIRCUITPY_AESIO = 0
+CIRCUITPY_AUDIOBUSIO = 0
+CIRCUITPY_AUDIOCORE = 0
+CIRCUITPY_AUDIOIO = 0
+CIRCUITPY_AUDIOMIXER = 0
+CIRCUITPY_AUDIOMP3 = 0
+CIRCUITPY_BLEIO_HCI = 0
+CIRCUITPY_DISPLAYIO = 0
+CIRCUITPY_FLOPPYIO = 0
+CIRCUITPY_FRAMEBUFFERIO = 0
+CIRCUITPY_PIXELMAP = 0
+CIRCUITPY_GETPASS = 0
+CIRCUITPY_KEYPAD = 0
+CIRCUITPY_MSGPACK = 0
+CIRCUITPY_PIXELBUF = 0
+CIRCUITPY_PS2IO = 0
+CIRCUITPY_RGBMATRIX = 0
+CIRCUITPY_RAINBOWIO = 0
+CIRCUITPY_ROTARYIO = 0
+CIRCUITPY_TOUCHIO = 0
+CIRCUITPY_USB_HID = 0
+CIRCUITPY_USB_MIDI = 0
+CIRCUITPY_WARNINGS = 0
+CIRCUITPY_ULAB = 0

ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/sdkconfig

@@ -1,6 +1,4 @@
 CONFIG_MICRO_ROS_ESP_XRCE_DDS_MIDDLEWARE=y
 CONFIG_MICRO_ROS_ESP_NETIF_WLAN=y
-CONFIG_MICRO_ROS_AGENT_IP="192.168.1.100"
-CONFIG_MICRO_ROS_AGENT_PORT="8888"
 CONFIG_MICRO_ROS_APP_STACK=16000
 CONFIG_MICRO_ROS_APP_TASK_PRIO=5

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

@@ -6,6 +6,12 @@
 
 #include "shared-bindings/rclcpy/__init__.h"
 
+#include "esp_log.h"
+
+#define RCLCPY_DOMAIN_ID 3
+#define RCLCPY_AGENT_IP "192.168.10.111"
+#define RCLCPY_AGENT_PORT "8888"
+
 rclcpy_context_t rclcpy_default_context = {
     .initialized = false,
 };
@@ -44,16 +50,43 @@ void common_hal_rclcpy_init(void) {
     custom_allocator.deallocate = microros_deallocate;
     custom_allocator.reallocate = microros_reallocate;
     custom_allocator.zero_allocate =  microros_zero_allocate;
-
     // Set custom allocator as default
     if (!rcutils_set_default_allocator(&custom_allocator)) {
+        ESP_LOGW("RCLCPY", "allocator failure");
         mp_raise_RuntimeError(MP_ERROR_TEXT("ROS memory allocator failure"));
     }
-
     rclcpy_default_context.rcl_allocator = custom_allocator;
+
     rcl_ret_t ret;
-    ret = rclc_support_init(&rclcpy_default_context.rcl_support, 0, NULL, &rclcpy_default_context.rcl_allocator);
+
+    // Options Init
+    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) {
+        ESP_LOGW("RCLCPY", "Options init failure: %d", ret);
+    }
+	ret = rcl_init_options_set_domain_id(&rclcpy_default_context.init_options, RCLCPY_DOMAIN_ID);
     if (ret != RCL_RET_OK) {
+        ESP_LOGW("RCLCPY", "Options domain failure: %d", ret);
+    }
+
+    // Set up 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(RCLCPY_AGENT_IP, RCLCPY_AGENT_PORT, rclcpy_default_context.rmw_options);
+    if (ret != RCL_RET_OK) {
+        ESP_LOGW("RCLCPY", "Agent options failure: %d", ret);
+    }
+	//RCCHECK(rmw_uros_discover_agent(rmw_options));
+
+    // Support Init
+    // ret = rclc_support_init(&rclcpy_default_context.rcl_support, 0, NULL, &rclcpy_default_context.rcl_allocator);
+    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) {
+        ESP_LOGW("RCLCPY", "Initialization failure: %d", ret);
         mp_raise_RuntimeError(MP_ERROR_TEXT("ROS failed to initialize"));
     } else {
         rclcpy_default_context.initialized = true;
@@ -66,4 +99,4 @@ rclcpy_context_t * common_hal_rclcpy_get_default_context(void) {
 
 bool common_hal_rclcpy_default_context_is_initialized(void) {
     return rclcpy_default_context.initialized;
-}
+}

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

@@ -14,11 +14,14 @@
 #include <rcl/error_handling.h>
 #include <rclc/rclc.h>
 #include <rclc/executor.h>
+#include <rmw_microros/rmw_microros.h>
 
 typedef struct {
     bool initialized;
     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;