rmw_uros_ping_agent is acting as a delay in the code

[B00M3RSIX]

OK, so i tried to implement the implement the example (modified micro-ros_reconnection_example.ino) in my code and faced some weird behaviour as my code took a lot more executiontime than before. Then I was playing around with the example itself and could reproduce it.
I found out that rmw_uros_ping_agent is acting as a delay in the code.

Setup

• Hardware description: teensy 4.0
• RTOS: none
• Installation type: PlatformIO as IDE, uROS Agent running in Docker on XavierNX, teensy connected via USB-serial (/dev/ttyACM0)
• Version or commit hash: galactic 2.0.1

Steps to reproduce the issue

modified micro-ros_reconnection_example.ino
(see code listing below)

Expected behavior

Returning with RMW_RET_OK==true from rmw_uros_ping_agent as fast as possible, when ping succeeded.
Wait for timeout if Ping is not successful and retry the given amount of attemts and return with RMW_RET_OK==false.

Actual behavior

Wether Ping is successful or not, rmw_uros_ping_agent is blocking the code the full timeoutperiod and is acting as a delay.
if (RMW_RET_OK == rmw_uros_ping_agent(500, 2)) --> Loop takes alway ~500 ms if Ping is ok
if (RMW_RET_OK == rmw_uros_ping_agent(10, 2)) --> Loop takes alway ~10 ms also if Ping is ok
if (RMW_RET_OK == rmw_uros_ping_agent(1, 2)) --> Loop takes alway ~1 ms also if Ping is ok

Additional information

#include <micro_ros_arduino.h>
#include <Arduino.h>                                   //<--------------- Added (for PlatformIO)
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rmw_microros/rmw_microros.h>

#include <std_msgs/msg/int32.h>

#define LED_PIN 13
#define ROS_LED_PIN 23                                          //<--------------- Added
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){return false;}}

rclc_support_t support;
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;

elapsedMicros tc; // ADDED TO MEASURE CYCLETIME OF LOOP INC MICROS

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
	(void) last_call_time;
	if (timer != NULL) {
		rcl_publish(&publisher, &msg, NULL);
		msg.data++;
	}
}

// Functions create_entities and destroy_entities can take several seconds.
// In order to reduce this rebuild the library with 
// - RMW_UXRCE_ENTITY_CREATION_DESTROY_TIMEOUT=0
// - UCLIENT_MAX_SESSION_CONNECTION_ATTEMPTS=3

bool create_entities()
{	
	allocator = rcl_get_default_allocator();

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

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

	// create publisher
	RCCHECK(rclc_publisher_init_best_effort(
		&publisher,
		&node,
		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
		"std_msgs_msg_Int32"));

	// create timer,
	const unsigned int timer_timeout = 1;       //<--------------- Modified
	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, 1, &allocator));
	RCCHECK(rclc_executor_add_timer(&executor, &timer));

	micro_ros_init_successful = true;
}

void destroy_entities()
{
	rcl_publisher_fini(&publisher, &node);
	rcl_node_fini(&node);
	rcl_timer_fini(&timer);
	rclc_executor_fini(&executor);
	rclc_support_fini(&support);

	micro_ros_init_successful = false;
}

void setup() {
  Serial4.begin(115200);                                  //<--------------- Added
  set_microros_transports(); 
  pinMode(LED_PIN, OUTPUT);
  pinMode(ROS_LED_PIN, OUTPUT);              //<--------------- Added
  digitalWrite(LED_PIN, HIGH);  
  digitalWrite(ROS_LED_PIN, LOW);                //<--------------- Added

  micro_ros_init_successful = false;
  msg.data = 0;
}

void loop() {
   uint32_t delay = 100000;
  if (RMW_RET_OK == rmw_uros_ping_agent(1, 2))                          //<--------------- Modified
  {
    delay = 500000;
    if (!micro_ros_init_successful) {
      create_entities();
      digitalWrite(ROS_LED_PIN, HIGH);               //<--------------- Added
      Serial4.println("Create");                                                          //<--------------- Added
    } else {
      rclc_executor_spin_some(&executor, RCL_MS_TO_NS(1));     //<--------------- Modified
    }
  } 
  else if (micro_ros_init_successful)
  {
    digitalWrite(ROS_LED_PIN, LOW);               //<--------------- Added
    destroy_entities();
    Serial4.println("Destroy");                                      //<--------------- Added
  }
  digitalWrite(LED_PIN, !digitalRead(LED_PIN));
  //delayMicroseconds(delay);                           <--------------- removed

  Serial4.println(tc);                           //<--------------- Added
  tc = 0;                                            //<--------------- Added

}

[B00M3RSIX]

Right know im doing rmw_uros_ping_agent(1, 10) which gives me also a timeout of 10ms as a workaround but keeps the times low.

I continued playing around taking some metrics over serial... and found out something weired:
rmw_uros_ping_agent(1, 10):
If I use 1ms timeout I will get looptimes smaller than 1 millisecond... ranging from 22 micros to usually 150-350 micros. It seems the low 23 micros can only be achieved if at least one ping-attempt has been made. However this is a guess... the looptime went to this low number directly after it showed a looptime of ~2500 micros so i guess a new attemt has been made.
rmw_uros_ping_agent(5, 10):
If I use 5ms it shows looptimes close to 5000 micros but alway less (usually 48xx to 49xx).
Also here it went down to once to 2xx micros after I measured a time of more than 5000 micros.

Well, if i concider that any looptime over the specified timeout is a new ping attempt, then I also must say the Ping itself doesn't seem work from time to time, as the uROS agent is running all the time.

