merged ros2 -> humble-devel

.github/workflows/ros1.yml

@@ -1,67 +0,0 @@
-name: ros1
-
-on:
-  push:
-    branches: [ master ]
-  pull_request:
-    branches: [ master ]
-
-env:
-  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
-  BUILD_TYPE: Release
-
-jobs: 
-  build:
-    # Disabling because Ubuntu 20.04 doesn't exist anymore on CI:
-    #   This is a scheduled Ubuntu 20.04 retirement. Ubuntu 20.04 LTS 
-    #   runner will be removed on 2025-04-15. For more details, see https://github.com/actions/runner-images/issues/11101
-    if: false
-
-    # The CMake configure and build commands are platform agnostic and should work equally
-    # well on Windows or Mac.  You can convert this to a matrix build if you need
-    # cross-platform coverage.
-    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
-    name: Build on ros ${{ matrix.ros_distro }} and ${{ matrix.os }}
-    runs-on: ${{ matrix.os }}
-    strategy:
-      matrix:
-        os: [ubuntu-20.04]
-        include:
-        - os: ubuntu-20.04
-          ros_distro: 'noetic'
-
-
-    steps:   
-    - uses: ros-tooling/[email protected]
-      with:
-        required-ros-distributions: ${{ matrix.ros_distro }}
-
-    - name: Install dependencies
-      run: |
-        sudo apt-get update
-        sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros python3-catkin-tools
-        sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap
-        sudo pip3 uninstall empy --yes
-
-    - name: Setup catkin workspace
-      run: | 
-        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
-        mkdir -p ${{github.workspace}}/catkin_ws/src
-        cd ${{github.workspace}}/catkin_ws/src
-        cd ..
-        catkin config --init --cmake-args -DSETUPTOOLS_DEB_LAYOUT=OFF -DCMAKE_C_FLAGS="-Wformat -Werror=format-security" -DCMAKE_CXX_FLAGS="-Wformat -Werror=format-security"
-
-    - uses: actions/checkout@v2
-      with:
-        repository: 'introlab/rtabmap'
-        path: 'catkin_ws/src/rtabmap'
-
-    - uses: actions/checkout@v2
-      with:
-        path: 'catkin_ws/src/rtabmap_ros'
-
-    - name: caktkin build
-      run: |
-        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
-        cd ${{github.workspace}}/catkin_ws
-        catkin build -p 1 -i --verbose

.github/workflows/ros2.yml

@@ -17,6 +17,9 @@ jobs:
     strategy:
       matrix:
         ros_distro: [humble]
+        include:
+        - ros_distro: humble
+          skip_keys: ''
       fail-fast: false
     container:
       image: osrf/ros:${{ matrix.ros_distro }}-desktop-full
@@ -34,3 +37,4 @@ jobs:
         target-ros2-distro: ${{ matrix.ros_distro }}
         vcs-repo-file-url: /tmp/deps.repos
         rosdep-check: true
+        rosdep-skip-keys: "${{ matrix.skip_keys }}"

docker/humble/latest/Dockerfile

@@ -1,20 +1,17 @@
-FROM introlab3it/rtabmap:22.04
+FROM introlab3it/rtabmap:jammy
 
-RUN source /ros_entrypoint.sh && \
+RUN mkdir -p ros2_ws/src
-    mkdir -p ros2_ws/src && \
-    cd ros2_ws/src
 
 COPY . ros2_ws/src/rtabmap_ros
 
 RUN source /ros_entrypoint.sh && \
     cd ros2_ws && \
-    export MAKEFLAGS="-j1" && \
+    export MAKEFLAGS="-j2" && \
     rosdep init && \
     rosdep update && \
     apt-get update && \
-    rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool && \
+    rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap" && \
-    apt remove ros-$ROS_DISTRO-rtabmap* -y && \
     apt-get clean && rm -rf /var/lib/apt/lists/ && \
-    colcon build --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    colcon build --executor sequential --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args --debug-find -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
     cd && \
     rm -rf ros2_ws

docker/humble/latest/hooks/build

@@ -1,2 +0,0 @@
-#!/bin/bash 
-docker build --build-arg CACHE_DATE="$(date)" --cache-from $IMAGE_NAME -f $DOCKERFILE_PATH -t $IMAGE_NAME -t $DOCKER_REPO:humble-latest  .

docker/jazzy/latest/Dockerfile

@@ -1,4 +1,4 @@
-FROM introlab3it/rtabmap:24.04
+FROM introlab3it/rtabmap:noble
 
 RUN source /ros_entrypoint.sh && \
     mkdir -p ros2_ws/src && \
@@ -8,12 +8,12 @@ COPY . ros2_ws/src/rtabmap_ros
 
 RUN source /ros_entrypoint.sh && \
     cd ros2_ws && \
-    export MAKEFLAGS="-j1" && \
+    export MAKEFLAGS="-j2" && \
     rosdep init && \
     rosdep update && \
     apt-get update && \
     rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap grid_map_ros" && \
     apt-get clean && rm -rf /var/lib/apt/lists/ && \
-    colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args --debug-find -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
     cd && \
     rm -rf ros2_ws

docker/kilted/latest/Dockerfile

@@ -1,4 +1,4 @@
-FROM introlab3it/rtabmap:24.04
+FROM introlab3it/rtabmap:noble-kilted
 
 RUN source /ros_entrypoint.sh && \
     mkdir -p ros2_ws/src && \
@@ -8,12 +8,12 @@ COPY . ros2_ws/src/rtabmap_ros
 
 RUN source /ros_entrypoint.sh && \
     cd ros2_ws && \
