Problems with reconnection when using custom domain ID

[gsokoll]

Issue template

• Hardware description: Renesas RA6M5 dev kit
• RTOS: FreeRTOS
• Installation type: UDP transport to agent on ubuntu 20.04 PC
• Version or commit hash: galactic

Steps to reproduce the issue

We are attempting to build a test app to ensure the micro ros connection can tolerate either the host or client device restarting, and/or interruptions to the network. We have used code similar to https://docs.vulcanexus.org/en/humble/rst/tutorials/micro/handle_reconnections/handle_reconnections.html with the addition of specifying the custom domain id per code similar to https://github.com/micro-ROS/micro-ROS-demos/blob/45f342d4ef887f6a2e9f2dfa45d1d3388d7f6dbd/rclc/configuration_example/configured_publisher/main.c#L49

Our actual code is given below.

Expected behavior

If we do NOT attempt to use a custom domain (calling rclc_support_init() without any options), the app works as expected.

• We can start either the agent or the client app first, and client app will connect and work ok.
• We can stop and restart the agent, and the client app will reconnect and work ok.

Actual behavior

If we DO attempt to use a custom domain , the client will not reconnect after an interruption. This occurs irrespective of whether the custom domain ID is 0 or some other non-default value.

• If the agent is started before the client: the client will connect initially and work, but if the agent is then stopped and restarted, the client app will fail at the call to rclc_support_init_with_options() during the create_entities() function when it attempts to reconnect. The return value from rclc_support_init_with_options() is 1 (i.e. RMW_RET_ERROR 1 =Generic error to indicate operation could not complete successfully)

• If the client is started before the agent is running: the client app will spin in the WAITING_AGENT state until the agent is started. It will connect successfully when the agent is started. However if the agent is stopped and restarted, the client will fail at the call to rclc_support_init_with_options() during the create_entities() function when it attempts to reconnect. The return value from rclc_support_init_with_options() is 1.

Additional information

Response from the agent when the client app successfully connects:

