Estimate gsof header.stamp based on PostitionTime or INSSolution

[zthorson]

Description

This is an optional functionality improvment to add correct timestamp values to some ROS Gsof Messages that currently write 0,0 for the timestamp. It adds an additional flag to enable the feature, which defaults to off to maintain compatability with other releases.

Reason

In the current code, any Gsof ROS message that does not have a GPSTime field in the structure will not write a timestamp value to the header. In these cases the timestamp is always written as 0,0 in the GSOF message header. If you attempt to use a TimeSynchronized message filter to group GSOF messages with NavFix (or INS) messages, this will cause them to fail since the GSOF message timestamp will never match.

Method / Design

To address this a new parameter estimate_gsof_time was added to the ros parameters list. This defaults to false in order to retain the same default behavior.

When estimate_gsof_time is set, the callback will use either the GSOF PositionTime (#1) gps time, or the INSSolution (#49) time as an estimate for other packets (Such as Velocity (#8) or Attitude (#27)). It has a preference for using the INS message, as that use case is likely more common.

To get the best results, you would want the PositionTime (#1) or INSSolution (#49) message to have the highest update rate of all messages. Ideally, it would also be a multiple of your lower frequency messages so that there is a good chance than GSOF will bundle the messages together on different pages of the same packet. This would give the most accurate stamp. Though in the worst case the timestamp would be off by 1/Hz s.

There exists one GSOF message Attitude (#27) that contains a special case. This packet has a gps_time_ms field with no associated week. To get the best timestamp, this packet will only use the GPSWeek from the PositionTime (#1) or INSSolution (#49) packet and use it's own gps_time_ms. There exists a small chance of some odd behavior during the gps week rollover, but if the packets are synced well this should not occur.

Testing

Test Setup:

• ros:humble-ros-base Docker image
  • ROS2 Humble
  • Ubuntu 22.04
  • Foxglove Studio for Packet visualization
  • BX992-INS GPS Units x 3

** GPS Configuration **

---

estimate_gsof_time = false

Packets with no valid GPS time have a heading timestamp of 0,0:

---

estimate_gsof_time = true

Packets with no valid GPS time now estimate their time based on PositionTime (#1)

[andre-nguyen]

Thank you for the feature, looks pretty important for products that don't output gsof 49.

I tried it out on my unit and something doesn't seem to work for me. If I set the rate of gsof 1 to 1Hz and gsof 27 to 5Hz. My attitude info timestamp doesn't follow the time of week.

I'm thinking there is a problem where the template isn't being resolved properly?

[zthorson]

I tried it out on my unit and something doesn't seem to work for me. If I set the rate of gsof 1 to 1Hz and gsof 27 to 5Hz. My attitude info timestamp doesn't follow the time of week.

I'm thinking there is a problem where the template isn't being resolved properly?

I think I spot a problem with the template, the message I was trying to target uses a different name for the field:

trimble_driver_ros/trimble_driver_ros/include/trimble_driver/gsof/message.h

Lines 768 to 770 in ae97136

	struct AttitudeInfo {
	Header header;
	uint32_t gps_time;

Since I'm really only trying to target Attitude Info (the only one that has a gps time in ms but no week info), I think it may make more sense to just write a specific template override for that particular message and avoid adding a special templating case. I'll look into it.

[andre-nguyen]

Since I'm really only trying to target Attitude Info (the only one that has a gps time in ms but no week info), I think it may make more sense to just write a specific template override for that particular message and avoid adding a special templating case. I'll look into it.

I wouldn't be against renaming the field to gps_time_ms, but I can confirm that this is the only message that doesn't include the gps week. A template specialization would make sense.

[zthorson]

Since I'm really only trying to target Attitude Info (the only one that has a gps time in ms but no week info), I think it may make more sense to just write a specific template override for that particular message and avoid adding a special templating case. I'll look into it.

I wouldn't be against renaming the field to gps_time_ms, but I can confirm that this is the only message that doesn't include the gps week. A template specialization would make sense.

There are a few others that have specialized names and out of order fields. I ended up adding them to the specialized converter list as well, since I saw they were using the time estimate rather than their included time fields.

I still have more work on this PR for cleanup, but feel free to take a look at the new template approach. Otherwise I should have access to the hardware again late next week for more testing.

[andre-nguyen]

So last minor comments.

Thank you for your patience.

edit: Also tested against GSOF 1, 2, 16 but not 41 as I don't have a base station connection right now.

[andre-nguyen]

Can confirm GSOF 41 is functional on my end.

Have you had a chance to test this PR on your own hardware?