Working version of rosbag2 to mola

mola_demos/mola-cli-launchs/rosbag2_ouster_just_view.yaml

@@ -0,0 +1,64 @@
+# -----------------------------------------------------------------------------
+#                        SLAM system definition for MOLA
+# This file defines:
+# An input sensor from a ROS 2 topic, and the MOLA Visualizer.
+# -----------------------------------------------------------------------------
+
+modules:
+  # =====================
+  # MolaViz
+  # =====================
+  - name: viz
+    type: mola::MolaViz
+    #verbosity_level: DEBUG
+    params: ~ # none
+
+# Offline or online sensory data sources =====================
+  - type: mola::Rosbag2Dataset
+    name: rosbag2_input
+    # In Rosbag2Dataset, this execution rate (Hz) determines the
+    # maximum rate of publishing topics from the bag file.
+    # If a message in the bag has an actually rate higher than this, their
+    # messages will be still sent out with the correct timestamps, but physically
+    # in bursts.
+    execution_rate: 50 # Hz
+    #verbosity_level: DEBUG
+
+    # Optional visualization of raw sensor streams in the MolaViz GUI
+    gui_preview_sensors:
+      - raw_sensor_label: '/ouster/points'
+        decimation: 1
+        win_pos: 5 5 400 400
+
+      #- raw_sensor_label: '/ouster/range_image'
+      #  decimation: 1
+      #  win_pos: 5 420 400 400
+
+    params:
+      # We use an environment variable to force the user to specify a bag file:
+      rosbag_filename: ${ROSBAG2_FILE}
+
+      base_link_frame_id: 'base_footprint'
+
+      time_warp_scale: 1.0
+      #rosbag_storage_id: 'mcap'
+      #rosbag_serialization: 'cdr'
+
+      # 'sensors' contains maps from output "sensorLabel"s, to the
+      # configuration of that data source (mainly, 'type', 'topic'):
+      # If no 'sensors' entry is given, all ROSBAG topics will be exposed
+      # to the MOLA subsystem. Note that if you want to override sensor poses
+      # (or if you do not have /tf), it is mandatory to define 'sensors' and
+      # define 'fixed_sensor_pose' for each sensor here:
+      sensors:
+        - topic: /ouster/points
+          type: CObservationPointCloud
+          # If present, this will override whatever /tf tells about the sensor pose:
+          fixed_sensor_pose: "0 0 0 0 0 0"  # 'x y z yaw_deg pitch_deg roll_deg'
+
+        - topic: /ouster/range_image
+          type: CObservationImage
+          fixed_sensor_pose: "0 0 0 0 0 0"  # 'x y z yaw_deg pitch_deg roll_deg'
+
+        - topic: /odom
+          type: CObservationOdometry

mola_input_rosbag2/.clang-format