Here the code how i did the metrics:
Looptime is measuerd without the Serial4 prints (exept when creating or destroying entities - which i don't care in the metric for looptime.

• C: Counter for create_entities() calls
• D: Counter for destroy_entities() calls
• T_STATE: Loop is currently taking more time than a specified theshold
• T: Counter how often the threshold exceeded
• T_MAX: last Looptime when exeeding threshold
• LOOP: current Looptime (without serial prints)

#include <micro_ros_arduino.h>
#include <Arduino.h>                                   
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rmw_microros/rmw_microros.h>

#include <std_msgs/msg/int32.h>

#define LED_PIN 13
#define ROS_LED_PIN 23                                          
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){return false;}}

rclc_support_t support;
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;

elapsedMicros tc; // ADDED TO MEASURE CYCLETIME OF LOOP INC MICROS
int32_t c_count =-1;      //Counter for create_entities() calls
int32_t d_count =0;       //Counter for destroy_entities() calls
elapsedMillis ros_led_blink = 0;   //let the additional led blink if trying to connect (after entities got destroyed)
int32_t t_count =-1;      //Counting how often the looptime went over the a specified timeout
uint32_t tcval_max = 0;   //Looptime when over the timeout
bool t_count_state =false;//Helpingmarker

bool micro_ros_is_destroying; //Destroying takes forever 
bool micro_ros_OK;            //!micro_ros_is_destroying && micro_ros_init_successful

//
const int32_t threshold = 1000;//[micros] usually how much the Pingtimeout is 
const int32_t ping_t = 1;//[millis] Pingtimeout



void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
	(void) last_call_time;
	if (timer != NULL) {
		rcl_publish(&publisher, &msg, NULL);
		msg.data++;
	}
}

// Functions create_entities and destroy_entities can take several seconds.
// In order to reduce this rebuild the library with 
// - RMW_UXRCE_ENTITY_CREATION_DESTROY_TIMEOUT=0
// - UCLIENT_MAX_SESSION_CONNECTION_ATTEMPTS=3

bool create_entities()
{	
	allocator = rcl_get_default_allocator();

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

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

	// create publisher
	RCCHECK(rclc_publisher_init_best_effort(
		&publisher,
		&node,
		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
		"std_msgs_msg_Int32"));

	// create timer,
	const unsigned int timer_timeout = 1;      
	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, 1, &allocator));
	RCCHECK(rclc_executor_add_timer(&executor, &timer));

	micro_ros_init_successful = true;
  return true;
}

void destroy_entities()
{
	rcl_publisher_fini(&publisher, &node);
	rcl_node_fini(&node);
	rcl_timer_fini(&timer);
	rclc_executor_fini(&executor);
	rclc_support_fini(&support);

	micro_ros_init_successful = false;
}

void setup() {
  Serial4.begin(115200);                                  
  set_microros_transports(); 
  pinMode(LED_PIN, OUTPUT);
  pinMode(ROS_LED_PIN, OUTPUT);              
  digitalWrite(LED_PIN, HIGH);  
  digitalWrite(ROS_LED_PIN, LOW);                

  micro_ros_init_successful = false;
  msg.data = 0;
}

void loop() {
  if (RMW_RET_OK == rmw_uros_ping_agent(ping_t, 10))
  {
    if (!micro_ros_init_successful && !micro_ros_is_destroying) {
      Serial4.println("Create ...");
      if ( create_entities()) {
        c_count ++;
        Serial4.print("Create\t");
        Serial4.println(c_count);
      }      
    } else {
      rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));   
    }
  } 
  else if (micro_ros_init_successful)
  {
    micro_ros_is_destroying = true;
    d_count++;
    Serial4.println("Destroying ...");
    digitalWrite(ROS_LED_PIN, LOW);
    destroy_entities();
    Serial4.print("Destroyed\t");
    Serial4.println(d_count);
    micro_ros_is_destroying = false;
  }

  micro_ros_OK = !micro_ros_is_destroying && micro_ros_init_successful;

  if (micro_ros_OK) {
    digitalWrite(ROS_LED_PIN, HIGH);
  }
  else {
    if (ros_led_blink > 500) {
      digitalWrite(ROS_LED_PIN, !digitalRead(ROS_LED_PIN));
      ros_led_blink = 0;
    }
  }
  uint32_t tcval = tc;
  
  if ((tcval > threshold) &! t_count_state){
    t_count++;
    t_count_state = true;
  } else {
    t_count_state = (tcval > threshold);
  }
  if (t_count_state && (t_count > 0)){
    tcval_max = tc;
  }

  Serial4.print("C: ");
  Serial4.print(c_count);
  Serial4.print("\tD: ");
  Serial4.print(d_count);
  Serial4.print("\tT_STATE: ");
  Serial4.print(t_count_state);
  Serial4.print("\tT: ");
  Serial4.print(t_count);
  Serial4.print("\tT_MAX: ");
  Serial4.print(tcval_max);
  Serial4.print("\tLOOP: ");
  Serial4.println(tcval);
  digitalWrite(LED_PIN, !digitalRead(LED_PIN));

  tc = 0;

}

[Acuadros95]

Hello @B00M3RSIX,

This bug was fixed on the latest XRCE-Client version (eProsima/Micro-XRCE-DDS-Client#267), but this patch was not included in the 2.0.1 release.

I have bumped the release with the latest changes, download the v2.0.2 and give it a try.

[B00M3RSIX]

Hello @Acuadros95,

indeed this solved the issue. Thanks a lot!