Trouble running a stepper motor with micro-ros

[Marcal8]

I'm having some trouble trying to move a stepper motor with my teensy 4.1. I'm using ROS2 Iron on my Ubuntu 22.04 host computer and micro-ros Iron for the communication with the teensy.

I'm trying to move a stepper using the AccelStepper library and micro-ros. I want to send an especific position to the teensy and have the stepper move to that position. The issue I am having is with the function to call the subscriber, it stops the code for a few microseconds and forbids the necessary steps to be send to move the stepper. This results in the stepper moving in a very slow velocity.

The code I'm using is the following:

#include <AccelStepper.h>

#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 <geometry_msgs/msg/quaternion.h>

rcl_subscription_t subscriber;
geometry_msgs__msg__Quaternion msg;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;


const int numSteppers = 1;

const int stepperPullPins[numSteppers] = {7};
const int stepperDirPins[numSteppers] = {8};
const int stepperEnablePins[numSteppers] = {9};

AccelStepper stepper[numSteppers] {
  AccelStepper(1,stepperPullPins[0],stepperDirPins[0])
};


float steps = 8000.0;

int posObjectiu[numSteppers] = {0};


#define LED_PIN 13

#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_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

void subscription_callback(const void * msgin)
{ 
  const geometry_msgs__msg__Quaternion * msg = (const geometry_msgs__msg__Quaternion *)msgin;

  posObjectiu[0]=int(msg->w);
}


void setup() {
  set_microros_transports();
 
  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(&node, "micro_ros_arduino_node", "", &support));

  // create subscriber
  RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Quaternion),
    "steppers"));

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


  for (int i = 0; i < numSteppers; i++) {

    pinMode(stepperPullPins[i],OUTPUT);
    pinMode(stepperDirPins[i],OUTPUT);
    pinMode(stepperEnablePins[i],OUTPUT);
    digitalWrite(stepperEnablePins[i],HIGH);

    stepper[i].setMaxSpeed(5000.0);
    stepper[i].setAcceleration(500.0);
    stepper[i].setSpeed(100.0);

    stepper[i].setMinPulseWidth(10);
  }
 
  delay(1000);
 
  Serial.begin(9600);
  Serial.println("Test:");

}

void loop() {

  RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));

  for (int i = 0; i < numSteppers; i++) {
    stepper[i].moveTo(posObjectiu[i]);
    stepper[i].setSpeed(250.0);
    stepper[i].runSpeedToPosition();
  }
 
}

Is there a way to call

RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));

that won't stop the steps from being send?

I learned I can use an ESP32 and utilize its second core, is that the only solution? Because changing from a teensy 4.1 to an ESP32 would cause me many more problems.

I also noticed that, if I create a publisher instead of a subscriber, the stepper is not affected in any way.

I would appreciate any help and thanks in advance.