refactored project structure

CMakeLists.txt

@@ -5,6 +5,6 @@ SET(CMAKE_CXX_COMPILER /opt/zephyr-sdk/arm-zephyr-eabi/bin/arm-zephyr-eabi-gcc)
 list(APPEND ZEPHYR_EXTRA_MODULES ${CMAKE_CURRENT_SOURCE_DIR}/modules/libmicroros)
 find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
 
-project(sherpa_ros2_sensor_node)
+project(sherpa_ros2_sensor_node C CXX)
 
-target_sources(app PRIVATE src/main.cpp)
+add_subdirectory(src)

modules/libmicroros/Kconfig

@@ -83,15 +83,15 @@ if MICROROS
 
     config MICROROS_PUBLISHERS
         string "available micro-ROS publishers"
-        default "2"
+        default "16"
     
     config MICROROS_SUBSCRIBERS
         string "available micro-ROS subscribers"
-        default "2"
+        default "16"
 
     config MICROROS_CLIENTS
         string "available micro-ROS service clients"
-        default "3"
+        default "4"
 
     config MICROROS_SERVERS
         string "available micro-ROS service servers"

src/CMakeLists.txt

@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 3.20.0)
+
+target_include_directories(app PRIVATE ./)
+
+add_subdirectory(./microros)
+
+target_sources(app PRIVATE
+    main.cpp
+    devices.cpp
+)

src/devices.cpp

@@ -0,0 +1,12 @@
+#include "devices.hpp"
+
+bool verify_are_devices_available(void) {
+    bool devices_ready = true;
+
+    if (!device_is_ready(led0.port)) {
+        printk("LED not ready\n");
+        devices_ready = false;
+    }
+
+    return devices_ready;
+}

src/devices.hpp

@@ -0,0 +1,17 @@
+#pragma once
+
+#include <version.h>
+
+#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0)
+#include <zephyr/device.h>
+#include <zephyr/devicetree.h>
+#include <zephyr/drivers/gpio.h>
+#else
+#include <device.h>
+#include <devicetree.h>
+#include <drivers/gpio.h>
+#endif
+
+static const struct gpio_dt_spec led0 = GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios);
+
+bool verify_are_devices_available(void);

src/main.cpp

@@ -2,50 +2,26 @@
 
 #if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0)
 #include <zephyr/kernel.h>
-#include <zephyr/device.h>
-#include <zephyr/devicetree.h>
-#include <zephyr/drivers/gpio.h>
 #else
 #include <zephyr.h>
-#include <device.h>
-#include <devicetree.h>
-#include <drivers/gpio.h>
 #endif
 
-#include <rcl/rcl.h>
-#include <rcl/error_handling.h>
-#include <std_msgs/msg/bool.h>
-
-#include <rclc/rclc.h>
-#include <rclc/executor.h>
-
-#include <rmw_microros/rmw_microros.h>
 #include <microros_transports.h>
+#include <rmw_microros/rmw_microros.h>
 
-#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc);for(;;){};}}
+#include "devices.hpp"
-#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}
+#include "microros_node.hpp"
 
-rcl_subscription_t subscriber;
+#include "led0_subscriber.hpp"
 
-std_msgs__msg__Bool msg;
-static const struct gpio_dt_spec led0 = GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios);
-
-
-void led0_callback(const void *msgin)
-{
-    const std_msgs__msg__Bool *msg = (const std_msgs__msg__Bool *)msgin;
-
-    gpio_pin_configure_dt(&led0, msg->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
-}
 
 int main(void)
 {
-    if (!device_is_ready(led0.port)) {
+    if (!verify_are_devices_available()) {
-        printk("LED not ready\n");
+        printk("##### Not all devices are available, aborting launch #####");
-        return -1;
+        for (;;){}
     }
 
-    /* micro-ROS transport setup */
     rmw_uros_set_custom_transport(
         MICRO_ROS_FRAMING_REQUIRED,
         (void *)&default_params,
@@ -55,36 +31,15 @@ int main(void)
         zephyr_transport_read
     );
 
-    rcl_allocator_t allocator = rcl_get_default_allocator();
+    MicrorosExecutor node0 = MicrorosExecutor("nucleo_node0");
-    rclc_support_t support;
 
-    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
+    while (!node0.connect()) {
+        printk("Waiting for connection ...");
+    };
 
-    rcl_node_t node;
+    led0_register(&node0);
-    RCCHECK(rclc_node_init_default(&node, "zephyr_servo_node", "", &support));
 
-    /* Create subscriber for /servo/position */
+    node0.execute();
-    RCCHECK(rclc_subscription_init_default(
-        &subscriber,
-        &node,
-        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
-        "/nucleo/led0"
-    ));
-
-    /* Create executor */
-    rclc_executor_t executor;
-    RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
-    RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &led0_callback, ON_NEW_DATA));
-
-    /* Spin executor loop */
-    while (1) {
-        rclc_executor_spin_some(&executor, 100);
-        k_sleep(K_MSEC(10));
-    }
-
-    /* Free resources (unreachable in infinite loop) */
-    RCCHECK(rcl_subscription_fini(&subscriber, &node))
-    RCCHECK(rcl_node_fini(&node))
 
     return 0;
 }

