[CR-1159] Add changelog, bump version to v0.1.7, do miscellaneous cha…

…nges

.github/workflows/ci.yml

@@ -35,7 +35,7 @@ jobs:
     steps:
       - name: Set Env Variables
         run: |
-          echo "APT_PACKAGES=ros-humble-ros2bag ros-humble-rosbag2 ros-humble-rosbag2-compression ros-humble-rosbag2-compression-zstd ros-humble-rosbag2-cpp ros-humble-rosbag2-interfaces ros-humble-rosbag2-py ros-humble-rosbag2-storage ros-humble-rosbag2-storage-default-plugins ros-humble-rosbag2-storage-mcap ros-humble-rosbag2-transport" >> "$GITHUB_ENV"
+          echo "APT_PACKAGES=ros-humble-ros2bag ros-humble-rosbag2 ros-humble-rosbag2-compression ros-humble-rosbag2-compression-zstd ros-humble-rosbag2-cpp ros-humble-rosbag2-interfaces ros-humble-rosbag2-py ros-humble-rosbag2-storage ros-humble-rosbag2-storage-default-plugins ros-humble-rosbag2-storage-mcap ros-humble-rosbag2-transport ros-humble-gps-msgs" >> "$GITHUB_ENV"
       - uses: actions/checkout@v2
       - name: Setup ROS
         uses: ros-tooling/setup-ros@v0.6
@@ -61,7 +61,7 @@ jobs:
       - name: Install prerequisites
         run: "poetry version && poetry install --extras ros2"
       - name: Run tests
-        run: "source /opt/ros/humble/setup.bash && poetry run pytest -v --doctest-modules"
+        run: "source /opt/ros/humble/setup.bash && poetry run pytest"
 
   check_style:
     runs-on: ubuntu-22.04

CHANGELOG.md

@@ -4,6 +4,30 @@ All notable changes to this project will be documented in this file.
 
 The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/)
 
+## 0.1.7 (2023-09-25)
+
+### Added
+
+- slamcore-convert-rosbag2:
+
+  - Introduce internal plugins. Use an internal plugin to convert GNSS data
+  - Enable converting `PoseWithCovarianceStamped` messages
+
+### Fixed
+
+- slamcore-convert-rosbag2:
+
+  - Fix issue with unbounded memory usage during conversion of bulky rosbag
+    datasets
+
+### Changed
+
+- slamcore-convert-rosbag2:
+
+  - Also allow running with or without the package properly installed via `pip` or
+    `poetry`
+  - Allow writers to handle multiple types of ROS 2 messages
+
 ## 0.1.6 (2022-05-31)
 
 ### Added

README.md

@@ -72,10 +72,12 @@ format, it will go through the rosbag2 and convert the required streams to
 generate the Slamcore dataset. By default, the `slamcore-convert-rosbag2` is
 only aware of the following messages:
 
-- `sensor_msgs/Imu`
-- `sensor_msgs/Camera`
-- `nav_msgs/Odometry`
 - `geometry_msgs/PoseStamped`
