diff --git a/locale/circuitpython.pot b/locale/circuitpython.pot index 9e8043764288..a529680e0c7d 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 81df154687ac..2fbb36cf0576 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 fdc45293cd27..e5b3c48e88a3 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 e0e6fd8b4f4f..545b8144ea4b 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 000000000000..b2712d5e0225 --- /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 000000000000..7a71316807b3 --- /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 000000000000..debf8f6c8964 --- /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 000000000000..debf8f6c8964 --- /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 000000000000..debf8f6c8964 --- /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 000000000000..debf8f6c8964 --- /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 000000000000..45e954a67bfb --- /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 000000000000..45e954a67bfb --- /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 d6569bc39bd8..5a919e65ea3d 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 a2fa9bd36a65..2393c300ad24 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 be1e229cce4f..1440ed59b30c 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 21909fe12bed..5e940a15c436 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 a6631642ea7c..929d737cd6c3 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 1b4734216889..3dfcac226920 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 000000000000..9bd1ba89b19d --- /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 000000000000..dda995588b90 --- /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 000000000000..e2ed14f22b0d --- /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 000000000000..d081810d6020 --- /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 000000000000..d9dead13fc7d --- /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 000000000000..b1cde6ebe8ad --- /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 000000000000..ebbd74933a65 --- /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 000000000000..7ea76f72423c --- /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;