Agent can not reconnect to client when using rclc_support_init_with_options

[PrwTsrt]

Issue template

• Hardware description: RP2040, Teensy, ESP32S3
• RTOS: FreeRTOS, Non-RTOS
• Installation type:
• Version or commit hash: humble

When using rclc_support_init_with_options instead of rcl_init_options_init, the Micro-ROS client can successfully connect to the agent on the first and second attempt, but fails to connect on subsequent attempts.

#include <Arduino.h>
#include <micro_ros_platformio.h>

#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <micro_ros_utilities/string_utilities.h>
#include <rmw_microros/rmw_microros.h>

#include <std_msgs/msg/bool.h>
#include <std_msgs/msg/u_int64.h>
#include <sensor_msgs/msg/range.h>
#include <Ultrasonic.h>

#include <Wire.h>
#include <ACROBOTIC_SSD1306.h>
#include <Int64String.h>


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

#define ROS_DOMAIN_ID 57

...

rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;
rcl_init_options_t init_options;

#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)){}}

...

// Timeout for each ping attempt
const int timeout_ms = 100;
// Number of ping attempts
const uint8_t attempts = 1;
// Spin period
const unsigned int spin_timeout = RCL_MS_TO_NS(100);
// Enum with connection status
enum states {
    WAITING_AGENT,
    AGENT_AVAILABLE,
    AGENT_CONNECTED,
    AGENT_DISCONNECTED
} state;

// Error handle loop
void error_loop() {
  while(1) {
    delay(100);
  }
}

bool create_entities(void) {

  size_t domain_id = (size_t)(ROS_DOMAIN_ID);
  
  // Initialize micro-ROS allocator
  allocator = rcl_get_default_allocator();

  // Initialize and modify options 
  init_options = rcl_get_zero_initialized_init_options();
  RCCHECK(rcl_init_options_init(&init_options, allocator));
  RCCHECK(rcl_init_options_set_domain_id(&init_options, domain_id));

  // Initialize rclc support object with custom options
  RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));

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

  ////////////////////////////////////////////////////////////////////////////////////////////////////////////

  // 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_raspico_node", "", &support));
  
  ////////////////////////////////////////////////////////////////////////////////////////////////////////////

  // create publisher
 ...

  // create timer,
  const unsigned int timer_timeout = 20;
  RCCHECK(rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(timer_timeout),
    timer_callback));

  // create executor
  executor = rclc_executor_get_zero_initialized_executor();
  RCCHECK(rclc_executor_init(&executor, &support.context, 5, &allocator));
  RCCHECK(rclc_executor_add_timer(&executor, &timer));

  // Add a subscriber to the executor
 ...

  return true;
}

void destroy_entities(void){

  rmw_context_t * rmw_context = rcl_context_get_rmw_context(&support.context);
  (void) rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0);

  RCCHECK(rcl_publisher_fini
  ...
  RCCHECK(rcl_timer_fini(&timer));
  RCCHECK(rclc_executor_fini(&executor));
  RCCHECK(rcl_node_fini(&node)); 
  RCCHECK(rclc_support_fini(&support));

}

void setup() {

  Wire.begin();	

  ...

  // Configure serial transport
  Serial.begin(921600);
  set_microros_serial_transports(Serial);
  delay(2000);

  
}

void loop() {

  switch (state)
    {
        case WAITING_AGENT:
            // Check for agent connection
            state = (RMW_RET_OK == rmw_uros_ping_agent(timeout_ms, attempts)) ? AGENT_AVAILABLE : WAITING_AGENT;
            break;

        case AGENT_AVAILABLE:
            // Create micro-ROS entities
            state = (true == create_entities()) ? AGENT_CONNECTED : WAITING_AGENT;

            if (state == WAITING_AGENT)
            {
                // Creation failed, release allocated resources
                destroy_entities();
            };
            break;

        case AGENT_CONNECTED:
            // Check connection and spin on success
            state = (RMW_RET_OK == rmw_uros_ping_agent(timeout_ms, attempts)) ? AGENT_CONNECTED : AGENT_DISCONNECTED;
            if (state == AGENT_CONNECTED)
            {
                RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));            }
            break;

        case AGENT_DISCONNECTED:
            // Connection is lost, destroy entities and go back to first step
            destroy_entities();
            state = WAITING_AGENT;
            break;

        default:
            break;
    }

}