-    export MAKEFLAGS="-j1" && \
+    export MAKEFLAGS="-j2" && \
     rosdep init && \
     rosdep update && \
     apt-get update && \
     rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros" && \
     apt-get clean && rm -rf /var/lib/apt/lists/ && \
-    colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args --debug-find -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
     cd && \
     rm -rf ros2_ws

rtabmap_conversions/CMakeLists.txt

@@ -5,6 +5,16 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+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
+  )
+endif()
+
+
 find_package(ament_cmake REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(geometry_msgs REQUIRED)

rtabmap_demos/README.md

@@ -7,6 +7,7 @@
 + [Turtlebot3 Nav2 and 2D LiDAR SLAM](#turtlebot3-nav2-and-2d-lidar-slam)
 + [Turtlebot3 Nav2 and RGB-D SLAM](#turtlebot3-nav2-and-rgb-d-slam)
 + [Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-2d-lidar-and-rgb-d-slam)
++ [Turtlebot3 Nav2, Fake 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-fake-2d-lidar-and-rgb-d-slam)
 + [Champ Quadruped Nav2, Elevation Map and VSLAM](#champ-quadruped-nav2-elevation-map-and-vslam)
 + [Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-2d-lidar-and-rgb-d-slam)
 + [Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-3d-lidar-and-rgb-d-slam)
@@ -50,6 +51,14 @@
 [turtlebot3_sim_rgbd_scan_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py)
 
 ![Peek 2024-11-29 13-41](https://github.com/user-attachments/assets/2e878158-b1b6-48a4-801c-72cdb41b4783)
+### Turtlebot3 Nav2, Fake 2D LiDAR and RGB-D SLAM
+[turtlebot3_sim_rgbd_fake_scan_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py)
+
+ * Red: Scan generated from camera's depth.
+ * Orange: Locally assembled scans used for proximity detection.
+ * Yellow: The map.
+
+![Peek 2025-07-04 16-57](https://github.com/user-attachments/assets/8869cf57-35a1-4236-bdab-151b88ae2ea1)
 ### Champ Quadruped Nav2, Elevation Map and VSLAM
 [champ_sim_vslam.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/champ/champ_sim_vslam.launch.py)
 

rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py

@@ -0,0 +1,143 @@
+# Example:
+#
+#   Bringup turtlebot3:
+#     $ export TURTLEBOT3_MODEL=waffle
+#     $ export LDS_MODEL=LDS-01
+#     $ ros2 launch turtlebot3_bringup robot.launch.py
+#
+#   SLAM:
+#     $ ros2 launch rtabmap_demos turtlebot3_rgbd_fake_scan.launch.py
+#
+#   Navigation (install nav2_bringup package):
+#     $ ros2 launch nav2_bringup navigation_launch.py
+#     $ ros2 launch nav2_bringup rviz_launch.py
+#
+#   Teleop:
+#     $ ros2 run turtlebot3_teleop teleop_keyboard
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    localization = LaunchConfiguration('localization')
+
+    parameters={
+          'frame_id':'base_footprint',
+          'use_sim_time':use_sim_time,
+          'subscribe_rgbd':True,
+          'subscribe_scan_cloud':True,
+          'use_action_for_goal':True,
+          'scan_cloud_is_2d': True,
+          # RTAB-Map's parameters should be strings:
+          'Reg/Strategy':'1',
+          'Reg/Force3DoF':'true',
+          'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
+    }
+
+    remappings=[
+          ('rgb/image', '/camera/image_raw'),
+          ('rgb/camera_info', '/camera/camera_info'),
+          ('depth/image', '/camera/depth/image_raw'),
+          ('scan_cloud', 'assembled_cloud')]
+
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'use_sim_time', default_value='false',
+            description='Use simulation (Gazebo) clock if true'),
+
+        DeclareLaunchArgument(
+            'localization', default_value='false',
+            description='Launch in localization mode.'),
+
+        # Nodes to launch
+        Node(
+            package='rtabmap_sync', executable='rgbd_sync', output='screen',
+            parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
+            remappings=remappings),
+
+        # Convert middle row of depth pixels to a fake laser scan
+        Node(
+            package='depthimage_to_laserscan', executable='depthimage_to_laserscan_node', output='screen',
+            parameters=[{
+                'use_sim_time':use_sim_time,
+                'range_max': 5.0
+            }],
+            remappings=[
+                ('depth', '/camera/depth/image_raw'),
+                ('depth_camera_info', '/camera/camera_info'),
+                ('scan', '/camera/scan')
+            ]),
+        
+        # Just to convert the fake laser scan to PointCloud2
+        Node(
+            package='rtabmap_util', executable='lidar_deskewing', output='screen',
+            parameters=[{'use_sim_time':use_sim_time,
+                         'fixed_frame_id': 'camera_link'}], # use camera frame
+            remappings=[
+                ('input_scan', '/camera/scan')
+            ]),
+        
+        # Assemble the fake laser scans using a circular buffer, then feed that cloud to rtabmap
+        Node(
+            package='rtabmap_util', executable='point_cloud_assembler', output='screen',
+            parameters=[{'use_sim_time':use_sim_time,
+                         'max_clouds': 20,
+                         'voxel_size': 0.05,
+                         'wait_for_transform': 1.0,
+                         'linear_update': 0.3,
+                         'angular_update': 0.5,
+                         'circular_buffer': True,
+                         'frame_id': 'base_link'}],
+            remappings=[
+                ('assembled_cloud', 'assembled_cloud'),
+                ('cloud', '/camera/scan/deskewed')
+            ]),
+
+        # SLAM Mode:
+        Node(
+            condition=UnlessCondition(localization),
+            package='rtabmap_slam', executable='rtabmap', output='screen',
+            parameters=[parameters],
+            remappings=remappings,
+            arguments=['-d']),
+            
+        # Localization mode:
+        Node(
+            condition=IfCondition(localization),
+            package='rtabmap_slam', executable='rtabmap', output='screen',
+            parameters=[parameters,
+              {'Mem/IncrementalMemory':'False',
+               'Mem/InitWMWithAllNodes':'True'}],
+            remappings=remappings),
+
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            parameters=[parameters],
+            remappings=remappings),
+        
+        # Obstacle detection with the camera for nav2 local costmap.
+        # First, we need to convert depth image to a point cloud.
+        # Second, we segment the floor from the obstacles.
+        Node(
+            package='rtabmap_util', executable='point_cloud_xyz', output='screen',
+            parameters=[{'decimation': 2,
+                         'max_depth': 3.0,
+                         'voxel_size': 0.02}],
+            remappings=[('depth/image', '/camera/depth/image_raw'),
+                        ('depth/camera_info', '/camera/camera_info'),
+                        ('cloud', '/camera/cloud')]),
+        Node(
+            package='rtabmap_util', executable='obstacles_detection', output='screen',
+            parameters=[parameters],
+            remappings=[('cloud', '/camera/cloud'),
+                        ('obstacles', '/camera/obstacles'),
+                        ('ground', '/camera/ground')]),
+    ])

rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py

@@ -0,0 +1,117 @@
+# Requirements:
+#   Install Turtlebot3 packages
+#   Modify turtlebot3_waffle SDF:
+#     1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf
+#     2) Add
+#          <joint name="camera_rgb_optical_joint" type="fixed">
+#            <parent>camera_rgb_frame</parent>
+#            <child>camera_rgb_optical_frame</child>
+#            <pose>0 0 0 -1.57079632679 0 -1.57079632679</pose>
+#            <axis>
+#              <xyz>0 0 1</xyz>
+#            </axis>
+#          </joint> 
+#     3) Rename <link name="camera_rgb_frame"> to <link name="camera_rgb_optical_frame">
+#     4) Add <link name="camera_rgb_frame"/>
+#     5) Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
+#     6) Change image width/height from 1920x1080 to 640x480
+# Example:
+#   $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_fake_scan_demo.launch.py
+#
+#   Teleop:
+#     $ ros2 run turtlebot3_teleop teleop_keyboard
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackageShare
+
+import os
+
+def launch_setup(context, *args, **kwargs):
+    if not 'TURTLEBOT3_MODEL' in os.environ:
+        os.environ['TURTLEBOT3_MODEL'] = 'waffle'
+
+    # Directories
+    pkg_turtlebot3_gazebo = get_package_share_directory(
+        'turtlebot3_gazebo')
+    pkg_nav2_bringup = get_package_share_directory(
+        'nav2_bringup')
+    pkg_rtabmap_demos = get_package_share_directory(
+        'rtabmap_demos')
+
+    world = LaunchConfiguration('world').perform(context)
+    
+    nav2_params_file = PathJoinSubstitution(
+        [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml']
+    )
+
+    # Paths
+    gazebo_launch = PathJoinSubstitution(
+        [pkg_turtlebot3_gazebo, 'launch', f'turtlebot3_{world}.launch.py'])
+    nav2_launch = PathJoinSubstitution(
+        [pkg_nav2_bringup, 'launch', 'navigation_launch.py'])
+    rviz_launch = PathJoinSubstitution(
+        [pkg_nav2_bringup, 'launch', 'rviz_launch.py'])
+    rtabmap_launch = PathJoinSubstitution(
+        [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd_fake_scan.launch.py'])
+
+    # Includes
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([gazebo_launch]),
+        launch_arguments=[
+            ('x_pose', LaunchConfiguration('x_pose')),
+            ('y_pose', LaunchConfiguration('y_pose'))
+        ]
+    )
+    nav2 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([nav2_launch]),
+        launch_arguments=[
+            ('use_sim_time', 'true'),
+            ('params_file', nav2_params_file)
+        ]
+    )
+    rviz = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([rviz_launch])
+    )
+    rtabmap = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([rtabmap_launch]),
+        launch_arguments=[
+            ('localization', LaunchConfiguration('localization')),
+            ('use_sim_time', 'true')
+        ]
+    )
+    return [
+        # Nodes to launch
+        nav2,
+        rviz,
+        rtabmap,
+        gazebo
+    ]
+
+def generate_launch_description():
+    return LaunchDescription([
+        
+        # Launch arguments
+        DeclareLaunchArgument(
+            'localization', default_value='false',
+            description='Launch in localization mode.'),
+        
+        DeclareLaunchArgument(
+            'world', default_value='house',
+            choices=['world', 'house', 'dqn_stage1', 'dqn_stage2', 'dqn_stage3', 'dqn_stage4'],
+            description='Turtlebot3 gazebo world.'),
+        
+        DeclareLaunchArgument(
+            'x_pose', default_value='-2.0',
+            description='Initial position of the robot in the simulator.'),
+        
+        DeclareLaunchArgument(
+            'y_pose', default_value='0.5',
+            description='Initial position of the robot in the simulator.'),
+
+        OpaqueFunction(function=launch_setup)
+    ])

rtabmap_msgs/CMakeLists.txt

@@ -10,6 +10,15 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64")
+  # issues #1285 #1288
+  find_library(
+    rcutils_LIB NAMES rcutils
+    PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib"
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
+  )
+endif()
+
 ##################
 ## Dependencies ##
 ##################

rtabmap_msgs/package.xml

@@ -14,6 +14,8 @@
 
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
+  <build_depend>ros_environment</build_depend>
+
   <depend>builtin_interfaces</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>

rtabmap_odom/CMakeLists.txt

@@ -10,6 +10,15 @@ if(POLICY CMP0074)
     cmake_policy(SET CMP0074 NEW)
 endif()
 
+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
+  )
+endif()
+      
 find_package(ament_cmake_ros REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(image_geometry REQUIRED)

rtabmap_odom/include/rtabmap_odom/OdometryROS.h

@@ -173,6 +173,7 @@ private:
 	rtabmap::Transform guess_;
 	rtabmap::Transform guessPreviousPose_;
 	double previousStamp_;
+	double previousClockTime_;
 	double expectedUpdateRate_;
 	double maxUpdateRate_;
 	double minUpdateRate_;

rtabmap_odom/package.xml

@@ -12,6 +12,8 @@
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
+  <build_depend>ros_environment</build_depend>
+
   <depend>cv_bridge</depend>
   <depend>image_geometry</depend>
   <depend>laser_geometry</depend>

rtabmap_odom/src/OdometryROS.cpp

@@ -84,7 +84,11 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	paused_(false),
 	resetCountdown_(0),
 	resetCurrentCount_(0),
+	stereoParams_(false),
+	visParams_(false),
+	icpParams_(false),
 	previousStamp_(0.0),
+	previousClockTime_(0.0),
 	expectedUpdateRate_(0.0),
 	maxUpdateRate_(0.0),
 	minUpdateRate_(0.0),
@@ -577,7 +581,36 @@ void OdometryROS::mainLoop()
 	Transform groundTruth;
 	if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
 	{
-		if(previousStamp_>0.0 && previousStamp_ >= rtabmap_conversions::timestampFromROS(header.stamp))
+		// Detect time jump in the past
+		double clockNow = now().seconds();
+		if(previousClockTime_ > clockNow)
+		{
+			RCLCPP_WARN(this->get_logger(), "Odometry: Detected jump back in time of %f sec. Odometry is "
+				"automatically reset to latest computed pose!",
+				previousClockTime_ - clockNow);
+			SensorData dataCpy = dataToProcess_;
+			std_msgs::msg::Header headerCpy = dataHeaderToProcess_;
+			double previousCpy = previousClockTime_;
+			this->reset(odometry_->getPose());
+			if(clockNow > rtabmap_conversions::timestampFromROS(headerCpy.stamp)) {
+				// new frame is using new clock, process it now
+				dataToProcess_ = dataCpy;
+				dataHeaderToProcess_ = headerCpy;
+				dataReady_.release();
+				RCLCPP_WARN(this->get_logger(), "Odometry: Restarting with frame: %f (clock previous=%f, new=%f)",
+					rtabmap_conversions::timestampFromROS(headerCpy.stamp), previousCpy, clockNow);
+			}
+			else {
+				// skip that old frame
+				RCLCPP_WARN(this->get_logger(), "Odometry: skipping frame: %f (clock previous=%f, new=%f)",
+					rtabmap_conversions::timestampFromROS(headerCpy.stamp), previousCpy, clockNow);
+			}
+			previousClockTime_ = clockNow;
+			return;
+		}
+		previousClockTime_ = clockNow;
+
+		if(previousStamp_ >= rtabmap_conversions::timestampFromROS(header.stamp))
 		{
 			RCLCPP_WARN(this->get_logger(), "Odometry: Detected not valid consecutive stamps (previous=%fs new=%fs). "
 					"New stamp should be always greater than previous stamp. This new data is ignored.",
@@ -677,8 +710,18 @@ void OdometryROS::mainLoop()
 						correctionMsg.header.stamp = header.stamp;
 						Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse();
 						rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform);
+
+						double time_now = now().seconds();
+						if(time_now >= previousClockTime_) {
 							tfBroadcaster_->sendTransform(correctionMsg);
 						}
+						else {
+							RCLCPP_WARN(this->get_logger(), "TF %s->%s is not published because we detected a time jump in the past of %f sec.",
+								correctionMsg.header.frame_id.c_str(),
+								correctionMsg.child_frame_id.c_str(),
+								previousClockTime_ - time_now);
+						}
+					}
 					guessPreviousPose_ = guessCurrentPose;
 					return;
 				}
@@ -731,12 +774,31 @@ void OdometryROS::mainLoop()
 				correctionMsg.header.stamp = header.stamp;
 				Transform correction = pose * guessCurrentPose.inverse();
 				rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform);
