added micro ros tansport canfd

src/devices.cpp

@@ -1,5 +1,7 @@
 #include "devices.hpp"
 
+const struct device *fdcan1 = DEVICE_DT_GET(DT_NODELABEL(fdcan1));
+
 bool verify_are_devices_available(void) {
     bool devices_ready = true;
 
@@ -22,6 +24,10 @@ bool verify_are_devices_available(void) {
         printk("Servo0 not ready\n");
         devices_ready = false;
     }
+    if (!device_is_ready(fdcan1)) {
+        printk("CAN FD 1 not ready\n");
+        devices_ready = false;
+    }
 
     return devices_ready;
 }

src/devices.hpp

@@ -19,4 +19,6 @@ static const struct gpio_dt_spec led2 = GPIO_DT_SPEC_GET(DT_ALIAS(led2), gpios);
 
 static const struct pwm_dt_spec servo0 = PWM_DT_SPEC_GET(DT_ALIAS(servo0));
 
+extern const struct device *fdcan1;
+
 bool verify_are_devices_available(void);

src/main.cpp

@@ -36,8 +36,11 @@ int main(void)
 
     MicrorosExecutor node0 = MicrorosExecutor("nucleo_node0");
 
+    printk("Connecting to Agent....\n");
+
     while (!node0.connect()) {
         printk("Waiting for connection ...\n");
+        k_msleep(1000);
     };
 
     printk("Connection to Agent node established!\n");

src/microros/microros_node.cpp

@@ -35,14 +35,33 @@ MicrorosExecutor::~MicrorosExecutor() {
 }
 
 bool MicrorosExecutor::connect() {
-    if (rclc_support_init(&this->support, 0, NULL, &this->allocator) != RCL_RET_OK)
-        return false;
+    static bool support_init = false;
+    static bool defaults_init = false;
+    static bool executor_init = false;
 
-    if (rclc_node_init_default(&this->node, this->node_name, "", &this->support) != RCL_RET_OK)
-        return 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 (rclc_executor_init(&this->executor, &this->support.context, 32, &this->allocator) != RCL_RET_OK)
-        return false;
+    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;
 }