How to use rmw_uros_ping_agent() and rmw_uros_sync_session() correctly ?

Dear all,

I am currently implementing micro-ros client on a RP2040 Pico board to publish the IMU data every 5ms (i.e. 200Hz).
Each time I need to publish the data, I check the connection of which transport layer is serial over USB knowing that its typical latency is reported here : https://docs.vulcanexus.org/en/iron/rst/microros_documentation/benchmarking/benchmarking.html#latency

However, measuring the time (in microsecs) taken by critical code in my implementation gives:

proc_imu() already took : 415
proc_baro() already took : 9
check_connection() already took : 2635
publish() already took : 1645
Whole loop() already took : 6709

As one could see, the whole loop() exceeded the expected 5ms and the most critical function is check_connection() which is implementated as below :
void check_connection(){
switch (uros_conn_curr_state) {
case WAITING_AGENT:
EXECUTE_EVERY_N_MS(5, uros_conn_curr_state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_AVAILABLE : WAITING_AGENT;);
break;
case AGENT_AVAILABLE:
uros_conn_curr_state = (true == create_entities()) ? AGENT_CONNECTED : WAITING_AGENT;
if (uros_conn_curr_state == WAITING_AGENT) {
destroy_entities();
};
break;
case AGENT_CONNECTED:
EXECUTE_EVERY_N_MS(500, uros_conn_curr_state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_CONNECTED : AGENT_DISCONNECTED;);
if (uros_conn_curr_state == AGENT_CONNECTED) {
rclc_executor_spin_some(&executor, 1000);
};
break;
case AGENT_DISCONNECTED:
destroy_entities();
uros_conn_curr_state = WAITING_AGENT;
break;
default:
break;
}
}

And the rmw_uros_sync_session() is call within create_entities() :
bool create_entities()
{

..

// create executor
executor = rclc_executor_get_zero_initialized_executor();
RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator));
RCCHECK(rclc_executor_add_service(&executor, &service_fps, &req_fps, &res_fps, service_fps_callback));
RCCHECK(rclc_executor_add_service(&executor, &service_exp, &req_exp, &res_exp, service_exp_callback));

RCSOFTCHECK(rmw_uros_sync_session(timeout_ms));

return true;
}

I put my doubt on the rmw_uros_ping_agent() function according to some issue reports, one of them is : https://github.com/micro-ROS/micro_ros_arduino/issues/506

To update, I also found cases that the check_connection() took less than 1ms but the publish() took > 6ms :
proc_imu() already took : 384
proc_baro() already took : 8
check_connection() already took : 953
publish() already took : 6375
Whole loop() already took : 9727

proc_imu() already took : 411
proc_baro() already took : 7
check_connection() already took : 986
publish() already took : 6668
Whole loop() already took : 10062

Thanks in advance for you advice / suggestion.
Khang

Beginne bei check_connection() und create_entities() und verfolge dann rmw_uros_ping_agent(), rmw_uros_sync_session(), publish() und rclc_executor_spin_some(). Reproduziere die gemeldeten Zeitmessungen auf dem RP2040 Pico mit serial-over-USB und grenze ein, ob der Ping, die Sitzungssynchronisierung, die Executor-Arbeit oder das Publizieren die Verzögerung verursacht. Als erledigt gilt die Aufgabe, wenn das Zeitverhalten und die korrekte API-Nutzung dokumentiert oder das zugrunde liegende Problem identifiziert sind.