+
+				double time_now = now().seconds();
+				if(time_now >= previousClockTime_) {
 					tfBroadcaster_->sendTransform(correctionMsg);
 				}
+				else {
+					RCLCPP_WARN(this->get_logger(), "TF %s->%s is not published because we detected a time jump in the past of %f sec.",
+						correctionMsg.header.frame_id.c_str(),
+						correctionMsg.child_frame_id.c_str(),
+						previousClockTime_ - time_now);
+				}
+			}
 			else
 			{
+				double time_now = now().seconds();
+				if(time_now >= previousClockTime_) {
 					tfBroadcaster_->sendTransform(poseMsg);
 				}
+				else {
+					RCLCPP_WARN(this->get_logger(), "TF %s->%s is not published because we detected a time jump in the past of %f sec.",
+						poseMsg.header.frame_id.c_str(),
+						poseMsg.child_frame_id.c_str(),
+						previousClockTime_ - time_now);
+				}
+			}
 		}
 
 		if(odomPub_->get_subscription_count())
@@ -927,8 +989,19 @@ void OdometryROS::mainLoop()
 			correctionMsg.header.stamp = header.stamp;
 			Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse();
 			rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform);
+			double time_now = now().seconds();
+			if(time_now >= previousClockTime_) {
 				tfBroadcaster_->sendTransform(correctionMsg);
 			}
