Add rtabmap_ros

distros/humble/patch/ros2-rtabmap-util.patch

@@ -0,0 +1,14 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index 9104af7..b0c0be8 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -28,7 +28,8 @@ find_package(rtabmap_msgs REQUIRED)
+ find_package(rtabmap_conversions REQUIRED)
+ find_package(rtabmap_sync REQUIRED)
+
+-find_package(RTABMap COMPONENTS gui REQUIRED)
++# nothing here uses rtabmap's GUI, and humble's rtabmap is built without Qt
++find_package(RTABMap REQUIRED)
+
+ # Optional components
+ find_package(octomap_msgs)

distros/humble/patch/ros2-rtabmap-util.win.patch

@@ -0,0 +1,13 @@
+diff --git a/src/nodelets/db_player.cpp b/src/nodelets/db_player.cpp
+index cacd679..e906d61 100644
+--- a/src/nodelets/db_player.cpp
++++ b/src/nodelets/db_player.cpp
+@@ -543,7 +543,7 @@ bool DbPlayer::publishNextFrame()
+         UASSERT(odomMsg.pose.covariance.size() == 36 &&
+                 odom.covariance().total() == 36 &&
+                 odom.covariance().type() == CV_64FC1);
+-        memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
++        memcpy(odomMsg.pose.covariance.data(), odom.covariance().data, 36*sizeof(double));
+         odometryPub_->publish(odomMsg);
+     }
+

distros/humble/vinca.yaml

@@ -12,6 +12,8 @@ mutex_package:
   version: "0.11.0"
 
 packages_exclude:
+  - rtabmap_rviz_plugins  # links PCL, whose VTK is Qt 6 on conda-forge, into a Qt 5 RViz plugin (humble's RViz is Qt 5)
+  - rtabmap_viz  # needs rtabmap's GUI, which follows VTK's Qt 6, while humble's RViz is Qt 5 (rtabmap is built without Qt here)
 
   - if: not linux
     then:

distros/jazzy/vinca.yaml

@@ -12,3 +12,7 @@ build_number: 27
 
 mutex_package:
   version: "0.20.0"
+
+packages_exclude:
+  - rtabmap_rviz_plugins  # links PCL, whose VTK is Qt 6 on conda-forge, into a Qt 5 RViz plugin (jazzy's RViz is Qt 5); rtabmap_viz is fine, it uses only rtabmap's Qt 6
+

shared/patch/ros2-rtabmap-conversions.patch

@@ -0,0 +1,30 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index 4dc4190..ed08e21 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -9,8 +9,7 @@ if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64")
+   # issues #1285 #1288
+   find_library(
+     builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c
+-    PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib"
+-    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
++    REQUIRED
+   )
+ endif()
+
+diff --git a/include/rtabmap_conversions/MsgConversion.h b/include/rtabmap_conversions/MsgConversion.h
+index 4e82e38..6f657f3 100644
+--- a/include/rtabmap_conversions/MsgConversion.h
++++ b/include/rtabmap_conversions/MsgConversion.h
+@@ -39,7 +39,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ #include <sensor_msgs/msg/point_cloud2.hpp>
+
+ #include <opencv2/opencv.hpp>
++#if CV_MAJOR_VERSION < 5
+ #include <opencv2/features2d/features2d.hpp>
++#else
++#include <opencv2/features.hpp>
++#endif
+ #ifdef PRE_ROS_IRON
+ #include <cv_bridge/cv_bridge.h>
+ #else

shared/patch/ros2-rtabmap-costmap-plugins.patch

