LGSVL ImuSensor updates getting lost or duplicated

[henrychoi]

I am noticing the time stamp of the ImuSensor streams (bridged through ros2-lgsvl-bridge foxy-devel branch; same result with the apt package ros-foxy-lgsvl-bridge) are duplicated or jumps forward when received on the bridge's node. In LGSVL simulator ImuSensor.cs, I inserted the following line:

UnityEngine.Debug.Log($"IMU {time}");

I confirmed that the time increments by 10 ms pretty consistently. In ros2-lgsvl-bridge node.cc publish() method, I inserted the following code to print the received timestamp.

if (topic.find("imu") != std::string::npos) {
    const uint32_t* p = (const uint32_t*)data.data();
    DEBUG(boost::str(boost::format("imu@%u.%09u") % p[2] % p[1]));
}

What I see is a lot of duplicated timestamps (up to 10 times easily) of a given timestamp, and sometimes a jump of 1~10 samples.

It's not obvious what the problem between the simulator and the bridge is.

[luqiang21]

@henrychoi It should be a bug in Simulator IMU sensor. Because ImuSensor has ImuData data as member of the class, and when publish is called it will send whatever latest is written into ImuData data class member. That is why you are seeing duplicated timestamps. In ImuSensor.cs, make data a local variable like below and it should fix your problem. These fixes will be available in next release.

[henrychoi]

Thanks for the quick comment @luqiang21 , could you possibly explain what latestData above is used for? I have not yet understood your explanation what the data is being trashed in MT sense, because it is being sent into a message queue, to the publish thread. I would have guessed that what is passed into a message queue is copied, and therefore immune from further changes in the FixedUpdate(). I can see perhaps the data no longer being a class member variable would help, but what is the purpose of the latestData?

[luqiang21]

@henrychoi OnVisualize() is still using global variable data. So I replaced the name data to be latestData for this purpose.

[henrychoi]

Thanks @luqiang21 , I think using a local variable called var imu = new ImuData() is slightly fewer lines of change. Instead of latestData = data, you could just do data = imu, and then later Publish(imu) instead of Publish(data). Tried out this change, and the dups are gone!

HOWEVER, some occasional drops are still there; the worst I've seen so far is 7 consecutive drops (70 ms). Any insight on this?

[luqiang21]

@henrychoi Glad to hear the duplicates are gone. I am not sure about the drops. Maybe because the fps is too low, so some messages were skipped?

[henrychoi]

@luqiang21 I don't think so; the nominal time difference is 10 ms exactly, so the fixed rate must be 100 Hz. I am not sure if FixedUpdate() sometimes doesn't get called?

[luqiang21]

@henrychoi Sorry I don't know much about this. @EricBoiseLGSVL or @hadiTab do you know?

[EricBoiseLGSVL]

@luqiang21 Yooooooooo! @henrychoi We are looking into an issue with large data being lost with bridge, We hope to have a fix soon.

[henrychoi]

Hi @EricBoiseLGSVL the ImuData is only ~320 B per packet (according to the ros2-lgsvl-bridge DEBUG line). Is this considered large?

[EricBoiseLGSVL]

Sorry large or with a high frequency. "I would have guessed that what is passed into a message queue is copied, and therefore immune from further changes" This ends up not being the case :)