Add dynamic message type support

Adds ROS2 message type support to the rclcpy module. Creating a Publisher now
requires a ROS message type - all future messages must adhere to that message
type. A small group of test messages are available in the std_msgs submodule of
rclcpy, including Int32 and Bool. Message type detection is done through a type
registry, and the appropriate C level information passed to the Micro-ROS
interface. Ideally, future messages and message packages will be supported with
an automated class and registry generator. Due to MicroROS restrictions, custom
user message types cannot be supported without rebuilding Circuitpython.

API changes:
Removes the test publisher function `publish_int32` and replaces it with the
correct `publish` function, which accepts a message object. Adds the MsgType
parameter to publisher creation. Adds the std_msgs module to the module root.

Author: hierophect

locale/circuitpython.pot

@@ -1300,6 +1300,10 @@ msgstr ""
 msgid "Invalid ROS domain ID"
 msgstr ""
 
+#: ports/espressif/common-hal/rclcpy/Publisher.c shared-bindings/rclcpy/Node.c
+msgid "Invalid ROS message type"
+msgstr ""
+
 #: ports/espressif/common-hal/espidf/__init__.c py/moderrno.c
 msgid "Invalid argument"
 msgstr ""
@@ -1330,6 +1334,10 @@ msgstr ""
 msgid "Invalid hex password"
 msgstr ""
 
+#: ports/espressif/common-hal/rclcpy/Publisher.c
+msgid "Invalid message type"
+msgstr ""
+
 #: ports/espressif/common-hal/wifi/Radio.c
 #: ports/zephyr-cp/common-hal/wifi/Radio.c
 msgid "Invalid multicast MAC address"
@@ -1854,6 +1862,10 @@ msgstr ""
 msgid "Program too long"
 msgstr ""
 
+#: shared-bindings/rclcpy/Publisher.c
+msgid "Publish message does not match topic"
+msgstr ""
+
 #: shared-bindings/rclcpy/Publisher.c
 msgid "Publishers can only be created from a parent node"
 msgstr ""
@@ -2341,6 +2353,10 @@ msgstr ""
 msgid "Unsupported hash algorithm"
 msgstr ""
 
+#: ports/espressif/common-hal/rclcpy/Publisher.c
+msgid "Unsupported message type"
+msgstr ""
+
 #: ports/espressif/common-hal/socketpool/Socket.c
 #: ports/zephyr-cp/common-hal/socketpool/Socket.c
 msgid "Unsupported socket type"

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

@@ -5,15 +5,17 @@
 // SPDX-License-Identifier: MIT
 
 #include "shared-bindings/rclcpy/Publisher.h"