+			else {
+				RCLCPP_WARN(this->get_logger(), "TF %s->%s is not published because its stamp (%f) is greater "
+					"than current time (%f), possible time jump happened!",
+					correctionMsg.header.frame_id.c_str(),
+					correctionMsg.child_frame_id.c_str(),
+					rtabmap_conversions::timestampFromROS(correctionMsg.header.stamp),
+					time_now);
+			}
+		}
 
 	}
 
@@ -1167,6 +1240,7 @@ void OdometryROS::reset(const Transform & pose)
 	guess_.setNull();
 	guessPreviousPose_.setNull();
 	previousStamp_ = 0.0;
+	previousClockTime_ = 0.0;
 	resetCurrentCount_ = resetCountdown_;
 	imuProcessed_ = false;
 	dataToProcess_ = SensorData();

rtabmap_rviz_plugins/CMakeLists.txt

@@ -5,6 +5,15 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+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
+  )
+endif()
+
 find_package(ament_cmake_ros REQUIRED)
 find_package(pcl_conversions REQUIRED)
 find_package(pluginlib REQUIRED)

rtabmap_rviz_plugins/package.xml

@@ -12,6 +12,8 @@
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
+  <build_depend>ros_environment</build_depend>
+
   <depend>pcl_conversions</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>

rtabmap_slam/CMakeLists.txt

