Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Cannot sync accurate time #131

Open
Pansamic opened this issue Nov 17, 2023 · 8 comments
Open

Cannot sync accurate time #131

Pansamic opened this issue Nov 17, 2023 · 8 comments

Comments

@Pansamic
Copy link

Pansamic commented Nov 17, 2023

Detailed Description

I use rmw_uros_sync_session() to sync time with Micro-ROS-Agent. But when I use rmw_uros_epoch_millis() and rmw_uros_epoch_nanos() to get time, I got seconds starting from 0, not 170xxxxxx (Linux timestamp seconds) as expected.

Thinking

This result was no surprise because I cannot find any API that can change local time of MCU, while there is an API to get time called clock_gettime() in micro_ros_stm32cubemx_utils/extra_sources/microros_time.c.

I want Micro-ROS to set local time that high accuracy hardware timer ( DWT, TIM or RTC ) holds, rather than FreeRTOS ticks.

it's said that rmw_uros_sync_session() uses NTP to sync time so it must has high accurate and less latency, but it cannot function properly, pity.

Trials

My clock_gettime() is now modified to set RTC time. Micro-ROS subscribes /time_reference topic from Micro-ROS-Agent and transform timestamp to RTC Binary format. I use the following code to get current timestamp and it works well (superficially). This function seems to call clock_gettime() and get time from RTC. Actually this method introduces huge latency.

rcutils_time_point_value_t current_time;
rcutils_ret_t ret;
ret = rcutils_system_time_now(&current_time);
if(ret != RCUTILS_RET_OK)
    printf("[ERROR]read systime failed!\r\n");
msg->header.stamp.sec = RCUTILS_NS_TO_S(current_time);
msg->header.stamp.nanosec = (uint32_t)current_time;

Question

  1. How does Micro-ROS set time?
  2. Can maintainers expose some API to set local time?
@pablogs9
Copy link
Member

How does Micro-ROS set time?

rmw_uros_sync_session will not change your local time it just stores a time offset of the XRCE session that is used when rmw_uros_epoch_nanos and rmw_uros_epoch_millis are called.

Can maintainers expose some API to set local time?

Out of micro-ROS scope.

@bnbhat
Copy link

bnbhat commented Oct 1, 2024

Hi @Pansamic,

Were you able to find a solution for the time synchronization issue? I'm facing a similar challenge, as I need to timestamp all the messages published by a micro-ROS node running on an MCU.

@pablogs9, could you please advise on the recommended approach for timestamping messages within a micro-ROS node?

Best Regards

@bnbhat
Copy link

bnbhat commented Oct 2, 2024

If anyone stumbles upon this issue looking for a way to timestamp messages in micro-ROS, here's a solution that worked for me:

  1. Synchronize the Time with the Agent
#include <rmw_microros/rmw_microros.h>

// Sync timeout
const int timeout_ms = 1000;

// Synchronize time with the agent
rmw_uros_sync_session(timeout_ms);
  1. Get the Synchronized Time
#include <rmw_microros/rmw_microros.h>

// Get corrected epoch time 
int64_t time_ns = rmw_uros_epoch_nanos();

// Convert to required format
msg.header.stamp.sec = (int32_t)(time_ns / 1000000000);
msg.header.stamp.nanosec = (uint32_t)(time_ns % 1000000000);

@l4es
Copy link

l4es commented Nov 21, 2024

Hi @bnbhat,

Does one need to call rmw_uros_sync_session() everytime prior rmw_uros_epoch_nanos() for filling the timestamp of the topic being published ?

In the following example, rmw_uros_sync_session() is called once when the agent is available : https://github.com/adityakamath/akros2_firmware/blob/akros2_humble/akros2_firmware.ino#L331

And the time offset is calculated for being used later (filling the timestamp of published topics) : https://github.com/adityakamath/akros2_firmware/blob/akros2_humble/akros2_firmware.ino#L554

@bnbhat
Copy link

bnbhat commented Nov 21, 2024

Hi @l4es

You don't need to call rmw_uros_sync_session() every time as it stores the offset. But you can call it once in a while to sync time if you want to.

When you call rmw_uros_epoch_nanos(), it adds the stored offset to the system time and gives you the synced time.

@flux0-0
Copy link

flux0-0 commented Nov 28, 2024

hi @bnbhat , If I pio run my code same you about rmw_uros_sync_session(timeout_ms) and int64_t time_ns = rmw_uros_epoch_nanos();, I have error "collect2: error: ld returned 1 exit status" . Have you get similar problem before?

@bnbhat
Copy link

bnbhat commented Nov 28, 2024

Hi @l4es , could you share your code? or more details on your setup?

have you synced the time before calling rmw_uros_epoch_nanos() ?

@flux0-0
Copy link

flux0-0 commented Nov 28, 2024

This is my code " https://github.com/flux0-0/test_microros_time/blob/main/main.cpp " . I tried "pio run " in platform IO and I have " collect2: error: ld returned 1 exit status
*** [.pio/build/esp32doit-devkit-v1/firmware.elf] Error 1"

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

5 participants