No voltage from LED pins - Multiple subscribers on one node

edit

I got this code from the example subscriber code on the Arduino IDE. I modified the code so it would have multiple subscribers on the same node. Somewhere along the way, this caused the pins to stop outputting. I followed all the tutorials and my code compiles just fine, but it seems that the digitalWrite functions are not being called because I get no voltage from any of the pins. It could also be an issue with the callback functions because I tried digitalWrite(LED_PIN0, HIGH) and still got nothing.

I'm running this code on a Portenta H7 microcontroller and Arduino IDE 2.0.4 It is plugged into a Latte Panda running ubuntu 20.04 and ROS2. According to rqt_graph, everything looks correct. All the subscribers are subscribed to the correct topics.

Here is my code:

#include <micro_ros_arduino.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int16.h>
#include <std_msgs/msg/int32.h>

std_msgs__msg__Int16 msgA;
std_msgs__msg__Int16 msgB;
std_msgs__msg__Int16 msgX;
std_msgs__msg__Int16 msgY;
std_msgs__msg__Int16 msgUp;
std_msgs__msg__Int16 msgDown;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_timer_t timer;

#define LED_PIN0 0
#define LED_PIN1 1
#define LED_PIN2 2
#define LED_PIN3 3
#define LED_PIN4 4
#define LED_PIN5 5

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


void error_loop(){
  while(1){
    digitalWrite(LED_PIN0, !digitalRead(LED_PIN0));
    digitalWrite(LED_PIN1, !digitalRead(LED_PIN1));
    digitalWrite(LED_PIN2, !digitalRead(LED_PIN2));
    digitalWrite(LED_PIN3, !digitalRead(LED_PIN3));
    digitalWrite(LED_PIN4, !digitalRead(LED_PIN4));
    digitalWrite(LED_PIN5, !digitalRead(LED_PIN5));

    delay(100);
  }
}

void btnA_callback(const void * msgin)
{  
  const std_msgs__msg__Int16 * msg = (const std_msgs__msg__Int16 *)msgin;
  digitalWrite(LED_PIN0, (msg->data == 0) ? LOW : HIGH);  
}
void btnB_callback(const void * msgin)
{  
  const std_msgs__msg__Int16 * msg = (const std_msgs__msg__Int16 *)msgin;
  digitalWrite(LED_PIN1, (msg->data == 0) ? LOW : HIGH);  
}
void btnX_callback(const void * msgin)
{  
  const std_msgs__msg__Int16 * msg = (const std_msgs__msg__Int16 *)msgin;
  digitalWrite(LED_PIN2, (msg->data == 0) ? LOW : HIGH);
}
void btnY_callback(const void * msgin)
{  
  const std_msgs__msg__Int16 * msg = (const std_msgs__msg__Int16 *)msgin;
  digitalWrite(LED_PIN3, (msg->data == 0) ? LOW : HIGH);  
}
void Up_callback(const void * msgin)
{  
  const std_msgs__msg__Int16 * msg = (const std_msgs__msg__Int16 *)msgin;
  digitalWrite(LED_PIN4, (msg->data == 0) ? LOW : HIGH);  
}
void Down_callback(const void * msgin)
{  
  const std_msgs__msg__Int16 * msg = (const std_msgs__msg__Int16 *)msgin;
  digitalWrite(LED_PIN5, (msg->data == 0) ? LOW : HIGH);  
}

void setup() {
  set_microros_transports();

  allocator = rcl_get_default_allocator();

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

  // create node
  rcl_node_t node; //= rcl_get_zero_initialized_node()
  RCCHECK(rclc_node_init_default(&node, "micro_ros_btn_sub_node", "", &support));


  // create subscribers
  rcl_subscription_t controllerSubA;  
  RCCHECK(rclc_subscription_init_default(&controllerSubA, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16), "/btn_ctl/A"));
  rcl_subscription_t controllerSubB; 
  RCCHECK(rclc_subscription_init_default(&controllerSubB, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16), "/btn_ctl/B"));
  rcl_subscription_t controllerSubX; 
  RCCHECK(rclc_subscription_init_default(&controllerSubX, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16), "/btn_ctl/X"));
  rcl_subscription_t controllerSubY; 
  RCCHECK(rclc_subscription_init_default(&controllerSubY, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16), "/btn_ctl/Y"));
  rcl_subscription_t controllerSubUp; 
  RCCHECK(rclc_subscription_init_default(&controllerSubUp, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16), "/btn_ctl/Up"));
  rcl_subscription_t controllerSubDown; 
  RCCHECK(rclc_subscription_init_default(&controllerSubDown, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16), "/btn_ctl/Down"));



  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 6, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor, &controllerSubA, &msgA, &btnA_callback, ON_NEW_DATA));
  RCCHECK(rclc_executor_add_subscription(&executor, &controllerSubB, &msgB, &btnB_callback, ON_NEW_DATA));
  RCCHECK(rclc_executor_add_subscription(&executor, &controllerSubX, &msgX, &btnX_callback, ON_NEW_DATA));
  RCCHECK(rclc_executor_add_subscription(&executor, &controllerSubY, &msgY, &btnY_callback, ON_NEW_DATA));
  RCCHECK(rclc_executor_add_subscription(&executor, &controllerSubUp, &msgUp, &Up_callback, ON_NEW_DATA ...