@@ -10,6 +10,15 @@ if(POLICY CMP0074)
     cmake_policy(SET CMP0074 NEW)
 endif()
 
+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
+  )
+endif()
+
 find_package(ament_cmake REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -29,6 +38,10 @@ find_package(rtabmap_sync REQUIRED)
 
 #optional
 find_package(apriltag_msgs)
+find_package(aruco_msgs)
+find_package(aruco_markers_msgs)
+find_package(aruco_opencv_msgs)
+find_package(ros2_aruco_interfaces)
 find_package(nav2_msgs)
 
 IF(WIN32)
@@ -79,6 +92,46 @@ SET(Libraries
 )
 ENDIF(apriltag_msgs_FOUND)
 
+# If aruco_msgs is found, add definition
+IF(aruco_msgs_FOUND)
+MESSAGE(STATUS "WITH aruco_msgs")
+ADD_DEFINITIONS("-DWITH_ARUCO_MSGS")
+SET(Libraries
+   ${Libraries}
+   aruco_msgs
+)
+ENDIF(aruco_msgs_FOUND)
+
+# If aruco_opencv_msgs is found, add definition
+IF(aruco_opencv_msgs_FOUND)
+MESSAGE(STATUS "WITH aruco_opencv_msgs")
+ADD_DEFINITIONS("-DWITH_ARUCO_OPENCV_MSGS")
+SET(Libraries
+   ${Libraries}
+   aruco_opencv_msgs
+)
+ENDIF(aruco_opencv_msgs_FOUND)
+
+# If aruco_markers_msgs is found, add definition
+IF(aruco_markers_msgs_FOUND)
+MESSAGE(STATUS "WITH aruco_markers_msgs")
+ADD_DEFINITIONS("-DWITH_ARUCO_MARKERS_MSGS")
+SET(Libraries
+   ${Libraries}
+   aruco_markers_msgs
+)
+ENDIF(aruco_markers_msgs_FOUND)
+
+# If ros2_aruco_interfaces is found, add definition
+IF(ros2_aruco_interfaces_FOUND)
+MESSAGE(STATUS "WITH ros2_aruco_interfaces")
+ADD_DEFINITIONS("-DWITH_ROS2_ARUCO_INTERFACES")
+SET(Libraries
+   ${Libraries}
+   ros2_aruco_interfaces
+)
+ENDIF(ros2_aruco_interfaces_FOUND)
+
 # If nav2_msgs is found, add definition
 IF(nav2_msgs_FOUND)
 MESSAGE(STATUS "WITH nav2_msgs")

rtabmap_slam/include/rtabmap_slam/CoreWrapper.h

@@ -88,6 +88,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <apriltag_msgs/msg/april_tag_detection_array.hpp>
 #endif
 
+#ifdef WITH_ARUCO_MSGS
+#include <aruco_msgs/msg/marker_array.hpp>
+#endif
+
+#ifdef WITH_ARUCO_OPENCV_MSGS
+#include <aruco_opencv_msgs/msg/aruco_detection.hpp>
+#endif
+
+#ifdef WITH_ARUCO_MARKERS_MSGS
+#include <aruco_markers_msgs/msg/marker_array.hpp>
+#endif
+
+#ifdef WITH_ROS2_ARUCO_INTERFACES
+#include <ros2_aruco_interfaces/msg/aruco_markers.hpp>
+#endif
+
 #ifdef WITH_NAV2_MSGS
 #include <nav2_msgs/action/navigate_to_pose.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
@@ -178,7 +194,20 @@ private:
 	void landmarkDetectionAsyncCallback(const rtabmap_msgs::msg::LandmarkDetection::SharedPtr landmarkDetection);
 	void landmarkDetectionsAsyncCallback(const rtabmap_msgs::msg::LandmarkDetections::SharedPtr landmarkDetections);
 #ifdef WITH_APRILTAG_MSGS
-	void tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr tagDetections);
+	void tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg);
+	void apriltagAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg);
+#endif
+#ifdef WITH_ARUCO_MSGS
+	void arucoAsyncCallback(const aruco_msgs::msg::MarkerArray::SharedPtr msg);
+#endif
+#ifdef WITH_ARUCO_OPENCV_MSGS
+	void arucoOpencvAsyncCallback(const aruco_opencv_msgs::msg::ArucoDetection::SharedPtr msg);
+#endif
+#ifdef WITH_ARUCO_MARKERS_MSGS
+	void arucoMarkersAsyncCallback(const aruco_markers_msgs::msg::MarkerArray::SharedPtr msg);
+#endif
+#ifdef WITH_ROS2_ARUCO_INTERFACES
+	void arucoInterfacesAsyncCallback(const ros2_aruco_interfaces::msg::ArucoMarkers::SharedPtr msg);
 #endif
 #ifdef WITH_FIDUCIAL_MSGS
 	void fiducialDetectionsAsyncCallback(const fiducial_msgs::msgs::FiducialTransformArray::SharedPtr fiducialDetections);
