feat: add time filter (#342)

* feat: add time filter

Signed-off-by: MasatoSaeki <masato.saeki@tier4.jp>

* reset original params

Signed-off-by: MasatoSaeki <masato.saeki@tier4.jp>

* chore

Signed-off-by: MasatoSaeki <masato.saeki@tier4.jp>

---------

Signed-off-by: MasatoSaeki <masato.saeki@tier4.jp>

Author: MasatoSaeki

docs/scenario_format/index.en.md

@@ -44,6 +44,7 @@ Evaluation:
         DirectInitialPose: Dictionary # optional; omit this key or use {} to disable the pose
         GoalPose: Dictionary # optional; omit this key or use {} to disable the pose
         goal_method: String # optional, default: set_goal_from_scenario
+        time_offset: Dictionary # optional, default: {}
 include_use_case:
   UseCaseName: String
   UseCaseFormatVersion: String
@@ -185,6 +186,16 @@ GoalPose:
 | `set_goal_from_scenario` | (Default) Use `GoalPose` from scenario to set goal. Skip setting goal if `GoalPose` is not specified.                                  |
 | `set_goal_from_rosbag`   | Read the last `/localization/kinematic_state` from rosbag and use its position as the goal. Even if `GoalPose` is specified ignore it. |
 
+#### time_offset
+
+(Optional) Specify the start time and duration of rosbag playback. The start time is specified as a relative time from the start of the rosbag, not based on timestamp. Place it at the same level as `GoalPose` in the dataset section of the scenario YAML.
+
+```yaml
+time_offset:
+  start: float # the start time [s]
+  duration: float # the duration [s]
+```
+
 ### include_use_case
 
 Use this when you want to perform evaluation with a different use case simultaneously with the use case specified in Evaluation.

docs/scenario_format/index.ja.md

@@ -44,6 +44,7 @@ Evaluation:
         DirectInitialPose: Dictionary # optional, 省略時は {} として扱われる (無効化する場合はキーを省略するか {} を指定すること)
         GoalPose: Dictionary # optional, 省略時は {} として扱われる (無効化する場合はキーを省略するか {} を指定すること)
         goal_method: String # optional, default: set_goal_from_scenario
+        time_offset: Dictionary # optional, default: {}
 include_use_case:
   UseCaseName: String
   UseCaseFormatVersion: String
@@ -184,6 +185,16 @@ GoalPose:
 | `set_goal_from_scenario` | （デフォルト）シナリオの `GoalPose` を使用してゴールを設定する。`GoalPose` が未指定の場合は何もしない。                             |
 | `set_goal_from_rosbag`   | rosbagの最後の `/localization/kinematic_state` を読み取り、その位置をゴールとして使用する。 `GoalPose` が指定されていても無視する。 |
 
+#### time_offset
+
+（オプション）rosbagの再生開始時刻と再生時間を指定する。前者はtimestamp基準ではなく、rosbagの開始からの相対時間で指定する。シナリオYAMLのデータセットセクションで `GoalPose` と同じ階層に配置する。
+
+```yaml
+time_offset:
+  start: float # rosbag再生開始時刻 [s]
+  duration: float # rosbag再生時間 [s]
+```
+
 ### include_use_case
 
 Evaluationで指定したユースケースとは別のユースケースの評価を同時に行う場合に使用する。

driving_log_replayer_v2/driving_log_replayer_v2/launch/argument.py

@@ -232,6 +232,7 @@ def update_conf_with_dataset_info(
     conf["direct_initial_pose"] = json.dumps(dataset_info.get("DirectInitialPose", {}))
     conf["goal_pose"] = json.dumps(dataset_info.get("GoalPose", {}))
     conf["goal_method"] = dataset_info.get("goal_method", "set_goal_from_scenario")
+    conf["time_offset"] = json.dumps(dataset_info.get("time_offset", {}))
     conf["t4_dataset_path"] = t4_dataset_path.as_posix()
     if "vehicle_model" not in conf:
         conf["vehicle_model"] = yaml_obj["VehicleModel"]
@@ -299,6 +300,9 @@ def ensure_arg_compatibility(context: LaunchContext) -> list:
         LogInfo(
             msg=f"{check_launch_component(conf)=}",
         ),
+        LogInfo(
+            msg=f"{conf['time_offset']=}",
+        ),
         LogInfo(
             msg=f"{conf['initial_pose']=}, {conf['direct_initial_pose']=}",
         ),

driving_log_replayer_v2/driving_log_replayer_v2/launch/rosbag.py

@@ -13,6 +13,7 @@
 # limitations under the License.
 
 from importlib import import_module
+import json
 from pathlib import Path
 
 from ament_index_python.packages import get_package_prefix
@@ -139,6 +140,20 @@ def get_pre_task_before_play_rosbag(
     )
 
 
+def get_rosbag_timestamp_offset(play_cmd: list[str], context: LaunchContext) -> list[str]:
+    conf = context.launch_configurations
+    if conf["time_offset"] == "{}":
+        return play_cmd
+    time_offset = json.loads(conf["time_offset"])
+    start = time_offset.get("start", None)
+    if start is not None:
+        play_cmd.extend(["--start-offset", str(start)])
+    duration = time_offset.get("duration", None)
+    if duration is not None:
+        play_cmd = ["timeout", "--preserve-status", str(duration), *play_cmd]
+    return play_cmd
+
+
 def launch_bag_player(
     context: LaunchContext,
 ) -> IncludeLaunchDescription:
@@ -155,6 +170,8 @@ def launch_bag_player(
         "--qos-profile-overrides-path",
         QOS_PROFILE_PATH_STR,
     ]
+    # timestamp offset
+    play_cmd = get_rosbag_timestamp_offset(play_cmd, context)
     # topics
     publish_list = ["--topics"]
     publish_list.extend(user_defined_publish(conf))