dell@GDS-T5810:~/quasor_ws$ ROS_DOMAIN_ID=0 ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888 -v6
[1678948109.461594] info     | UDPv4AgentLinux.cpp | init                     | running...             | port: 8888
[1678948109.461952] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6
[1678948109.532862] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data: 
0000: 80 00 00 00 02 01 08 00 00 0A FF FD 02 00 00 00
[1678948109.533276] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x00000000, len: 36, data: 
0000: 80 00 00 00 06 01 1C 00 00 0A FF FD 00 00 01 0D 58 52 43 45 01 00 01 0F 00 01 0D 00 01 00 00 00
0020: 00 00 00 00
[1678948109.632792] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 24, data: 
0000: 80 00 00 00 00 01 10 00 58 52 43 45 01 00 01 0F 49 8B 8B B5 81 00 FC 01
[1678948109.632960] info     | Root.cpp           | create_client            | create                 | client_key: 0x498B8BB5, session_id: 0x81
[1678948109.633046] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x498B8BB5, address: 192.168.0.125:4501
[1678948109.633177] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x498B8BB5, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1678948109.633623] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x498B8BB5, len: 48, data: 
0000: 81 01 00 00 01 07 26 00 00 0A 00 01 01 03 00 00 18 00 00 00 00 01 00 00 10 00 00 00 6D 79 5F 72
0020: 65 6E 65 73 61 73 5F 6E 6F 64 65 00 00 00 00 00
[1678948109.633733] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x498B8BB5, len: 80, data: 
0000: 81 01 01 00 01 07 46 00 00 0B 00 02 02 03 00 00 38 00 00 00 11 00 00 00 72 74 2F 69 6E 74 5F 70
0020: 75 62 6C 69 73 68 65 72 00 00 01 00 1C 00 00 00 73 74 64 5F 6D 73 67 73 3A 3A 6D 73 67 3A 3A 64
0040: 64 73 5F 3A 3A 49 6E 74 33 32 5F 00 00 01 00 00
[1678948109.633831] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x498B8BB5, len: 24, data: 
0000: 81 01 02 00 01 07 10 00 00 0C 00 03 03 03 00 00 02 00 00 00 00 00 00 01
[1678948109.633918] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x498B8BB5, len: 36, data: 
0000: 81 01 03 00 01 07 1C 00 00 0D 00 05 05 03 00 00 0E 00 00 00 00 02 01 73 03 00 01 73 0A 00 00 00
0020: 00 00 00 03
[1678948109.634007] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x498B8BB5, len: 16, data: 
0000: 81 80 00 00 07 01 08 00 00 0E 00 05 00 00 00 00
[1678948109.647293] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x498B8BB5, participant_id: 0x000(1)
[1678948109.647382] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x498B8BB5, len: 14, data: 
0000: 81 01 00 00 05 01 06 00 00 0A 00 01 00 00
[1678948109.647399] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x498B8BB5, topic_id: 0x000(2), participant_id: 0x000(1)
[1678948109.647448] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x498B8BB5, len: 14, data: 
0000: 81 01 01 00 05 01 06 00 00 0B 00 02 00 00
[1678948109.647489] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x498B8BB5, publisher_id: 0x000(3), participant_id: 0x000(1)
[1678948109.647522] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x498B8BB5, len: 14, data: 
0000: 81 01 02 00 05 01 06 00 00 0C 00 03 00 00
[1678948109.647773] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x498B8BB5, datawriter_id: 0x000(5), publisher_id: 0x000(3)
[1678948109.647821] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000000, len: 4, data: 
0000: 00 00 00 00
[1678948109.647879] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x498B8BB5, len: 14, data: 
0000: 81 01 03 00 05 01 06 00 00 0D 00 05 00 00
[1678948109.647913] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x498B8BB5, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1678948109.747705] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x498B8BB5, len: 16, data: 
0000: 81 80 01 00 07 01 08 00 00 0F 00 05 01 00 00 00
[1678948109.747824] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000000, len: 4, data: 
0000: 01 00 00 00
[1678948109.747963] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x498B8BB5, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1678948109.847716] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x498B8BB5, len: 16, data: 
0000: 81 80 02 00 07 01 08 00 00 10 00 05 02 00 00 00
[1678948109.847837] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000000, len: 4, data: 
0000: 02 00 00 00
[1678948109.847978] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x498B8BB5, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80

Response from the client when an unsuccessful attempt is made by the client to connect:

dell@GDS-T5810:~/quasor_ws$ ROS_DOMAIN_ID=0 ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888 -v6
[1678948068.808037] info     | UDPv4AgentLinux.cpp | init                     | running...             | port: 8888
[1678948068.808388] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6
[1678948069.340981] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 16, data: 
0000: 80 00 00 00 02 01 08 00 00 0A FF FD 02 00 00 00
[1678948069.341427] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0x00000000, len: 36, data: 
0000: 80 00 00 00 06 01 1C 00 00 0A FF FD 00 00 01 0D 58 52 43 45 01 00 01 0F 00 01 0D 00 01 00 00 00
0020: 00 00 00 00

Our test code follows below.

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){bool l = true; while(1){set_led_status(LED_RED, l = !l); vTaskDelay(pdMS_TO_TICKS(100));}}}

rclc_support_t support;
rcl_init_options_t init_options;
rcl_node_t node;
rcl_timer_t timer;
rclc_executor_t executor;
rcl_allocator_t allocator;
rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;
bool micro_ros_init_successful;

bool redLedOn = true;
bool blueLedOn = true;

enum states
    {
    WAITING_AGENT,
    AGENT_AVAILABLE,
    AGENT_CONNECTED,
    AGENT_DISCONNECTED
    } state;

