diff --git a/.vscode/launch.json b/.vscode/launch.json index 0cc4706..f30f09f 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -6,7 +6,7 @@ "configurations": [ { "name": "Python: Current File", - "type": "python", + "type": "debugpy", "request": "launch", "program": "${file}", "console": "integratedTerminal", @@ -14,11 +14,11 @@ }, { "name": "main (Workspace must be sourced before execution)", - "type": "python", + "type": "debugpy", "request": "launch", "program": "autoware_msg_bag_converter/main.py", "console": "integratedTerminal", - "args": ["$HOME/data/sample-rosbag", "$HOME/data/converted-rosbag"] + "args": ["$HOME/convert_test/input_bag", "$HOME/convert_test/converted_bag"] } ] } diff --git a/.vscode/settings.json b/.vscode/settings.json index a3cdfb3..729e43c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -42,7 +42,9 @@ "${env:HOME}/ros_ws/converter/install/autoware_auto_perception_msgs/local/lib/python3.10/dist-packages", "${env:HOME}/ros_ws/converter/install/autoware_auto_planning_msgs/local/lib/python3.10/dist-packages", "${env:HOME}/ros_ws/converter/install/autoware_auto_system_msgs/local/lib/python3.10/dist-packages", - "${env:HOME}/ros_ws/converter/install/autoware_auto_vehicle_msgs/local/lib/python3.10/dist-packages" + "${env:HOME}/ros_ws/converter/install/autoware_auto_vehicle_msgs/local/lib/python3.10/dist-packages", + + "${env:HOME}/ros_ws/converter/install/tier4_planning_msgs/local/lib/python3.10/dist-packages" ], "python.analysis.extraPaths": [ "/opt/ros/humble/lib/python3.10/site-packages", @@ -62,7 +64,9 @@ "${env:HOME}/ros_ws/converter/install/autoware_auto_perception_msgs/local/lib/python3.10/dist-packages", "${env:HOME}/ros_ws/converter/install/autoware_auto_planning_msgs/local/lib/python3.10/dist-packages", "${env:HOME}/ros_ws/converter/install/autoware_auto_system_msgs/local/lib/python3.10/dist-packages", - "${env:HOME}/ros_ws/converter/install/autoware_auto_vehicle_msgs/local/lib/python3.10/dist-packages" + "${env:HOME}/ros_ws/converter/install/autoware_auto_vehicle_msgs/local/lib/python3.10/dist-packages", + + "${env:HOME}/ros_ws/converter/install/tier4_planning_msgs/local/lib/python3.10/dist-packages" ], "ros.distro": "humble" } diff --git a/README.md b/README.md index 7d3c418..86e44df 100644 --- a/README.md +++ b/README.md @@ -37,10 +37,22 @@ source install/setup.bash cd src/autoware_msg_bag_converter/autoware_msg_bag_converter # convert one bag -python3 main.py ${input_bag} ${output_bag} +python3 main.py ${input_bag_dir} ${output_bag_dir} # convert multi bags in directory -python3 main.py ${input_bag_dir} ${output_bag_dir} -d +python3 main.py ${input_bag_dir_root} ${output_bag_dir_root} -d +``` + +```shell +# example +$ tree +bag_root # <- input_bag_dir_root +├── sample_mcap # <- input_bag_dir +│   ├── metadata.yaml +│   └── sample_mcap_0.db3 +└── sample_sqlite3 # <- input_bag_dir + ├── metadata.yaml + └── sample_sqlite3_0.db3 ``` ## demo diff --git a/autoware_msg_bag_converter/converter.py b/autoware_msg_bag_converter/converter.py index aa690fc..afc3c8e 100644 --- a/autoware_msg_bag_converter/converter.py +++ b/autoware_msg_bag_converter/converter.py @@ -17,17 +17,55 @@ # https://github.com/ros2/rosbag2/blob/rolling/rosbag2_py/test/test_reindexer.py from pathlib import Path +from typing import TYPE_CHECKING +from autoware_control_msgs.msg import Control +from autoware_control_msgs.msg import Lateral +from autoware_control_msgs.msg import Longitudinal +from autoware_planning_msgs.msg import PathPoint +from rclpy.serialization import deserialize_message +from rclpy.serialization import serialize_message from rosbag2_py import Reindexer from rosbag2_py import TopicMetadata +from rosidl_runtime_py.utilities import get_message +from tier4_planning_msgs.msg import PathPointWithLaneId +from tier4_planning_msgs.msg import PathWithLaneId as T4PathWithLaneId from autoware_msg_bag_converter.bag import create_reader from autoware_msg_bag_converter.bag import create_writer from autoware_msg_bag_converter.bag import get_storage_options +if TYPE_CHECKING: + from autoware_auto_control_msgs.msg import AckermannControlCommand + from autoware_auto_planning_msgs.msg import PathWithLaneId as AutoPathWithLaneId + +TYPES_NOT_SIMPLY_REPLACED = { + "autoware_auto_control_msgs/msg/AckermannControlCommand": "autoware_control_msgs/msg/Control", + "autoware_auto_planning_msgs/msg/PathWithLaneId": "tier4_planning_msgs/msg/PathWithLaneId", +} + +# Define the type of message you want autoware prefixes to be attached to (forward matching) +TYPES_TO_ADD_AUTOWARE_PREFIX = [ + "control_validator/msg", + "planning_validator/msg", + "vehicle_cmd_gate/msg", +] + def change_topic_type(old_type: TopicMetadata) -> TopicMetadata: - # If old_type is not of type autoware_auto, the original message type remains + if old_type.type in TYPES_NOT_SIMPLY_REPLACED: + return TopicMetadata( + name=old_type.name, + type=TYPES_NOT_SIMPLY_REPLACED[old_type.type], + serialization_format="cdr", + ) + if any(old_type.type.startswith(prefix) for prefix in TYPES_TO_ADD_AUTOWARE_PREFIX): + return TopicMetadata( + name=old_type.name, + type=f"autoware_{old_type.type}", + serialization_format="cdr", + ) + # If old_type is not in the conversion rules, simply remove "auto_" and use that as the new type. return TopicMetadata( name=old_type.name, type=old_type.type.replace("autoware_auto_", "autoware_"), @@ -35,6 +73,61 @@ def change_topic_type(old_type: TopicMetadata) -> TopicMetadata: ) +def convert_msg(topic_name: str, msg: bytes, type_map: dict) -> bytes: + # get old msg type + old_type: str = type_map[topic_name] + if old_type not in TYPES_NOT_SIMPLY_REPLACED: + return msg + old_msg = deserialize_message( + msg, + get_message(type_map[topic_name]), + ) + if old_type == "autoware_auto_control_msgs/msg/AckermannControlCommand": + old_msg: AckermannControlCommand + lateral = Lateral( + stamp=old_msg.lateral.stamp, + steering_tire_angle=old_msg.lateral.steering_tire_angle, + steering_tire_rotation_rate=old_msg.lateral.steering_tire_rotation_rate, + is_defined_steering_tire_rotation_rate=True, + ) + longitudinal = Longitudinal( + stamp=old_msg.longitudinal.stamp, + velocity=old_msg.longitudinal.speed, + acceleration=old_msg.longitudinal.acceleration, + jerk=old_msg.longitudinal.jerk, + is_defined_acceleration=True, + is_defined_jerk=False, + ) + return serialize_message( + Control( + stamp=old_msg.stamp, + lateral=lateral, + longitudinal=longitudinal, + ), + ) + if old_type == "autoware_auto_planning_msgs/msg/PathWithLaneId": + old_msg: AutoPathWithLaneId + points: list[PathPointWithLaneId] = [] + for old_point in old_msg.points: + point = PathPoint( + pose=old_point.point.pose, + longitudinal_velocity_mps=old_point.point.longitudinal_velocity_mps, + lateral_velocity_mps=old_point.point.lateral_velocity_mps, + heading_rate_rps=old_point.point.heading_rate_rps, + is_final=old_point.point.is_final, + ) + points.append(PathPointWithLaneId(point=point, lane_ids=old_point.lane_ids)) + return serialize_message( + T4PathWithLaneId( + header=old_msg.header, + points=points, + left_bound=old_msg.left_bound, + right_bound=old_msg.right_bound, + ), + ) + return None + + def convert_bag(input_bag_path: str, output_bag_path: str) -> None: p_input = Path(input_bag_path) storage_type = "mcap" @@ -47,7 +140,7 @@ def convert_bag(input_bag_path: str, output_bag_path: str) -> None: writer = create_writer(output_bag_path, storage_type) # create topic - type_map = {} + type_map = {} # key: topic_name value: old_type's msg type for topic_type in reader.get_all_topics_and_types(): type_map[topic_type.name] = topic_type.type new_topic_type = change_topic_type( @@ -58,7 +151,8 @@ def convert_bag(input_bag_path: str, output_bag_path: str) -> None: # copy data from input bag to output bag while reader.has_next(): topic_name, msg, stamp = reader.read_next() - writer.write(topic_name, msg, stamp) + new_msg = convert_msg(topic_name, msg, type_map) + writer.write(topic_name, new_msg, stamp) # reindex to update metadata.yaml del writer diff --git a/autoware_msg_bag_converter/main.py b/autoware_msg_bag_converter/main.py index d8ace1b..40a6aa4 100644 --- a/autoware_msg_bag_converter/main.py +++ b/autoware_msg_bag_converter/main.py @@ -19,6 +19,18 @@ from autoware_msg_bag_converter.converter import convert_bag +EXAMPLE_MESSAGE = """ +# example +$ tree +bag_root # <- input_bag_dir_root +├── sample_mcap # <- input_bag_dir +│ ── metadata.yaml +│ └── sample_mcap_0.db3 +└── sample_sqlite3 # <- input_bag_dir + ├── metadata.yaml + └── sample_sqlite3_0.db3 +""" + def convert_bag_in_directory(input_dir: str, output_dir: str) -> None: input_root = Path(input_dir) @@ -35,8 +47,8 @@ def convert_bag_in_directory(input_dir: str, output_dir: str) -> None: def main() -> None: parser = argparse.ArgumentParser() - parser.add_argument("input", help="path of input bag with autoware_auto_msgs") - parser.add_argument("output", help="path of output bag with autoware_msgs") + parser.add_argument("input", help="path of input bag directory with autoware_auto_msgs") + parser.add_argument("output", help="path of output bag directory with autoware_msgs") parser.add_argument( "--directory", "-d", @@ -44,6 +56,10 @@ def main() -> None: help="If this option is specified, all rosbags under the directory specified in input will be converted", ) args = parser.parse_args() + if not Path(args.input).is_dir(): + print(f"{args.input=} is not directory") # noqa + print(EXAMPLE_MESSAGE) # noqa + return if not args.directory: convert_bag(expandvars(args.input), expandvars(args.output)) else: diff --git a/dependency.repos b/dependency.repos index 030c8e3..ba69cbd 100644 --- a/dependency.repos +++ b/dependency.repos @@ -12,3 +12,7 @@ repositories: type: git url: https://github.com/tier4/autoware_auto_msgs.git version: tier4/main + autoware/tier4_autoware_msgs: + type: git + url: https://github.com/tier4/tier4_autoware_msgs.git + version: tier4/universe diff --git a/test/test_converter.py b/test/test_converter.py index 3d8cab2..6bfd1b8 100644 --- a/test/test_converter.py +++ b/test/test_converter.py @@ -21,16 +21,26 @@ def test_change_topic_type() -> None: - old_type = TopicMetadata( + auto_type = TopicMetadata( name="/vehicle/status/control_mode", type="autoware_auto_vehicle_msgs/msg/ControlModeReport", serialization_format="cdr", ) - new_type = change_topic_type(old_type) + new_type = change_topic_type(auto_type) assert new_type.name == "/vehicle/status/control_mode" assert new_type.type == "autoware_vehicle_msgs/msg/ControlModeReport" assert new_type.serialization_format == "cdr" + not_auto_type = TopicMetadata( + name="/tf", + type="tf2_msgs/msg/TFMessage", + serialization_format="cdr", + ) + not_changed_type = change_topic_type(not_auto_type) + assert not_auto_type.name == not_changed_type.name + assert not_auto_type.type == not_changed_type.type + assert not_auto_type.serialization_format == not_changed_type.serialization_format + def test_get_rosbag_path() -> None: # Test to confirm bag path acquisition in directory mode