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

Bắt đầu với ví dụ int32_publisher mở rộng và vòng lặp micro_ros_task, sau đó kiểm tra timer_callback_1, timer_callback_2 và lời gọi rclc_executor_spin_some trong khi tái hiện tình trạng mất agent. So sánh thời điểm callback và đầu ra lỗi publish khi agent được kết nối và khi bị ngắt kết nối; hoàn thành nghĩa là ghi lại hoặc xác nhận hành vi timer dự kiến và xác định ranh giới thực thi liên quan.

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);
}