microROS service hangs on first call

edit

Good day all,

I've been away sick in the hospital for a while, but am slowly recovering and just getting back into all of this to keep my head focused and from spinning around.

Anyway, it seems I've never been able to get around fixing an issue with the service routine I have written in microRos running on a teensy.

Manually calling the service works and everything, but it always hangs on the very first request, ending with me hitting ctrl+c. After that there are absolutely no issues.

I have included the service callback and also console output below.

Setup:

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

service callback

// ------------------------------------------------------------------------------------------------
// srv.FrontLights callback
void service_FrontLights_callback(const void * req_FrontLights, void * res_FrontLights) {
  bool previousState = digitalRead(FRONT_LIGHT);
  delayMicroseconds(1000);
  example_interfaces__srv__SetBool_Request * req_in = (example_interfaces__srv__SetBool_Request *) req_FrontLights;
  example_interfaces__srv__SetBool_Response * res_in = (example_interfaces__srv__SetBool_Response *) res_FrontLights;

  digitalWrite(FRONT_LIGHT, (req_in->data == 0) ? LOW:HIGH);

  delayMicroseconds(1000);

  if (previousState != digitalRead(FRONT_LIGHT)) {
    res_in->success = true;
    res_in->message.data = const_cast<char*>("Front Lights succeeded to update");
  }
  else {
    res_in->success = false;
    res_in->message.data = const_cast<char*>("Front Lights FAILED to update");
  }
}

I have added the delays above, without the delays, if always reports FAILED, as per the else statement. I believe this to be due to timing it takes for actual outputs to switch state, aka rising/falling edge and making the logic levels.

Console output:

administrator@dev01:~/microros_ws$ ros2 service call /frontlights example_interfaces/srv/SetBool data:\ true
waiting for service to become available...
requester: making request: example_interfaces.srv.SetBool_Request(data=True)

^Cadministrator@dev01:~/microros_wsros2 service call /frontlights example_interfaces/srv/SetBool data:\ false
requester: making request: example_interfaces.srv.SetBool_Request(data=False)

response:
example_interfaces.srv.SetBool_Response(success=True, message='Front Lights succeeded to update')

administrator@dev01:~/microros_ws$ ros2 service call /frontlights example_interfaces/srv/SetBool data:\ true
waiting for service to become available...
requester: making request: example_interfaces.srv.SetBool_Request(data=True)

response:
example_interfaces.srv.SetBool_Response(success=True, message='Front Lights succeeded to update')

administrator@dev01:~/microros_ws$ ros2 service call /frontlights example_interfaces/srv/SetBool data:\ false
requester: making request: example_interfaces.srv.SetBool_Request(data=False)

response:
example_interfaces.srv.SetBool_Response(success=True, message='Front Lights succeeded to update')

administrator@dev01:~/microros_ws$

Please let me know if there is anything else that I need to share here and I will. Thanks for any suggestions

.