Throw error if /tf messages are not tf2. (#617)

Closes #613
MichaelGrupp · Jan 4, 2024 · 329fdd6 · 329fdd6
1 parent cda8b4b
commit 329fdd6
Showing 1 changed file with 6 additions and 0 deletions.
diff --git a/evo/tools/tf_cache.py b/evo/tools/tf_cache.py
@@ -40,6 +40,8 @@
 
 logger = logging.getLogger(__name__)
 
+SUPPORTED_TF_MSG = "tf2_msgs/msg/TFMessage"
+
 
 class TfCacheException(EvoException):
     pass
@@ -91,6 +93,10 @@ def from_bag(self, reader: Rosbag1Reader, topic: str = "/tf",
             ]
             for connection, _, rawdata in reader.messages(
                     connections=connections):
+                if connection.msgtype != SUPPORTED_TF_MSG:
+                    raise TfCacheException(
+                        f"Expected {SUPPORTED_TF_MSG} message type for topic "
+                        f"{tf_topic}, got: {connection.msgtype}")
                 msg = deserialize_cdr(ros1_to_cdr(rawdata, connection.msgtype),
                                       connection.msgtype)
                 for tf in msg.transforms: