From fa342bd85350a887454d8105b9a29eae8e2c16c2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 8 Jun 2025 16:15:37 -0700 Subject: [PATCH 1/7] removed ros1 workflow --- .github/workflows/ros1.yml | 67 -------------------------------------- 1 file changed, 67 deletions(-) delete mode 100644 .github/workflows/ros1.yml diff --git a/.github/workflows/ros1.yml b/.github/workflows/ros1.yml deleted file mode 100644 index c0d6c707..00000000 --- a/.github/workflows/ros1.yml +++ /dev/null @@ -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/setup-ros@v0.2 - 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 From d336369ca4ac4f979ed2836658137acf3b3677c2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 16 Jun 2025 19:20:21 -0700 Subject: [PATCH 2/7] Debug builtin interfaces issue (#1328) * debugging builtin_interfaces issue on arm64 * add logh * cmake debug * show arch * refactored * explicit find_library * explicitly search builtin_interfaces__rosidl_generator_c * fixing in rtabmap_slam * rtabmap_sync * openssl error * rtabmap odom * rtabmap_viz * adding missing deps instead * testing on all distros * limit fix to humble-aarch64 build * remvoed debug code * Running aarch64 workaround for all distros * message_filters error * added cmake --debug-find * using 2 threads * reseting ros2 ci --- docker/humble/latest/Dockerfile | 8 +++----- docker/jazzy/latest/Dockerfile | 4 ++-- docker/kilted/latest/Dockerfile | 4 ++-- rtabmap_conversions/CMakeLists.txt | 10 ++++++++++ rtabmap_msgs/CMakeLists.txt | 9 +++++++++ rtabmap_msgs/package.xml | 2 ++ rtabmap_odom/CMakeLists.txt | 9 +++++++++ rtabmap_odom/package.xml | 2 ++ rtabmap_rviz_plugins/CMakeLists.txt | 9 +++++++++ rtabmap_rviz_plugins/package.xml | 2 ++ rtabmap_slam/CMakeLists.txt | 9 +++++++++ rtabmap_sync/CMakeLists.txt | 14 ++++++++++++++ rtabmap_sync/package.xml | 2 ++ rtabmap_viz/CMakeLists.txt | 19 +++++++++++++++++++ rtabmap_viz/package.xml | 2 ++ 15 files changed, 96 insertions(+), 9 deletions(-) diff --git a/docker/humble/latest/Dockerfile b/docker/humble/latest/Dockerfile index eb7ee5e8..d24ddaf5 100644 --- a/docker/humble/latest/Dockerfile +++ b/docker/humble/latest/Dockerfile @@ -1,19 +1,17 @@ FROM introlab3it/rtabmap:jammy -RUN source /ros_entrypoint.sh && \ - mkdir -p ros2_ws/src && \ - cd ros2_ws/src +RUN mkdir -p 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 --skip-keys="rtabmap" && \ 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 diff --git a/docker/jazzy/latest/Dockerfile b/docker/jazzy/latest/Dockerfile index 3b60aaac..e2b06208 100644 --- a/docker/jazzy/latest/Dockerfile +++ b/docker/jazzy/latest/Dockerfile @@ -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 diff --git a/docker/kilted/latest/Dockerfile b/docker/kilted/latest/Dockerfile index 2a20a5ea..97cc6c46 100644 --- a/docker/kilted/latest/Dockerfile +++ b/docker/kilted/latest/Dockerfile @@ -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 diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index f51f841b..3df9516a 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/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) diff --git a/rtabmap_msgs/CMakeLists.txt b/rtabmap_msgs/CMakeLists.txt index 7ae7eead..01e7cebb 100644 --- a/rtabmap_msgs/CMakeLists.txt +++ b/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 ## ################## diff --git a/rtabmap_msgs/package.xml b/rtabmap_msgs/package.xml index ca45d59a..136fb15a 100644 --- a/rtabmap_msgs/package.xml +++ b/rtabmap_msgs/package.xml @@ -14,6 +14,8 @@ rosidl_default_generators + ros_environment + builtin_interfaces std_msgs std_srvs diff --git a/rtabmap_odom/CMakeLists.txt b/rtabmap_odom/CMakeLists.txt index f9859b44..96130502 100644 --- a/rtabmap_odom/CMakeLists.txt +++ b/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) diff --git a/rtabmap_odom/package.xml b/rtabmap_odom/package.xml index ffa9cd90..c51f592f 100644 --- a/rtabmap_odom/package.xml +++ b/rtabmap_odom/package.xml @@ -12,6 +12,8 @@ ament_cmake_ros + ros_environment + cv_bridge image_geometry laser_geometry diff --git a/rtabmap_rviz_plugins/CMakeLists.txt b/rtabmap_rviz_plugins/CMakeLists.txt index d466c1b2..c1808ebe 100644 --- a/rtabmap_rviz_plugins/CMakeLists.txt +++ b/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) diff --git a/rtabmap_rviz_plugins/package.xml b/rtabmap_rviz_plugins/package.xml index 32b779c4..ada5c080 100644 --- a/rtabmap_rviz_plugins/package.xml +++ b/rtabmap_rviz_plugins/package.xml @@ -12,6 +12,8 @@ ament_cmake_ros + ros_environment + pcl_conversions pluginlib rclcpp diff --git a/rtabmap_slam/CMakeLists.txt b/rtabmap_slam/CMakeLists.txt index d843f4f6..df0eeb6f 100644 --- a/rtabmap_slam/CMakeLists.txt +++ b/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) diff --git a/rtabmap_sync/CMakeLists.txt b/rtabmap_sync/CMakeLists.txt index 6b45dcd7..5bbf39fd 100644 --- a/rtabmap_sync/CMakeLists.txt +++ b/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) diff --git a/rtabmap_sync/package.xml b/rtabmap_sync/package.xml index 8eee2e6a..769155cc 100644 --- a/rtabmap_sync/package.xml +++ b/rtabmap_sync/package.xml @@ -12,6 +12,8 @@ ament_cmake_ros + ros_environment + cv_bridge image_transport message_filters diff --git a/rtabmap_viz/CMakeLists.txt b/rtabmap_viz/CMakeLists.txt index 23b66ad8..a7707f22 100644 --- a/rtabmap_viz/CMakeLists.txt +++ b/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) diff --git a/rtabmap_viz/package.xml b/rtabmap_viz/package.xml index de89ccd5..b8e2ce78 100644 --- a/rtabmap_viz/package.xml +++ b/rtabmap_viz/package.xml @@ -12,6 +12,8 @@ ament_cmake_ros + ros_environment + cv_bridge geometry_msgs rclcpp From aa7f42a57e86da2f6a1b86691389d44b40d86921 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 28 Jun 2025 16:46:10 -0700 Subject: [PATCH 3/7] Adding aruco msgs input support (#1334) * Adding aruco msgs input support * updated topic names --- rtabmap_slam/CMakeLists.txt | 44 +++++++ .../include/rtabmap_slam/CoreWrapper.h | 44 ++++++- rtabmap_slam/package.xml | 3 + rtabmap_slam/src/CoreWrapper.cpp | 122 ++++++++++++++++-- 4 files changed, 204 insertions(+), 9 deletions(-) diff --git a/rtabmap_slam/CMakeLists.txt b/rtabmap_slam/CMakeLists.txt index df0eeb6f..f53b7aaa 100644 --- a/rtabmap_slam/CMakeLists.txt +++ b/rtabmap_slam/CMakeLists.txt @@ -38,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) @@ -88,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") diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index a0eb6e73..157a9b26 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -88,6 +88,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif +#ifdef WITH_ARUCO_MSGS +#include +#endif + +#ifdef WITH_ARUCO_OPENCV_MSGS +#include +#endif + +#ifdef WITH_ARUCO_MARKERS_MSGS +#include +#endif + +#ifdef WITH_ROS2_ARUCO_INTERFACES +#include +#endif + #ifdef WITH_NAV2_MSGS #include #include @@ -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::SharedPtr landmarkDetectionsSub_; #ifdef WITH_APRILTAG_MSGS rclcpp::Subscription::SharedPtr tagDetectionsSub_; + rclcpp::Subscription::SharedPtr apriltagSub_; +#endif +#ifdef WITH_ARUCO_MSGS + rclcpp::Subscription::SharedPtr arucoSub_; +#endif +#ifdef WITH_ARUCO_OPENCV_MSGS + rclcpp::Subscription::SharedPtr arucoOpencvSub_; +#endif +#ifdef WITH_ARUCO_MARKERS_MSGS + rclcpp::Subscription::SharedPtr arucoMarkersSub_; +#endif +#ifdef WITH_ROS2_ARUCO_INTERFACES + rclcpp::Subscription::SharedPtr arucoInterfacesSub_; #endif #ifdef WITH_FIDUCIAL_MSGS rclcpp::Subscription::SharedPtr fiducialTransfromsSub_; diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml index 38d38b64..08543237 100644 --- a/rtabmap_slam/package.xml +++ b/rtabmap_slam/package.xml @@ -27,6 +27,9 @@ tf2_ros visualization_msgs apriltag_msgs + aruco_msgs + aruco_opencv_msgs + rtabmap_msgs rtabmap_util diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index bb86ad56..c6a20333 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -886,6 +886,19 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : landmarkDetectionsSub_ = this->create_subscription("landmark_detections", 1, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); #ifdef WITH_APRILTAG_MSGS tagDetectionsSub_ = this->create_subscription("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); + apriltagSub_ = this->create_subscription("apriltag/detections", 5, std::bind(&CoreWrapper::apriltagAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); +#endif +#ifdef WITH_ARUCO_MSGS + arucoSub_ = this->create_subscription("aruco/detections", 5, std::bind(&CoreWrapper::arucoAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); +#endif +#ifdef WITH_ARUCO_OPENCV_MSGS + arucoOpencvSub_ = this->create_subscription("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/detections", 5, std::bind(&CoreWrapper::arucoMarkersAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); +#endif +#ifdef WITH_ROS2_ARUCO_INTERFACES + arucoInterfacesSub_ = this->create_subscription("aruco_interfaces/detections", 5, std::bind(&CoreWrapper::arucoInterfacesAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); #endif #ifdef WITH_FIDUCIAL_MSGS fiducialTransfromsSub_ = this->create_subscription("fiducial_transforms", 5, std::bind(&CoreWrapper::fiducialDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); @@ -2651,18 +2664,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; idetections.size(); ++i) + for(unsigned int i=0; idetections.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 +2695,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; imarkers.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; imarkers.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; imarkers.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; imarker_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))); } } From 3cc9db8f87ade67eb5a80a2a2caabfe9a83d056c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 3 Jul 2025 17:50:38 -0700 Subject: [PATCH 4/7] Odom reset on time jump in the past (#1333) * Odom reset on time jump * Adding more logs to debug * refactored * dont skip frame on clock jump * Making clock check independent of the topic stamp check * fixed post check * Making diagnostic more robust to time jump * Added node name to warning * make sync warning msg working in case of time jump * not need to reset timer * reset timer * timer auto reset already * typo * Added more time checks to make sure we don't republish a tf frame with stamp from a topic in the future * dont send tf if time jump happened while processing * fixed errors * addressing comments * fixing time comparison --- .../include/rtabmap_odom/OdometryROS.h | 5 +- rtabmap_odom/src/OdometryROS.cpp | 106 ++++++++++++++---- rtabmap_odom/src/nodelets/icp_odometry.cpp | 12 +- rtabmap_slam/src/CoreWrapper.cpp | 18 +++ .../include/rtabmap_sync/SyncDiagnostic.h | 27 ++++- 5 files changed, 136 insertions(+), 32 deletions(-) diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index c5672d36..24d20160 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -87,7 +87,7 @@ protected: tf::TransformListener & tfListener() {return tfListener_;} double waitForTransformDuration() const {return waitForTransform_?waitForTransformDuration_:0.0;} rtabmap::Transform velocityGuess() const; - double previousStamp() const {return previousStamp_;} + ros::Time previousStamp() const {return previousStamp_;} virtual void postProcessData(const rtabmap::SensorData & data, const std_msgs::Header & header) const {} private: @@ -158,7 +158,8 @@ private: bool icpParams_; rtabmap::Transform guess_; rtabmap::Transform guessPreviousPose_; - double previousStamp_; + ros::Time previousStamp_; + ros::Time previousClockTime_; double expectedUpdateRate_; double maxUpdateRate_; double minUpdateRate_; diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index f79fca44..f5b54ba9 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -78,7 +78,6 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) : stereoParams_(stereoParams), visParams_(visParams), icpParams_(icpParams), - previousStamp_(0.0), expectedUpdateRate_(0.0), maxUpdateRate_(0.0), minUpdateRate_(0.0), @@ -543,27 +542,56 @@ void OdometryROS::mainLoop() Transform groundTruth; if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()) { - if(previousStamp_>0.0 && previousStamp_ >= header.stamp.toSec()) + // Detect time jump in the past + ros::Time clockNow = ros::Time::now(); + if(previousClockTime_ > clockNow) + { + NODELET_WARN("Odometry: Detected jump back in time of %f sec. Odometry is " + "automatically reset to latest computed pose!", + (previousClockTime_ - clockNow).toSec()); + SensorData dataCpy = dataToProcess_; + std_msgs::Header headerCpy = dataHeaderToProcess_; + ros::Time previousCpy = previousClockTime_; + this->reset(odometry_->getPose()); + if(previousCpy > headerCpy.stamp) { + // new frame is using new clock, process it now + dataToProcess_ = dataCpy; + dataHeaderToProcess_ = headerCpy; + dataReady_.release(); + NODELET_WARN("Odometry: Restarting with frame: %f (clock previous=%f, new=%f)", + headerCpy.stamp.toSec(), previousCpy.toSec(), clockNow.toSec()); + } + else { + // skip that old frame + NODELET_WARN("Odometry: skipping frame: %f (clock previous=%f, new=%f)", + headerCpy.stamp.toSec(), previousCpy.toSec(), clockNow.toSec()); + } + previousClockTime_ = clockNow; + return; + } + previousClockTime_ = clockNow; + + if(previousStamp_ >= header.stamp) { NODELET_WARN("Odometry: Detected not valid consecutive stamps (previous=%fs new=%fs). " - "New stamp should be always greater than previous stamp. This new data is ignored.", - previousStamp_, header.stamp.toSec()); + "New stamp should be always greater than previous stamp. This new data is ignored. ", + previousStamp_.toSec(), header.stamp.toSec()); return; } else if(maxUpdateRate_ > 0 && - previousStamp_ > 0 && - (header.stamp.toSec()-previousStamp_+(expectedUpdateRate_ > 0?1.0/expectedUpdateRate_:0)) < 1.0/maxUpdateRate_) + previousStamp_.toSec() > 0 && + ((header.stamp-previousStamp_).toSec()+(expectedUpdateRate_ > 0?1.0/expectedUpdateRate_:0)) < 1.0/maxUpdateRate_) { // throttling return; } else if(maxUpdateRate_ == 0 && expectedUpdateRate_ > 0 && - previousStamp_ > 0 && - (header.stamp.toSec()-previousStamp_) < 1.0/expectedUpdateRate_) + previousStamp_.toSec() > 0 && + (header.stamp-previousStamp_).toSec() < 1.0/expectedUpdateRate_) { NODELET_WARN("Odometry: Aborting odometry update, higher frame rate detected (%f Hz) than the expected one (%f Hz). (stamps: previous=%fs new=%fs)", - 1.0/(header.stamp.toSec()-previousStamp_), expectedUpdateRate_, previousStamp_, header.stamp.toSec()); + 1.0/(header.stamp-previousStamp_).toSec(), expectedUpdateRate_, previousStamp_.toSec(), header.stamp.toSec()); return; } @@ -632,7 +660,7 @@ void OdometryROS::mainLoop() guess_.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); if((guessMinTranslation_ <= 0.0 || uMax3(fabs(x), fabs(y), fabs(z)) < guessMinTranslation_) && (guessMinRotation_ <= 0.0 || uMax3(fabs(roll), fabs(pitch), fabs(yaw)) < guessMinRotation_) && - (guessMinTime_ <= 0.0 || (previousStamp_>0.0 && header.stamp.toSec()-previousStamp_ < guessMinTime_))) + (guessMinTime_ <= 0.0 || (previousStamp_.toSec()>0.0 && (header.stamp-previousStamp_).toSec() < guessMinTime_))) { // Ignore odometry update, we didn't move enough if(publishTf_) @@ -643,7 +671,16 @@ void OdometryROS::mainLoop() correctionMsg.header.stamp = header.stamp; Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse(); rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); - tfBroadcaster_.sendTransform(correctionMsg); + ros::Time time_now = ros::Time::now(); + if(time_now >= previousClockTime_) { + tfBroadcaster_.sendTransform(correctionMsg); + } + else { + ROS_WARN("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).toSec()); + } } guessPreviousPose_ = guessCurrentPose; return; @@ -658,7 +695,7 @@ void OdometryROS::mainLoop() } } - bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && (header.stamp.toSec()-previousStamp_) > 1.0/minUpdateRate_; + bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_.toSec() > 0 && (header.stamp-previousStamp_).toSec() > 1.0/minUpdateRate_; // process data ros::WallTime time = ros::WallTime::now(); @@ -697,11 +734,29 @@ void OdometryROS::mainLoop() correctionMsg.header.stamp = header.stamp; Transform correction = pose * guessCurrentPose.inverse(); rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); - tfBroadcaster_.sendTransform(correctionMsg); + ros::Time time_now = ros::Time::now(); + if(time_now >= previousClockTime_) { + tfBroadcaster_.sendTransform(correctionMsg); + } + else { + ROS_WARN("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).toSec()); + } } else { - tfBroadcaster_.sendTransform(poseMsg); + ros::Time time_now = ros::Time::now(); + if(time_now >= previousClockTime_) { + tfBroadcaster_.sendTransform(poseMsg); + } + else { + ROS_WARN("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).toSec()); + } } } @@ -893,7 +948,18 @@ void OdometryROS::mainLoop() correctionMsg.header.stamp = header.stamp; Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse(); rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); - tfBroadcaster_.sendTransform(correctionMsg); + ros::Time time_now = ros::Time::now(); + if(time_now >= previousClockTime_) { + tfBroadcaster_.sendTransform(correctionMsg); + } + else { + ROS_WARN("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(), + correctionMsg.header.stamp.toSec(), + time_now.toSec()); + } } } @@ -903,7 +969,7 @@ void OdometryROS::mainLoop() { NODELET_WARN( "Odometry lost! Odometry will be reset because last update " "is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.", - header.stamp.toSec() - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, header.stamp.toSec()); + (header.stamp - previousStamp_).toSec(), 1.0/minUpdateRate_, minUpdateRate_, previousStamp_.toSec(), header.stamp.toSec()); } else if(--resetCurrentCount_>0) { @@ -1098,10 +1164,10 @@ void OdometryROS::mainLoop() syncDiagnostic_->tick(header.stamp, maxUpdateRate_>0 ? maxUpdateRate_: expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_: - previousStamp_ == 0.0 || header.stamp.toSec() - previousStamp_ > 1.0/curentRate?0:curentRate); + previousStamp_.toSec() == 0.0 || (header.stamp - previousStamp_).toSec() > 1.0/curentRate?0:curentRate); } - previousStamp_ = header.stamp.toSec(); + previousStamp_ = header.stamp; } bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) @@ -1125,7 +1191,8 @@ void OdometryROS::reset(const Transform & pose) odometry_->reset(pose); guess_.setNull(); guessPreviousPose_.setNull(); - previousStamp_ = 0.0; + previousStamp_ = ros::Time(); + previousClockTime_ = ros::Time(); resetCurrentCount_ = resetCountdown_; imuProcessed_ = false; dataToProcess_ = SensorData(); @@ -1135,6 +1202,7 @@ void OdometryROS::reset(const Transform & pose) imus_.clear(); imuMutex_.unlock(); this->flushCallbacks(); + this->tfListener().clear(); } bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&) diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index 23bb280e..1c712e1c 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp @@ -380,11 +380,11 @@ private: -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp); - if(guessFrameId().empty() && previousStamp() > 0 && !velocityGuess().isNull()) + if(guessFrameId().empty() && previousStamp().toSec() > 0.0 && !velocityGuess().isNull()) { // deskew with constant velocity model (we are in frameId) sensor_msgs::PointCloud2 scanOutDeskewed; - if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess())) + if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp().toSec(), velocityGuess())) { ROS_ERROR("Failed to deskew input cloud, aborting odometry update!"); return; @@ -405,11 +405,11 @@ private: { projection.projectLaser(*scanMsg, scanOut, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp); - if(deskewing_ && previousStamp() > 0 && !velocityGuess().isNull()) + if(deskewing_ && previousStamp().toSec() > 0.0 && !velocityGuess().isNull()) { // deskew with constant velocity model sensor_msgs::PointCloud2 scanOutDeskewed; - if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess())) + if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp().toSec(), velocityGuess())) { ROS_ERROR("Failed to deskew input cloud, aborting odometry update!"); return; @@ -628,7 +628,7 @@ private: return; } } - else if(previousStamp() > 0 && !velocityGuess().isNull()) + else if(previousStamp().toSec() > 0.0 && !velocityGuess().isNull()) { // deskew with constant velocity model bool alreadyInBaseFrame = frameId().compare(pointCloudMsg->header.frame_id) == 0; @@ -648,7 +648,7 @@ private: } sensor_msgs::PointCloud2::Ptr cloudDeskewed(new sensor_msgs::PointCloud2); - if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, previousStamp(), velocityGuess())) + if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, previousStamp().toSec(), velocityGuess())) { ROS_ERROR("Failed to deskew input cloud, aborting odometry update!"); return; diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 6a3ab207..ff930a44 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -1023,6 +1023,15 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Ti { if(!paused_) { + // Check time jump in the past + if(stamp < previousStamp_) { + ROS_WARN("Detected time jump in the past of %f sec (previous stamp=%f, current stamp=%f). Resetting internal stamps and abort!", + previousStamp_.toSec() - stamp.toSec(), previousStamp_.toSec(), stamp.toSec()); + previousStamp_ = ros::Time(); + tfListener_.clear(); + return false; + } + Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); if(!odom.isNull()) { @@ -1134,6 +1143,15 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp) { if(!paused_) { + // Check time jump in the past + if(stamp < previousStamp_) { + ROS_WARN("Detected time jump in the past of %f sec (previous stamp=%f, current stamp=%f). Resetting internal stamps and abort!", + previousStamp_.toSec() - stamp.toSec(), previousStamp_.toSec(), stamp.toSec()); + previousStamp_ = ros::Time(); + tfListener_.clear(); + return false; + } + // Odom TF ready? Transform odom = rtabmap_conversions::getTransform(odomFrameId_, frameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0); if(odom.isNull()) diff --git a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h index dfc3e384..59f9181f 100644 --- a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h +++ b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h @@ -19,7 +19,9 @@ class SyncDiagnostic { compositeTask_("Sync status"), lastCallbackCalledStamp_(ros::Time::now().toSec()-1), targetFrequency_(0.0), - windowSize_(windowSize) + windowSize_(windowSize), + lastTickTime_(0.0), + nodeName_(nodeName) { UASSERT(windowSize_ >= 1); } @@ -46,7 +48,7 @@ class SyncDiagnostic { } diagnosticUpdater_.setHardwareID(strList.empty()?"none":uJoin(strList, "/")); diagnosticUpdater_.force_update(); - diagnosticTimer_ = ros::NodeHandle().createTimer(ros::Duration(1), &SyncDiagnostic::diagnosticTimerCallback, this); + diagnosticTimer_ = ros::NodeHandle().createTimer(ros::Duration(5), &SyncDiagnostic::diagnosticTimerCallback, this); } void tick(const ros::Time & stamp, double targetFrequency = 0) @@ -79,16 +81,29 @@ class SyncDiagnostic { targetFrequency_ = targetFrequency; } lastCallbackCalledStamp_ = stamp.toSec(); + + double clockNow = ros::Time::now().toSec(); + if(lastTickTime_ > clockNow) + { + ROS_WARN("%s: Detected time jump in the past of %f sec, forcing diagnostic update.", + nodeName_.c_str(), lastTickTime_ - clockNow); + frequencyStatus_.clear(); + diagnosticUpdater_.force_update(); + lastCallbackCalledStamp_ = clockNow; + } + else + { + diagnosticUpdater_.update(); + } + lastTickTime_ = clockNow; } private: void diagnosticTimerCallback(const ros::TimerEvent& event) { - diagnosticUpdater_.update(); - if(ros::Time::now().toSec()-lastCallbackCalledStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty()) { - ROS_WARN_THROTTLE(5, "%s", topicsNotReceivedWarningMsg_.c_str()); + ROS_WARN("%s", topicsNotReceivedWarningMsg_.c_str()); } } @@ -103,6 +118,8 @@ private: double targetFrequency_; int windowSize_; std::deque window_; + double lastTickTime_; + std::string nodeName_; }; From 0ba59996e695ade7b20a182fbce0bc447537cc86 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 4 Jul 2025 22:01:22 +0000 Subject: [PATCH 5/7] refactored time jump detection --- rtabmap_odom/src/OdometryROS.cpp | 1 - rtabmap_slam/src/CoreWrapper.cpp | 18 ------------------ 2 files changed, 19 deletions(-) diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index f5b54ba9..81d08076 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -1202,7 +1202,6 @@ void OdometryROS::reset(const Transform & pose) imus_.clear(); imuMutex_.unlock(); this->flushCallbacks(); - this->tfListener().clear(); } bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index ff930a44..6a3ab207 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -1023,15 +1023,6 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Ti { if(!paused_) { - // Check time jump in the past - if(stamp < previousStamp_) { - ROS_WARN("Detected time jump in the past of %f sec (previous stamp=%f, current stamp=%f). Resetting internal stamps and abort!", - previousStamp_.toSec() - stamp.toSec(), previousStamp_.toSec(), stamp.toSec()); - previousStamp_ = ros::Time(); - tfListener_.clear(); - return false; - } - Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); if(!odom.isNull()) { @@ -1143,15 +1134,6 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp) { if(!paused_) { - // Check time jump in the past - if(stamp < previousStamp_) { - ROS_WARN("Detected time jump in the past of %f sec (previous stamp=%f, current stamp=%f). Resetting internal stamps and abort!", - previousStamp_.toSec() - stamp.toSec(), previousStamp_.toSec(), stamp.toSec()); - previousStamp_ = ros::Time(); - tfListener_.clear(); - return false; - } - // Odom TF ready? Transform odom = rtabmap_conversions::getTransform(odomFrameId_, frameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0); if(odom.isNull()) From bc8123089e3f28aadadc61bcae050df25ff20039 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 4 Jul 2025 16:59:24 -0700 Subject: [PATCH 6/7] Added new demo turtlebot3_sim_rgbd_fake_scan_demo.launch.py --- .../turtlebot3_rgbd_fake_scan.launch.py | 143 ++++++++++++++++++ ...rtlebot3_sim_rgbd_fake_scan_demo.launch.py | 117 ++++++++++++++ .../src/nodelets/point_cloud_assembler.cpp | 2 +- 3 files changed, 261 insertions(+), 1 deletion(-) create mode 100644 rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py create mode 100644 rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py new file mode 100644 index 00000000..1ccacd65 --- /dev/null +++ b/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')]), + ]) diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py new file mode 100644 index 00000000..314ad3eb --- /dev/null +++ b/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 +# +# camera_rgb_frame +# camera_rgb_optical_frame +# 0 0 0 -1.57079632679 0 -1.57079632679 +# +# 0 0 1 +# +# +# 3) Rename to +# 4) Add +# 5) Change to +# 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) + ]) diff --git a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp index 2edf5509..99b64ccf 100644 --- a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp +++ b/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); } From 0918c60dc1e276a2cf9d2ca800131d20ed1e442b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 4 Jul 2025 17:20:25 -0700 Subject: [PATCH 7/7] Update README.md --- rtabmap_demos/README.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/rtabmap_demos/README.md b/rtabmap_demos/README.md index b1eb14ce..4f37769c 100644 --- a/rtabmap_demos/README.md +++ b/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)