src/microros/CMakeLists.txt

@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 3.20.0)
+
+add_subdirectory(publishers)
+add_subdirectory(subscribers)
+
+target_include_directories(app PRIVATE .)
+
+target_sources(app PRIVATE 
+    microros_node.cpp
+)

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;
+}

src/microros/microros_node.hpp

@@ -0,0 +1,40 @@
+#pragma once
+
+#include <zephyr/sys/slist.h>
+#include <rcl/rcl.h>
+#include <rclc/rclc.h>
+#include <rclc/executor.h>
+#include <rosidl_runtime_c/message_type_support_struct.h>
+#include <zephyr/kernel.h>
+
+class MicrorosExecutor {
+private:
+    const char *node_name;
+
+    rcl_allocator_t allocator;
+    rclc_executor_t executor;
+    rcl_node_t node;
+    rclc_support_t support;
+
+    sys_slist_t publishers_head;
+    sys_slist_t subscribers_head;
+
+    struct publisher_node {
+        rcl_publisher_t publisher;
+        sys_snode_t node;  // embedded Zephyr list node
+    };
+
+    struct subscription_node {
+        rcl_subscription_t subscription;
+        sys_snode_t node;  // embedded Zephyr list node
+    };
+
+public:
+    MicrorosExecutor(const char *name);
+    ~MicrorosExecutor(void);
+
+    bool connect(void);
+    bool execute(void);
+    rcl_publisher_t* add_publisher(const rosidl_message_type_support_t *type_support, const char *topic_name);
+    rcl_subscription_t* add_subscription(const rosidl_message_type_support_t *type_support, const char *topic_name, void* msg, rclc_callback_t callback);
+};

src/microros/publishers/CMakeLists.txt

@@ -0,0 +1 @@
+cmake_minimum_required(VERSION 3.20.0)

src/microros/subscribers/CMakeLists.txt

@@ -0,0 +1,3 @@
+cmake_minimum_required(VERSION 3.20.0)
+
+add_subdirectory(led0)

src/microros/subscribers/led0/CMakeLists.txt

@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 3.20.0)
+
+target_include_directories(app PRIVATE .)
+
+target_sources(app PRIVATE 
+    led0_subscriber.cpp
+)

src/microros/subscribers/led0/led0_subscriber.cpp

@@ -0,0 +1,25 @@
+#include "led0_subscriber.hpp"
+
+#include <zephyr/drivers/gpio.h>
+#include <std_msgs/msg/bool.h>
+
+#include "devices.hpp"
+
+
+std_msgs__msg__Bool msg;
+
+void led0_callback(const void *msgin){
+    const std_msgs__msg__Bool *msg = (const std_msgs__msg__Bool *)msgin;
+
+    gpio_pin_configure_dt(&led0, msg->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
+}
+
+
+void led0_register(MicrorosExecutor* node) {
+    node->add_subscription(
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
+        "/nucleo/led0",
+        &msg,
+        led0_callback
+    );
+}

src/microros/subscribers/led0/led0_subscriber.hpp

@@ -0,0 +1,5 @@
+#pragma once
+
+#include "microros_node.hpp"
+
+void led0_register(MicrorosExecutor* node);