diff --git a/CMakeLists.txt b/CMakeLists.txt index 9cfd7bc..b44f908 100644 --- a/CMakeLists.txt +++ b/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) \ No newline at end of file diff --git a/modules/libmicroros/Kconfig b/modules/libmicroros/Kconfig index c421a8f..b18c21c 100644 --- a/modules/libmicroros/Kconfig +++ b/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" diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..3e6ef70 --- /dev/null +++ b/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 +) \ No newline at end of file diff --git a/src/devices.cpp b/src/devices.cpp new file mode 100644 index 0000000..9e07776 --- /dev/null +++ b/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; +} \ No newline at end of file diff --git a/src/devices.hpp b/src/devices.hpp new file mode 100644 index 0000000..4c10b95 --- /dev/null +++ b/src/devices.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include + +#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0) +#include +#include +#include +#else +#include +#include +#include +#endif + +static const struct gpio_dt_spec led0 = GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios); + +bool verify_are_devices_available(void); \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 3e587b6..589571b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -2,50 +2,26 @@ #if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0) #include -#include -#include -#include #else #include -#include -#include -#include #endif -#include -#include -#include - -#include -#include - -#include #include +#include -#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(;;){};}} -#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 "devices.hpp" +#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)) { - printk("LED not ready\n"); - return -1; + if (!verify_are_devices_available()) { + printk("##### Not all devices are available, aborting launch #####"); + 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(); - rclc_support_t support; + MicrorosExecutor node0 = MicrorosExecutor("nucleo_node0"); - RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); + while (!node0.connect()) { + printk("Waiting for connection ..."); + }; - rcl_node_t node; - RCCHECK(rclc_node_init_default(&node, "zephyr_servo_node", "", &support)); + led0_register(&node0); - /* Create subscriber for /servo/position */ - 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)) + node0.execute(); return 0; } diff --git a/src/microros/CMakeLists.txt b/src/microros/CMakeLists.txt new file mode 100644 index 0000000..58bce52 --- /dev/null +++ b/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 +) \ No newline at end of file diff --git a/src/microros/microros_node.cpp b/src/microros/microros_node.cpp new file mode 100644 index 0000000..2f0f422 --- /dev/null +++ b/src/microros/microros_node.cpp @@ -0,0 +1,92 @@ +#include "microros_node.hpp" + +#include +#include +#include +#include +#include +#include // 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; +} diff --git a/src/microros/microros_node.hpp b/src/microros/microros_node.hpp new file mode 100644 index 0000000..1cf9ee3 --- /dev/null +++ b/src/microros/microros_node.hpp @@ -0,0 +1,40 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +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); +}; diff --git a/src/microros/publishers/CMakeLists.txt b/src/microros/publishers/CMakeLists.txt new file mode 100644 index 0000000..d3b7c32 --- /dev/null +++ b/src/microros/publishers/CMakeLists.txt @@ -0,0 +1 @@ +cmake_minimum_required(VERSION 3.20.0) diff --git a/src/microros/subscribers/CMakeLists.txt b/src/microros/subscribers/CMakeLists.txt new file mode 100644 index 0000000..9280b19 --- /dev/null +++ b/src/microros/subscribers/CMakeLists.txt @@ -0,0 +1,3 @@ +cmake_minimum_required(VERSION 3.20.0) + +add_subdirectory(led0) \ No newline at end of file diff --git a/src/microros/subscribers/led0/CMakeLists.txt b/src/microros/subscribers/led0/CMakeLists.txt new file mode 100644 index 0000000..1d0de19 --- /dev/null +++ b/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 +) \ No newline at end of file diff --git a/src/microros/subscribers/led0/led0_subscriber.cpp b/src/microros/subscribers/led0/led0_subscriber.cpp new file mode 100644 index 0000000..c3808cb --- /dev/null +++ b/src/microros/subscribers/led0/led0_subscriber.cpp @@ -0,0 +1,25 @@ +#include "led0_subscriber.hpp" + +#include +#include + +#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 + ); +} diff --git a/src/microros/subscribers/led0/led0_subscriber.hpp b/src/microros/subscribers/led0/led0_subscriber.hpp new file mode 100644 index 0000000..6123c58 --- /dev/null +++ b/src/microros/subscribers/led0/led0_subscriber.hpp @@ -0,0 +1,5 @@ +#pragma once + +#include "microros_node.hpp" + +void led0_register(MicrorosExecutor* node);