// custom ROS domain ID used for the robot
#define CUSTOM_DOMAIN_ID    10
#define USE_CUSTOM_DOMAIN   false

bool create_entities();
void destroy_entities();
void test_microros_recon();

bool create_entities()
    {
    allocator = rcl_get_default_allocator();
    if (true == USE_CUSTOM_DOMAIN)
    {
    init_options = rcl_get_zero_initialized_init_options();
    // connect to agent, specifying options for custom domain, client key etc
    RCCHECK(rcl_init_options_init(&init_options, allocator));
    rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options);
    RCCHECK(rcl_init_options_set_domain_id(&init_options, CUSTOM_DOMAIN_ID));
    RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));
    }
    else
    {
    // connect to agent, without specifying options
    RCCHECK(temp_rc = rclc_support_init (&support, 0, NULL, &allocator));
    }
    // create node
    RCCHECK(rclc_node_init_default(&node, "my_renesas_node", "", &support));
    // create publisher
    RCCHECK(rclc_publisher_init_default(&publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "int_publisher"));
    return true;
    }

void destroy_entities()
    {
    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(&publisher, &node));
    RCCHECK(rcl_timer_fini(&timer));
    RCCHECK(rclc_executor_fini(&executor));
    RCCHECK(rcl_node_fini(&node));
    // clean up init_options ????
    RCCHECK(rclc_support_fini(&support));
    }

void test_microros_recon()
    {
    // Timeout for each ping attempt
    const int timeout_ms = 20;
    // Number of ping attempts
    const uint8_t attempts = 1;
    rcl_ret_t res;
    msg.data = 0;
    state = WAITING_AGENT;
    while (1)
    {
    switch (state)
        {
    case WAITING_AGENT:
        state = (RMW_RET_OK == rmw_uros_ping_agent(timeout_ms, attempts)) ? AGENT_AVAILABLE : WAITING_AGENT;
        if (WAITING_AGENT == state)
        {
        set_led_status(LED_RED, redLedOn = !redLedOn);
        vTaskDelay(pdMS_TO_TICKS(1000));
        }
        else
        {
        vTaskDelay(pdMS_TO_TICKS(100));
        }

        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();
        set_led_status(LED_GREEN, false);
        vTaskDelay(pdMS_TO_TICKS(1000));
        }
        else
        {
        set_led_status(LED_RED, false);
        set_led_status(LED_GREEN, true);
        }

        break;
    case AGENT_CONNECTED:
        res = rcl_publish(&publisher, &msg, NULL);
        msg.data++;
        // Ping agent if previous publish attempt gave error
        if (RCL_RET_OK != res)
        {
        state = (RMW_RET_OK == rmw_uros_ping_agent(timeout_ms, attempts)) ? AGENT_CONNECTED : AGENT_DISCONNECTED;
        if (AGENT_CONNECTED == state)
            {
            // Spin ROS executor only once and return immediately to process any incoming messages
            rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
            set_led_status(LED_GREEN, true);
            }
        else
            {
            blueLedOn = false;
            set_led_status(LED_BLUE, blueLedOn);
            set_led_status(LED_GREEN, false);
            }
        }
        else
        {
        set_led_status(LED_BLUE, blueLedOn = !blueLedOn);
        }
        vTaskDelay(pdMS_TO_TICKS(100));
        break;
    case AGENT_DISCONNECTED:
        destroy_entities();
        set_led_status(LED_GREEN, false);
        state = WAITING_AGENT;
        break;
    default:
        break;
        }
    }
    }

[pablogs9]

Probably you will need to use rcl_init_options_fini somewhere when using custom domains. Please read the RCL API documentation.

[gsokoll]

I suspected there might need to be a fini for the init_options (and had a comment in my code to that effect), but didn't find it when googling. Might have googled for rclc_init_options_fini by mistake.

In any case - I have added rcl_init_options_fini to my destroy_entities function now and it seems to be working reliably. Thanks.