Trouble running a stepper motor with micro-ros

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

#include

#include
#include
#include
#include
#include

#include

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.

Start with the provided loop(), especially rclc_executor_spin_some() and the AccelStepper runSpeedToPosition() call, and reproduce the behavior on a Teensy 4.1 with ROS2 Iron and micro-ROS Iron. Determine whether the executor call blocks step generation and document a supported fix or limitation, with a focused reproduction if the repository has relevant examples or tests.

Written by the indexing model from the issue text.