diff --git a/cartographer_ros/scripts/remove_leading_slashes.py b/cartographer_ros/scripts/remove_leading_slashes.py new file mode 100755 index 0000000..388f3a9 --- /dev/null +++ b/cartographer_ros/scripts/remove_leading_slashes.py @@ -0,0 +1,53 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- + +# Copyright 2016 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""A simple tool to remove leading slashes from frame names.""" + +import argparse +import os +import rosbag + + +def ParseArgs(): + argument_parser = argparse.ArgumentParser( + description="Removes leading slashes from frame names.") + argument_parser.add_argument("input", type=str, help="Input bag") + return argument_parser.parse_args() + + +def RewriteMsg(msg): + if hasattr(msg, "header"): + if msg.header.frame_id.startswith("/"): + msg.header.frame_id = msg.header.frame_id[1:] + if hasattr(msg, "child_frame_id"): + if msg.child_frame_id.startswith("/"): + msg.child_frame_id = msg.child_frame_id[1:] + if hasattr(msg, "transforms"): + for transform_msg in msg.transforms: + RewriteMsg(transform_msg) + + +def Main(): + options = ParseArgs() + with rosbag.Bag(os.path.splitext(options.input)[0] + ".filtered.bag", + "w") as outbag: + for topic, msg, t in rosbag.Bag(options.input).read_messages(): + RewriteMsg(msg) + outbag.write(topic, msg, msg.header.stamp if msg._has_header else t) + + +if __name__ == "__main__": + Main()