Optionally use stored timestamps when publishing from svo files

[lucasw]

Haven't tested this extensively or inspected the timestamp in every possible published message, with some feedback could move this out of draft.

Rebased version in https://github.com/lucasw/zed-ros-wrapper/tree/svo_playback_timestamps_rebased but haven't tested it yet.

The way the node is generating sim time by using the last timestamp out of the svo, then stepping forward in small increments, then skipping forward to the next stored timestamp when it becomes available is maybe not the best, but seems to work ok.

Resolves #411

[Myzhar]

What's the goal of this PR? Please describe it in detail because I cannot understand the logic behind it.

[lucasw]

I want to play back svo files through the nodelet, but use the timestamps stored in the svo instead of the current time in every published message header. Mostly I plan on recording out selected topics to bag files (ideally at faster than real time if possible, that could be in a future PR), then those bag files ought to be time synced to other bag files for non-zed topics recorded at the same time as the svo.

[Myzhar]

OK, I supposed it. To use simulated time you need to set a global parameter /use_sim_time that should be set before the node starts and then you must start publishing the timestamp read from the SVO file for each grabbed frame on the /clock topic, acting as a clock server.
The problem is that when you set the /use_sim_time parameter to false the nodelet server does not start waiting for someone publishing the simulated timestamp...
Can you explain how you can solve this problem?

[lucasw]

I pushed in a few more commits that clean up my changes a little, hopefully make it more clear what I'm doing. I'm not sure in every instance on usage of zed ::CURRENT vs. ros::Time::now(), or what happens when using IMAGE time on startup when no images have arrived yet- if this is done wrong I can fix it (though probably at the cost of complicating the code more).

The problem is that when you set the /use_sim_time parameter to false the nodelet server does not start waiting for someone publishing the simulated timestamp...
Can you explain how you can solve this problem?

I'm not understanding you here. If /use_sim_time is false then the intent is that svo playback will behave like it does without this PR, it publishes the current time (though again I could have switched zed CURRENT time for ros::Time::now() in a few places, that can be fixed if it is a problem).

It is odd the way the same node is both publishing a clock and subscribing to it when use_sim_time is true. If that isn't done properly the node locks up and waits forever without doing anything, I've avoided that with a WallTimer that operates off wall-clock time instead of sim time, and moves time forward between frames like what I described at the top.

There could be some case where /use_sim_time is true and there is another clock source (like gazebo, but using a saved svo with a sim seems like an odd use case, but if there is no way to feed sim stereo images into this node maybe it's a good test case), if that is a concern there needs to be another param to enable the zed sim time clock publisher.

[Myzhar]

The problem is that if you do not set /use_sim_time to true the other nodes of the system continue to use the system clock instead of the timestamp published on the /clock topic.
When you call the ROS API ros::Time::now() the ROS time system automatically switches between the real and the simulated time according to the use_sim_time value

[lucasw]

The problem is that if you do not set /use_sim_time to true the other nodes of the system continue to use the system clock instead of the timestamp published on the /clock topic.

I've made it in this PR that /clock isn't published out of the zed node if use_sim_time is false, so every node is using consistent time as long as they all start up with the same use_sim_time. If ros nodes are started before and after a change to use_sim_time then time across the system gets confused, but that has always been true.

I think this function encapsulates the intent here, it could be missing strange cases where mSvoMode is false, but use_sim_time is true- the user wants to connect to a real camera, but inject sim timestamps into all the topics, so in that case use ros::Time::now()?

ros::Time ZEDWrapperNodelet::getTimestamp()
{
  ros::Time stamp;
  if (mSvoMode && !mUseSimTime)
  {
    // TODO(lucasw) does it matter which one?
    stamp = ros::Time::now();
    // mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT));
    NODELET_WARN_STREAM_ONCE("Using current time instead of image time " << stamp);
  }
  else
  {
    // TODO(lucasw) if no images have arrived yet, this is zero, or something else?
    // In the svo file it appears to be something else, slightly in advance of the first
    // frame.
    stamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE));
    NODELET_WARN_STREAM_ONCE("Using zed image time " << stamp);
  }
  return stamp;
}

I'm trying to parallel rosbag play --clock + /use_sim_time true but for svo playback- maybe a separate parameter to publish the clock makes that more clear, more consistent with rosbag play.

If I'm not describing the changes well it could be worth compiling this and playing back an svo with use_sim_time true and then false and comparing the header stamps of different zed topic outputs. And maybe there is some combination of other parameters or nodes I'm not testing with that demonstrates the issue that could be listed in detail?

[Myzhar]

OK, I begin to understand how it works and it seems promising.
I still have a doubt about /use_sim_time are you setting it to true just before starting the node and everything correctly works?
I'm asking this because I had many problems when I tested a similar solution as you can see in this question on ROS answers:
https://answers.ros.org/question/301128/clock-server-as-nodelet/

[lucasw]

ros/ros_comm#1492 appears to describe that same issue, and there was a PR fix to use wall clock that was merged. I didn't see it in noetic but maybe the launch file I was using didn't use zed in nodelet form.

[Myzhar]

ros/ros_comm#1492 appears to describe that same issue, and there was a PR fix to use wall clock that was merged.

Interesting, I was not aware of it. Thank you