Add agent information to init

Expands rclcpy init function to accept agent IP and port information, as well as
a new domain_id value. Contains minor changes to prepare for reset support.

Author: hierophect

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

@@ -8,10 +8,6 @@
 
 #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,
 };
@@ -41,7 +37,10 @@ static void* microros_zero_allocate(size_t number_of_elements, size_t size_of_el
     return ptr;
 }
 
-void common_hal_rclcpy_init(void) {
+void rclcpy_reset(void) {
+}
+
+void common_hal_rclcpy_init(const char *agent_ip, const char *agent_port, int16_t domain_id) {
     // Get empty allocator
     rcl_allocator_t custom_allocator = rcutils_get_zero_initialized_allocator();
 
@@ -65,29 +64,30 @@ void common_hal_rclcpy_init(void) {
     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 (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) {
         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);
+	ret = rmw_uros_options_set_udp_address(agent_ip, 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"));
+        mp_raise_RuntimeError(MP_ERROR_TEXT("ROS failed to initialize. Is agent connected?"));
     } else {
         rclcpy_default_context.initialized = true;
     }

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

@@ -24,4 +24,5 @@ typedef struct {
     rmw_init_options_t* rmw_options;
 } rclcpy_context_t;
 
-extern rclcpy_context_t rclcpy_default_context;
+extern rclcpy_context_t rclcpy_default_context;
+void rclcpy_reset(void);

shared-bindings/rclcpy/__init__.c

@@ -13,11 +13,24 @@
 #include "py/objstr.h"
 #include "py/runtime.h"
 
-static mp_obj_t rclcpy_init(void) {
-    common_hal_rclcpy_init();
+// static mp_obj_t rclcpy_init(void) {
+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_0(rclcpy_init_obj, rclcpy_init);
+static MP_DEFINE_CONST_FUN_OBJ_KW(rclcpy_init_obj, 2, rclcpy_init);
 
 
 // TODO: parallel implementation to Node constructor

shared-bindings/rclcpy/__init__.h

@@ -7,6 +7,6 @@
 #pragma once
 #include "common-hal/rclcpy/__init__.h"
 
-void common_hal_rclcpy_init(void);
+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);