From 31d5a0eb95203efe6e8263e0edea6c4c55054370 Mon Sep 17 00:00:00 2001 From: Hayato Mizushima Date: Thu, 7 Nov 2024 16:21:43 +0900 Subject: [PATCH 1/2] feat: remap topics Signed-off-by: Hayato Mizushima --- .../launch/driving_log_replayer_v2.launch.py | 34 +++++++++++++------ 1 file changed, 23 insertions(+), 11 deletions(-) diff --git a/driving_log_replayer_v2/launch/driving_log_replayer_v2.launch.py b/driving_log_replayer_v2/launch/driving_log_replayer_v2.launch.py index bf7b4fce..f5f0fc2f 100644 --- a/driving_log_replayer_v2/launch/driving_log_replayer_v2.launch.py +++ b/driving_log_replayer_v2/launch/driving_log_replayer_v2.launch.py @@ -49,6 +49,7 @@ def get_launch_arguments() -> list: record_only override_topics_regex storage + remap """ launch_arguments = [] @@ -109,6 +110,11 @@ def add_launch_arg( default_value="sqlite3", # Settings are adjusted to ros distro standards. Currently autoware is humble, so use sqlite3. Change to mcap when updated to jazzy. description="select storage type mcap or sqlite3", ) + add_launch_arg( + "remap", + default_value="", # Settings are adjusted to ros distro standards. Currently autoware is humble, so use sqlite3. Change to mcap when updated to jazzy. + description="use comma separated string. Ex: remap:=/tf,/sensing/lidar/concatenated/pointcloud", + ) return launch_arguments @@ -341,35 +347,41 @@ def launch_bag_player( "qos.yaml", ).as_posix(), ] - remap_list = ["--remap"] + remap_set = {"--remap"} if conf.get("sensing", "true") == "true": - remap_list.append( + remap_set.add( "/sensing/lidar/concatenated/pointcloud:=/unused/concatenated/pointcloud", ) if conf.get("localization", "true") == "true": - remap_list.append( + remap_set.add( "/tf:=/unused/tf", ) - remap_list.append( + remap_set.add( "/localization/kinematic_state:=/unused/localization/kinematic_state", ) - remap_list.append( + remap_set.add( "/localization/acceleration:=/unused/localization/acceleration", ) if conf.get("perception", "true") == "true": # remap perception msgs in bag - remap_list.append( + remap_set.add( "/perception/obstacle_segmentation/pointcloud:=/unused/perception/obstacle_segmentation/pointcloud", ) - remap_list.append( + remap_set.add( "/perception/object_recognition/objects:=/unused/perception/object_recognition/objects", ) if conf.get("goal_pose") is not None: - remap_list.append( + remap_set.add( "/planning/mission_planning/route:=/unused/planning/mission_planning/route", ) - if len(remap_list) != 1: - play_cmd.extend(remap_list) + # user defined remap + if conf["remap"] != "": + remap_topics: list[str] = conf["remap"].split(",") + for topic in remap_topics: + if topic.startswith("/"): + remap_set.add(f"{topic}:=/unused{topic}") + if len(remap_set) != 1: + play_cmd.extend(list(remap_set)) bag_player = ( ExecuteProcess( cmd=play_cmd, @@ -379,7 +391,7 @@ def launch_bag_player( if conf["record_only"] == "true" else ExecuteProcess(cmd=play_cmd, output="screen") ) - return [bag_player] + return [bag_player, LogInfo(msg=f"The displayed topics are remapped {remap_set}")] def launch_bag_recorder(context: LaunchContext) -> list: From c33dd8d18e8cd09e2a0b89bf07429a53695bad7e Mon Sep 17 00:00:00 2001 From: Hayato Mizushima Date: Thu, 7 Nov 2024 16:55:26 +0900 Subject: [PATCH 2/2] fix: remap command Signed-off-by: Hayato Mizushima --- .../launch/driving_log_replayer_v2.launch.py | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/driving_log_replayer_v2/launch/driving_log_replayer_v2.launch.py b/driving_log_replayer_v2/launch/driving_log_replayer_v2.launch.py index f5f0fc2f..92b16eaf 100644 --- a/driving_log_replayer_v2/launch/driving_log_replayer_v2.launch.py +++ b/driving_log_replayer_v2/launch/driving_log_replayer_v2.launch.py @@ -347,41 +347,41 @@ def launch_bag_player( "qos.yaml", ).as_posix(), ] - remap_set = {"--remap"} + remap_list = ["--remap"] if conf.get("sensing", "true") == "true": - remap_set.add( + remap_list.append( "/sensing/lidar/concatenated/pointcloud:=/unused/concatenated/pointcloud", ) if conf.get("localization", "true") == "true": - remap_set.add( + remap_list.append( "/tf:=/unused/tf", ) - remap_set.add( + remap_list.append( "/localization/kinematic_state:=/unused/localization/kinematic_state", ) - remap_set.add( + remap_list.append( "/localization/acceleration:=/unused/localization/acceleration", ) if conf.get("perception", "true") == "true": # remap perception msgs in bag - remap_set.add( + remap_list.append( "/perception/obstacle_segmentation/pointcloud:=/unused/perception/obstacle_segmentation/pointcloud", ) - remap_set.add( + remap_list.append( "/perception/object_recognition/objects:=/unused/perception/object_recognition/objects", ) if conf.get("goal_pose") is not None: - remap_set.add( + remap_list.append( "/planning/mission_planning/route:=/unused/planning/mission_planning/route", ) # user defined remap if conf["remap"] != "": remap_topics: list[str] = conf["remap"].split(",") for topic in remap_topics: - if topic.startswith("/"): - remap_set.add(f"{topic}:=/unused{topic}") - if len(remap_set) != 1: - play_cmd.extend(list(remap_set)) + if topic.startswith("/") and topic not in remap_list: + remap_list.append(f"{topic}:=/unused{topic}") + if len(remap_list) != 1: + play_cmd.extend(remap_list) bag_player = ( ExecuteProcess( cmd=play_cmd, @@ -391,7 +391,7 @@ def launch_bag_player( if conf["record_only"] == "true" else ExecuteProcess(cmd=play_cmd, output="screen") ) - return [bag_player, LogInfo(msg=f"The displayed topics are remapped {remap_set}")] + return [bag_player, LogInfo(msg=f"remap_command is {remap_list}")] def launch_bag_recorder(context: LaunchContext) -> list: