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

check_connection() と create_entities() から開始し、続いて rmw_uros_ping_agent()、rmw_uros_sync_session()、publish()、rclc_executor_spin_some() を追跡します。serial-over-USB を使用する RP2040 Pico で報告されたタイミングを再現し、遅延の原因が ping、セッション同期、executor の処理、公開のいずれであるかを切り分けます。タイミングの挙動と正しい API の使用方法が文書化されるか、根本的な問題が特定されれば完了です。

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