Fixes to handle durations

Signed-off-by: Patrick Roncagliolo <[email protected]>
ros2 · Nov 10, 2023 · dfa534a · dfa534a
1 parent b67e16a
commit dfa534a
Show file tree

Hide file tree

Showing 4 changed files with 65 additions and 26 deletions.
diff --git a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp
@@ -138,35 +138,53 @@ PlayOptions get_play_options_from_node_params(rclcpp::Node * node)
     "clock_trigger_topics",
     std::vector<std::string>());
 
-  auto delay_ = node->declare_parameter<double>(
-    "delay",
+  auto delay_sec = node->declare_parameter<int32_t>(
+    "delay.sec",
     0.0);
-  play_options.delay = rclcpp::Duration::from_seconds(delay_);
+  auto delay_nsec = node->declare_parameter<int32_t>(
+    "delay.nsec",
+    0.0);
+  play_options.delay = rclcpp::Duration(delay_sec, delay_nsec);
 
-  auto playback_duration_ = node->declare_parameter<double>(
-    "playback_duration",
-    -1);
-  play_options.playback_duration = rclcpp::Duration::from_seconds(playback_duration_);
+  auto playback_duration_sec = node->declare_parameter<int32_t>(
+    "playback_duration.sec",
+    -1.0);
+  auto playback_duration_nsec = node->declare_parameter<int32_t>(
+    "playback_duration.nsec",
+    0.0);
+  play_options.playback_duration = rclcpp::Duration(playback_duration_sec, playback_duration_nsec);
 
-  play_options.playback_until_timestamp = node->declare_parameter<int64_t>(
-    "playback_until_timestamp",
-    -1);
+  auto playback_until_timestamp_sec = node->declare_parameter<int32_t>(
+    "playback_until_timestamp.sec",
+    -1.0);
+  auto playback_until_timestamp_nsec = node->declare_parameter<int32_t>(
+    "playback_until_timestamp.nsec",
+    0.0);
+  play_options.playback_until_timestamp = rclcpp::Duration(playback_until_timestamp_sec, playback_until_timestamp_nsec).nanoseconds();
 
   play_options.start_paused = node->declare_parameter<bool>(
     "start_paused",
     false);
 
-  play_options.start_offset = node->declare_parameter<int64_t>(
-    "start_offset",
-    0);
+  auto start_offset_sec = node->declare_parameter<int32_t>(
+    "start_offset.sec",
+    -1.0);
+  auto start_offset_nsec = node->declare_parameter<int32_t>(
+    "start_offset.nsec",
+    0.0);
+  play_options.start_offset = rclcpp::Duration(start_offset_sec, start_offset_nsec).nanoseconds();
 
   play_options.disable_keyboard_controls = node->declare_parameter<bool>(
     "disable_keyboard_controls",
     false);
 
-  play_options.wait_acked_timeout = node->declare_parameter<int64_t>(
-    "wait_acked_timeout",
-    -1);
+  auto wait_acked_timeout_sec = node->declare_parameter<int32_t>(
+    "wait_acked_timeout.sec",
+    -1.0);
+  auto wait_acked_timeout_nsec = node->declare_parameter<int32_t>(
+    "wait_acked_timeout.nsec",
+    0.0);
+  play_options.wait_acked_timeout = rclcpp::Duration(wait_acked_timeout_sec, wait_acked_timeout_nsec).nanoseconds();
 
   play_options.disable_loan_message = node->declare_parameter<bool>(
     "disable_loan_message",

diff --git a/rosbag2_transport/src/rosbag2_transport/play_options.cpp b/rosbag2_transport/src/rosbag2_transport/play_options.cpp
@@ -29,16 +29,16 @@ struct convert<rosbag2_transport::Rosbag2Duration>
   static Node encode(const rosbag2_transport::Rosbag2Duration & duration)
   {
     Node node;
-    node["seconds"] = duration.seconds();
-    node["nanoseconds"] = duration.nanoseconds();
+    node["sec"] = duration.nanoseconds() / 1000000000;
+    node["nsec"] = duration.nanoseconds() % 1000000000;
     return node;
   }
 
   static bool decode(const Node & node, rosbag2_transport::Rosbag2Duration & duration)
   {
     duration = rosbag2_transport::Rosbag2Duration(
-      node["seconds"].as<int32_t>(),
-      node["nanoseconds"].as<uint32_t>()
+      node["sec"].as<int32_t>(),
+      node["nsec"].as<uint32_t>()
     );
     return true;
   }

diff --git a/rosbag2_transport/test/resources/params.yaml b/rosbag2_transport/test/resources/params.yaml
@@ -11,24 +11,29 @@ player_params_node:
     clock_publish_frequency: 19.0
     clock_publish_on_topic_publish: true
     clock_trigger_topics: ["/triggers/clock"]
+    # Negative durations are invalid. 
     delay:
       sec: 1
       nsec: 0
+    # Negative timestamps will make the playback to not stop.  
     playback_duration:
-      sec: 5
+      sec: -5
       nsec: 800000000
+    # Negative timestamps will make the playback to not stop.
     playback_until_timestamp:
-      sec: 5
+      sec: -5
       nsec: 800000000
     start_paused: true
+    # Negative durations are invalid.
     start_offset: 
       sec: 0
       nsec: 300000000
     disable_keyboard_controls: true
+    # Negative value means that published messages do not need to be acknowledged.
     wait_acked_timeout:
       sec: 0
-      nsec: 300000000
-    disable_loan_message: true
+      nsec: -300000000
+    disable_loan_message: false
 
     storage_id: sqlite3
     config_uri: ""

diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_params.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_params.cpp
@@ -50,9 +50,25 @@ TEST_F(RosBag2PlayTestFixture, parse_parameter_from_file) {
   YAML::Node yaml_storage_opt = YAML::convert<rosbag2_storage::StorageOptions>().encode(
     storage_options);
 
+  auto param_node = YAML::LoadFile(_SRC_RESOURCES_DIR_PATH "/params.yaml");
+  auto qos_node = YAML::LoadFile(_SRC_RESOURCES_DIR_PATH "/overrides.yaml");
+
   YAML::Emitter emitter;
-  emitter << yaml_play_opt;
-  std::cout << "Node :" << emitter.c_str() << std::endl;
+  emitter 
+  << YAML::Comment("params.yaml")
+  << param_node << YAML::Newline 
+  << YAML::Comment("overrides.yaml")
+  << qos_node << YAML::Newline 
+  << YAML::Comment("node parameters")
+  << yaml_play_opt << YAML::Newline;
+
+  std::cout << emitter.c_str() << std::endl;
 
   // TODO(roncapat): compare YAML trees (from file vs from struct)
 }
+
+/*
+TEST_F(RosBag2PlayTestFixture, test_negative_durations) {
+// TODO(roncapat): implement
+}
+*/