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 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_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) 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_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/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index fe515e05..4a1b7a81 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/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_; 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_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index cfbbf7b5..c9955d99 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/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,7 +710,17 @@ void OdometryROS::mainLoop() correctionMsg.header.stamp = header.stamp; Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse(); rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); - tfBroadcaster_->sendTransform(correctionMsg); + + 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,11 +774,30 @@ void OdometryROS::mainLoop() correctionMsg.header.stamp = header.stamp; Transform correction = pose * guessCurrentPose.inverse(); rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); - tfBroadcaster_->sendTransform(correctionMsg); + + 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 { - tfBroadcaster_->sendTransform(poseMsg); + 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); + } } } @@ -927,7 +989,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); + 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(); 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..f53b7aaa 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) @@ -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") 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 4042b9e7..db537e67 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..c214d91a 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/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; - rtabmap_conversions::transformToGeometryMsg(mapToOdom_, msg.transform); + msg.header.stamp = now() + rclcpp::Duration::from_seconds(tfTolerance); tfBroadcaster_->sendTransform(msg); } mapToOdomMutex_.unlock(); @@ -886,6 +885,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 +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; 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 +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; 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))); } } 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/include/rtabmap_sync/SyncDiagnostic.h b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h index 2cafd411..03b49348 100644 --- a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h +++ b/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 inWindow_; std::deque outWindow_; UMutex tickMutex_; + double lastTickTime_; }; 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_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); } 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