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

Adding a timestamp to debug messages #61

Merged
merged 14 commits into from
Jul 5, 2023

Conversation

SejalBehere
Copy link
Contributor

Changes were made to Debug.c to include a timestamp (Format - Day Y:M:D H:M:S.ms) in front of the debug messages being printed. Change was made to MotoROS_PlatformLib.h to add the function localtime_r. These changes were "Fixes #38" to resolve the issue "Add timestamp to debug udp broadcasts" .

Copy link
Collaborator

@gavanderhoorn gavanderhoorn left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for the PR 👍

I've left a couple in-line comments.

Besides those: IIUC, this returns time in 'local time'. Many controllers will not be configured with the correct timezone (if that is even possible), and will likely not have NTP setup.

This means stamps prefixed to debug output would not necessarily show the correct time, when compared to the time of the receiving systems.

In a previous version I believe you used the time from the host, synced using the micro-ROS Agent. That would seem to avoid the timezone issue.

Did that not work correctly?

src/Debug.c Outdated
@@ -36,6 +36,7 @@ void Ros_Debug_Init()
void Ros_Debug_BroadcastMsg(char* fmt, ...)
{
char str[MAX_DEBUG_MESSAGE_SIZE];
char Formatted_Time[300];
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please use a named constant (ie: #define) for the buffer size instead of a magic nr.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I will make that change

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It doesn't appear that Formatted_Time is used in the code below. Looks like a leftover artifact from debugging. I recommend removing this unused variable.
(If you do want to use it for something, be sure to replace the 300 with FORMATTED_TIME_SIZE.)

src/Debug.c Outdated Show resolved Hide resolved
src/Debug.c Outdated Show resolved Hide resolved
src/Debug.c Outdated Show resolved Hide resolved
src/Debug.c Outdated Show resolved Hide resolved
src/Debug.c Outdated Show resolved Hide resolved
src/Debug.c Show resolved Hide resolved
src/Debug.c Outdated Show resolved Hide resolved
src/Debug.c Outdated Show resolved Hide resolved
src/Debug.c Outdated Show resolved Hide resolved
@ted-miller
Copy link
Collaborator

In a previous version I believe you used the time from the host, synced using the micro-ROS Agent. That would seem to avoid the timezone issue.

gavanderhoorn is right, we have used another time function that is synchronized with the PC running the microROS Agent.

INT64 theTime = rmw_uros_epoch_nanos();
Ros_Nanos_To_Time_Msg(theTime, &g_messages_PositionMonitor.jointStateAllGroups->header.stamp);

rmw_uros_epoch_nanos gets nanoseconds. Then Ros_Nanos_To_Time_Msg will break that into seconds and remaining nanoseconds.

@gavanderhoorn
Copy link
Collaborator

gavanderhoorn commented Jun 15, 2023

I was looking at Yaskawa-Global:9f1c599...SejalBehere:7f163ac.

That seems to do what you also suggest @ted-miller, but there must be a reason this didn't make it into this PR. Hence my question.

@SejalBehere
Copy link
Contributor Author

SejalBehere commented Jun 15, 2023

In a previous version I did use time from the host, synced using the micro-ROS Agent. I was able to get the timestamp in the format of H:M:S.ms but was not able to add the day to it without adding the whole of what I have used now as well.

I will make some more changes to the previous version and get the current timestamp format using the host time.

src/Debug.c Outdated
builtin_interfaces__msg__Time debug_msg_timestamp;

Ros_Nanos_To_Time_Msg(nanosecs, &debug_msg_timestamp);
time(&debug_msg_timestamp);
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

IIUC, the time function will put the current time into debug_msg_timestamp. But, this is not needed because the current timestamp was already obtained above with rmw_uros_epoch_nanos. I think that this line can be deleted.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I did try by removing that line but what is happening is that for the fist few messages, before the message "Attempting to connect to the micro-ROS agent", the timestamp being showed is the epoch time (THU 1970-01-01 00:00:00.000) and it is only after this message that it shows the current timestamp.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I see. Then I would suggest making the time source conditional on whether the agent is connected.

if (g_Ros_Communication_AgentIsConnected)
{
    //get synchronized time from the agent
    int64_t nanosecs = rmw_uros_epoch_nanos();
    Ros_Nanos_To_Time_Msg(nanosecs, &debug_msg_timestamp);
}
else
{
    //rmw_uros_epoch_nanos cannot sync with agent because it's not connected
    time(&debug_msg_timestamp);
}

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Okay, I will replace the time function with the conditional

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not sure you can pass an instance of builtin_interfaces__msg__Time to time(..).

@gavanderhoorn
Copy link
Collaborator

@SejalBehere: if this is ready for another review please either request one or comment.

@SejalBehere
Copy link
Contributor Author

@gavanderhoorn This is ready for another review.

src/Debug.c Outdated Show resolved Hide resolved
src/Debug.c Show resolved Hide resolved
src/Debug.c Outdated Show resolved Hide resolved
@SejalBehere
Copy link
Contributor Author

I have made the changes to address the points raised in the above review. Also, I have removed the +1 as like @ted-miller said str buffer is getting zeroed at the top and hence there's no need for considering the null terminator.

Copy link
Collaborator

@ted-miller ted-miller left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The only other question I have is to confirm whether you have a tested this on a live controller. If so, please post an example copy/paste of the output so we can see the final product.
Thanks!

src/Debug.c Outdated
@@ -36,6 +36,7 @@ void Ros_Debug_Init()
void Ros_Debug_BroadcastMsg(char* fmt, ...)
{
char str[MAX_DEBUG_MESSAGE_SIZE];
char Formatted_Time[300];
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It doesn't appear that Formatted_Time is used in the code below. Looks like a leftover artifact from debugging. I recommend removing this unused variable.
(If you do want to use it for something, be sure to replace the 300 with FORMATTED_TIME_SIZE.)

@ted-miller
Copy link
Collaborator

Also, I owe you an updated version of MotoROS_PlatformLib.h

@SejalBehere
Copy link
Contributor Author

I have made the changes to the branch. I did test this on a live controller and the output was obtained in the following format:

[1687983037.99751401] [192.168.1.31:58649]: WED 2023-06-28 16:10:25.768 Using ROS domain ID: 0
[1687983037.99761701] [192.168.1.31:58649]: WED 2023-06-28 16:10:25.768 Using client key: 0x73B9D4ED
[1687983037.99765611] [192.168.1.31:58649]: WED 2023-06-28 16:10:25.768 Attempting to connect to Micro-ROS PC Agent (at udp://192.168.1.50:8888)
[1687983039.01968718] [192.168.1.31:58649]: THU 1970-01-01 00:00:00.000 Found Micro-ROS PC Agent
[1687983039.02444792] [192.168.1.31:58649]: WED 2023-06-28 16:10:26.795 rclc_support_init_with_options = 0
[1687983039.02451229] [192.168.1.31:58649]: WED 2023-06-28 16:10:26.795 remap_rules str: ''
[1687983039.02464890] [192.168.1.31:58649]: WED 2023-06-28 16:10:26.795 len(remap_rules str): 0
[1687983039.02472425] [192.168.1.31:58649]: WED 2023-06-28 16:10:26.795 num parsed remap rules: 0

ted-miller
ted-miller previously approved these changes Jun 30, 2023
@ted-miller
Copy link
Collaborator

Looks good to me. Thanks for the contribution!

I'll work on resolving the conflicts this afternoon and get this merged.

Copy link
Collaborator

@gavanderhoorn gavanderhoorn left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

See my comment.

@ted-miller ted-miller dismissed gavanderhoorn’s stale review July 5, 2023 15:10

Header updated with just the changes to localtime_r

@ted-miller ted-miller merged commit 9f68b48 into Yaskawa-Global:main Jul 5, 2023
@gavanderhoorn gavanderhoorn mentioned this pull request Jun 29, 2023
9 tasks
@gavanderhoorn
Copy link
Collaborator

Building this for DX200 I get several warnings, with one sticking out:

src/Debug.c:63: warning: passing argument 1 of 'localtime_r' from incompatible pointer type

that would be this line.

@SejalBehere: could you confirm the YRC build doesn't warn on this?

@ted-miller
Copy link
Collaborator

I can confirm that YRC does raise a warning. It's wanting a time_t instead of int32_t. I guess a simple explicit cast should resolve it.

@ted-miller
Copy link
Collaborator

@gavanderhoorn, I'm also getting a warning for gettimeofday. Is that not defined somewhere? (It seems to work/run...)

@gavanderhoorn
Copy link
Collaborator

gavanderhoorn commented Jul 10, 2023

Not on DX200.

See #18 (comment).

gavanderhoorn added a commit to gavanderhoorn/motoros2 that referenced this pull request Aug 17, 2023
PR Yaskawa-Global#61 added a timestamp to the log message sent out by MotoROS2. Use that timestamp instead of calculating a new one upon reception.

The format of the timestamp prefixed by MotoROS2 is exactly the same as what `debug_listener.py` used to prefix. But due to limitations on the VxWorks side, will likely not take time-zones into account.
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

Successfully merging this pull request may close these issues.

Add timestamp to debug udp broadcasts
3 participants