@@ -0,0 +1,13 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index eab748d..233a90d 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -55,7 +55,7 @@ ENDIF()
+
+ # Causes the visibility macros to use dllexport rather than dllimport,
+ # which is appropriate when building the dll but not consuming it.
+-target_compile_definitions(rtabmap_costmap_plugins PRIVATE "RTABMAP_ROS_BUILDING_LIBRARY")
++target_compile_definitions(rtabmap_costmap_plugins PRIVATE "RTABMAP_COSTMAP_PLUGINS_BUILDING_DLL")
+
+ # prevent pluginlib from using boost
+ target_compile_definitions(rtabmap_costmap_plugins PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")

shared/patch/ros2-rtabmap-odom.patch

@@ -0,0 +1,14 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index 7b10ad2..6ef5020 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -14,8 +14,7 @@ if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64")
+   # issues #1285 #1288
+   find_library(
+     builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c
+-    PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib"
+-    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
++    REQUIRED
+   )
+ endif()
+

shared/patch/ros2-rtabmap-odom.win.patch

@@ -0,0 +1,16 @@
+diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp
+index 6a44b86..f0b5ba5 100644
+--- a/src/OdometryROS.cpp
++++ b/src/OdometryROS.cpp
+@@ -56,6 +56,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ #include "rtabmap/utilite/UFile.h"
+ #include "rtabmap/utilite/UMath.h"
+
++// windows.h (wingdi.h) defines ERROR, which breaks diagnostic_msgs' DiagnosticStatus::ERROR
++#ifdef ERROR
++#undef ERROR
++#endif
++
+ #define BAD_COVARIANCE 9999
+
+ using namespace rtabmap;

shared/patch/ros2-rtabmap-rviz-plugins.patch

@@ -0,0 +1,23 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index 4e47a68..4658ddb 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -9,8 +9,7 @@ if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64")
+   # issues #1285 #1288
+   find_library(
+     message_filters_LIB NAMES message_filters
+-    PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib"
+-    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
++    REQUIRED
+   )
+ endif()
+
+@@ -96,7 +95,7 @@ ENDIF()
+
+ # Causes the visibility macros to use dllexport rather than dllimport,
+ # which is appropriate when building the dll but not consuming it.
+-target_compile_definitions(rtabmap_rviz_plugins PRIVATE "RTABMAP_ROS_BUILDING_LIBRARY")
++target_compile_definitions(rtabmap_rviz_plugins PRIVATE "RTABMAP_RVIZ_PLUGINS_BUILDING_DLL")
+
+ # prevent pluginlib from using boost
+ target_compile_definitions(rtabmap_rviz_plugins PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")

shared/patch/ros2-rtabmap-slam.patch

@@ -0,0 +1,27 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index 6107b23..97ea89f 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -14,8 +14,7 @@ if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64")
+   # issues #1285 #1288
+   find_library(
+     builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c
+-    PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib"
+-    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
++    REQUIRED
+   )
+ endif()
+
+diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
+index c1fb75b..bb732d6 100644
+--- a/src/CoreWrapper.cpp
++++ b/src/CoreWrapper.cpp
+@@ -1474,7 +1474,7 @@ void CoreWrapper::commonMultiCameraCallbackImpl(
+ 		cv::Mat leftMono;
+ 		if(rgb.channels() == 3)
+ 		{
+-			cv::cvtColor(rgb, leftMono, CV_BGR2GRAY);
++			cv::cvtColor(rgb, leftMono, cv::COLOR_BGR2GRAY);
+ 		}
+ 		else
+ 		{

shared/patch/ros2-rtabmap-slam.win.patch

@@ -0,0 +1,16 @@
+diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
+index 7321ccc..912832d 100644
+--- a/src/CoreWrapper.cpp
++++ b/src/CoreWrapper.cpp
+@@ -90,6 +90,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+ #include "rtabmap_conversions/MsgConversion.h"
+
++// windows.h (wingdi.h) defines ERROR, which breaks diagnostic_msgs' DiagnosticStatus::ERROR
++#ifdef ERROR
++#undef ERROR
++#endif
++
+ #ifdef PRE_ROS_JAZZY
+ namespace rclcpp {
+ 	rmw_qos_profile_t ServicesQoS() {return rmw_qos_profile_services_default;}

shared/patch/ros2-rtabmap-sync.win.patch

@@ -0,0 +1,38 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index 5e1b4da..fc0f9e7 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -146,6 +146,12 @@ ELSE()
+ ENDIF()
+ target_link_libraries(rtabmap_sync_plugins PUBLIC rtabmap_sync)
+
++# Windows: export CommonDataSubscriber explicitly (exporting every symbol of
++# rtabmap_sync exceeds the 65535 limit of an import library, LNK1189), and
++# everything of the plugins, whose node classes the executables construct.
++target_compile_definitions(rtabmap_sync PRIVATE RTABMAP_SYNC_BUILDING_DLL)
++set_target_properties(rtabmap_sync_plugins PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS ON)
++
+ rclcpp_components_register_nodes(rtabmap_sync_plugins "rtabmap_sync::RGBDSync")
+ rclcpp_components_register_nodes(rtabmap_sync_plugins "rtabmap_sync::StereoSync")
+ rclcpp_components_register_nodes(rtabmap_sync_plugins "rtabmap_sync::RGBSync")
+diff --git a/include/rtabmap_sync/CommonDataSubscriber.h b/include/rtabmap_sync/CommonDataSubscriber.h
+index 1282a4c..ff46ecd 100644
+--- a/include/rtabmap_sync/CommonDataSubscriber.h
++++ b/include/rtabmap_sync/CommonDataSubscriber.h
+@@ -130,7 +130,7 @@ namespace rtabmap_sync {
+  * publishes -- which needs no rebuild and has no camera-count limit, at the cost of one
+  * extra node and one full-frame copy per camera.
+  */
+-class CommonDataSubscriber {
++class RTABMAP_SYNC_PUBLIC CommonDataSubscriber {
+ public:
+ 	/**
+ 	 * @brief Declares the `subscribe_*`, queue and QoS parameters on @p node.
+@@ -142,7 +142,6 @@ public:
+ 	 * @param gui  true for a visualization node: `subscribe_depth` and `subscribe_rgb`
+ 	 *             then default to false, leaving odometry as the only default input
+ 	 */
+-	RTABMAP_SYNC_PUBLIC
+ 	CommonDataSubscriber(rclcpp::Node & node, bool gui);
+ 	virtual ~CommonDataSubscriber();
+

shared/patch/ros2-rtabmap-util.win.patch

@@ -0,0 +1,13 @@
+diff --git a/src/nodelets/db_player.cpp b/src/nodelets/db_player.cpp
+index cacd679..e906d61 100644
+--- a/src/nodelets/db_player.cpp
++++ b/src/nodelets/db_player.cpp
+@@ -543,7 +543,7 @@ bool DbPlayer::publishNextFrame()
+         UASSERT(odomMsg.pose.covariance.size() == 36 &&
+                 odom.covariance().total() == 36 &&
+                 odom.covariance().type() == CV_64FC1);
+-        memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
++        memcpy(odomMsg.pose.covariance.data(), odom.covariance().data, 36*sizeof(double));
+         odometryPub_->publish(odomMsg);
+     }
+

shared/patch/ros2-rtabmap-viz.patch

@@ -0,0 +1,30 @@
+diff --git a/src/GuiNode.cpp b/src/GuiNode.cpp
+index 88895ed..e62af21 100644
+--- a/src/GuiNode.cpp
++++ b/src/GuiNode.cpp
+@@ -32,6 +32,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ #include <rtabmap/gui/MainWindow.h>
+ #include <rtabmap/utilite/ULogger.h>
+ #include <signal.h>
++#include <QSurfaceFormat>
++#include <vtkVersionMacros.h>
++#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1)
++#include <QVTKRenderWidget.h>
++#endif
+
+ QApplication * app = 0;
+
+@@ -49,6 +54,13 @@ int main(int argc, char** argv)
+
+ 	rclcpp::init(argc, argv);
+
++#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1)
++	// needed to ensure appropriate OpenGL context is created for VTK rendering
++	// (without it, macOS creates a legacy OpenGL 2.1 context and VTK crashes).
++	QSurfaceFormat::setDefaultFormat(QVTKRenderWidget::defaultFormat());
++#endif
++	QApplication::setAttribute(Qt::AA_ShareOpenGLContexts);
++
+ 	app = new QApplication(argc, argv);
+ 	app->connect( app, SIGNAL( lastWindowClosed() ), app, SLOT( quit() ) );
+

shared/patch/ros2-rtabmap-viz.win.patch

@@ -0,0 +1,42 @@
+diff --git a/src/GuiNode.cpp b/src/GuiNode.cpp
+index e62af21..2ee0cea 100644
+--- a/src/GuiNode.cpp
++++ b/src/GuiNode.cpp
+@@ -78,11 +78,16 @@ int main(int argc, char** argv)
+
+ 		// Catch ctrl-c to close the gui
+ 		// (Place this after QApplication's constructor)
++#ifdef _WIN32
++		// no sigaction on Windows
++		signal(SIGINT, my_handler);
++#else
+ 		struct sigaction sigIntHandler;
+ 		sigIntHandler.sa_handler = my_handler;
+ 		sigemptyset(&sigIntHandler.sa_mask);
+ 		sigIntHandler.sa_flags = 0;
+ 		sigaction(SIGINT, &sigIntHandler, NULL);
++#endif
+
+ 		// Here start the ROS events loop
+ 		rclcpp::executors::SingleThreadedExecutor executor; //Use 1 thread
+diff --git a/src/RGBDImageViewerNode.cpp b/src/RGBDImageViewerNode.cpp
+index f277ac8..7a0326d 100644
+--- a/src/RGBDImageViewerNode.cpp
++++ b/src/RGBDImageViewerNode.cpp
+@@ -55,11 +55,16 @@ int main(int argc, char** argv)
+
+ 		// Catch ctrl-c to close the gui
+ 		// (Place this after QApplication's constructor)
++#ifdef _WIN32
++		// no sigaction on Windows
++		signal(SIGINT, my_handler);
++#else
+ 		struct sigaction sigIntHandler;
+ 		sigIntHandler.sa_handler = my_handler;
+ 		sigemptyset(&sigIntHandler.sa_mask);
+ 		sigIntHandler.sa_flags = 0;
+ 		sigaction(SIGINT, &sigIntHandler, NULL);
++#endif
+
+ 		// Here start the ROS events loop
+ 		rclcpp::executors::SingleThreadedExecutor executor; //Use 1 thread

shared/pkg_additional_info.yaml

@@ -134,6 +134,27 @@ robot_state_publisher:
   additional_cmake_args: "-DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=ON"
 rosx_introspection:
   additional_cmake_args: "-DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=ON"
+rtabmap_conversions:
+  additional_cmake_args: "-DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=ON"
+# rtabmap_odom/slam/util/viz on Windows: rtabmap::core links some PCL libraries by bare
+# name (pcl_visualization, pcl_keypoints; RTABMapConfig.cmake creates no imported target
+# for them), so add the library directory to the link path; NOMINMAX for Eigen vs windows.h.
+rtabmap_odom:
+  additional_cmake_args:
+    if: win
+    then: '-DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=ON -DCMAKE_CXX_FLAGS_INIT=/DNOMINMAX -DCMAKE_SHARED_LINKER_FLAGS_INIT=/LIBPATH:%LIBRARY_LIB% -DCMAKE_EXE_LINKER_FLAGS_INIT=/LIBPATH:%LIBRARY_LIB%'
+rtabmap_slam:
+  additional_cmake_args:
+    if: win
+    then: '-DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=ON -DCMAKE_CXX_FLAGS_INIT=/DNOMINMAX -DCMAKE_SHARED_LINKER_FLAGS_INIT=/LIBPATH:%LIBRARY_LIB% -DCMAKE_EXE_LINKER_FLAGS_INIT=/LIBPATH:%LIBRARY_LIB%'
+rtabmap_util:
+  additional_cmake_args:
+    if: win
+    then: '-DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=ON -DCMAKE_CXX_FLAGS_INIT=/DNOMINMAX -DCMAKE_SHARED_LINKER_FLAGS_INIT=/LIBPATH:%LIBRARY_LIB% -DCMAKE_EXE_LINKER_FLAGS_INIT=/LIBPATH:%LIBRARY_LIB%'
+rtabmap_viz:
+  additional_cmake_args:
+    if: win
+    then: '-DCMAKE_CXX_FLAGS_INIT=/DNOMINMAX -DCMAKE_SHARED_LINKER_FLAGS_INIT=/LIBPATH:%LIBRARY_LIB% -DCMAKE_EXE_LINKER_FLAGS_INIT=/LIBPATH:%LIBRARY_LIB%'
 sbg_driver:
   additional_cmake_args:
     if: win
@@ -158,6 +179,10 @@ urdfdom_py:
   generate_dummy_package_with_run_deps:
     dep_name: urdfdom-py
     max_pin: 'x.x'
+velodyne_laserscan:  # Windows (pulled in by rtabmap_examples): M_PI, and its library exports no symbols
+  additional_cmake_args:
+    if: win
+    then: '-DCMAKE_CXX_FLAGS_INIT=/D_USE_MATH_DEFINES -DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=ON'
 visp:
   generate_dummy_package_with_run_deps:
     dep_name: visp

shared/tests/ros2-rtabmap-slam.yaml

@@ -0,0 +1,10 @@
+tests:
+  # Smoke test: `rtabmap_slam/rtabmap` (the SLAM node) starts (loads its libraries and the RMW) and is still running after 5 s.
+  - if: unix
+    then:
+      script:
+        - python -c "import os, subprocess, sys, time; p = subprocess.Popen([os.path.join(os.environ['PREFIX'], 'lib', 'rtabmap_slam', 'rtabmap')]); time.sleep(5); r = p.poll(); p.terminate(); sys.exit(0 if r is None else 1)"
+  - if: win
+    then:
+      script:
+        - if not exist %LIBRARY_LIB%\rtabmap_slam\rtabmap.exe exit 1

shared/vinca.yaml

@@ -322,6 +322,7 @@ packages_select_by_deps:
   - rqt_robot_steering
   - rqt_runtime_monitor
   - rtabmap
+  - rtabmap_ros # requested in https://github.com/RoboStack/ros-lyrical/pull/83
   - rtcm_msgs
   - rtest
   - rviz2
@@ -604,6 +605,14 @@ packages_select_by_deps:
 
 packages_exclude:
 
+  - if: win
+    then:
+      # velodyne_driver reads the sensor over POSIX sockets (and needs libpcap); also left
+      # out of the packages depending on them (rtabmap_examples)
+      - velodyne
+      - velodyne_driver
+      - velodyne_pointcloud
+
   - if: not linux
     then:
       - pendulum_control