114 lines
3.5 KiB
C++
114 lines
3.5 KiB
C++
#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() {
|
|
static bool support_init = false;
|
|
static bool defaults_init = false;
|
|
static bool executor_init = false;
|
|
|
|
if (!support_init){
|
|
if (rclc_support_init(&this->support, 0, NULL, &this->allocator) != RCL_RET_OK) {
|
|
printk("Can't init support!\r\n");
|
|
return false;
|
|
}
|
|
support_init = true;
|
|
}
|
|
|
|
if (!defaults_init){
|
|
if (rclc_node_init_default(&this->node, this->node_name, "", &this->support) != RCL_RET_OK){
|
|
printk("Can't init defaults!\r\n");
|
|
return false;
|
|
}
|
|
defaults_init = true;
|
|
}
|
|
|
|
if (!executor_init){
|
|
if (rclc_executor_init(&this->executor, &this->support.context, 32, &this->allocator) != RCL_RET_OK){
|
|
printk("Can't init executor!\r\n");
|
|
return false;
|
|
}
|
|
executor_init = true;
|
|
}
|
|
|
|
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);
|
|
|
|
printk("registered publisher %s\n", topic_name);
|
|
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;
|
|
}
|
|
|
|
printk("registered subscriber %s\n", topic_name);
|
|
return &sub_node->subscription;
|
|
}
|