+#include "shared-bindings/rclcpy/registry.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) {
+    const mp_obj_type_t *message_type, 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);
+    const rosidl_message_type_support_t *type_support = common_hal_rclcpy_registry_get_msg_ros_typesupport(message_type);
+    if (!type_support) {
+        mp_raise_ValueError(MP_ERROR_TEXT("Invalid ROS message type"));
+    }
 
     // Creates a reliable Int32 publisher
     rcl_ret_t rc = rclc_publisher_init_default(
@@ -24,6 +26,7 @@ void common_hal_rclcpy_publisher_construct(rclcpy_publisher_obj_t *self, rclcpy_
     }
 
     self->node = node;
+    self->message_type = message_type;
 }
 
 bool common_hal_rclcpy_publisher_deinited(rclcpy_publisher_obj_t *self) {
@@ -45,15 +48,36 @@ void common_hal_rclcpy_publisher_deinit(rclcpy_publisher_obj_t *self) {
     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;
+void common_hal_rclcpy_publisher_publish(rclcpy_publisher_obj_t *self, mp_obj_t message_obj) {
+
+    const rclcpy_registry_msg_entry_t *entry = common_hal_rclcpy_registry_get_msg_entry(self->message_type);
+    if (!entry) {
+        mp_raise_ValueError(MP_ERROR_TEXT("Invalid message type"));
+    }
 
-    // Set message value
-    msg.data = data;
+    rcl_ret_t rc = RCL_RET_ERROR;
+
+    // This will eventually be moved to an autogenerated dispatcher file.
+    switch (entry->msg_kind) {
+        case RCLCPY_MSG_TYPE_BOOL: {
+            rclcpy_std_msgs_bool_obj_t *bool_msg = MP_OBJ_TO_PTR(message_obj);
+            std_msgs__msg__Bool ros_msg;
+            ros_msg.data = bool_msg->data;
+            rc = rcl_publish(&self->rcl_publisher, &ros_msg, NULL);
+            break;
+        }
+        case RCLCPY_MSG_TYPE_INT32: {
+            rclcpy_std_msgs_int32_obj_t *int32_msg = MP_OBJ_TO_PTR(message_obj);
+            std_msgs__msg__Int32 ros_msg;
+            ros_msg.data = int32_msg->data;
+            rc = rcl_publish(&self->rcl_publisher, &ros_msg, NULL);
+            break;
+        }
+        default:
+            mp_raise_ValueError(MP_ERROR_TEXT("Unsupported message type"));
+            return;
+    }
 
-    // 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"));
     }

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

@@ -10,17 +10,21 @@
 
 #include "common-hal/rclcpy/Node.h"
 #include "common-hal/rclcpy/__init__.h"
+#include "shared-bindings/rclcpy/std_msgs/Int32.h"
+#include "shared-bindings/rclcpy/std_msgs/Bool.h"
 
 #include <rcl/error_handling.h>
 #include <rcl/rcl.h>
 #include <rclc/executor.h>
 #include <rclc/rclc.h>
 #include <stdio.h>
 #include <std_msgs/msg/int32.h>
+#include <std_msgs/msg/bool.h>
 
 
 typedef struct {
     mp_obj_base_t base;
     rclcpy_node_obj_t *node;
     rcl_publisher_t rcl_publisher;
+    const mp_obj_type_t *message_type;
 } rclcpy_publisher_obj_t;

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

@@ -5,6 +5,7 @@
 // SPDX-License-Identifier: MIT
 
 #include "shared-bindings/rclcpy/__init__.h"
+#include "common-hal/rclcpy/registry.h"
 
 #include "esp_log.h"
 
@@ -59,6 +60,7 @@ void rclcpy_reset(void) {
         memset(&rclcpy_default_context, 0, sizeof(rclcpy_default_context));
         rclcpy_default_context.initialized = false;
     }
+    deinitialize_registry();
 }
 
 void common_hal_rclcpy_init(const char *agent_ip, const char *agent_port, int16_t domain_id) {

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

@@ -0,0 +1,67 @@
+// 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/registry.h"
+#include "shared-bindings/rclcpy/std_msgs/__init__.h"
+
+static rclcpy_registry_msg_entry_t msg_registry[RCLCPY_MSG_TYPE_COUNT];
+static bool registry_initialized = false;
+
+// static const rclcpy_registry_msg_entry_t msg_registry[] = {
+//     // stg_msg
+//     {
+//         .cpy_type = &rclcpy_std_msgs_bool_type,
+//         .ros_type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
+//         .msg_kind = RCLCPY_MSG_TYPE_BOOL
+//     },
+//     {
+//         .cpy_type = &rclcpy_std_msgs_int32_type,
+//         .ros_type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
+//         .msg_kind = RCLCPY_MSG_TYPE_INT32
+//     },
+// };
+
+// Called on demand, not at init
+static void initialize_registry(void) {
+    if (registry_initialized) {
+        return;
+    }
+
+    msg_registry[0] = (rclcpy_registry_msg_entry_t) {
+        .cpy_type = &rclcpy_std_msgs_bool_type,
+        .ros_type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
+        .msg_kind = RCLCPY_MSG_TYPE_BOOL
+    };
+
+    msg_registry[1] = (rclcpy_registry_msg_entry_t) {
+        .cpy_type = &rclcpy_std_msgs_int32_type,
+        .ros_type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
+        .msg_kind = RCLCPY_MSG_TYPE_INT32
+    };
+
+    registry_initialized = true;
+}
+
+// Used at reset (just to be careful with ROS support pointers)
+void deinitialize_registry(void) {
+    registry_initialized = false;
+}
+
+const rclcpy_registry_msg_entry_t * common_hal_rclcpy_registry_get_msg_entry(const mp_obj_type_t *cpy_type) {
+    initialize_registry();
+    for (size_t i = 0; i < RCLCPY_MSG_TYPE_COUNT; i++) {
+        if (msg_registry[i].cpy_type == cpy_type) {
+            return &msg_registry[i];
+        }
+    }
+    return NULL;
+}
+
+const rosidl_message_type_support_t * common_hal_rclcpy_registry_get_msg_ros_typesupport(const mp_obj_type_t *cpy_type) {
+    initialize_registry();
+    const rclcpy_registry_msg_entry_t *entry = common_hal_rclcpy_registry_get_msg_entry(cpy_type);
+    return entry ? entry->ros_type_support : NULL;
+}

ports/espressif/common-hal/rclcpy/registry.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 <std_msgs/msg/bool.h>
+#include <std_msgs/msg/int32.h>
+#include <std_msgs/msg/string.h>
+
+typedef enum {
+    RCLCPY_MSG_TYPE_BOOL,
+    RCLCPY_MSG_TYPE_INT32,
+    RCLCPY_MSG_TYPE_COUNT
+} rclcpy_msg_kind_t;
+
+typedef struct {
+    const mp_obj_type_t *cpy_type;
+    const rosidl_message_type_support_t *ros_type_support;
+    rclcpy_msg_kind_t msg_kind;
+} rclcpy_registry_msg_entry_t;
+
+void deinitialize_registry(void);

ports/espressif/common-hal/rclcpy/std_msgs/Bool.c

@@ -0,0 +1,5 @@
+// This file is part of the CircuitPython project: https://circuitpython.org
+//
+// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland
+//
+// SPDX-License-Identifier: MIT

ports/espressif/common-hal/rclcpy/std_msgs/Bool.h

@@ -0,0 +1,5 @@
+// This file is part of the CircuitPython project: https://circuitpython.org
+//
+// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland
+//
+// SPDX-License-Identifier: MIT

ports/espressif/common-hal/rclcpy/std_msgs/Int32.c

@@ -0,0 +1,5 @@
+// This file is part of the CircuitPython project: https://circuitpython.org
+//
+// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland
+//
+// SPDX-License-Identifier: MIT

ports/espressif/common-hal/rclcpy/std_msgs/Int32.h

@@ -0,0 +1,5 @@
+// This file is part of the CircuitPython project: https://circuitpython.org
+//
+// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland
+//
+// SPDX-License-Identifier: MIT