Don't record sim-time messages before first /clock

[emersonknapp]

Fixes #1276

When using sim-time, check that now != 0 before writing any messages into the bag, to avoid the time jump that then happens on first /clock message. This potentially drops some messages that it could have written, but is more correct, in that those messages are "out of time" and have no temporal standing in simulated time.

[MichaelOrlov]

Thanks for the PR.

[MichaelOrlov]

@emersonknapp A bunch of tests for use-sim-time start failing

 build/rosbag2_transport/test_results/rosbag2_transport/test_record_all_use_sim_time__rmw_fastrtps_cpp.gtest.xml: 1 test, 0 errors, 1 failure, 0 skipped
2023-05-26T01:21:28.7555631Z - rosbag2_transport.RecordIntegrationTestFixture record_all_with_sim_time
2023-05-26T01:21:28.7555951Z   <<< failure message
2023-05-26T01:21:28.7556306Z     /__w/rosbag2/rosbag2/ros_ws/src/nn632imx3xs/rosbag2/rosbag2_transport/test/rosbag2_transport/test_record_all_use_sim_time.cpp:85
2023-05-26T01:21:28.7556696Z     Value of: ret
2023-05-26T01:21:28.7556882Z       Actual: false
2023-05-26T01:21:28.7557085Z     Expected: true
2023-05-26T01:21:28.7557361Z     failed to capture expected messages in time. recorded messages = 9
2023-05-26T01:21:28.7557817Z     /__w/rosbag2/rosbag2/ros_ws/src/nn632imx3xs/rosbag2/rosbag2_transport/test/rosbag2_transport/test_record_all_use_sim_time.cpp:90
2023-05-26T01:21:28.7558171Z     Expected equality of these values:
2023-05-26T01:21:28.7558424Z       messages_per_topic[clock_topic]
2023-05-26T01:21:28.7558635Z         Which is: 4
2023-05-26T01:21:28.7558827Z       5u
2023-05-26T01:21:28.7559016Z         Which is: 5
2023-05-26T01:21:28.7559350Z     /__w/rosbag2/rosbag2/ros_ws/src/nn632imx3xs/rosbag2/rosbag2_transport/test/rosbag2_transport/test_record_all_use_sim_time.cpp:93
2023-05-26T01:21:28.7559698Z     Value of: recorded_messages
2023-05-26T01:21:28.7559927Z     Expected: size is >= 10
2023-05-26T01:21:28.7561306Z       Actual: { 16-byte object <10-7E D0-B8 3D-56 00-00 00-7E D0-B8 3D-56 00-00>, 16-byte object <60-50 D0-B8 3D-56 00-00 50-50 D0-B8 3D-56 00-00>, 16-byte object <80-55 D0-B8 3D-56 00-00 70-55 D0-B8 3D-56 00-00>, 16-byte object <40-47 D0-B8 3D-56 00-00 30-47 D0-B8 3D-56 00-00>, 16-byte object <A0-3F CD-B8 3D-56 00-00 90-3F CD-B8 3D-56 00-00>, 16-byte object <90-40 D0-B8 3D-56 00-00 80-40 D0-B8 3D-56 00-00>, 16-byte object <20-41 D0-B8 3D-56 00-00 10-41 D0-B8 3D-56 00-00>, 16-byte object <30-43 D0-B8 3D-56 00-00 20-43 D0-B8 3D-56 00-00>, 16-byte object <C0-43 D0-B8 3D-56 00-00 B0-43 D0-B8 3D-56 00-00> }, whose size 9 doesn't match
2023-05-26T01:21:28.7561953Z   >>>

Could you please investigate?

[MichaelOrlov]

Test fails need investigation

[MichaelOrlov]

LGTM with green CI if failing tests will be addressed.

[MichaelOrlov]

@emersonknapp I found out that node's clock has method started() - which is indicating if first message on the "/clock" topic has arrived It looks like a better way to determine if clock was primed rather than relying on the clock()->now().nanoseconds == 0 condition.

Also, I found out that the node's clock has wait_until_started(timeout) method

RCLCPP_PUBLIC
  bool
  wait_until_started(
    const rclcpp::Duration & timeout,
    Context::SharedPtr context = contexts::get_global_default_context(),
    const rclcpp::Duration & wait_tick_ns = rclcpp::Duration(0, static_cast<uint32_t>(1e7)));

Perhaps it would be even better to wait until clock will be started before creating subscriptions if using use_sim_time option. We can wait in the loop and check for rclcpp::ok in the Recorder::record() method and in the discovery thread just skip trying to subscribe on the topics and continue to sleep in the discovery thread.

What do you think?

[emersonknapp]

Thanks for looking into that - I think that waiting for clock before starting discovery is a cleaner approach, I am rewriting to do that, and will address test issues

[emersonknapp]

Pulls: #1354
Gist: https://gist.githubusercontent.com/emersonknapp/e213a931a39a2f287543f266a7a9fdcc/raw/4f65cca6c2058ec607b06da56eef3d4da03c62eb/ros2.repos
BUILD args: --packages-above-and-dependencies rosbag2_transport ros2bag
TEST args: --packages-above rosbag2_transport ros2bag
ROS Distro: rolling
Job: ci_launcher
ci_launcher ran: https://ci.ros2.org/job/ci_launcher/12167

• Linux
• Linux-aarch64
• Windows

[emersonknapp]

@Mergifyio backport iron humble

[mergify]

backport iron humble

✅ Backports have been created

• #1374 Don't record sim-time messages before first /clock (backport #1354) has been created for branch iron
• #1375 Don't record sim-time messages before first /clock (backport #1354) has been created for branch humble but encountered conflicts

[emersonknapp]

Oops, that was premature - I thought of a couple edge cases that this implementation did not cover, while doing the backports. Reverting for now (#1377) - and reopened PR as #1378