Behavior of micro-ROS when losing the connection to the micro-ROS-agent

[tostbrot1]

Hello,

I am working on an application where I want to use micro-ROS on an ESP32-S3. In the application I need to realize a real-time controller with a cycle of 10 ms. As micro-ROS is advertised as a solution for real-time applications, I expected that I can use the micro-ROS timer for my controller and for understanding I firstly tested the behavior of micro-ROS a little bit.

• Hardware description: ESP32-S3
• RTOS: FreeRTOS
• Installation type: micro-ROS-ESP-IDF component in a PlatformIO project micro-ROS-agent on an Ubuntu 20.04
• ROS2 and MicroROS Verison: Galactic

Steps to reproduce the issue

For testing I expanded the int32_publisher example with a second timer. This timer only sets a led on or off at each callback. The timer 1 (publisher) is set to a cycle time of 1000 ms and the timer 2 (led) is set to a cycle time of 10 ms.

Then I tested my example and while the micro-ROS-agent was active it worked great. The led shines constantly so the timer callback seems to be fast and constant. Also the printf output is shown every second. After that I tested the behavior of the timer when losing the agent by disabling the Wi-Fi of my Ubuntu. But after the ESP32 lost the connection to the agent, micro-ROS did not behave like I expected. The printf output is still displayed every 1000 ms. But also it prints “Failed status on line 36: 1. Continuing” every 1000 ms. My conclusion is that the publisher cannot publish anymore because it needs an agent. Am I understanding this right? Also I noticed that the callback of my second timer does not work anymore with the right frequency after stopping the agent. The led blinks with a frequency of 1 Hz. This is way too slow for my application because the controller always needs to be called with a constant frequency. After I activated the Wi-Fi, the publisher and the second timer worked again correctly.

Expected behavior

The timer frequency is independend wether the micro-ROS-agent ist available or not. The timer 2 is called every 10 ms even if the micro-ROS-agent is not connected.

Actual behavior

The callback frequency of the timer 2 drops to 1 Hz instead of the wanted 100 Hz, while the agent is not connected .

Additional information

Did I do something wrong in my application? Is it possible to make the micro-ROS-timer work independently from the micro-ROS-agent or do I need to do a workaroud like a using a GPT Timer?

Thank you for your reply.

#include <string.h>
#include <stdio.h>
#include <unistd.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "esp_system.h"

#include <uros_network_interfaces.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <std_msgs/msg/int32.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#ifdef CONFIG_MICRO_ROS_ESP_XRCE_DDS_MIDDLEWARE
#include <rmw_microros/rmw_microros.h>
#endif

#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);vTaskDelete(NULL);}}
#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);}}

#include "driver/gpio.h"
#define led_green (gpio_num_t) 48
bool toogle_led_green = 0;

rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;

void timer_callback_1(rcl_timer_t * timer, int64_t last_call_time)
{
    RCLC_UNUSED(last_call_time);
    if (timer != NULL) {
        printf("Publishing: %d\n", msg.data); 
        RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
        msg.data++;
    }
}

void timer_callback_2(rcl_timer_t * timer, int64_t last_call_time)
{
    RCLC_UNUSED(last_call_time);
    if (timer != NULL) {
        gpio_set_level(led_green, toogle_led_green);
        toogle_led_green = !toogle_led_green;
    }
}

void micro_ros_task(void * arg)
{
    rcl_allocator_t allocator = rcl_get_default_allocator();
    rclc_support_t support;

    rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
    RCCHECK(rcl_init_options_init(&init_options, allocator));

#ifdef CONFIG_MICRO_ROS_ESP_XRCE_DDS_MIDDLEWARE
    rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options);

    // Static Agent IP and port can be used instead of autodisvery.
    RCCHECK(rmw_uros_options_set_udp_address(CONFIG_MICRO_ROS_AGENT_IP, CONFIG_MICRO_ROS_AGENT_PORT, rmw_options));
    //RCCHECK(rmw_uros_discover_agent(rmw_options));
#endif

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

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

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

    // create timer,
    rcl_timer_t timer_1;
    const unsigned int timer_timeout_1 = 1000;
    RCCHECK(rclc_timer_init_default(
        &timer_1,
        &support,
        RCL_MS_TO_NS(timer_timeout_1),
        timer_callback_1));

    rcl_timer_t timer_2;
    const unsigned int timer_timeout_2 = 10;
    RCCHECK(rclc_timer_init_default(
        &timer_2,
        &support,
        RCL_MS_TO_NS(timer_timeout_2),
        timer_callback_2));

    // create executor
    rclc_executor_t executor;
    RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator));
    RCCHECK(rclc_executor_add_timer(&executor, &timer_1));
    RCCHECK(rclc_executor_add_timer(&executor, &timer_2));

    msg.data = 0;

    while(1){
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(1));
        //usleep(10000);
    }

    // free resources
    RCCHECK(rcl_publisher_fini(&publisher, &node));
    RCCHECK(rcl_node_fini(&node));

    vTaskDelete(NULL);
}

void app_main(void)
{
    //Initialize LED
    gpio_pad_select_gpio(led_green);
    gpio_set_direction(led_green, GPIO_MODE_OUTPUT);
    gpio_set_level(led_green, 0);

#if defined(CONFIG_MICRO_ROS_ESP_NETIF_WLAN) || defined(CONFIG_MICRO_ROS_ESP_NETIF_ENET)
    ESP_ERROR_CHECK(uros_network_interface_initialize());
#endif

    //pin micro-ros task in APP_CPU to make PRO_CPU to deal with wifi:
    xTaskCreate(micro_ros_task,
            "uros_task",
            CONFIG_MICRO_ROS_APP_STACK,
            NULL,
            CONFIG_MICRO_ROS_APP_TASK_PRIO,
            NULL);
}

[pablogs9]

But also it prints “Failed status on line 36: 1. Continuing” every 1000 ms.

You are printing it here #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);}}

Also, depending on the task priority design you have, maybe it is important to have a sleep in every loop, so the micro-ROS task allows low-priority tasks to execute.

Regarding the bad timing of the second timer, I will try to find a slot to take a look, but in the meanwhile can you check if keeping your code as it is and adding only the LED timer to the executor also fails depending on the agent status?

[tostbrot1]

Thanks for your fast reply. When I am adding only the LED timer to the executor, the LED timer does not fail. The LED does not start to blink when the connection to the agent is lost. So I think the Publisher causes the LED timer to fail.

[pablogs9]

Yes, it seems that somewhere inside our RMW is waiting too long when dealing with DDS entities. Let me check