Simple microROS pub/sub failing at init

edit

Hi everyone, Working on a ROS Robot project and I only have the weekends to work on this. So far I have wasted away 3 consecutive weekends bashing on a simple issue.

Anyone kind enough to take a quick look? I promise it is a very simple question, any of you experts likely can answer within a few minutes of your valuable time, saving me hours of headache!

Thanks to any kind soul providing advice! I will merge my working code back into the github repository under examples, so other can benefit from this as well! Don't believe me? Check out my pull requests on this repo: https://github.com/micro-ROS/micro_ro...

Issue with the implementation of a publisher and subscriber in same program: Meaning if I delete everything related to the subscriber / publisher, running the other one works.

This is achieved, following the "Teensy with Arduino" tutorial on micro.ros.org

Ubuntu 22.04.1 with ROS2 Humble install (desktop)
Visual Studio Code Version: 1.71.2
PlatformIO Core 6.1.4·Home 3.4.3
Platform Teensy 4.1 and micro_ROS

The line of code below causes it to get stuck in the "error_loop". (around line 121)

RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg_sub, &subscription_callback, ON_NEW_DATA));

Here the entire source code:

#include <Arduino.h>
#include <Wire.h>
#include <micro_ros_platformio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int32.h>

#if !defined(MICRO_ROS_TRANSPORT_ARDUINO_SERIAL)
#error This example is only avaliable for Arduino framework with serial transport.
#endif

// debug
#ifndef DebugMonitor
#define DebugMonitor Serial6
#endif

#define debug(x) DebugMonitor.print(x)
#define debugln(x) DebugMonitor.println(x)

//publisher
rcl_publisher_t publisher;
std_msgs__msg__Int32 msg_pub;

// subscriber
rcl_subscription_t subscriber;
std_msgs__msg__Int32 msg_sub;

// publisher and subscriber common
rcl_node_t node;
rclc_support_t support;
rcl_allocator_t allocator;
rclc_executor_t executor;
rcl_timer_t timer;

#define LED_PIN 13

#define RCCHECK(fn){rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){debugln("error: ");debugln(temp_rc);debugln(RCL_RET_OK);error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

// Error handle loop
void error_loop() {
  while(1) {
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(500);
  }
}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time) {
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    RCSOFTCHECK(rcl_publish(&publisher, &msg_pub, NULL));
    msg_pub.data++;
  }
}

void subscription_callback(const void * msgin)
{  
  const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin;
  digitalWrite(LED_PIN, (msg->data == 0) ? LOW : HIGH);  
  debugln("=================================");
  debugln(" RECEIVED:");
  debugln("=================================");
  debugln(msg->data);

}

void setup() {
  // Configure serial transport
  Serial.begin(115200);
  set_microros_serial_transports(Serial);

  // Debug
  DebugMonitor.begin(115200);

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH); 

  delay(2000);

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
  debugln("Debug 0");

  // create node
  RCCHECK(rclc_node_init_default(&node, "uros_platformio_node", "", &support));
  debugln("Debug 1");

  // create publisher
  RCCHECK(rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "micro_ros_platformio_node_publisher"));
  debugln("Debug 2");

  // create subscriber
  RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "micro_ros_platformio_node_subscriber"));
  debugln("Debug 3");

  // create timer,
  const unsigned int timer_timeout = 1000;
  RCCHECK(rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(timer_timeout),
    timer_callback));
  debugln("Debug 4");

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  debugln("Debug 5");
  RCCHECK(rclc_executor_add_timer(&executor, &timer));
  debugln("Debug 6");
  // RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg_sub, &subscription_callback, ON_NEW_DATA)); 
  debugln("Debug 7");


  msg_pub.data ...