No service response when node has both service server and subscriber

[anaelle-sw]

Hi! I encountered an issue that seems really problematic, but quite strange. When my node's executor has both a subscriber and a service server, the response of the server is never sent.

System:

• Teensy 3.5
• Ubuntu 20.04, ROS2 Foxy
• micro-ROS agent is installed from snap
• to be sure my micro-ROS Arduino lib was up-to-date, I rebuilt it from the latest version

Steps to reproduce

I have a node quite big, with several service severs, publishers, subscribers. Recently, I moved the add_service_to_executor function after the add_service_to_executor functions in the setup. And this causes my node to not sent anymore the service responses.
So I tried to isolate the problem with this minimal code inspired by the micro-ROS Arduino examples:

#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_srvs/srv/trigger.h>
#include <std_msgs/msg/int32.h>

rcl_subscription_t subscriber;
std_msgs__msg__Int32 msg;

rcl_service_t service;
std_srvs__srv__Trigger_Response res;
std_srvs__srv__Trigger_Request req;

rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

#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 std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin;
  digitalWrite(LED_PIN, (msg->data == 0) ? LOW : HIGH);  
}

void service_callback(const void * req, void * res){
  std_srvs__srv__Trigger_Request * req_in = (std_srvs__srv__Trigger_Request *) req;
  std_srvs__srv__Trigger_Response * res_in = (std_srvs__srv__Trigger_Response *) res;
  res_in->success = true;
}


void setup() {
  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(std_msgs, msg, Int32), "topic_int"));
    
  // create service
  RCCHECK(rclc_service_init_default(&service, &node,  ROSIDL_GET_SRV_TYPE_SUPPORT(std_srvs, srv, Trigger), "srv_trigger"));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_service(&executor, &service, &req, &res, service_callback));    // LINE A
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));    // LINE B
}

void loop() {
  delay(100);
  RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

This code is uploaded on a Teensy 3.5.
Then I launch a micro-ROS agent (with verbose): micro-ros-agent serial --dev /dev/teensy -v6
And I call the service with CLI: ros2 service call /srv_trigger std_srvs/srv/Trigger '{}'

Issue description

In my complete code, when I invert LINE A and LINE B, the server does receive the request, but no response is sent.

But the "strange" thing is that with this example code, no matter if LINE B is before or after LINE A, the service response won't be sent. Actually, what happens is that immediately after the board reboots (after the upload or hardware reboot), an error happpens when spinning node, and the board is stuck in the error loop.
But when only LINE A or only LINE B is un-commented, everything works as expected.
So my questions are: can someone reproduce this? Why this example code cannot run with both a subscriber and a server?
If I did something wrong when initializing the subscriber or the server in this example, there could be the same errors in my complete code, which may explains the unconsistent behavior depending on the order of the add_X_to_executor functions...

Precisions

• I tried to configure the subscriber to best effort QoS (rclc_subscription_init_best_effort), the result is the same
• I cannot explain why the order of lines within my complete code change something, but not within this example. I am working on it and will update this issue if I got something

Thanks a lot for helping!

[pablogs9]

I start working on this. I'll be back when having some answers.

[pablogs9]

In this line:

  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));

You are setting number_of_handles to 1 but then you are adding 2 handles (the subscriber and the service)

In this other line:

  RCCHECK(rclc_executor_add_service(&executor, &service, &req, &res, service_callback));    // LINE A

You are using service_callback instead of &service_callback... I have just check that all our examples are like this, but I'm not sure about that. I will investigate about function pointers.

And finally, the problem here is that std_srvs/Trigger has two fields: a boolean and a string for a status message. micro-ROS does not handle well uninitialized string messages in order to avoid dynamic memory allocation. I have tested this and it works, please check how I'm initializing the string in the response struct provided to the executor:

#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_srvs/srv/trigger.h>
#include <std_msgs/msg/int32.h>

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); return 1;}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}

