Improve rosbag ROS1 and ROS2 readers

Some small styling mistakes + guess ros2 bagfile topic

cpp/kiss_icp/CMakeLists.txt

@@ -21,7 +21,7 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
 cmake_minimum_required(VERSION 3.16...3.24)
-project(kiss_icp_cpp VERSION 0.2.1 LANGUAGES CXX)
+project(kiss_icp_cpp VERSION 0.2.2 LANGUAGES CXX)
 
 # Setup build options
 option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" ON)

python/CMakeLists.txt

@@ -21,7 +21,7 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
 cmake_minimum_required(VERSION 3.16...3.24)
-project(kiss_icp_pybind VERSION 0.2.1 LANGUAGES CXX)
+project(kiss_icp_pybind VERSION 0.2.2 LANGUAGES CXX)
 
 # Set build type
 set(CMAKE_BUILD_TYPE Release)

python/kiss_icp/__init__.py

@@ -20,4 +20,4 @@
 # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
-__version__ = "0.2.1"
+__version__ = "0.2.2"

python/kiss_icp/datasets/rosbag.py

@@ -24,8 +24,6 @@
 from pathlib import Path
 import sys
 
-import numpy as np
-
 
 class RosbagDataset:
     def __init__(self, data_dir: Path, topic: str, *_, **__):
@@ -36,6 +34,7 @@ def __init__(self, data_dir: Path, topic: str, *_, **__):
             sys.exit(1)
 
         from kiss_icp.tools.point_cloud2 import read_point_cloud
+
         self.read_point_cloud = read_point_cloud
         self.sequence_id = os.path.basename(data_dir).split(".")[0]
 
@@ -72,7 +71,8 @@ def check_topic(self, topic: str) -> str:
         ]
 
         if len(point_cloud_topics) == 1:
-            return point_cloud_topics[0][0]  # this is the string topic name, go figure out
+            # this is the string topic name, go figure out
+            return point_cloud_topics[0][0]
 
         # In any other case we consider this an error
         if len(point_cloud_topics) == 0:

python/kiss_icp/datasets/rosbag2.py

@@ -20,14 +20,11 @@
 # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
-
 import importlib
 import os
 from pathlib import Path
 import sys
 
-import numpy as np
-
 
 class RosbagDataset:
     def __init__(self, data_dir: Path, topic: str, *_, **__):
@@ -36,8 +33,9 @@ def __init__(self, data_dir: Path, topic: str, *_, **__):
         except ImportError:
             print('rosbag2 reader is not installed, run "pip install rosbags"')
             sys.exit(1)
-        
+
         from kiss_icp.tools.point_cloud2 import read_point_cloud
+
         self.read_point_cloud = read_point_cloud
 
         self.deserialize_cdr = importlib.import_module("rosbags.serde").deserialize_cdr
@@ -49,19 +47,18 @@ def __init__(self, data_dir: Path, topic: str, *_, **__):
         self.bagfile = data_dir
         self.bag = rosbag2.Reader(self.bagfile)
         self.bag.open()
-        self.topic = topic
-        self.check_for_topics()
+        self.topic = self.check_topic(topic)
         self.n_scans = self.bag.topics[self.topic].msgcount
 
         # limit connections to selected topic
-        connections = [x for x in self.bag.connections if x.topic == topic]
+        connections = [x for x in self.bag.connections if x.topic == self.topic]
         self.msgs = self.bag.messages(connections=connections)
 
         # Visualization Options
         self.use_global_visualizer = True
 
     def __del__(self):
-        if hasattr(self, 'bag'):
+        if hasattr(self, "bag"):
             self.bag.close()
 
     def __len__(self):
@@ -72,12 +69,32 @@ def __getitem__(self, idx):
         msg = self.deserialize_cdr(rawdata, connection.msgtype)
         return self.read_point_cloud(msg)
 
