cras_bag_tools: Added StampTwist filter.

Author: peci1

cras_bag_tools/src/cras_bag_tools/bag_filter.py

@@ -65,7 +65,8 @@ def filter_bag(bags, out, bag_filter=Passthrough(), params=None, start_time=None
     # apply topic filters
     topics = [t for t in topics if bag_filter.topic_filter(t)]
     # apply connection filters
-    topics = [c.topic for c in bags._get_connections(topics, bag_filter.connection_filter)]  # noqa
+    if len(topics) > 0:
+        topics = [c.topic for c in bags._get_connections(topics, bag_filter.connection_filter)]  # noqa
 
     bag_filter.on_filtering_start()
 

cras_bag_tools/src/cras_bag_tools/bag_utils.py

@@ -47,6 +47,12 @@ def __del__(self):
     def __getattr__(self, item):
         return getattr(self.bag, item)
 
+    def read_messages(self, topics, start_time, end_time, topic_filter, raw, return_connection_header=False):
+        if topics is not None and len(topics) == 0:
+            return
+        for msg in self.bag.read_messages(topics, start_time, end_time, topic_filter, raw, return_connection_header):
+            yield msg
+
     def _get_entries(self, connections=None, start_time=None, end_time=None):
         all_ranges = None
         if start_time is not None and isinstance(start_time, TimeRanges):
@@ -251,6 +257,8 @@ def _read_messages(self,
                        return_connection_header=False,  # type: bool
                        ):
         # type: (...) -> Iterator[Union[rosbag.bag.BagMessage, rosbag.bag.BagMessageWithConnectionHeader]]
+        if topics is not None and len(topics) == 0:
+            return
         connections = dict(self._get_connections(topics, connection_filter, True))
         for bag, entry, _ in self._get_entries(connections, start_time, end_time):
             msg = bag._reader.seek_and_read_message_data_record(  # noqa

cras_bag_tools/src/cras_bag_tools/message_filters.py

@@ -30,7 +30,7 @@
 from cv_bridge import CvBridge, CvBridgeError
 from dynamic_reconfigure.encoding import decode_config
 from dynamic_reconfigure.msg import Config
-from geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3
+from geometry_msgs.msg import Quaternion, Transform, TransformStamped, Twist, TwistStamped, Vector3
 from image_transport_codecs import decode, encode
 from image_transport_codecs.compressed_depth_codec import has_rvl
 from image_transport_codecs.parse_compressed_format import guess_any_compressed_image_transport_format
@@ -61,6 +61,19 @@ def dict_to_str(d, sep='='):
         k, sep, str(v) if not isinstance(v, dict) else dict_to_str(v, sep)) for k, v in d.items()) + "}"
 
 
+def create_connection_header(topic, msg_type, latch=False):
+    header = {
+        "callerid": "/bag_filter",
+        "topic": topic,
+        "message_definition": msg_type._full_text,  # noqa
+        "type": msg_type._type,  # noqa
+        "md5sum": msg_type._md5sum,  # noqa
+    }
+    if latch:
+        header["latching"] = "1"
+    return header
+
+
 class SetFields(DeserializedMessageFilter):
     """Change values of some fields of a message (pass the fields to change as kwargs)."""
 
@@ -1128,6 +1141,46 @@ def _str_params(self):
         return ", ".join(parts)
 
 
+class StampTwist(DeserializedMessageFilter):
+    """Adjust some static transforms."""
+
+    def __init__(self, source_topic, stamped_topic=None, frame_id="base_link", *args, **kwargs):
+        # type: (STRING_TYPE, Optional[STRING_TYPE], STRING_TYPE, Any, Any) -> None
+        """
+        :param source_topic: The Twist topic to stamp.
+        :param stamped_topic: The stamped Twist topic to create.
+        :param frame_id: The frame_id to use in the stamped messages.
+        :param args: Standard include/exclude topics/types and min/max stamp args.
+        :param kwargs: Standard include/exclude topics/types and min/max stamp kwargs.
+        """
+        super(StampTwist, self).__init__(include_topics=[source_topic], *args, **kwargs)
+
+        self._source_topic = source_topic
+        self._stamped_topic = stamped_topic if stamped_topic is not None else (source_topic + "_stamped")
+        self._frame_id = frame_id
+
+        self._connection_header = create_connection_header(self._stamped_topic, TwistStamped)
+
+    def filter(self, topic, msg, stamp, header):
+        stamped_msg = TwistStamped()
+        stamped_msg.header.frame_id = self._frame_id
+        stamped_msg.header.stamp = stamp
+        stamped_msg.twist = msg
+
+        return [
+            (topic, msg, stamp, header),
+            (self._stamped_topic, stamped_msg, stamp, self._connection_header)
+        ]
+
+    def _str_params(self):
+        parts = []
+        parts.append('%s=>%s (frame %s)' % (self._source_topic, self._stamped_topic, self._frame_id))
+        parent_params = self._default_str_params(include_types=False)
+        if len(parent_params) > 0:
+            parts.append(parent_params)
+        return ", ".join(parts)
+
+
 class FixExtrinsicsFromIKalibr(DeserializedMessageFilterWithTF):
     """Apply extrinsic calibrations from IKalibr."""
 
@@ -1325,16 +1378,7 @@ def parse_time_str(time_str):
         return subtitles
 
     def extra_initial_messages(self):
-        connection_header = {
-            "callerid": "/bag_filter",
-            "topic": self._topic,
-            "message_definition": String._full_text,
-            "type": String._type,
-            "md5sum": String._md5sum,
-        }
-        if self._latch:
-            connection_header["latching"] = "1"
-
+        connection_header = create_connection_header(self._topic, String, self._latch)
         for start_time, end_time, subtitle in self._subtitles:
             msg = String(data=subtitle)
             abs_time = self._start_time + start_time