From 4ab8adc2c0f68685d4587503cd37b69b4b8ad30f Mon Sep 17 00:00:00 2001 From: Lucian Copeland Date: Fri, 1 Aug 2025 16:06:33 -0400 Subject: [PATCH] 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. --- locale/circuitpython.pot | 16 +++++ ports/espressif/common-hal/rclcpy/Publisher.c | 46 ++++++++++--- ports/espressif/common-hal/rclcpy/Publisher.h | 4 ++ ports/espressif/common-hal/rclcpy/__init__.c | 2 + ports/espressif/common-hal/rclcpy/registry.c | 67 +++++++++++++++++++ ports/espressif/common-hal/rclcpy/registry.h | 26 +++++++ .../common-hal/rclcpy/std_msgs/Bool.c | 5 ++ .../common-hal/rclcpy/std_msgs/Bool.h | 5 ++ .../common-hal/rclcpy/std_msgs/Int32.c | 5 ++ .../common-hal/rclcpy/std_msgs/Int32.h | 5 ++ .../common-hal/rclcpy/std_msgs/__init__.c | 7 ++ .../common-hal/rclcpy/std_msgs/__init__.h | 7 ++ py/circuitpy_defns.mk | 4 ++ shared-bindings/rclcpy/Node.c | 14 +++- shared-bindings/rclcpy/Publisher.c | 21 +++--- shared-bindings/rclcpy/Publisher.h | 4 +- shared-bindings/rclcpy/__init__.c | 1 + shared-bindings/rclcpy/__init__.h | 2 + shared-bindings/rclcpy/registry.c | 7 ++ shared-bindings/rclcpy/registry.h | 11 +++ shared-bindings/rclcpy/std_msgs/Bool.c | 55 +++++++++++++++ shared-bindings/rclcpy/std_msgs/Bool.h | 15 +++++ shared-bindings/rclcpy/std_msgs/Int32.c | 55 +++++++++++++++ shared-bindings/rclcpy/std_msgs/Int32.h | 15 +++++ shared-bindings/rclcpy/std_msgs/__init__.c | 26 +++++++ shared-bindings/rclcpy/std_msgs/__init__.h | 13 ++++ 26 files changed, 413 insertions(+), 25 deletions(-) create mode 100644 ports/espressif/common-hal/rclcpy/registry.c create mode 100644 ports/espressif/common-hal/rclcpy/registry.h create mode 100644 ports/espressif/common-hal/rclcpy/std_msgs/Bool.c create mode 100644 ports/espressif/common-hal/rclcpy/std_msgs/Bool.h create mode 100644 ports/espressif/common-hal/rclcpy/std_msgs/Int32.c create mode 100644 ports/espressif/common-hal/rclcpy/std_msgs/Int32.h create mode 100644 ports/espressif/common-hal/rclcpy/std_msgs/__init__.c create mode 100644 ports/espressif/common-hal/rclcpy/std_msgs/__init__.h create mode 100644 shared-bindings/rclcpy/registry.c create mode 100644 shared-bindings/rclcpy/registry.h create mode 100644 shared-bindings/rclcpy/std_msgs/Bool.c create mode 100644 shared-bindings/rclcpy/std_msgs/Bool.h create mode 100644 shared-bindings/rclcpy/std_msgs/Int32.c create mode 100644 shared-bindings/rclcpy/std_msgs/Int32.h create mode 100644 shared-bindings/rclcpy/std_msgs/__init__.c create mode 100644 shared-bindings/rclcpy/std_msgs/__init__.h diff --git a/locale/circuitpython.pot b/locale/circuitpython.pot index 9e8043764288f..a529680e0c7d7 100644 --- a/locale/circuitpython.pot +++ b/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" diff --git a/ports/espressif/common-hal/rclcpy/Publisher.c b/ports/espressif/common-hal/rclcpy/Publisher.c index 81df154687ace..2fbb36cf0576d 100644 --- a/ports/espressif/common-hal/rclcpy/Publisher.c +++ b/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")); } diff --git a/ports/espressif/common-hal/rclcpy/Publisher.h b/ports/espressif/common-hal/rclcpy/Publisher.h index fdc45293cd270..e5b3c48e88a32 100644 --- a/ports/espressif/common-hal/rclcpy/Publisher.h +++ b/ports/espressif/common-hal/rclcpy/Publisher.h @@ -10,6 +10,8 @@ #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 #include @@ -17,10 +19,12 @@ #include #include #include +#include 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; diff --git a/ports/espressif/common-hal/rclcpy/__init__.c b/ports/espressif/common-hal/rclcpy/__init__.c index e0e6fd8b4f4fc..545b8144ea4be 100644 --- a/ports/espressif/common-hal/rclcpy/__init__.c +++ b/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) { diff --git a/ports/espressif/common-hal/rclcpy/registry.c b/ports/espressif/common-hal/rclcpy/registry.c new file mode 100644 index 0000000000000..b2712d5e02250 --- /dev/null +++ b/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; +} diff --git a/ports/espressif/common-hal/rclcpy/registry.h b/ports/espressif/common-hal/rclcpy/registry.h new file mode 100644 index 0000000000000..7a71316807b3b --- /dev/null +++ b/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 +#include +#include + +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); diff --git a/ports/espressif/common-hal/rclcpy/std_msgs/Bool.c b/ports/espressif/common-hal/rclcpy/std_msgs/Bool.c new file mode 100644 index 0000000000000..debf8f6c8964b --- /dev/null +++ b/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 diff --git a/ports/espressif/common-hal/rclcpy/std_msgs/Bool.h b/ports/espressif/common-hal/rclcpy/std_msgs/Bool.h new file mode 100644 index 0000000000000..debf8f6c8964b --- /dev/null +++ b/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 diff --git a/ports/espressif/common-hal/rclcpy/std_msgs/Int32.c b/ports/espressif/common-hal/rclcpy/std_msgs/Int32.c new file mode 100644 index 0000000000000..debf8f6c8964b --- /dev/null +++ b/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 diff --git a/ports/espressif/common-hal/rclcpy/std_msgs/Int32.h b/ports/espressif/common-hal/rclcpy/std_msgs/Int32.h new file mode 100644 index 0000000000000..debf8f6c8964b --- /dev/null +++ b/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 diff --git a/ports/espressif/common-hal/rclcpy/std_msgs/__init__.c b/ports/espressif/common-hal/rclcpy/std_msgs/__init__.c new file mode 100644 index 0000000000000..45e954a67bfb2 --- /dev/null +++ b/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 diff --git a/ports/espressif/common-hal/rclcpy/std_msgs/__init__.h b/ports/espressif/common-hal/rclcpy/std_msgs/__init__.h new file mode 100644 index 0000000000000..45e954a67bfb2 --- /dev/null +++ b/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 diff --git a/py/circuitpy_defns.mk b/py/circuitpy_defns.mk index d6569bc39bd8e..5a919e65ea3d7 100644 --- a/py/circuitpy_defns.mk +++ b/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 \ diff --git a/shared-bindings/rclcpy/Node.c b/shared-bindings/rclcpy/Node.c index a2fa9bd36a656..2393c300ad246 100644 --- a/shared-bindings/rclcpy/Node.c +++ b/shared-bindings/rclcpy/Node.c @@ -7,6 +7,7 @@ #include #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. diff --git a/shared-bindings/rclcpy/Publisher.c b/shared-bindings/rclcpy/Publisher.c index be1e229cce4f8..1440ed59b30cf 100644 --- a/shared-bindings/rclcpy/Publisher.c +++ b/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); diff --git a/shared-bindings/rclcpy/Publisher.h b/shared-bindings/rclcpy/Publisher.h index 21909fe12bed9..5e940a15c4362 100644 --- a/shared-bindings/rclcpy/Publisher.h +++ b/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); diff --git a/shared-bindings/rclcpy/__init__.c b/shared-bindings/rclcpy/__init__.c index a6631642ea7c0..929d737cd6c3c 100644 --- a/shared-bindings/rclcpy/__init__.c +++ b/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); diff --git a/shared-bindings/rclcpy/__init__.h b/shared-bindings/rclcpy/__init__.h index 1b4734216889c..3dfcac2269203 100644 --- a/shared-bindings/rclcpy/__init__.h +++ b/shared-bindings/rclcpy/__init__.h @@ -7,6 +7,8 @@ #pragma once #include "common-hal/rclcpy/__init__.h" +extern const mp_obj_module_t rclcpy_std_msgs_module; + 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); diff --git a/shared-bindings/rclcpy/registry.c b/shared-bindings/rclcpy/registry.c new file mode 100644 index 0000000000000..9bd1ba89b19d1 --- /dev/null +++ b/shared-bindings/rclcpy/registry.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 + +// No python level registry functions diff --git a/shared-bindings/rclcpy/registry.h b/shared-bindings/rclcpy/registry.h new file mode 100644 index 0000000000000..dda995588b90d --- /dev/null +++ b/shared-bindings/rclcpy/registry.h @@ -0,0 +1,11 @@ +// 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 "common-hal/rclcpy/registry.h" + +const rclcpy_registry_msg_entry_t * common_hal_rclcpy_registry_get_msg_entry(const mp_obj_type_t *cpy_type); +const rosidl_message_type_support_t * common_hal_rclcpy_registry_get_msg_ros_typesupport(const mp_obj_type_t *cpy_type); diff --git a/shared-bindings/rclcpy/std_msgs/Bool.c b/shared-bindings/rclcpy/std_msgs/Bool.c new file mode 100644 index 0000000000000..e2ed14f22b0d0 --- /dev/null +++ b/shared-bindings/rclcpy/std_msgs/Bool.c @@ -0,0 +1,55 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#include +#include "py/runtime.h" +#include "py/obj.h" + +#include "shared-bindings/rclcpy/std_msgs/Bool.h" + +static void bool_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) { + (void)kind; + rclcpy_std_msgs_bool_obj_t *self = MP_OBJ_TO_PTR(self_in); + mp_print_str(print, "Bool(data="); + mp_obj_print_helper(print, mp_obj_new_bool(self->data), PRINT_REPR); + mp_print_str(print, ")"); +} + +static mp_obj_t bool_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) { + mp_arg_check_num(n_args, n_kw, 0, 1, true); + rclcpy_std_msgs_bool_obj_t *self = m_new_obj(rclcpy_std_msgs_bool_obj_t); + self->base.type = &rclcpy_std_msgs_bool_type; + + if (n_args == 1) { + self->data = mp_obj_is_true(args[0]); + } else { + self->data = false; + } + return MP_OBJ_FROM_PTR(self); +} + +static void bool_attr(mp_obj_t self_in, qstr attribute, mp_obj_t *destination) { + rclcpy_std_msgs_bool_obj_t *self = MP_OBJ_TO_PTR(self_in); + if (attribute == MP_QSTR_data) { + if (destination[0] == MP_OBJ_NULL) { + // Read access: msg.data + destination[0] = mp_obj_new_bool(self->data); + } else { + // Write access: msg.data = value + self->data = mp_obj_is_true(destination[1]); + destination[0] = MP_OBJ_NULL; + } + } +} + +MP_DEFINE_CONST_OBJ_TYPE( + rclcpy_std_msgs_bool_type, + MP_QSTR_Bool, + MP_TYPE_FLAG_HAS_SPECIAL_ACCESSORS, + make_new, bool_make_new, + print, bool_print, + attr, bool_attr + ); diff --git a/shared-bindings/rclcpy/std_msgs/Bool.h b/shared-bindings/rclcpy/std_msgs/Bool.h new file mode 100644 index 0000000000000..d081810d6020c --- /dev/null +++ b/shared-bindings/rclcpy/std_msgs/Bool.h @@ -0,0 +1,15 @@ +// 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" + +typedef struct { + mp_obj_base_t base; + bool data; +} rclcpy_std_msgs_bool_obj_t; + +extern const mp_obj_type_t rclcpy_std_msgs_bool_type; diff --git a/shared-bindings/rclcpy/std_msgs/Int32.c b/shared-bindings/rclcpy/std_msgs/Int32.c new file mode 100644 index 0000000000000..d9dead13fc7dc --- /dev/null +++ b/shared-bindings/rclcpy/std_msgs/Int32.c @@ -0,0 +1,55 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#include +#include "py/runtime.h" +#include "py/obj.h" + +#include "shared-bindings/rclcpy/std_msgs/Int32.h" + +static void int32_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) { + (void)kind; + rclcpy_std_msgs_int32_obj_t *self = MP_OBJ_TO_PTR(self_in); + mp_print_str(print, "Int32(data="); + mp_obj_print_helper(print, mp_obj_new_int(self->data), PRINT_REPR); + mp_print_str(print, ")"); +} + +static mp_obj_t int32_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) { + mp_arg_check_num(n_args, n_kw, 0, 1, true); + rclcpy_std_msgs_int32_obj_t *self = m_new_obj(rclcpy_std_msgs_int32_obj_t); + self->base.type = &rclcpy_std_msgs_int32_type; + + if (n_args == 1) { + self->data = mp_obj_get_int(args[0]); + } else { + self->data = 0; + } + return MP_OBJ_FROM_PTR(self); +} + +static void int32_attr(mp_obj_t self_in, qstr attribute, mp_obj_t *destination) { + rclcpy_std_msgs_int32_obj_t *self = MP_OBJ_TO_PTR(self_in); + if (attribute == MP_QSTR_data) { + if (destination[0] == MP_OBJ_NULL) { + // Read access: msg.data + destination[0] = mp_obj_new_int(self->data); + } else { + // Write access: msg.data = value + self->data = mp_obj_get_int(destination[1]); + destination[0] = MP_OBJ_NULL; + } + } +} + +MP_DEFINE_CONST_OBJ_TYPE( + rclcpy_std_msgs_int32_type, + MP_QSTR_Int32, + MP_TYPE_FLAG_HAS_SPECIAL_ACCESSORS, + make_new, int32_make_new, + print, int32_print, + attr, int32_attr + ); diff --git a/shared-bindings/rclcpy/std_msgs/Int32.h b/shared-bindings/rclcpy/std_msgs/Int32.h new file mode 100644 index 0000000000000..b1cde6ebe8ade --- /dev/null +++ b/shared-bindings/rclcpy/std_msgs/Int32.h @@ -0,0 +1,15 @@ +// 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" + +typedef struct { + mp_obj_base_t base; + int32_t data; +} rclcpy_std_msgs_int32_obj_t; + +extern const mp_obj_type_t rclcpy_std_msgs_int32_type; diff --git a/shared-bindings/rclcpy/std_msgs/__init__.c b/shared-bindings/rclcpy/std_msgs/__init__.c new file mode 100644 index 0000000000000..ebbd74933a651 --- /dev/null +++ b/shared-bindings/rclcpy/std_msgs/__init__.c @@ -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 + +#include "py/obj.h" +#include "py/objproperty.h" +#include "py/objstr.h" +#include "py/runtime.h" + +#include "shared-bindings/rclcpy/std_msgs/__init__.h" + +static const mp_rom_map_elem_t rclcpy_std_msgs_globals_table[] = { + { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_rclcpy_dot_std_msgs) }, + { MP_OBJ_NEW_QSTR(MP_QSTR_Bool), (mp_obj_t)&rclcpy_std_msgs_bool_type }, + { MP_OBJ_NEW_QSTR(MP_QSTR_Int32), (mp_obj_t)&rclcpy_std_msgs_int32_type }, +}; +static MP_DEFINE_CONST_DICT(rclcpy_std_msgs_globals, rclcpy_std_msgs_globals_table); + +const mp_obj_module_t rclcpy_std_msgs_module = { + .base = { &mp_type_module }, + .globals = (mp_obj_dict_t *)&rclcpy_std_msgs_globals, +}; + +MP_REGISTER_MODULE(MP_QSTR_rclcpy_dot_std_msgs, rclcpy_std_msgs_module); diff --git a/shared-bindings/rclcpy/std_msgs/__init__.h b/shared-bindings/rclcpy/std_msgs/__init__.h new file mode 100644 index 0000000000000..7ea76f72423cc --- /dev/null +++ b/shared-bindings/rclcpy/std_msgs/__init__.h @@ -0,0 +1,13 @@ +// 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" + +extern const mp_obj_module_t rclcpy_std_msgs_module; + +extern const mp_obj_type_t rclcpy_std_msgs_bool_type; +extern const mp_obj_type_t rclcpy_std_msgs_int32_type;