rcl_subscription_t subscriber;
std_msgs__msg__Int32 msg;

rcl_service_t service;
std_srvs__srv__Trigger_Response res;
std_srvs__srv__Trigger_Request req;

rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;

static const char res_message = '\0';

void error_loop(){
  while(1){
    printf("error\n");
  }
}

void subscription_callback(const void * msgin){  
  const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin;
  printf("received topic\n");
}


void service_callback(const void * req, void * res){
  std_srvs__srv__Trigger_Request * req_in = (std_srvs__srv__Trigger_Request *) req;
  std_srvs__srv__Trigger_Response * res_in = (std_srvs__srv__Trigger_Response *) res;
  printf("received service\n");

  res_in->success = true;
}


void setup() {

  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(std_msgs, msg, Int32), "topic_int"));
    
  // create service
  RCCHECK(rclc_service_init_default(&service, &node,  ROSIDL_GET_SRV_TYPE_SUPPORT(std_srvs, srv, Trigger), "srv_trigger"));

  // create executor
  res.message.data = &res_message;
  res.message.size = 1;
  res.message.capacity = 1;  
  RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator));
  RCCHECK(rclc_executor_add_service(&executor, &service, &req, &res, &service_callback));    // LINE A
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));    // LINE B
}

void loop() {
  RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

[JanStaschulat]

// create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_service(&executor, &service, &req, &res, service_callback));    // LINE A
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));    // LINE B

Yes, I caught this mistake too: you need to define the number of handles in init function as 2.
You did not get any error message when calling rclc_executor_add_subscription?

[anaelle-sw]

Hi @pablogs9 , thanks a lot for your quick answer

Indeed, I have completely forgotten to change the number of handles when initializing the executor, in the minimal code and in my complete code (no problem seems to happen because of this, until now). The number of handles in the number of ROS entities an executor manages, except the node, that's right? (If in my complete application my executor manages a node, 3 publishers, 2 subscribers, and 2 servers, the number of handles has to be 7?)

About the callbacks' pointers, I followed the examples: callback for a service callbacks and &callback for subscriber callbacks

Thanks for the string initialization example. It was correctly initialized in my complete code but I forgot it in the minimal one.

Your example works great for me. I also replaced the number of handles in my complete code, now it works properly. So, my error was to let the number of handles set to 1, thank you for pointed that out and for your help.

[pablogs9]

Cool, as far as I know, the number of handles is the number of entities that the executor will manage in terms of subscriptions, timers, services... a node is not handled by the executor.

Maybe @JanStaschulat can give us a more precise answer.

[JanStaschulat]

@pablogs9 We might want to change the RCCHECK to output the error message if any, like here The error message is set by the rclc-executor, but it is the responsibility of the user to print it. Currently the RCCHECK macro only reports THAT there was an error.

@anaelle-sw
"If in my complete application my executor manages a node, 3 publishers, 2 subscribers, and 2 servers, the number of handles has to be 7?)"
No. You specify the number of handles the executor has to manage. That is the total number of subscriptions, timers, services, clients and guard conditions.

In your example that would be 4. If you specify more (7) that will also work fine. For these handles the executor has "to do something" if a message arrives, e.g. call the callback of the topic. As micro-ros is for microcontrollers, memory is scarce. Therefore memory for the total number of handles is pre-allocated before calling any of the spin-methods.

• Nodes is a concept in rclcpp but is not used in rclc-API regarding the executor.
• For publishers, the user only calls the function rcl_publish(), which is an API of rcl-library. It is so-to-say an out-going signalling path. The executor is not involved.

[anaelle-sw]

Hi @JanStaschulat , sorry I didn't see you answer before closing.

You did not get any error message when calling rclc_executor_add_subscription?

After checking it, yes the error_loop was reached at this line in the minimal code. I didn't pay attention to it in my complete code, as I only use RCSOFTCHECK to not stuck the application.

Thanks a lot for your detailed answer about handles, I got it now!