-    def check_for_topics(self):
-        if self.topic:
-            return
-        print("Please provide one of the following topics with the --topic flag")
-        print("PointCloud2 topics available:")
-        for topic, topic_info in self.bag.topics.items():
-            if topic_info.msgtype == "sensor_msgs/msg/PointCloud2":
-                print(topic)
+    def check_topic(self, topic: str) -> str:
+        # when user specified the topic don't check
+        if topic:
+            return topic
+
+        # Extract all PointCloud2 msg topics from the bagfile
+        point_cloud_topics = [
+            topic
+            for topic in self.bag.topics.items()
+            if topic[1].msgtype == "sensor_msgs/msg/PointCloud2"
+        ]
+
+        if len(point_cloud_topics) == 1:
+            # this is the string topic name, go figure out
+            return point_cloud_topics[0][0]
+
+        # In any other case we consider this an error
+        if len(point_cloud_topics) == 0:
+            print("[ERROR] Your bagfile does not contain any sensor_msgs/PointCloud2 topic")
+        if len(point_cloud_topics) > 1:
+            print("Multiple sensor_msgs/msg/PointCloud2 topics available.")
+            print("Please provide one of the following topics with the --topic flag")
+            for topic_tuple in point_cloud_topics:
+                print(50 * "-")
+                print(f"Topic {topic_tuple[0]}")
+                print(f"\tType      {topic_tuple[1].msgtype}")
+                print(f"\tMessages  {topic_tuple[1].count}")
+            print(50 * "-")
         sys.exit(1)

python/kiss_icp/tools/point_cloud2.py

@@ -1,4 +1,3 @@
-
 # Copyright 2008 Willow Garage, Inc.
 #
 # Redistribution and use in source and binary forms, with or without
@@ -56,15 +55,15 @@
 _DATATYPES[PointField.FLOAT32] = np.dtype(np.float32)
 _DATATYPES[PointField.FLOAT64] = np.dtype(np.float64)
 
-DUMMY_FIELD_PREFIX = 'unnamed_field'
+DUMMY_FIELD_PREFIX = "unnamed_field"
 
 
 def read_point_cloud(msg: PointCloud2) -> Tuple[np.ndarray, np.ndarray]:
     """
     Extract poitns and timestamps from a PointCloud2 message.
-    
+
     :return: Tuple of [points, timestamps]
-        points: array of x, y z points, shape: (N, 3) 
+        points: array of x, y z points, shape: (N, 3)
         timestamps: array of per-pixel timestamps, shape: (N,)
     """
     field_names = ["x", "y", "z"]
@@ -74,9 +73,11 @@ def read_point_cloud(msg: PointCloud2) -> Tuple[np.ndarray, np.ndarray]:
             t_field = field.name
             field_names.append(t_field)
             break
-    
+
     points_structured = read_points(msg, field_names=field_names)
-    points = np.column_stack([points_structured["x"], points_structured["y"], points_structured["z"]])
+    points = np.column_stack(
+        [points_structured["x"], points_structured["y"], points_structured["z"]]
+    )
 
     if t_field:
         timestamps = points_structured[t_field]
@@ -87,11 +88,12 @@ def read_point_cloud(msg: PointCloud2) -> Tuple[np.ndarray, np.ndarray]:
 
 
 def read_points(
-        cloud: PointCloud2,
-        field_names: Optional[List[str]] = None,
-        skip_nans: bool = False,
-        uvs: Optional[Iterable] = None,
-        reshape_organized_cloud: bool = False) -> np.ndarray:
+    cloud: PointCloud2,
+    field_names: Optional[List[str]] = None,
+    skip_nans: bool = False,
+    uvs: Optional[Iterable] = None,
+    reshape_organized_cloud: bool = False,
+) -> np.ndarray:
     """
     Read points from a sensor_msgs.PointCloud2 message.
     :param cloud: The point cloud to read from sensor_msgs.PointCloud2.
@@ -106,19 +108,21 @@ def read_points(
     """
     # Cast bytes to numpy array
     points = np.ndarray(
-        shape=(cloud.width * cloud.height, ),
+        shape=(cloud.width * cloud.height,),
         dtype=dtype_from_fields(cloud.fields, point_step=cloud.point_step),
-        buffer=cloud.data)
+        buffer=cloud.data,
+    )
 
     # Keep only the requested fields
     if field_names is not None:
