Add ROS2 message support

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

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

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

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

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

py/circuitpy_defns.mk

@@ -546,6 +546,10 @@ SRC_COMMON_HAL_ALL = \
 	rclcpy/__init__.c \
 	rclcpy/Node.c \
 	rclcpy/Publisher.c \
+	rclcpy/registry.c \
+	rclcpy/std_msgs/__init__.c \
+	rclcpy/std_msgs/Bool.c \
+	rclcpy/std_msgs/Int32.c \
 	rgbmatrix/RGBMatrix.c \
 	rgbmatrix/__init__.c \
 	rotaryio/IncrementalEncoder.c \

shared-bindings/rclcpy/Node.c

@@ -7,6 +7,7 @@
 #include <stdint.h>
 #include "shared-bindings/rclcpy/Node.h"
 #include "shared-bindings/rclcpy/Publisher.h"
+#include "shared-bindings/rclcpy/registry.h"
 #include "shared-bindings/util.h"
 #include "py/objproperty.h"
 #include "py/objtype.h"
@@ -83,16 +84,23 @@ static void check_for_deinit(rclcpy_node_obj_t *self) {
 //|         """
 //|         ...
 //|
-static mp_obj_t rclcpy_node_create_publisher(mp_obj_t self_in, mp_obj_t topic) {
+static mp_obj_t rclcpy_node_create_publisher(mp_obj_t self_in, mp_obj_t msg_type, 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);
 
+    // Validate msg type
+    const mp_obj_type_t *message_type = MP_OBJ_TO_PTR(msg_type);
+    if (!common_hal_rclcpy_registry_get_msg_ros_typesupport(message_type)) {
+        mp_raise_ValueError(MP_ERROR_TEXT("Invalid ROS message type"));
+    }
+
     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);
+    common_hal_rclcpy_publisher_construct(publisher, self, message_type, topic_name);
     return (mp_obj_t)publisher;
 }
-static MP_DEFINE_CONST_FUN_OBJ_2(rclcpy_node_create_publisher_obj, rclcpy_node_create_publisher);
+static MP_DEFINE_CONST_FUN_OBJ_3(rclcpy_node_create_publisher_obj, rclcpy_node_create_publisher);
 
 //|     def get_name(self) -> str:
 //|         """Get the name of the node.

shared-bindings/rclcpy/Publisher.c

@@ -47,22 +47,25 @@ static void check_for_deinit(rclcpy_publisher_obj_t *self) {
     }
 }
 
-//|     def publish_int32(self, message: int) -> None:
-//|         """Publish a 32-bit signed integer message to the topic.
+//|     def publish(self, message: MsgObj) -> None:
+//|         """Publish a 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)
+//|         :param MsgObj message: ROS message instance with same type as the topic
 //|         """
 //|         ...
 //|
-static mp_obj_t rclcpy_publisher_publish_int32(mp_obj_t self_in, mp_obj_t in_msg) {
+static mp_obj_t rclcpy_publisher_publish(mp_obj_t self_in, mp_obj_t msg_obj) {
     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);
+
+    // Verify the message type matches what this publisher expects
+    if (mp_obj_get_type(msg_obj) != self->message_type) {
+        mp_raise_ValueError(MP_ERROR_TEXT("Publish message does not match topic"));
+    }
+    common_hal_rclcpy_publisher_publish(self, msg_obj);
     return mp_const_none;
 }
-static MP_DEFINE_CONST_FUN_OBJ_2(rclcpy_publisher_publish_int32_obj, rclcpy_publisher_publish_int32);
+static MP_DEFINE_CONST_FUN_OBJ_2(rclcpy_publisher_publish_obj, rclcpy_publisher_publish);
 
 //|     def get_topic_name(self) -> str:
 //|         """Get the name of the topic this publisher publishes to.
@@ -84,7 +87,7 @@ static MP_DEFINE_CONST_FUN_OBJ_1(rclcpy_publisher_get_topic_name_obj, rclcpy_pub
 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_publish), MP_ROM_PTR(&rclcpy_publisher_publish_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);

shared-bindings/rclcpy/Publisher.h

@@ -11,8 +11,8 @@
 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);
+    const mp_obj_type_t *message_type, 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);
+void common_hal_rclcpy_publisher_publish(rclcpy_publisher_obj_t *self, mp_obj_t msg_obj);
 const char *common_hal_rclcpy_publisher_get_topic_name(rclcpy_publisher_obj_t *self);

shared-bindings/rclcpy/__init__.c

@@ -122,6 +122,7 @@ static const mp_rom_map_elem_t rclcpy_module_globals_table[] = {
     { 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) },
+    { MP_ROM_QSTR(MP_QSTR_std_msgs), MP_ROM_PTR(&rclcpy_std_msgs_module) },
 };
 
 static MP_DEFINE_CONST_DICT(rclcpy_module_globals, rclcpy_module_globals_table);