Inconsistent sim-time stamps

[rhaschke]

The inconsistent timestamps reported by Lara might originate from asynchronous processing of /clock messages. As far as I can see, you use a clock publisher to publish and ros::Time::now() to retrieve the current sim time. Thus, the message needs to published and received via the topic to yield the current sim time in your simulation node. To allow for fast intra-process message delivery (without actual sending), you should use a shared message pointer here: https://github.com/ubi-agni/mujoco_ros_pkgs/blob/e8baa92b317878501f4199e34bf6c7b7074639b1/mujoco_ros/src/mujoco_sim.cpp#L395-L397

Additionally, if you have different threads in your MuJuCo node, this might result in additional synchronization issues.

[rhaschke]

I think the correct way to configure sim_time and update your time in the simulation node is to call ros::Time::setNow(). Then, you don't need to subscribe to /clock at all.

[DavidPL1]

Running a few tests I could confirm that some calls to ros::Time::now() in the publishing stage of the sensors plugin returned stale values, which would result in sensor messages of the same sim step having differing timestamps. While ensuring that use_sim_time was set did not fix the issue, ~using a shared message pointer for the clock updates seems to do the trick.~

EDIT: Turns out even though it's faster, using the shared pointe for publication is not fast enough. In a simpler setup running with a higher step frequency ros::Time::now() changes in between steps. Probably that's due to some asynchronous call still involved when relying on the publish function.

Using ros::Time:setNow() before publishing the clock update really fixes the issue, because it's a blocking call that sets the g_sim_time variable which is directly accessed by ros::Time::now().