rclc single node failing to init secondary service

[andrew-cra2019]

Hardware description: Teensy 4.1
Installation type: Ubuntu 22.04.1, ROS2 (humble)
Version or commit hash: ROS-humble
Platformio Core 6.1.4·Home 3.4.3 withing VS-Code 1.72.2

Steps to reproduce the issue

Take Source code below, check how service AddTwoInts was "duplicated" and adjusted for Service "SetBool"
Create project in platformio
set-up your platformio.ini as the below code-sample
use proper build workflow: (a) clean, or delete .pio folder, b) pio lib install, c) pio run, d) pio run

Expected behavior Compile without error and actually work on target, creating two individual services under single node

Actual behavior Compiles just fine, but once loaded into target, the initialization of second service (setBool) will cause the error_loop to be called. This is in line RCCHECK(rclc_service_init_default(&service_setBool, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, SetBool), "/setbool"));

Additional Information Each service works by itself. Removing (commenting) one or the other results in proper function. There must be something wrong with the RCCHECK? Tracking it back, it returns error code RCL_RET_OK

I found the definition in file types.h:

/// Success return code.
#define RCL_RET_OK RMW_RET_OK
/// Unspecified error return code.
#define RCL_RET_ERROR RMW_RET_ERROR
/// Timeout occurred return code.
#define RCL_RET_TIMEOUT RMW_RET_TIMEOUT
/// Failed to allocate memory return code.
#define RCL_RET_BAD_ALLOC RMW_RET_BAD_ALLOC
/// Invalid argument return code.
#define RCL_RET_INVALID_ARGUMENT RMW_RET_INVALID_ARGUMENT
/// Unsupported return code.
#define RCL_RET_UNSUPPORTED RMW_RET_UNSUPPORTED

Hovering with the mouse over the individual #defines, the tooltip revealed the "1" seen in my debug output resolves to

RMW_RET_ERROR

with the comment above:

/// Unspecified error return code.

platfomio.ini

[env:teensy41]
platform = teensy
board = teensy41
framework = arduino
board_microros_transport = serial
monitor_port = /dev/ttyUSB0
board_microros_distro = humble

lib_deps =
    https://github.com/micro-ROS/micro_ros_platformio

Source code

#include <Arduino.h>
#include <Wire.h>
#include <micro_ros_platformio.h>
#include <example_interfaces/srv/add_two_ints.h>
#include <example_interfaces/srv/set_bool.h>
#include <stdio.h>
#include <rcl/error_handling.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int32.h>
#include <std_msgs/msg/int64.h>
#include <std_msgs/msg/bool.h>

#if !defined(MICRO_ROS_TRANSPORT_ARDUINO_SERIAL)
#error This example is only avaliable for Arduino framework with serial transport.
#endif

// ============================================================================
// INSTANCES, OBJECTS
// ============================================================================
// publisher and subscriber common
rcl_node_t node;
rclc_support_t support;
rcl_allocator_t allocator;
rclc_executor_t executor;
rcl_timer_t timer;
unsigned int num_handles = 4;   // 1 subscriber, 1 publisher, 2 service

// service addTwoInts
rcl_service_t service_addTwoInts;

// service setBool
rcl_service_t service_setBool;

rcl_wait_set_t wait_set;

//publisher
rcl_publisher_t publisher;
std_msgs__msg__Int32 msg_pub;

// subscriber
rcl_subscription_t subscriber;
std_msgs__msg__Int32 msg_sub;

example_interfaces__srv__AddTwoInts_Response res;
example_interfaces__srv__AddTwoInts_Request req;

example_interfaces__srv__SetBool_Response res_setBool;
example_interfaces__srv__SetBool_Request req_setBool;

// ============================================================================
// DEFINES
// ============================================================================
// I/O - Definitions
#define LED_PIN 13

// ============================================================================
// FUNCTION PROTOTYPES
// ============================================================================
#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)){}}

// Error handle loop
void error_loop() {
  while(1) {
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(500);
  }
}
// --------------------------------------------------------
// timer callback (currently just increments msg_pub.data)
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_pub, NULL));
    msg_pub.data++;
  }
}
// --------------------------------------------------------
// subscriber callback
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);  
}
// --------------------------------------------------------
// srv.addTwoInts callback
void service_addTwoInts_callback(const void * req, void * res) {
  example_interfaces__srv__AddTwoInts_Request * req_in = (example_interfaces__srv__AddTwoInts_Request *) req;
  example_interfaces__srv__AddTwoInts_Response * res_in = (example_interfaces__srv__AddTwoInts_Response *) res;

  //printf("Service request value: %d + %d.\n", (int) req_in->a, (int) req_in->b);

  res_in->sum = req_in->a + req_in->b;
}
// --------------------------------------------------------
// srv.setBool callback
void service_setBool_callback(const void * req_setBool, void * res_setBool) {
  bool previousState = digitalRead(LED_PIN);
  example_interfaces__srv__SetBool_Request * req_in = (example_interfaces__srv__SetBool_Request *) req_setBool;
  example_interfaces__srv__SetBool_Response * res_in = (example_interfaces__srv__SetBool_Response *) res_setBool;

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

  if (previousState != digitalRead(LED_PIN)) {
    res_in->success = true;
  }
  else {
    res_in->success = false;
  }
}