+- `geometry_msgs/PoseWithCovarianceStamped`
+- `gps_msgs/GPSFix`
+- `nav_msgs/Odometry`
+- `sensor_msgs/Camera`
+- `sensor_msgs/Imu`
 
 The user can also specify plugins for conversions of arbitrary messages - see
 the [plugins](#rosbag-2-converter-plugins) section for more.
@@ -365,6 +367,7 @@ its dependencies isolated in a virtual environment:
 
 ```sh
 git clone https://github.com/slamcore/slamcore_utils
+cd slamcore_utils
 poetry install
 poetry shell
 

pyproject.toml

@@ -1,6 +1,6 @@
 [tool.poetry]
 name = "slamcore_utils"
-version = "0.1.6"
+version = "0.1.7"
 description = "SLAMcore SLAM Utilities"
 authors = ["Nikos Koukis <nikolaos@slamcore.com>"]
 license = "BSD License"
@@ -19,7 +19,7 @@ classifiers = [
 ]
 
 [tool.poetry.dependencies]
-python = ">=3.6.8,<3.11"
+python = ">=3.6.8,<3.12"
 questionary = "^1.10.0"
 prompt-toolkit = ">3.0,<=3.0.23"
 numpy = [ { version = "^1.19.5", python = "<3.9" }, { version = "*", python = ">=3.9" } ]
@@ -88,6 +88,10 @@ pythonPlatform = "Linux"
 line-length = 95
 target-version = "py38"
 
+[tool.pytest.ini_options]
+addopts = ["-v", "--doctest-modules"]
+
 [build-system]
 requires = ["poetry-core"]
 build-backend = "poetry.core.masonry.api"
+

slamcore_utils/__init__.py

@@ -15,4 +15,4 @@
 ]
 
 # TODO Find a way to use this version in pyproject.toml
-__version__ = "0.1.6"
+__version__ = "0.1.7"

slamcore_utils/dataset_subdir_writer.py

@@ -23,6 +23,7 @@
 
 import abc
 from pathlib import Path
+from typing import Type
 
 
 class DatasetSubdirWriter(abc.ABC):
@@ -36,6 +37,18 @@ def __init__(self, directory: Path):
         self.directory = directory
         self.directory.mkdir(parents=True, exist_ok=False)
 
+    def register_ros_msg_type(self, msg_type: Type) -> None:
+        """
+        In case the writer supports multiple ROS 2 msg types and changes its behavior based on
+        the particular ROS 2 msg type being passed, the derived class can implement this
+        function to become aware of the concrete message type that it will receive.
+        """
+
+    def prepare_write(self) -> None:
+        """
+        Initialization actions for the writer at hand.
+        """
+
     @abc.abstractmethod
     def write(self, msg):
         pass

slamcore_utils/math.py

@@ -0,0 +1,43 @@
+import numpy as np
+
+
+def is_symmetric(arr: np.ndarray, tol=1e-8) -> bool:
+    """
+    Check if a numpy array is approximately symmetric.
+
+    A matrix is symmetric if its transpose is approximately equal to itself.
+
+    Examples:
+
+        >>> arr1 = np.array([[1, 2, 3],
+        ...                  [2, 4, 5],
+        ...                  [3, 5, 6]])
+        >>> is_symmetric(arr1)
+        True
+
+        >>> arr2 = np.array([[1, 2, 3],
+        ...                  [4, 5, 6],
+        ...                  [7, 8, 9]])
+        >>> is_symmetric(arr2)
+        False
+
+        >>> arr3 = np.array([[1.0, 2.0],
+        ...                  [2.0, 1.0]])
+        >>> is_symmetric(arr3)
+        True
+
+        >>> arr4 = np.array([[1.0, 2.0, 3.0],
+        ...                  [2.0, 1.0, 4.0]])
+        >>> is_symmetric(arr4)
+        False
+
+    """
+    if arr.ndim != 2:
+        raise ValueError(f"Input array must be 2D - Current #dims: {arr.ndim}")
+
+    rows, cols = arr.shape
+    if rows != cols:
+        return False
+
+    return np.allclose(arr, arr.T, atol=tol)
+

slamcore_utils/measurement_type.py

@@ -30,7 +30,11 @@ class UnknownMeasurementTypeError(BaseException):
     """Exception raised when the measurement type cannot be found."""
 
     def __init__(self, name):
-        super().__init__(f"No MeasurementType found for {name}")
+        digit_stripped_name = re.sub(r"\d+$", "", name)
+        super().__init__(
+            f"No MeasurementType found for {digit_stripped_name}. "
+            f'The following measurement types are defined: {", ".join([m.shortname for m in measurement_types])}'
+        )
 
 
 class MeasurementType:

slamcore_utils/ros2/ros2_converter_plugins/gnss.py

@@ -0,0 +1,147 @@
+__copyright__ = """
+
+    SLAMcore Confidential
+    ---------------------
+
+    SLAMcore Limited
+    All Rights Reserved.
+    (C) Copyright 2023
+
+    NOTICE:
+
+    All information contained herein is, and remains the property of SLAMcore
+    Limited and its suppliers, if any. The intellectual and technical concepts
+    contained herein are proprietary to SLAMcore Limited and its suppliers and
+    may be covered by patents in process, and are protected by trade secret or
+    copyright law. Dissemination of this information or reproduction of this
+    material is strictly forbidden unless prior written permission is obtained
+    from SLAMcore Limited.
+"""
+
+__license__ = "SLAMcore Confidential"
+
+
+import csv
+from pathlib import Path
+from geometry_msgs.msg import PoseStamped
+
+import numpy as np
+
+from slamcore_utils import DatasetSubdirWriter, MeasurementType, setup_pkg_logging
+from slamcore_utils.ros2 import Ros2ConverterPlugin, Ros2PluginInitializationFailureError
+
+plugin_name = Path(__file__).name
+logger = setup_pkg_logging(plugin_name)
+
+try:
+    from gps_msgs.msg import GPSFix
+except ModuleNotFoundError:
+    raise Ros2PluginInitializationFailureError(
+        plugin_name=plugin_name,
+        msg=(
+            "Could not import the gps_msgs module. On Ubuntu this is shipped with the ros-<ros-version>-gps-msgs package"
+            "Please make sure that the latter is installed (or that the gps-msgs are installed via source and retry"
+        ),
+    )
+
+
+class GPSAsPoseStampedWriter(DatasetSubdirWriter):
+    """Convert and save GPSFix messages in the Slamcore dataset format."""
+
+    # constants for the potential conversion of LLA -> XYZ if given GPS data
+    EARTH_RADIUS_M = 6378137.0
+    FLATTENING_RATIO = 1.0 / 298.257224
+
+    def __init__(self, directory):
+        super().__init__(directory=directory)
+
+    def prepare_write(self) -> None:
+        self.ofs_data = (self.directory / "data.csv").open("w", newline="")
+        self.csv_writer = csv.writer(self.ofs_data, delimiter=",")
+
+        cols = [
+            "#timestamp [ns]",
+            "p_RS_R_x [m]",
+            "p_RS_R_y [m]",
+            "p_RS_R_z [m]",
+            "q_RS_w []",
+            "q_RS_x []",
+            "q_RS_y []",
+            "q_RS_z []",
+        ]
+
+        self.csv_writer.writerow(cols)
+
+    def write_pose_stamped(self, msg: PoseStamped):
+        ts = int(msg.header.stamp.sec * 1e9) + int(msg.header.stamp.nanosec)
+        p = msg.pose.position
+        q = msg.pose.orientation
+        self.csv_writer.writerow([ts, p.x, p.y, p.z, q.w, q.x, q.y, q.z])
+
+    def write(self, msg: GPSFix) -> None:
+        """
+        Convert LLA GPS coordinates XYZ coordinates
+
+        Consult the following two resources for the conversion equations:
+
+        https://web.archive.org/web/20181018072749/http://mathforum.org/library/drmath/view/51832.html
+        https://stackoverflow.com/a/8982005/2843583
+        """
+
+        lat_rad = np.deg2rad(msg.latitude)
+        lon_rad = np.deg2rad(msg.longitude)
+        alt_m = msg.altitude
+
+        cos_lat = np.cos(lat_rad)
+        sin_lat = np.sin(lat_rad)
+
+        cos_lon = np.cos(lon_rad)
+        sin_lon = np.sin(lon_rad)
+
+        C = 1.0 / np.sqrt(
+            cos_lat * cos_lat
+            + (1 - self.FLATTENING_RATIO) * (1 - self.FLATTENING_RATIO) * sin_lat * sin_lat
+        )
+        S = (1.0 - self.FLATTENING_RATIO) * (1.0 - self.FLATTENING_RATIO) * C
+
+        pose_out: PoseStamped = PoseStamped()
+        pose_out.header = msg.header
+        pose_out.pose.position.x = (self.EARTH_RADIUS_M * C + alt_m) * cos_lat * cos_lon
+        pose_out.pose.position.y = (self.EARTH_RADIUS_M * C + alt_m) * cos_lat * sin_lon
+        pose_out.pose.position.z = (self.EARTH_RADIUS_M * S + alt_m) * sin_lat
+
+        # # Cache first pose received and always substract from it so that we start from (0,0,0)
+        # if self._first_pose is None:
+        #     x = pose_out.pose.position.x
+        #     y = pose_out.pose.position.y
+        #     z = pose_out.pose.position.z
+        #     logger.warning(
+        #         f"Setting the first GPS received pose from ({x}, {y}, {z}) to to (0,0,0) ..."
+        #     )
+
+        #     self._first_pose = Pose()
+        #     self._first_pose.position.x = pose_out.pose.position.x
+        #     self._first_pose.position.y = pose_out.pose.position.y
+        #     self._first_pose.position.z = pose_out.pose.position.z
+
+        # pose_out.pose.position.x -= self._first_pose.position.x
+        # pose_out.pose.position.y -= self._first_pose.position.y
+        # pose_out.pose.position.z -= self._first_pose.position.z
+
+        self.write_pose_stamped(pose_out)
+
+    def teardown(self):
+        self.ofs_data.close()
+
+
+# define plugins for converter to use ------------------------------------------
+# mandatory key in module namespace
+converter_plugins = [
+    Ros2ConverterPlugin(
+        writer_type=GPSAsPoseStampedWriter,
+        measurement_type=MeasurementType(
+            name="GNSSGroundTruth", shortname="gnss_groundtruth", is_camera=False
+        ),
+        msg_type="gps_msgs/msg/GPSFix",
+    ),
+]