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

[l4es]

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 : micro-ROS/micro_ros_arduino#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

[hippo5329]

You may follow this re-connection example.

https://github.com/hippo5329/micro_ros_arduino_examples_platformio/blob/main/micro-ros_reconnection_example/micro-ros_reconnection_example.ino

You should increase the i2c clock to the maximum supported by your IMU, eg, 400K or 1MHz.
Wire.setClock(400000);

You may try low-latency kernel.

https://github.com/hippo5329/micro_ros_arduino_examples_platformio/wiki#install-ubuntu-2404-and-low-latency-kernel

[l4es]

Hi @hippo5329,

Thanks for your suggestion.

I think my check_connection() function would be the same as the micro-ros reconnection example that you shared.

By the way, there should be no worry about the I2C and SPI interfaces as they are at 500Khz and 16MHz respectively.

And the reading time of the data from the IMU is hundreds of microseconds (<500us), while the conversion time of pressure and temperature (ADC conversion) of the barometer takes longer time (depending on the Oversamplingrate/resolution) but is optimized by using timer interrupt. Therefore, data from barometer is read from its internal registers when the conversion is done, and this reading takes only some microseconds (< 10us).

What I am worrying about is the way I do the pinging to the agent (repeatedly, sometime not necessarily !?!?) and the following publishing that usually take huge delay compared to the typical latency of the selected transport layer : which is serial over USB. I wonder if it could come from the agent running on the Linux side as the USB driver in particular and the kernel in general are not real-time?

Best Regards,
Khang

[hippo5329]

You may remove the re-connection check to see if it is the issue.