refactored project structure

src/microros/microros_node.cpp

@@ -0,0 +1,92 @@
+#include "microros_node.hpp"
+
+#include <zephyr/kernel.h>
+#include <rcl/rcl.h>
+#include <rclc/rclc.h>
+#include <zephyr/sys/slist.h>
+#include <rosidl_runtime_c/message_type_support_struct.h>
+#include <stdlib.h>  // for malloc/free
+
+MicrorosExecutor::MicrorosExecutor(const char *name) {
+    this->node_name = name;
+    this->allocator = rcl_get_default_allocator();
+
+    // Initialize empty lists
+    sys_slist_init(&this->publishers_head);
+    sys_slist_init(&this->subscribers_head);
+}
+
+MicrorosExecutor::~MicrorosExecutor() {
+    // Free all publishers
+    struct publisher_node *pub;
+    SYS_SLIST_FOR_EACH_CONTAINER(&this->publishers_head, pub, node) {
+        rcl_publisher_fini(&pub->publisher, &this->node);
+        free(pub);
+    }
+
+    // Free all subscribers
+    struct subscription_node *sub;
+    SYS_SLIST_FOR_EACH_CONTAINER(&this->subscribers_head, sub, node) {
+        rcl_subscription_fini(&sub->subscription, &this->node);
+        free(sub);
+    }
+
+    rcl_node_fini(&this->node);
+}
+
+bool MicrorosExecutor::connect() {
+    if (rclc_support_init(&this->support, 0, NULL, &this->allocator) != RCL_RET_OK)
+        return false;
+
+    if (rclc_node_init_default(&this->node, this->node_name, "", &this->support) != RCL_RET_OK)
+        return false;
+
+    if (rclc_executor_init(&this->executor, &this->support.context, 1, &this->allocator) != RCL_RET_OK)
+        return false;
+
+    return true;
+}
+
+bool MicrorosExecutor::execute() {
+    while (1) {
+        rclc_executor_spin_some(&this->executor, 100);
+        k_sleep(K_MSEC(10));
+    }
+}
+
+rcl_publisher_t* MicrorosExecutor::add_publisher(const rosidl_message_type_support_t *type_support, const char *topic_name) {
+    // Allocate new list node
+    struct publisher_node *pub_node = (struct publisher_node *)malloc(sizeof(struct publisher_node));
+    if (!pub_node) return nullptr;
+
+    if (rclc_publisher_init_default(&pub_node->publisher, &this->node, type_support, topic_name) != RCL_RET_OK) {
+        free(pub_node);
+        return nullptr;
+    }
+
+    // Append to list
+    sys_slist_append(&this->publishers_head, &pub_node->node);
+
+    return &pub_node->publisher;
+}
+
+rcl_subscription_t* MicrorosExecutor::add_subscription(const rosidl_message_type_support_t *type_support, const char *topic_name, void* msg, rclc_callback_t callback) {
+    // Allocate new list node
+    struct subscription_node *sub_node = (struct subscription_node *)malloc(sizeof(struct subscription_node));
+    if (!sub_node) return nullptr;
+
+    if (rclc_subscription_init_default(&sub_node->subscription, &this->node, type_support, topic_name) != RCL_RET_OK) {
+        free(sub_node);
+        return nullptr;
+    }
+
+    // Append to list
+    sys_slist_append(&this->subscribers_head, &sub_node->node);
+
+    if (rclc_executor_add_subscription(&this->executor, &sub_node->subscription, msg, callback, ON_NEW_DATA) != RCL_RET_OK) {
+        free(sub_node);
+        return nullptr;
+    }
+
+    return &sub_node->subscription;
+}