@@ -420,6 +449,19 @@ private:
 	rclcpp::Subscription<rtabmap_msgs::msg::LandmarkDetections>::SharedPtr landmarkDetectionsSub_;
 #ifdef WITH_APRILTAG_MSGS
 	rclcpp::Subscription<apriltag_msgs::msg::AprilTagDetectionArray>::SharedPtr tagDetectionsSub_;
+	rclcpp::Subscription<apriltag_msgs::msg::AprilTagDetectionArray>::SharedPtr apriltagSub_;
+#endif
+#ifdef WITH_ARUCO_MSGS
+	rclcpp::Subscription<aruco_msgs::msg::MarkerArray>::SharedPtr arucoSub_;
+#endif
+#ifdef WITH_ARUCO_OPENCV_MSGS
+	rclcpp::Subscription<aruco_opencv_msgs::msg::ArucoDetection>::SharedPtr arucoOpencvSub_;
+#endif
+#ifdef WITH_ARUCO_MARKERS_MSGS
+	rclcpp::Subscription<aruco_markers_msgs::msg::MarkerArray>::SharedPtr arucoMarkersSub_;
+#endif
+#ifdef WITH_ROS2_ARUCO_INTERFACES
+	rclcpp::Subscription<ros2_aruco_interfaces::msg::ArucoMarkers>::SharedPtr arucoInterfacesSub_;
 #endif
 #ifdef WITH_FIDUCIAL_MSGS
 	rclcpp::Subscription<fiducial_msgs::msg::FiducialTransformArray>::SharedPtr fiducialTransfromsSub_;

rtabmap_slam/package.xml

@@ -12,6 +12,8 @@
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
+  <build_depend>ros_environment</build_depend>
+
   <depend>cv_bridge</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
@@ -25,6 +27,9 @@
   <depend>tf2_ros</depend>
   <depend>visualization_msgs</depend>
   <depend>apriltag_msgs</depend>
+  <depend>aruco_msgs</depend>
+  <depend>aruco_opencv_msgs</depend>
+  <!-- depend>aruco_markers_msgs</depend --> <!-- binaries only available on humble -->
   
   <depend>rtabmap_msgs</depend>
   <depend>rtabmap_util</depend>

rtabmap_slam/src/CoreWrapper.cpp

@@ -718,12 +718,11 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 				mapToOdomMutex_.lock();
 				if(!odomFrameId_.empty())
 				{
-					rclcpp::Time tfExpiration = now() + rclcpp::Duration::from_seconds(tfTolerance);
 					geometry_msgs::msg::TransformStamped msg;
+					rtabmap_conversions::transformToGeometryMsg(mapToOdom_, msg.transform);
 					msg.child_frame_id = odomFrameId_;
 					msg.header.frame_id = mapFrameId_;
-					msg.header.stamp = tfExpiration;
+					msg.header.stamp = now() + rclcpp::Duration::from_seconds(tfTolerance);
-					rtabmap_conversions::transformToGeometryMsg(mapToOdom_, msg.transform);
 					tfBroadcaster_->sendTransform(msg);
 				}
 				mapToOdomMutex_.unlock();
