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

Start at check_connection() and create_entities(), then trace rmw_uros_ping_agent(), rmw_uros_sync_session(), publish(), and rclc_executor_spin_some(). Reproduce the reported timings on the RP2040 Pico with serial-over-USB and isolate whether ping, session synchronization, executor work, or publishing causes the delay. Done means the timing behavior and correct API usage are documented or the underlying issue is identified.

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