Add MOLA on MRPT 3 (Linux, macOS)

distros/humble/patch/dependencies.yaml

@@ -66,6 +66,11 @@ mrpt2:
   add_host: ["assimp", "octomap", "tinyxml2", "libboost-devel", "jsoncpp", "gtest", "libboost-python-devel", "libdc1394", "xorg-libxcomposite", "libftdi", "ros2-octomap"]
   add_run: ["assimp", "octomap", "tinyxml2", "libboost-devel", "jsoncpp", "gtest", "libboost-python-devel", "libdc1394", "xorg-libxcomposite", "libftdi", "ros2-octomap"]
   add_build: ["${{ cdt('libxcomposite-devel') if linux }}"]
+mrpt_gui:
+  # Overrides shared/: this distribution maps Qt to qt-main 5.15, whose builds need
+  # gstreamer <1.27, while the pinned wxwidgets (3.3.3) needs gstreamer 1.28. A version
+  # spec bypasses that pin, so the solver picks a wxwidgets that coexists with qt-main.
+  add_host: ["wxwidgets >=3.2"]
 nav2_constrained_smoother:
   add_host: ["${{ 'openblas' if win }}"]
 nav2_util:

distros/jazzy/patch/dependencies.yaml

@@ -102,6 +102,11 @@ mrpt2:
   add_host: ["assimp", "octomap", "tinyxml2", "libboost-devel", "jsoncpp", "gtest", "libboost-python-devel", "libdc1394", "xorg-libxcomposite", "libftdi", "ros2-octomap"]
   add_run: ["assimp", "octomap", "tinyxml2", "libboost-devel", "jsoncpp", "gtest", "libboost-python-devel", "libdc1394", "xorg-libxcomposite", "libftdi", "ros2-octomap"]
   add_build: ["${{ cdt('libxcomposite-devel') if linux }}"]
+mrpt_gui:
+  # Overrides shared/: this distribution maps Qt to qt-main 5.15, whose builds need
+  # gstreamer <1.27, while the pinned wxwidgets (3.3.3) needs gstreamer 1.28. A version
+  # spec bypasses that pin, so the solver picks a wxwidgets that coexists with qt-main.
+  add_host: ["wxwidgets >=3.2"]
 nav2_constrained_smoother:
   add_host: ["${{ 'openblas' if win }}"]
 nav2_mppi_controller:

shared/patch/dependencies.yaml

@@ -86,6 +86,14 @@ moveit_setup_framework:
   add_host: ["${{ 'libgl-devel' if linux }}", "${{ 'libopengl-devel' if linux }}"]
 moveit_setup_srdf_plugins:
   add_host: ["${{ 'libgl-devel' if linux }}", "${{ 'libopengl-devel' if linux }}"]
+mrpt_common:
+  # CMakeLists.txt calls find_package(ament_cmake) when ROS_VERSION=2 (set by ros_environment),
+  # but package.xml (build_type cmake) does not declare it
+  add_host: ["ros2-ament-cmake"]
+mrpt_gui:
+  # links libwx_*; the wxwidgets/wx-common keys map to wxpython, so wxwidgets is only a
+  # transitive host dep and its (weak) run_exports never reach the run requirements
+  add_host: ["wxwidgets"]
 mujoco_ros2_control_plugins:
   add_host:
   - if: linux

shared/patch/ros2-mola-launcher.patch