@@ -886,6 +885,19 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	landmarkDetectionsSub_ = this->create_subscription<rtabmap_msgs::msg::LandmarkDetections>("landmark_detections", 1, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions);
 #ifdef WITH_APRILTAG_MSGS
 	tagDetectionsSub_ = this->create_subscription<apriltag_msgs::msg::AprilTagDetectionArray>("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions);
+	apriltagSub_ = this->create_subscription<apriltag_msgs::msg::AprilTagDetectionArray>("apriltag/detections", 5, std::bind(&CoreWrapper::apriltagAsyncCallback, this, std::placeholders::_1), landmarkSubOptions);
+#endif
+#ifdef WITH_ARUCO_MSGS
+	arucoSub_ = this->create_subscription<aruco_msgs::msg::MarkerArray>("aruco/detections", 5, std::bind(&CoreWrapper::arucoAsyncCallback, this, std::placeholders::_1), landmarkSubOptions);
+#endif
+#ifdef WITH_ARUCO_OPENCV_MSGS
+	arucoOpencvSub_ = this->create_subscription<aruco_opencv_msgs::msg::ArucoDetection>("aruco_opencv/detections", 5, std::bind(&CoreWrapper::arucoOpencvAsyncCallback, this, std::placeholders::_1), landmarkSubOptions);
+#endif
+#ifdef WITH_ARUCO_MARKERS_MSGS
+	arucoMarkersSub_ = this->create_subscription<aruco_markers_msgs::msg::MarkerArray>("aruco_markers/detections", 5, std::bind(&CoreWrapper::arucoMarkersAsyncCallback, this, std::placeholders::_1), landmarkSubOptions);
+#endif
+#ifdef WITH_ROS2_ARUCO_INTERFACES
+	arucoInterfacesSub_ = this->create_subscription<ros2_aruco_interfaces::msg::ArucoMarkers>("aruco_interfaces/detections", 5, std::bind(&CoreWrapper::arucoInterfacesAsyncCallback, this, std::placeholders::_1), landmarkSubOptions);
 #endif
 #ifdef WITH_FIDUCIAL_MSGS
 	fiducialTransfromsSub_ = this->create_subscription<fiducial_msgs::msg::FiducialTransformArray>("fiducial_transforms", 5, std::bind(&CoreWrapper::fiducialDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions);
@@ -2651,18 +2663,30 @@ void CoreWrapper::landmarkDetectionsAsyncCallback(const rtabmap_msgs::msg::Landm
 }
 
 #ifdef WITH_APRILTAG_MSGS
-void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr tagDetections)
+void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg)
+{
+	if(!paused_)
+	{
+		static bool warningShow = false;
+		if(!warningShow) {
+			RCLCPP_WARN(this->get_logger(), "\"tag_detections\" input topic name for apriltag_msgs is deprecated, remap \"apriltag\" input topic name instead. This message is only printed once.");
+			warningShow = true;
+		}
+		apriltagAsyncCallback(msg);
+	}
+}
+void CoreWrapper::apriltagAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg)
 {
 	if(!paused_)
 	{
 		UScopeMutex lock(landmarksMutex_);
-		for(unsigned int i=0; i<tagDetections->detections.size(); ++i)
+		for(unsigned int i=0; i<msg->detections.size(); ++i)
 		{
-			std::string tagFrameId = tagDetections->detections[i].family+":"+uNumber2Str(tagDetections->detections[i].id);
+			std::string tagFrameId = msg->detections[i].family+":"+uNumber2Str(msg->detections[i].id);
 			Transform camToTag = rtabmap_conversions::getTransform(
-				tagDetections->header.frame_id, // e.g., camera_optical_frame
+				msg->header.frame_id, // e.g., camera_optical_frame
 				tagFrameId,                     // e.g., tag36h11:42
-				tagDetections->header.stamp,
+				msg->header.stamp,
 				*tfBuffer_,
 				waitForTransform_);
 			if(camToTag.isNull())
@@ -2670,16 +2694,97 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagD
 				RCLCPP_WARN(get_logger(), "Could not get TF between %s and %s frames for tag detection %d.",
 					frameId_.c_str(),
 					tagFrameId.c_str(),
-					tagDetections->detections[i].id);
+					msg->detections[i].id);
 					continue;
 			}
 
 			geometry_msgs::msg::PoseWithCovarianceStamped p;
 			rtabmap_conversions::transformToPoseMsg(camToTag, p.pose.pose);
-			p.header = tagDetections->header;
+			p.header = msg->header;
 			
 			uInsert(landmarks_,
-					std::make_pair(tagDetections->detections[i].id,
+					std::make_pair(msg->detections[i].id,
+							std::make_pair(p, 0.0f)));
+		}
+	}
+}
+#endif
+
+#ifdef WITH_ARUCO_MSGS
+void CoreWrapper::arucoAsyncCallback(const aruco_msgs::msg::MarkerArray::SharedPtr msg)
+{
+	if(!paused_)
+	{
+		UScopeMutex lock(landmarksMutex_);
+		for(unsigned int i=0; i<msg->markers.size(); ++i)
+		{
+			geometry_msgs::msg::PoseWithCovarianceStamped p;
+			p.pose = msg->markers[i].pose;
+			p.header = msg->markers[i].header;
+
+			uInsert(landmarks_,
+					std::make_pair((int)msg->markers[i].id,
+							std::make_pair(p, 0.0f)));
+		}
+	}
+}
+#endif
+
+#ifdef WITH_ARUCO_OPENCV_MSGS
+void CoreWrapper::arucoOpencvAsyncCallback(const aruco_opencv_msgs::msg::ArucoDetection::SharedPtr msg)
+{
+	if(!paused_)
+	{
+		UScopeMutex lock(landmarksMutex_);
+		for(unsigned int i=0; i<msg->markers.size(); ++i)
+		{
+			geometry_msgs::msg::PoseWithCovarianceStamped p;
+			p.pose.pose = msg->markers[i].pose;
+			p.header = msg->header;
+			
+			uInsert(landmarks_,
+					std::make_pair((int)msg->markers[i].marker_id,
+							std::make_pair(p, 0.0f)));
+		}
+	}
+}
+#endif
+
+#ifdef WITH_ARUCO_MARKERS_MSGS
+void CoreWrapper::arucoMarkersAsyncCallback(const aruco_markers_msgs::msg::MarkerArray::SharedPtr msg)
+{
+	if(!paused_)
+	{
+		UScopeMutex lock(landmarksMutex_);
+		for(unsigned int i=0; i<msg->markers.size(); ++i)
+		{
+			geometry_msgs::msg::PoseWithCovarianceStamped p;
+			p.pose.pose = msg->markers[i].pose.pose;
+			p.header = msg->markers[i].pose.header;
+			
+			uInsert(landmarks_,
+					std::make_pair((int)msg->markers[i].id,
+							std::make_pair(p, 0.0f)));
+		}
+	}
+}
+#endif
+
+#ifdef WITH_ROS2_ARUCO_INTERFACES
+void CoreWrapper::arucoInterfacesAsyncCallback(const ros2_aruco_interfaces::msg::ArucoMarkers::SharedPtr msg)
+{
+	if(!paused_)
+	{
+		UScopeMutex lock(landmarksMutex_);
+		UASSERT(msg->marker_ids.size() == msg->poses.size());
+		for(unsigned int i=0; i<msg->marker_ids.size(); ++i)
+		{
+			geometry_msgs::msg::PoseWithCovarianceStamped p;
+			p.pose.pose = msg->poses[i];
+			p.header = msg->header;
+			
+			uInsert(landmarks_,
+					std::make_pair((int)msg->marker_ids[i],
 							std::make_pair(p, 0.0f)));
 		}
 	}

rtabmap_sync/CMakeLists.txt

@@ -1,6 +1,20 @@
 cmake_minimum_required(VERSION 3.5)
 project(rtabmap_sync)
 
+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
+  )
+  find_library(
+    crypto_LIB NAMES crypto
+    PATHS "/usr/lib/${CMAKE_SYSTEM_PROCESSOR}-linux-gnu"
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
+  )
+endif()
+
 find_package(cv_bridge REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(message_filters REQUIRED)

rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h

@@ -26,10 +26,10 @@ class SyncDiagnostic {
 		inCompositeTask_("Input Status"),
         outCompositeTask_("Output Status"),
         lastTickInputStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1),
-		lastTickOutputStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1),
         inTargetFrequency_(0.0),
 		outTargetFrequency_(0.0),
