Problem with rclc implementation combined publisher and subscriber

Question

Issue with the implementation of a publisher and subscriber in same program: I have both parts working individually. Meaning if I delete everything related to the subscriber, I can run the publisher and if I delete everything related to the publisher, then I can run the subscriber.

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

In the program below is an attempt to get both a subscriber and publisher working in the same program. Expected behavior: publish a number counting up continuously

Actual behavior: After loading the program into the teensy, it immediately goes into the error routine and flashes the LED.

I'm not sure what I am missing.

#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)

rcl_publisher_t publisher;
rcl_subscription_t subscriber;
std_msgs__msg__Int32 msg;

// publisher and subscriber common
rclc_support_t support;
rcl_allocator_t allocator;

//publisher
rclc_executor_t executor_pub;
rcl_node_t pub_node;

// subscriber
rclc_executor_t executor_sub;
rcl_node_t sub_node;

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(100);
  }
}

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, NULL));
    msg.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);  
}

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));

  // create node
  RCCHECK(rclc_node_init_default(&pub_node, "uros_platformio_pub_node", "", &support));
  RCCHECK(rclc_node_init_default(&sub_node, "uros_platformio_sub_node", "", &support));

  // create publisher
  RCCHECK(rclc_publisher_init_default(
    &publisher,
    &pub_node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "micro_ros_platformio_node_publisher"));
  
  // create subscriber
  RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &sub_node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "micro_ros_arduino_subscriber"));

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

  // create executor_pub
  RCCHECK(rclc_executor_init(&executor_pub, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_timer(&executor_pub, &timer));

  // create executor_sub
  RCCHECK(rclc_executor_init(&executor_sub, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor_sub, &subscriber, &msg, &subscription_callback, ON_NEW_DATA)); 
  

  msg.data = 0;
}

void loop() {
  delay(100);
  RCSOFTCHECK(rclc_executor_spin_some(&executor_pub, RCL_MS_TO_NS(100)));
  RCSOFTCHECK(rclc_executor_spin_some(&executor_sub, RCL_MS_TO_NS(100)));
  debugln("Scan");
}

Any hints will be greatly appreciated. Please let me know if anything needs clarification.

Problem with rclc implementation combined publisher and subscriber

0 Answers0