@@ -0,0 +1,66 @@
+diff --git a/src/MolaDLL_Loader.cpp b/src/MolaDLL_Loader.cpp
+--- a/src/MolaDLL_Loader.cpp
++++ b/src/MolaDLL_Loader.cpp
+@@ -24,7 +24,7 @@
+
+ #include <map>
+
+-#if defined(__unix__)
++#if defined(__unix__) || defined(__APPLE__)
+ #include <dlfcn.h>
+ #endif
+
+@@ -39,7 +39,9 @@
+ {
+   MRPT_TRY_START
+
+-#if defined(__unix__)
++#if defined(__APPLE__)
++#define DLL_EXT "dylib"
++#elif defined(__unix__)
+ #define DLL_EXT "so"
+ #else
+ #define DLL_EXT "dll"
+@@ -59,11 +61,14 @@
+     for (const auto& lib : lst)
+     {
+       if (lib.name.find("mola") == std::string::npos) continue;  // skip
++      // ROS interface libraries (e.g. mola_msgs) are never MOLA modules, and the
++      // Python ones cannot be dlopen()ed from a non-Python process:
++      if (lib.name.find("__rosidl_") != std::string::npos) continue;  // skip
+
+       // Already loaded?
+       if (loaded_lib_handled.count(lib.name) != 0) continue;  // skip
+
+-#if defined(__unix__)
++#if defined(__unix__) || defined(__APPLE__)
+       // Check if already loaded?
+       {
+         void* handle = dlopen(lib.wholePath.c_str(), RTLD_NOLOAD);
+@@ -109,7 +114,7 @@
+   {
+     // printf("[mola-launcher] Unloading: %s\n",
+     // lib.second.lib_path.c_str());
+-#if defined(__unix__)
++#if defined(__unix__) || defined(__APPLE__)
+     dlclose(lib.second.handle);
+ #else
+     FreeLibrary(lib.second.handle);
+diff --git a/src/MolaLauncherApp.cpp b/src/MolaLauncherApp.cpp
+--- a/src/MolaLauncherApp.cpp
++++ b/src/MolaLauncherApp.cpp
+@@ -119,6 +119,14 @@
+   // do not filter these ones by *directory* name, since *filenames* will be
+   // filtered later on:
+   from_env_var_to_list("LD_LIBRARY_PATH", lib_search_paths_);
++
++  // conda/RoboStack environments do not set LD_LIBRARY_PATH: also search
++  // <prefix>/lib for every prefix in AMENT_PREFIX_PATH.
++  {
++    std::vector<std::string> prefixes;
++    from_env_var_to_list("AMENT_PREFIX_PATH", prefixes);
++    for (const auto& prefix : prefixes) safe_add_to_list(prefix + "/lib", lib_search_paths_);
++  }
+ }
+
+ MolaLauncherApp::~MolaLauncherApp()

shared/patch/ros2-mp2p-icp-core.patch

@@ -0,0 +1,108 @@
+diff --git a/mp2p_icp/include/mp2p_icp/LogRecord.h b/mp2p_icp/include/mp2p_icp/LogRecord.h
+--- a/mp2p_icp/include/mp2p_icp/LogRecord.h
++++ b/mp2p_icp/include/mp2p_icp/LogRecord.h
+@@ -80,7 +80,7 @@ class LogRecord : public mrpt::serialization::CSerializable
+
+         DECLARE_TTYPENAME_CLASSNAME(mp2p_icp::LogRecord::DebugInfoPerIteration)
+     };
+-    using iteration_idx_t   = std::size_t;
++    using iteration_idx_t   = std::uint64_t;
+     using IterationsDetails = std::map<iteration_idx_t, DebugInfoPerIteration>;
+
+     std::optional<IterationsDetails> iterationsDetails;
+diff --git a/mp2p_icp/src/Pairings.cpp b/mp2p_icp/src/Pairings.cpp
+--- a/mp2p_icp/src/Pairings.cpp
++++ b/mp2p_icp/src/Pairings.cpp
+@@ -57,7 +57,7 @@ void Pairings::serializeFrom(mrpt::serialization::CArchive& in)
+     {
+         // Removed in v3: per-point weights. Read and discard, for backward
+         // compatibility with older serialized icplog files.
+-        std::vector<std::pair<std::size_t, double>> unusedPointWeights;
++        std::vector<std::pair<std::uint64_t, double>> unusedPointWeights;
+         in >> unusedPointWeights;
+     }
+     if (readVersion >= 1)
+diff --git a/mp2p_icp/src/Results.cpp b/mp2p_icp/src/Results.cpp
+--- a/mp2p_icp/src/Results.cpp
++++ b/mp2p_icp/src/Results.cpp
+@@ -25,7 +25,7 @@ static const uint8_t SERIALIZATION_VERSION = 1;
+ void Results::serializeTo(mrpt::serialization::CArchive& out) const
+ {
+     out.WriteAs<uint8_t>(SERIALIZATION_VERSION);
+-    out << optimal_tf << optimalScale << nIterations;
++    out << optimal_tf << optimalScale << static_cast<uint64_t>(nIterations);
+     out << static_cast<uint8_t>(terminationReason);
+     out << quality;
+     finalPairings.serializeTo(out);
+@@ -37,7 +37,8 @@ void Results::serializeFrom(mrpt::serialization::CArchive& in)
+
+     ASSERT_LE_(readVersion, SERIALIZATION_VERSION);
+
+-    in >> optimal_tf >> optimalScale >> nIterations;
++    in >> optimal_tf >> optimalScale;
++    nIterations = static_cast<size_t>(in.ReadAs<uint64_t>());
+     terminationReason = static_cast<IterTermReason>(in.ReadAs<uint8_t>());
+     in >> quality;
+     finalPairings.serializeFrom(in);
+diff --git a/mp2p_icp_common/src/load_plugin.cpp b/mp2p_icp_common/src/load_plugin.cpp
+--- a/mp2p_icp_common/src/load_plugin.cpp
++++ b/mp2p_icp_common/src/load_plugin.cpp
+@@ -27,7 +27,7 @@
+ #include <string>
+ namespace fs = std::filesystem;
+
+-#if defined(__unix__)
++#if defined(__unix__) || defined(__APPLE__)
+ #include <dlfcn.h>
+ #endif
+
+@@ -72,6 +72,14 @@ void mp2p_icp::load_plugin(
+ {
+     std::string absPath;
+
++#if defined(__APPLE__)
++    // Plugin names are usually given as "lib<name>.so": use the macOS suffix.
++    if (fs::path(moduleToLoad).extension() == ".so")
++    {
++        return load_plugin(fs::path(moduleToLoad).replace_extension(".dylib").string(), logger);
++    }
++#endif
++
+     if (!fs::path(moduleToLoad).is_relative())
+     {
+         // already absolute:
+@@ -83,6 +91,13 @@ void mp2p_icp::load_plugin(
+         // search for it:
+         std::vector<std::string> lstPath;
+         from_env_var_to_list("LD_LIBRARY_PATH", lstPath);
++        // conda/RoboStack environments do not set LD_LIBRARY_PATH: also search
++        // <prefix>/lib for every prefix in AMENT_PREFIX_PATH.
++        {
++            std::vector<std::string> prefixes;
++            from_env_var_to_list("AMENT_PREFIX_PATH", prefixes);
++            for (const auto& prefix : prefixes) safe_add_to_list(prefix + "/lib", lstPath);
++        }
+
+         for (const auto& p : lstPath)
+         {
+@@ -101,7 +116,7 @@ void mp2p_icp::load_plugin(
+         }
+     }
+
+-#if defined(__unix__)
++#if defined(__unix__) || defined(__APPLE__)
+     // Check if already loaded?
+     {
+         void* handle = dlopen(absPath.c_str(), RTLD_NOLOAD);
+diff --git a/tests/test-mp2p_Pairings.cpp b/tests/test-mp2p_Pairings.cpp
+--- a/tests/test-mp2p_Pairings.cpp
++++ b/tests/test-mp2p_Pairings.cpp
+@@ -149,7 +149,7 @@ void test_Pairings_BackwardCompatDeserialization()
+     MatchedLineList       ln2ln;
+     MatchedPlaneList      pl2pl;
+     // Old-format per-point weights block, no longer used by current code:
+-    std::vector<std::pair<std::size_t, double>> point_weights;
++    std::vector<std::pair<std::uint64_t, double>> point_weights;
+     point_weights.emplace_back(1, 2.5);
+     const uint64_t          potential_pairings = 42;
+     MatchedPointWithCovList cov2cov;

shared/patch/ros2-mp2p-icp-viz.patch

@@ -0,0 +1,13 @@
+diff --git a/apps/imgui_app_common/CMakeLists.txt b/apps/imgui_app_common/CMakeLists.txt
+--- a/apps/imgui_app_common/CMakeLists.txt
++++ b/apps/imgui_app_common/CMakeLists.txt
+@@ -20,4 +20,9 @@ find_package(Threads REQUIRED)
+
+ target_link_libraries(${PROJECT_NAME} PUBLIC imgui::imgui Threads::Threads)
+
++# libcfgpath (used here and by the apps) calls FSFindFolder() on macOS:
++if(APPLE)
++  target_link_libraries(${PROJECT_NAME} PUBLIC "-framework CoreServices")
++endif()
++
+ target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)

shared/patch/ros2-mrpt-gui.patch

@@ -0,0 +1,48 @@
+diff --git a/3rdparty/nanogui/CMakeLists.txt b/3rdparty/nanogui/CMakeLists.txt
+--- a/3rdparty/nanogui/CMakeLists.txt
++++ b/3rdparty/nanogui/CMakeLists.txt
+@@ -64,9 +64,9 @@ endif()
+ # -------------------------------------------------------------------------
+ # Resource / Font Compilation
+ # -------------------------------------------------------------------------
+-find_file(ENTYPO_TTF_FILE           entypo.ttf          PATHS "${CMAKE_CURRENT_SOURCE_DIR}/resources" REQUIRED)
+-find_file(ROBOTO_BOLD_TTF_FILE      Roboto-Bold.ttf     PATHS /usr/share/fonts/truetype/roboto-fontface/roboto/ "${CMAKE_CURRENT_SOURCE_DIR}/resources" REQUIRED)
+-find_file(ROBOTO_REGULAR_TTF_FILE   Roboto-Regular.ttf  PATHS /usr/share/fonts/truetype/roboto-fontface/roboto/ "${CMAKE_CURRENT_SOURCE_DIR}/resources" REQUIRED)
++find_file(ENTYPO_TTF_FILE           entypo.ttf          PATHS "${CMAKE_CURRENT_SOURCE_DIR}/resources" NO_CMAKE_FIND_ROOT_PATH REQUIRED)
++find_file(ROBOTO_BOLD_TTF_FILE      Roboto-Bold.ttf     PATHS /usr/share/fonts/truetype/roboto-fontface/roboto/ "${CMAKE_CURRENT_SOURCE_DIR}/resources" NO_CMAKE_FIND_ROOT_PATH REQUIRED)
++find_file(ROBOTO_REGULAR_TTF_FILE   Roboto-Regular.ttf  PATHS /usr/share/fonts/truetype/roboto-fontface/roboto/ "${CMAKE_CURRENT_SOURCE_DIR}/resources" NO_CMAKE_FIND_ROOT_PATH REQUIRED)
+
+ set(resources ${ENTYPO_TTF_FILE} ${ROBOTO_BOLD_TTF_FILE} ${ROBOTO_REGULAR_TTF_FILE})
+ message(STATUS "Font resources: ${resources}")
+diff --git a/cmake/script_nanogui.cmake b/cmake/script_nanogui.cmake
+--- a/cmake/script_nanogui.cmake
++++ b/cmake/script_nanogui.cmake
+@@ -4,11 +4,9 @@
+ set(CMAKE_MRPT_HAS_NANOGUI 0)
+ set(CMAKE_MRPT_HAS_NANOGUI_SYSTEM 0)
+
+-if (APPLE)
+-  set(def_ OFF)
+-else()
+-  set(def_ ON)
+-endif()
++# RoboStack: also build nanogui on macOS (needed by mola_viz); it builds with GLFW and
++# OpenGL.framework.
++set(def_ ON)
+ set(MRPT_WITH_NANOGUI ${def_} CACHE BOOL "Build an embedded version of nanogui (OpenGL GUIs)")
+ unset(def_)
+
+diff --git a/src/error_box.cpp b/src/error_box.cpp
+--- a/src/error_box.cpp
++++ b/src/error_box.cpp
+@@ -22,7 +22,9 @@
+ #else
+ #include <iostream>
+ #endif  // MRPT_HAS_Qt5
+-#if MRPT_HAS_WXWIDGETS
++// Only needed without Qt (see showErrorMessage()); on macOS the wx and Qt 5 headers
++// clash (NSData/NSString forward declarations).
++#if MRPT_HAS_WXWIDGETS && !MRPT_HAS_Qt5
+ #include <mrpt/gui/WxUtils.h>
+ #endif
+

shared/patch/ros2-mrpt-imgui-vendor.patch

@@ -0,0 +1,13 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -141,6 +141,9 @@
+ # Always PIC: a static vendor library gets linked into MRPT shared libraries.
+ set_target_properties(${PROJECT_NAME} PROPERTIES POSITION_INDEPENDENT_CODE TRUE)
+
++# The GLFW/OpenGL3 backends call dlopen()/dlsym(), which live in libdl on glibc < 2.34.
++target_link_libraries(${PROJECT_NAME} PUBLIC ${CMAKE_DL_LIBS})
++
+ # Upstream headers are included unqualified (`#include "imgui.h"`), so the
+ # directory holding them must itself be on the interface include path. SYSTEM
+ # keeps third-party warnings out of consumers' builds.

shared/patch/ros2-mrpt-maps.patch

@@ -0,0 +1,15 @@
+diff --git a/include/mrpt/maps/bonxai/bonxai.hpp b/include/mrpt/maps/bonxai/bonxai.hpp
+--- a/include/mrpt/maps/bonxai/bonxai.hpp
++++ b/include/mrpt/maps/bonxai/bonxai.hpp
+@@ -538,7 +538,10 @@
+ {
+   dim_ = other.dim_;
+   size_ = other.size_;
+-  mask_ = std::move(other.mask_);
++  // Mask has const members, so it is not assignable (clang rejects this even if
++  // the operator is never instantiated): re-construct it in place instead.
++  mask_.~Mask();
++  new (&mask_) Bonxai::Mask(std::move(other.mask_));
+   std::swap(data_, other.data_);
+   return *this;
+ }

shared/robostack.yaml

@@ -56,6 +56,8 @@ clang-format:
   robostack: [clang-format]
 clang-tidy:
   robostack: [clang-tools]
+cli11:
+  robostack: [cli11]
 cmake:
   robostack: [cmake]
 coinor-libcbc-dev:

shared/tests/ros2-mola.yaml

@@ -0,0 +1,7 @@
+tests:
+  # mola-cli must find the module libraries of a conda environment without any environment
+  # variable (shared/patch/ros2-mola-launcher.patch) and register their classes.
+  - if: unix
+    then:
+      script:
+        - python -c "import subprocess; out = subprocess.run(['mola-cli', '--rtti-list-all'], capture_output=True, text=True).stdout; missing = [c for c in ('mola::MolaViz', 'mola::BridgeROS2', 'mola::Rosbag2Dataset', 'mola::KeyframePointCloudMap') if c not in out]; assert not missing, missing"

shared/vinca.yaml

@@ -563,6 +563,7 @@ packages_select_by_deps:
       - med14_moveit_config
       - med7_moveit_config
       - mobile_robot_simulator
+      - mola # requested in https://github.com/RoboStack/robostack/issues/106; Windows not tried yet
       - octomap_server
       - openvdb_vendor
       - orbbec_camera_msgs