-        assert all(field_name in points.dtype.names for field_name in field_names), \
-            'Requests field is not in the fields of the PointCloud!'
+        assert all(
+            field_name in points.dtype.names for field_name in field_names
+        ), "Requests field is not in the fields of the PointCloud!"
         # Mask fields
         points = points[list(field_names)]
 
     # Swap array if byte order does not match
-    if bool(sys.byteorder != 'little') != bool(cloud.is_bigendian):
+    if bool(sys.byteorder != "little") != bool(cloud.is_bigendian):
         points = points.byteswap(inplace=True)
 
     # Check if we want to drop points with nan values
@@ -127,8 +131,7 @@ def read_points(
         not_nan_mask = np.ones(len(points), dtype=bool)
         for field_name in points.dtype.names:
             # Only keep points without any non values in the mask
-            not_nan_mask = np.logical_and(
-                not_nan_mask, ~np.isnan(points[field_name]))
+            not_nan_mask = np.logical_and(not_nan_mask, ~np.isnan(points[field_name]))
         # Select these points
         points = points[not_nan_mask]
 
@@ -164,8 +167,8 @@ def dtype_from_fields(fields: Iterable[PointField], point_step: Optional[int] =
         # Datatype as numpy datatype
         datatype = _DATATYPES[field.datatype]
         # Name field
-        if field.name == '':
-            name = f'{DUMMY_FIELD_PREFIX}_{i}'
+        if field.name == "":
+            name = f"{DUMMY_FIELD_PREFIX}_{i}"
         else:
             name = field.name
         # Handle fields with count > 1 by creating subfields with a suffix consiting
@@ -174,21 +177,17 @@ def dtype_from_fields(fields: Iterable[PointField], point_step: Optional[int] =
         for a in range(field.count):
             # Add suffix if we have multiple subfields
             if field.count > 1:
-                subfield_name = f'{name}_{a}'
+                subfield_name = f"{name}_{a}"
             else:
                 subfield_name = name
-            assert subfield_name not in field_names, 'Duplicate field names are not allowed!'
+            assert subfield_name not in field_names, "Duplicate field names are not allowed!"
             field_names.append(subfield_name)
             # Create new offset that includes subfields
             field_offsets.append(field.offset + a * datatype.itemsize)
             field_datatypes.append(datatype.str)
 
     # Create dtype
-    dtype_dict = {
-            'names': field_names,
-            'formats': field_datatypes,
-            'offsets': field_offsets
-    }
+    dtype_dict = {"names": field_names, "formats": field_datatypes, "offsets": field_offsets}
     if point_step is not None:
-        dtype_dict['itemsize'] = point_step
+        dtype_dict["itemsize"] = point_step
     return np.dtype(dtype_dict)

python/setup.cfg

@@ -1,6 +1,6 @@
 [metadata]
 name = kiss_icp
-version = 0.2.1
+version = 0.2.2
 author = Ignacio Vizzo
 author_email = ignaciovizzo@gmail.com
 description = Simple yet effective 3D LiDAR-Odometry registration pipeline

ros/CMakeLists.txt

@@ -21,7 +21,7 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
 cmake_minimum_required(VERSION 3.16...3.24)
-project(kiss_icp VERSION 0.2.1 LANGUAGES CXX)
+project(kiss_icp VERSION 0.2.2 LANGUAGES CXX)
 
 set(ignore ${CATKIN_INSTALL_INTO_PREFIX_ROOT})
 set(CMAKE_BUILD_TYPE Release)

ros/package.xml

@@ -25,7 +25,7 @@
 -->
 <package format="3">
   <name>kiss_icp</name>
-  <version>0.2.1</version>
+  <version>0.2.2</version>
   <description>KISS-ICP ROS Wrappers</description>
   <maintainer email="ignaciovizzo@gmail.com">ivizzo</maintainer>
   <license>MIT</license>