@@ -0,0 +1,83 @@
+Language:        Cpp
+BasedOnStyle: Google
+# ---
+#AccessModifierOffset: -4
+AlignAfterOpenBracket: AlwaysBreak # Values: Align, DontAlign, AlwaysBreak
+AlignConsecutiveAssignments: true
+AlignConsecutiveDeclarations: true
+#AlignEscapedNewlinesLeft: true
+#AlignOperands:   false
+AlignTrailingComments: false # Should be off, causes many dummy problems!!
+#AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: true
+#AllowShortCaseLabelsOnASingleLine: false
+#AllowShortFunctionsOnASingleLine: Empty
+#AllowShortIfStatementsOnASingleLine: false
+#AllowShortLoopsOnASingleLine: false
+#AlwaysBreakAfterDefinitionReturnType: None
+#AlwaysBreakAfterReturnType: None
+#AlwaysBreakBeforeMultilineStrings: true
+#AlwaysBreakTemplateDeclarations: true
+#BinPackArguments: false
+#BinPackParameters: false
+#BraceWrapping:
+  #AfterClass:      false
+  #AfterControlStatement: false
+  #AfterEnum:       false
+  #AfterFunction:   false
+  #AfterNamespace:  false
+  #AfterObjCDeclaration: false
+  #AfterStruct:     false
+  #AfterUnion:      false
+  #BeforeCatch:     false
+  #BeforeElse:      true
+  #IndentBraces:    false
+#BreakBeforeBinaryOperators: None
+BreakBeforeBraces: Allman
+#BreakBeforeTernaryOperators: true
+#BreakConstructorInitializersBeforeComma: false
+ColumnLimit: 80
+#CommentPragmas:  ''
+#ConstructorInitializerAllOnOneLineOrOnePerLine: true
+#ConstructorInitializerIndentWidth: 4
+#ContinuationIndentWidth: 4
+#Cpp11BracedListStyle: true
+#DerivePointerAlignment: false
+#DisableFormat:   false
+#ExperimentalAutoDetectBinPacking: false
+##FixNamespaceComments: true # Not applicable in 3.8
+#ForEachMacros:   [ foreach, Q_FOREACH, BOOST_FOREACH ]
+#IncludeCategories:
+  #- Regex:           '.*'
+    #Priority:        1
+IndentCaseLabels: true
+IndentWidth:     4
+IndentWrappedFunctionNames: true
+#KeepEmptyLinesAtTheStartOfBlocks: true
+#MacroBlockBegin: ''
+#MacroBlockEnd:   ''
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+#PenaltyBreakBeforeFirstCallParameter: 19
+#PenaltyBreakComment: 300
+#PenaltyBreakFirstLessLess: 120
+#PenaltyBreakString: 1000
+#PenaltyExcessCharacter: 1000000
+#PenaltyReturnTypeOnItsOwnLine: 200
+DerivePointerAlignment: false
+#PointerAlignment: Left
+ReflowComments:  true # Should be true, otherwise clang-format doesn't touch comments
+SortIncludes:    true
+#SpaceAfterCStyleCast: false
+SpaceBeforeAssignmentOperators: true
+#SpaceBeforeParens: ControlStatements
+#SpaceInEmptyParentheses: false
+#SpacesBeforeTrailingComments: 2
+#SpacesInAngles:  false
+#SpacesInContainerLiterals: true
+#SpacesInCStyleCastParentheses: false
+#SpacesInParentheses: false
+#SpacesInSquareBrackets: false
+Standard:        Cpp11
+TabWidth:        4
+UseTab:          Never # Available options are Never, Always, ForIndentation

mola_input_rosbag2/CHANGELOG.rst

@@ -0,0 +1,4 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package mola_input_rosbag2
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+

mola_input_rosbag2/CMakeLists.txt

@@ -0,0 +1,53 @@
+# ------------------------------------------------------------------------------
+#        A Modular Optimization framework for Localization and mApping
+#                               (MOLA)
+#
+# Copyright (C) 2018-2023, Jose Luis Blanco-Claraco, contributors (AUTHORS.md)
+# All rights reserved.
+# Released under GNU GPL v3. See LICENSE file
+# ------------------------------------------------------------------------------
+
+# Minimum CMake vesion: limited by CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS
+cmake_minimum_required(VERSION 3.4)
+
+# Tell CMake we'll use C++ for use in its tests/flags
+project(mola_input_rosbag2 LANGUAGES CXX)
+
+# MOLA CMake scripts: "mola_xxx()"
+find_package(mola_common REQUIRED)
+# find dependencies:
+find_package(mrpt-obs REQUIRED)
+find_package(mrpt-ros2bridge REQUIRED)
+
+find_package(tf2_ros REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(rosbag2_cpp REQUIRED)
+find_package(tf2_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)
+
+find_mola_package(mola_kernel)
+
+# -----------------------
+# define lib:
+file(GLOB_RECURSE LIB_SRCS src/*.cpp src/*.h)
+file(GLOB_RECURSE LIB_PUBLIC_HDRS include/*.h)
+
+mola_add_library(
+	TARGET ${PROJECT_NAME}
+	SOURCES ${LIB_SRCS} ${LIB_PUBLIC_HDRS}
+	PRIVATE_LINK_LIBRARIES
+		mola_kernel
+		mrpt::obs
+		mrpt::ros2bridge
+		tf2::tf2
+		rosbag2_cpp::rosbag2_cpp
+		#nav_msgs
+		tf2_msgs::tf2_msgs__rosidl_typesupport_cpp
+		sensor_msgs::sensor_msgs_library
+		cv_bridge::cv_bridge
+	CMAKE_DEPENDENCIES
+		mola_kernel
+		mrpt-obs
+		mrpt-ros2bridge
+)