-		windowSize_(windowSize)
+		windowSize_(windowSize),
+        lastTickTime_(0.0)
     {
         UASSERT(windowSize_ >= 1);
     }
@@ -76,6 +76,7 @@ class SyncDiagnostic {
 
     void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0)
     {
+        double lastTickOutputStamp;
         updateFrequency(
             stamp,
             expectedFrequency,
@@ -83,7 +84,7 @@ class SyncDiagnostic {
             outTimeStampStatus_,
             outWindow_,
             outTargetFrequency_,
-            lastTickOutputStamp_);
+            lastTickOutputStamp);
     }
 
 private:
@@ -140,6 +141,18 @@ private:
         }
 
         lastTickStamp = stampSec;
+
+        double clockNow = rtabmap_conversions::timestampFromROS(node_->now());
+        if(lastTickTime_ > clockNow)
+        {
+            RCLCPP_WARN(node_->get_logger(), "%s: Detected time jump in the past of %f sec, forcing diagnostic update.", 
+                node_->get_name(), lastTickTime_ - clockNow);
+            inFrequencyStatus_.clear();
+            outFrequencyStatus_.clear();
+            diagnosticUpdater_.force_update();
+            lastTickInputStamp_ = clockNow;
+        }
+        lastTickTime_ = clockNow;
     }
 
 private:
@@ -154,13 +167,13 @@ private:
     diagnostic_updater::CompositeDiagnosticTask outCompositeTask_;
 	rclcpp::TimerBase::SharedPtr diagnosticTimer_;
 	double lastTickInputStamp_;
-    double lastTickOutputStamp_;
 	double inTargetFrequency_;
     double outTargetFrequency_;
 	int windowSize_;
 	std::deque<double> inWindow_;
     std::deque<double> outWindow_;
     UMutex tickMutex_;
+    double lastTickTime_;
 
 };
 

rtabmap_sync/package.xml

@@ -12,6 +12,8 @@
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
+  <build_depend>ros_environment</build_depend>
+
   <depend>cv_bridge</depend>
   <depend>image_transport</depend>
   <depend>message_filters</depend>

rtabmap_util/package.xml

@@ -12,6 +12,8 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <build_depend>ros_environment</build_depend>
+
   <depend>cv_bridge</depend>
   <depend>image_transport</depend>
   <depend>rclcpp</depend>

rtabmap_util/src/nodelets/point_cloud_assembler.cpp

@@ -130,7 +130,7 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
 
 	if(maxClouds_==0 && assemblingTime_ ==0.0)
 	{
-		RCLCPP_ERROR(get_logger(), "point_cloud_assembler: max_cloud or assembling_time parameters should be set!");
+		RCLCPP_ERROR(get_logger(), "point_cloud_assembler: max_clouds or assembling_time parameters should be set!");
 		exit(-1);
 	}
 

rtabmap_viz/CMakeLists.txt

@@ -5,6 +5,25 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+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
+  )
+  find_library(
+    rcutils_LIB NAMES rcutils
+    PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib"
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
+  )
+  find_library(
+    crypto_LIB NAMES crypto
+    PATHS "/usr/lib/${CMAKE_SYSTEM_PROCESSOR}-linux-gnu"
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
+  )
+endif()
+
 find_package(ament_cmake REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(geometry_msgs REQUIRED)

rtabmap_viz/package.xml

@@ -12,6 +12,8 @@
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
+  <build_depend>ros_environment</build_depend>
+
   <depend>cv_bridge</depend>
   <depend>geometry_msgs</depend>
   <depend>rclcpp</depend>