// ============================================================================
// SETUP
// ============================================================================
void setup() {
  set_microros_serial_transports(Serial);

  // I/O setup
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);

  delay(1000); 

  allocator = rcl_get_default_allocator();

  // create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
  RCCHECK(rclc_node_init_default(&node, "uros_platformio_node", "", &support));

  // create service addTwoInts
  RCCHECK(rclc_service_init_default(&service_addTwoInts, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts), "/addtwoints"));

  // create service setBool
  RCCHECK(rclc_service_init_default(&service_setBool, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, SetBool), "/setbool"));

  // create publisher
  RCCHECK(rclc_publisher_init_default(&publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "micro_ros_platformio_node_publisher"));

  // create subscriber
  RCCHECK(rclc_subscription_init_default(&subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "micro_ros_platformio_node_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
  RCCHECK(rclc_executor_init(&executor, &support.context, num_handles, &allocator));  
  RCCHECK(rclc_executor_add_service(&executor, &service_addTwoInts, &req, &res, service_addTwoInts_callback));
  RCCHECK(rclc_executor_add_service(&executor, &service_setBool, &req_setBool, &res_setBool, service_setBool_callback));
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg_sub, &subscription_callback, ON_NEW_DATA));   
  RCCHECK(rclc_executor_add_timer(&executor, &timer));

  msg_pub.data = 0;
}

// ============================================================================
// MAIN LOOP
// ============================================================================
void loop() {
  delay(100);
  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

[Acuadros95]

You need to increase the max number of services on the default meta file: detail.

You can create your own .meta file with this value increased, check the Readme section Other configuration for the steps.

[andrew-cra2019]

Hi @Acuadros95 , Thanks for getting abck so quickly.

I have changed the colcon.meta and then again executed "pio run" before uploading to target. But the problem still persists.

Here is the colcon.meta

{
    "names": {
        "rmw_microxrcedds": {
            "cmake-args": [
                "-DRMW_UXRCE_MAX_NODES=1",
                "-DRMW_UXRCE_MAX_PUBLISHERS=10",
                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=5",
                "-DRMW_UXRCE_MAX_SERVICES=2",
                "-DRMW_UXRCE_MAX_CLIENTS=1",
                "-DRMW_UXRCE_MAX_HISTORY=4",
                "-DRMW_UXRCE_TRANSPORT=custom"
            ]
        }
    }
}

Anything else I'm missing? Thank you so much!

[andrew-cra2019]

Just noted the comment: Note: the [common.meta](https://github.com/micro-ROS/micro_ros_platformio/blob/main/metas/common.meta) file makes general adjustments to the library and shall not be modified by the user.

I will try following the documentation. Reporting back shortly

[andrew-cra2019]

Updated my platformio.ini to the below:

[env:teensy41]
platform = teensy
board = teensy41
framework = arduino
board_microros_transport = serial
monitor_port = /dev/ttyUSB0
board_microros_distro = humble

board_microros_user_meta = custom.meta

lib_deps =
    https://github.com/micro-ROS/micro_ros_platformio

And then I added a new file under the main project directory named accordingly: "custom.meta" with the following content:

{
    "names": {
        "rmw_microxrcedds": {
            "cmake-args": [
                "-DRMW_UXRCE_MAX_NODES=1",
                "-DRMW_UXRCE_MAX_PUBLISHERS=10",
                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=5",
                "-DRMW_UXRCE_MAX_SERVICES=2",
                "-DRMW_UXRCE_MAX_CLIENTS=1",
                "-DRMW_UXRCE_MAX_HISTORY=4",
                "-DRMW_UXRCE_TRANSPORT=custom"
            ]
        }
    }
}

After that, I removed the .pio folder in case my previous fiddling in the .meta file has caused some unwanted changes in the lib. did a clean and the followed the proper build steps:

1. pio lib install
2. pio run
3. pio run --target upload

And it worked, all services, subscriber and publisher are actually running together :-)

You're fantastic help with my project. I do so truly appreciate your help with this, mate!

I will create another example from this and promise to create a pull-request to main, as soon as my other pending PR is accepted. Just for the reason, I don't know how to amend to an existing PR :-)

Thank you so very much @Acuadros95