[SLAM with D435i] TF errors (TF_NAN_INPUT, TF_DENORMALIZED_QUATERNION) and incorrect 'camera_link' frame data

sooyoungmoon commented 3 weeks ago

Required Info
Camera Model	D435i
Firmware Version	5.14.0
Operating System & Version	Ubuntu 22.04.5 LTS
Kernel Version (Linux Only)	6.8.0-47-generic
Platform	PC
Librealsense SDK Version	2.55.1
Language	{C++ }
Segment	Robot
ROS Distro	Humble
RealSense ROS Wrapper Version	4.55.1

Issue Description

Hello, I'm following SLAM-with-D435i-wiki to test SLAM using Intel Realsense2 D435i on ROS2 (Humble) environment.

After modifying a launch file and a few parameters, I was able to execute all the necessary nodes. I can see the 'camera_link' frame moves as I change the position and direction of my D435i camera. However, soon after I move my camera, I can see on rviz the 'camera_link' frame 'floats away' from the real pose of the camera and the following error messages are printed on the screen.

스크린샷 2024-10-26 23-42-51

In addition to that, the /rtabmap/odom topic includes abnormal values (covariance matrix has very big (i.e. 9999) diagonal values) as soon as the errors occur .

I'm curious about the reason why those error occur simultaneously at multiple ROS2 nodes including: /rtabmap/rgbd_odometry, /rtabmap/rtabmap, /ukf_filter_node, and /rviz2.

Could you give me some hint to solve the issue? Thank you!!

For your reference, I attach the parameters I changed and the launch file I used (opensource_tracking_ros2.launch.py)

스크린샷 2024-10-26 23-44-54

-------------------- opensource_tracking_ros2.launch.py ------------------------------

import os

from ament_index_python import get_package_share_directory

from launch import LaunchContext, LaunchDescription from launch.actions import DeclareLaunchArgument from launch.actions import IncludeLaunchDescription from launch.actions import SetLaunchConfiguration from launch.actions import GroupAction

from launch.conditions import IfCondition, UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch.substitutions import TextSubstitution from launch.substitutions import ThisLaunchFileDir from launch_ros.actions import Node from launch_ros.actions import PushRosNamespace

def generate_launch_description():

remappings=[
    ('/camera/camera/aligned_depth_to_color/image_raw', '/camera/aligned_depth_to_color/image_raw'),
    ('/camera/camera/aligned_depth_to_color/camera_info', '/camera/aligned_depth_to_color/camera_info'),
    ('/camera/camera/aligned_depth_to_color/image_raw/compressed', '/camera/aligned_depth_to_color/image_raw/compressed'),
    ('/camera/camera/aligned_depth_to_color/image_raw/compressedDepth', '/camera/aligned_depth_to_color/image_raw/compressedDepth'),
    ('/camera/camera/aligned_depth_to_color/image_raw/theora', '/camera/aligned_depth_to_color/image_raw/theora'),
    ('/camera/camera/accel/imu_info', '/camera/accel/imu_info'),
    ('/camera/camera/accel/metadata', '/camera/accel/metadata'),
    ('/camera/camera/accel/sample', '/camera/accel/sample'),
    ('/camera/camera/color/image_raw/compressed', '/camera/color/image_raw/compressed'),
    ('/camera/camera/color/image_raw/compressedDepth', '/camera/color/image_raw/compressedDepth'),
    ('/camera/camera/color/image_raw/theora', '/camera/color/image_raw/theora'),
    ('/camera/camera/color/metadata', '/camera/color/metadata'),
    ('/camera/camera/depth/camera_info', '/camera/depth/camera_info'),
    ('/camera/camera/depth/image_rect_raw', '/camera/depth/image_rect_raw'),
    ('/camera/camera/depth/image_rect_raw/compressed', '/camera/depth/image_rect_raw/compressed'),
    ('/camera/camera/depth/image_rect_raw/compressedDepth', '/camera/depth/image_rect_raw/compressedDepth'),
    ('/camera/camera/depth/image_rect_raw/theora', '/camera/depth/image_rect_raw/theora'),
    ('/camera/camera/depth/metadata', '/camera/depth/metadata'),
    ('/camera/camera/extrinsics/depth_to_accel', '/camera/extrinsics/depth_to_accel'),
    ('/camera/camera/extrinsics/depth_to_color', '/camera/extrinsics/depth_to_color'),
    ('/camera/camera/extrinsics/depth_to_gyro', '/camera/extrinsics/depth_to_gyro'),
    ('/camera/camera/gyro/imu_info', '/camera/gyro/imu_info'),
    ('/camera/camera/gyro/metadata', '/camera/gyro/metadata'),
    ('/camera/camera/gyro/sample', '/camera/gyro/sample'),
    ('/camera/camera/imu', '/camera/imu'),
    ('/camera/camera/color/camera_info', '/camera/color/camera_info'),
    ('/camera/camera/color/image_raw', '/camera/color/image_raw')  
]

# args that can be set from the command line or a default will be used

emitter_enabled_arg = DeclareLaunchArgument(
    "emitter_enabled",
    default_value=TextSubstitution(text="false")
)

enable_infra1_arg = DeclareLaunchArgument(
    "enable_infra1",
    default_value=TextSubstitution(text="true")
)

enable_infra2_arg = DeclareLaunchArgument(
    "enable_infra2",
    default_value=TextSubstitution(text="true")
)

buffer_length_arg = DeclareLaunchArgument(
    "buffer_length",
    default_value=TextSubstitution(text="10")
)
enable_accel_arg = DeclareLaunchArgument(
    "enable_accel",
    default_value=TextSubstitution(text="true")
)
enable_gyro_arg = DeclareLaunchArgument(
    "enable_gyro",
    default_value=TextSubstitution(text="true")
)
offline_arg = DeclareLaunchArgument(
    "offline", 
    default_value=TextSubstitution(text="false")
)

enable_rgbd_arg = DeclareLaunchArgument( # 2024.09.27
    "enable_rgbd",
    default_value=TextSubstitution(text="true")
)

align_depth_arg = DeclareLaunchArgument(
    "align_depth.enable", 
    default_value=TextSubstitution(text="true")
)

enable_sync_arg = DeclareLaunchArgument( # 2024.09.27
    "enable_sync",
    default_value=TextSubstitution(text="true")
)

enable_color_arg = DeclareLaunchArgument( # 2024.09.27
    "enable_color",
    default_value=TextSubstitution(text="true")
)

enable_depth_arg = DeclareLaunchArgument( # 2024.09.27
    "enable_depth",
    default_value=TextSubstitution(text="true")
)    

linear_accel_cov_arg = DeclareLaunchArgument(
    "linear_accel_cov", 
    default_value=TextSubstitution(text="1.0")
)

unite_imu_method_arg = DeclareLaunchArgument(
    "unite_imu_method",
    default_value=TextSubstitution(text="2") # linear_interpolation
)

use_mag_arg = DeclareLaunchArgument(
    "use_mag",
    default_value=TextSubstitution(text="false")
)

_publish_tf_arg = DeclareLaunchArgument(
    "_publish_tf",
    default_value=TextSubstitution(text="false")
)

_world_frame_arg = DeclareLaunchArgument(
    "_world_frame",
    default_value=TextSubstitution(text="enu")
)

## arguments for rtabmap
# Choose between depth and stereo, set both to false to do only scan
stereo_arg = DeclareLaunchArgument(
    "stereo",
    default_value=TextSubstitution(text="false")
)
depth_arg = DeclareLaunchArgument(
    "depth",
    default_value=TextSubstitution(text="true")
)

subscribe_rgb_arg = DeclareLaunchArgument(
    "subscribe_rgb",
    default_value=LaunchConfiguration('depth')
)

#Localization-only mode 
localization_arg = DeclareLaunchArgument(
    "localization",
    default_value=TextSubstitution(text="false")
)
initial_pose_arg = DeclareLaunchArgument(
    "initial_pose",
    default_value=TextSubstitution(text="")
)
# sim time for convenience, if playing a rosbag
use_sim_time_arg = DeclareLaunchArgument(
    "use_sim_time",
    default_value=TextSubstitution(text="false")
)
# Corresponding config files
cfg_arg = DeclareLaunchArgument(
    "cfg",
    default_value=TextSubstitution(text="")
)
gui_cfg_arg = DeclareLaunchArgument(
    "gui_cfg",
    default_value=TextSubstitution(text="")
)
rviz_config_file = os.path.join(
    get_package_share_directory('rtabmap_launch'),
    'launch',
    'config',
    'rgbd.rviz'
)
rviz_cfg_arg = DeclareLaunchArgument(
    "rviz_cfg",
    default_value=rviz_config_file
)
frame_id_arg = DeclareLaunchArgument(
    "frame_id",
    default_value=TextSubstitution(text="camera_link") # 09.29 camera_link
)
odom_frame_id_arg = DeclareLaunchArgument(
    "odom_frame_id",
    default_value=TextSubstitution(text="") # 09.29 
)
odom_frame_id_init_arg = DeclareLaunchArgument(
    "odom_frame_id_init",
    default_value=TextSubstitution(text="")
)
map_frame_id_arg = DeclareLaunchArgument(
    "map_frame_id",
    default_value=TextSubstitution(text="map")
)
ground_truth_frame_id_arg = DeclareLaunchArgument(
    "ground_truth_frame_id",
    default_value=TextSubstitution(text="")
)
ground_truth_base_frame_id_arg = DeclareLaunchArgument(
    "ground_truth_base_frame_id",
    default_value=TextSubstitution(text="")
)
namespace_arg = DeclareLaunchArgument(
    "namespace",
    default_value=TextSubstitution(text="rtabmap")
)
database_path_arg = DeclareLaunchArgument(
    "database_path",
    default_value=TextSubstitution(text="~/.ros/rtabmap.db")
)
queue_size_arg = DeclareLaunchArgument(
    "queue_size",
    default_value=TextSubstitution(text="10")
)
wait_for_transform_arg = DeclareLaunchArgument(
    "wait_for_transform",
    default_value=TextSubstitution(text="0.2")
)
gdb_arg = DeclareLaunchArgument(
    "gdb",
    default_value=TextSubstitution(text="false")
)
launch_prefix_arg = DeclareLaunchArgument(
    "launch_prefix",
    default_value=TextSubstitution(text="")
)
#SetLaunchConfiguration(
#    'launch_prefix', 'true', condition=IfCondition(LaunchConfiguration('gdb'))
#)
#SetLaunchConfiguration(
#    'launch_prefix', 'false', condition=UnlessCondition(LaunchConfiguration('gdb'))
#)
clear_params_arg = DeclareLaunchArgument(
    "clear_params",
    default_value=TextSubstitution(text="true")
)
output_arg = DeclareLaunchArgument(
    "output",
    default_value=TextSubstitution(text="screen")
)
publish_tf_map_arg = DeclareLaunchArgument(
    "publish_tf_map",
    default_value=TextSubstitution(text="true")
)

args_arg = DeclareLaunchArgument(
    "args",
    default_value=TextSubstitution(text="--delete_db_on_start")
)

rgb_topic_arg = DeclareLaunchArgument(
    "rgb_topic",
    default_value=TextSubstitution(text="/camera/color/image_raw")
)

depth_topic_arg = DeclareLaunchArgument(
    "depth_topic",
    default_value=TextSubstitution(text="/camera/aligned_depth_to_color/image_raw")
)

camera_info_topic_arg = DeclareLaunchArgument(
    "camera_info_topic",
    default_value=TextSubstitution(text="/camera/color/camera_info")
)

depth_camera_info_topic_arg = DeclareLaunchArgument(
    "depth_camera_info_topic",
    default_value=TextSubstitution(text="/camera/depth/camera_info")   
)

rtabmap_viz_arg = DeclareLaunchArgument(
    "rtabmap_viz",
    default_value=TextSubstitution(text="true")
)
rtabmapviz_arg = DeclareLaunchArgument(
    "rtabmapviz",
    default_value=TextSubstitution(text="true")
)

rviz_arg = DeclareLaunchArgument(
    "rviz",
    default_value=TextSubstitution(text="true")
)

approx_sync_arg = DeclareLaunchArgument(
    "approx_sync",
    default_value= LaunchConfiguration('depth')
)
approx_sync_max_interval_arg = DeclareLaunchArgument(
    "approx_sync_max_interval",
    default_value=TextSubstitution(text="0.5")
)

# stereo related topics
stereo_namespace_arg = DeclareLaunchArgument(
    "stereo_namespace",
    default_value=TextSubstitution(text="/stereo_camera")
)
left_image_topic_arg = DeclareLaunchArgument(
    "left_image_topic",
    default_value=TextSubstitution(text="/stereo_camera/left/image_rect_color")
)
right_image_topic_arg = DeclareLaunchArgument(
    "right_image_topic",
    default_value=TextSubstitution(text="/stereo_camera/right/image_rect_color")
)
left_camera_info_topic_arg = DeclareLaunchArgument(
    "left_camera_info_topic",
    default_value=TextSubstitution(text="/stereo_camera/left/camera_info")
)
right_camera_info_topic_arg = DeclareLaunchArgument(
    "right_camera_info_topic",
    default_value=TextSubstitution(text="/stereo_camera/right/camera_info")
)

# Already synchronized RGB-D related topic, with rtabmap_sync/rgbd_sync nodelet
rgbd_sync_arg = DeclareLaunchArgument(
    "rgbd_sync",
    default_value=TextSubstitution(text="false")
)
approx_rgbd_sync_arg = DeclareLaunchArgument(
    "approx_rgbd_sync",
    default_value=TextSubstitution(text="true")
)
subscribe_rgbd_arg = DeclareLaunchArgument(
    "subscribe_rgbd",
    default_value = LaunchConfiguration('rgbd_sync')
)
rgbd_topic_arg = DeclareLaunchArgument(
    "rgbd_topic",
    default_value=TextSubstitution(text="rgbd_image")
)
depth_scale_arg = DeclareLaunchArgument(
    "depth_scale",
    default_value=TextSubstitution(text="1.0")
)
rgbd_depth_scale_arg = DeclareLaunchArgument(
    "rgbd_depth_scale",
    default_value=LaunchConfiguration('depth_scale')
)
rgbd_decimation_arg = DeclareLaunchArgument(
    "rgbd_decimation",
    default_value=TextSubstitution(text="1")
)
compressed_arg = DeclareLaunchArgument(
    "compressed",
    default_value=TextSubstitution(text="false")
)
rgb_image_transport_arg = DeclareLaunchArgument(
    "rgb_image_transport",
    default_value=TextSubstitution(text="compressed")
)
depth_image_transport_arg = DeclareLaunchArgument(
    "depth_image_transport",
    default_value=TextSubstitution(text="compressedDepth")
)
gen_cloud_arg = DeclareLaunchArgument(
    "gen_cloud",
    default_value=TextSubstitution(text="false")
)
gen_cloud_decimation_arg = DeclareLaunchArgument(
    "gen_cloud_decimation",
    default_value=TextSubstitution(text="4")
)
gen_cloud_voxel_arg = DeclareLaunchArgument(
    "gen_cloud_voxel",
    default_value=TextSubstitution(text="0.05")
)
subscribe_scan_arg = DeclareLaunchArgument(
    "subscribe_scan",
    default_value= TextSubstitution(text="false")
)
scan_cloud_topic_arg = DeclareLaunchArgument(
    "scan_cloud_topic",
    default_value=TextSubstitution(text="/scan_cloud") 
)
subscribe_scan_descriptor_arg = DeclareLaunchArgument(
    "subscribe_scan_descriptor",
    default_value=TextSubstitution(text="false")
)
scan_descriptor_topic_arg = DeclareLaunchArgument(
    "scan_descriptor_topic",
    default_value=TextSubstitution(text="/scan_descriptor")
)
scan_deskewing_arg = DeclareLaunchArgument(
    "scan_deskewing",
    default_value=TextSubstitution(text="false")
)
scan_deskewing_slerp_arg = DeclareLaunchArgument(
    "scan_deskewing_slerp",
    default_value=TextSubstitution(text="false")
)
scan_cloud_max_points_arg = DeclareLaunchArgument(
    "scan_cloud_max_points",
    default_value=TextSubstitution(text="0")
)
scan_cloud_filtered_arg = DeclareLaunchArgument(
    "scan_cloud_filtered",
    default_value=LaunchConfiguration('scan_deskewing')
)
gen_scan_arg = DeclareLaunchArgument(
    "gen_scan",
    default_value=TextSubstitution(text="false")
)
gen_depth_arg = DeclareLaunchArgument(
    "gen_depth",
    default_value=TextSubstitution(text="false")
)
gen_depth_decimation_arg = DeclareLaunchArgument(
    "gen_depth_decimation",
    default_value=TextSubstitution(text="1")
)
gen_depth_fill_holes_size_arg = DeclareLaunchArgument(
    "gen_depth_fill_holes_size",
    default_value=TextSubstitution(text="0")
)
gen_depth_fill_iterations_arg = DeclareLaunchArgument(
    "gen_depth_fill_iterations",
    default_value=TextSubstitution(text="1")
)
gen_depth_fill_holes_error_arg = DeclareLaunchArgument(
    "gen_depth_fill_holes_error",
    default_value=TextSubstitution(text="0.1")
)
visual_odometry_arg = DeclareLaunchArgument(
    "visual_odometry",
    default_value=TextSubstitution(text="true")
)
icp_odometry_arg = DeclareLaunchArgument(
    "icp_odometry",
    default_value=TextSubstitution(text="false")
)
odom_topic_arg = DeclareLaunchArgument(
    "odom_topic",
    default_value=TextSubstitution(text="odom")
)
vo_frame_id_arg = DeclareLaunchArgument(
    "vo_frame_id",
    default_value=LaunchConfiguration('odom_topic')
)
publish_tf_odom_arg = DeclareLaunchArgument(
    "publish_tf_odom",
    default_value=TextSubstitution(text="true")
)
odom_tf_angular_variance_arg = DeclareLaunchArgument(
    "odom_tf_angular_variance",
    default_value=TextSubstitution(text="0.001")
)
odom_tf_linear_variance_arg = DeclareLaunchArgument(
    "odom_tf_linear_variance",
    default_value=TextSubstitution(text="0.001")
)
odom_args_arg = DeclareLaunchArgument(
    "odom_args",
    default_value=TextSubstitution(text="")
)
odom_sensor_sync_arg = DeclareLaunchArgument(
    "odom_sensor_sync",
    default_value=TextSubstitution(text="false")
)
odom_guess_frame_id_arg = DeclareLaunchArgument(
    "odom_guess_frame_id",
    default_value=TextSubstitution(text="")
)
odom_guess_min_translation_arg = DeclareLaunchArgument(
    "odom_guess_min_translation",
    default_value=TextSubstitution(text="0.0")
)
odom_guess_min_rotation_arg = DeclareLaunchArgument(
    "odom_guess_min_rotation",
    default_value=TextSubstitution(text="0.0")
)
odom_max_rate_arg = DeclareLaunchArgument(
    "odom_max_rate",
    default_value=TextSubstitution(text="0.0")
)
odom_expected_rate_arg = DeclareLaunchArgument(
    "odom_expected_rate",
    default_value=TextSubstitution(text="0.0")
)
imu_topic_arg = DeclareLaunchArgument(
    "imu_topic",
    default_value=TextSubstitution(text="/imu/data")
)
wait_imu_to_init_arg = DeclareLaunchArgument(
    "wait_imu_to_init",
    default_value=TextSubstitution(text="false")
)
use_odom_features_arg = DeclareLaunchArgument(
    "use_odom_features",
    default_value=TextSubstitution(text="false")
)

scan_cloud_assembling_arg = DeclareLaunchArgument(
    "scan_cloud_assembling",
    default_value=TextSubstitution(text="false")
)
scan_cloud_assembling_time_arg = DeclareLaunchArgument(
    "scan_cloud_assembling_time",
    default_value=TextSubstitution(text="1")
)
scan_cloud_assembling_max_clouds_arg = DeclareLaunchArgument(
    "scan_cloud_assembling_max_clouds",
    default_value=TextSubstitution(text="0")
)
scan_cloud_assembling_fixed_frame_arg = DeclareLaunchArgument(
    "scan_cloud_assembling_fixed_frame",
    default_value=TextSubstitution(text="")
)
scan_cloud_assembling_voxel_size_arg = DeclareLaunchArgument(
    "scan_cloud_assembling_voxel_size",
    default_value=TextSubstitution(text="0.05")
)
scan_cloud_assembling_range_min_arg = DeclareLaunchArgument(
    "scan_cloud_assembling_range_min",
    default_value=TextSubstitution(text="0.0")
)
scan_cloud_assembling_range_max_arg = DeclareLaunchArgument(
    "scan_cloud_assembling_range_max",
    default_value=TextSubstitution(text="0.0")
)
scan_cloud_assembling_noise_radius_arg = DeclareLaunchArgument(
    "scan_cloud_assembling_noise_radius",
    default_value=TextSubstitution(text="0.0")
)
scan_cloud_assembling_noise_min_neighbors_arg = DeclareLaunchArgument(
    "scan_cloud_assembling_noise_min_neighbors",
    default_value=TextSubstitution(text="5")
)
subscribe_user_data_arg = DeclareLaunchArgument(
    "subscribe_user_data",
    default_value=TextSubstitution(text="false")
)
user_data_topic_arg = DeclareLaunchArgument(
    "user_data_topic",
    default_value=TextSubstitution(text="/user_data")
)
user_data_async_topic_arg = DeclareLaunchArgument(
    "user_data_async_topic",
    default_value=TextSubstitution(text="/user_data_async")
)
gps_topic_arg = DeclareLaunchArgument(
    "gps_topic",
    default_value=TextSubstitution(text="/gps/fix")
)
tag_topic_arg = DeclareLaunchArgument(
    "tag_topic",
    default_value=TextSubstitution(text="/tag_detections")
)
tag_linear_variance_arg = DeclareLaunchArgument(
    "tag_linear_variance",
    default_value=TextSubstitution(text="0.0001")
)
tag_angular_variance_arg = DeclareLaunchArgument(
    "tag_angular_variance",
    default_value=TextSubstitution(text="9999.0")
)
fiducial_topic_arg = DeclareLaunchArgument(
    "fiducial_topic",
    default_value=TextSubstitution(text="/fiducial_transforms")
)

rgb_topic_relay_arg = DeclareLaunchArgument(
    "rgb_topic_relay",
    default_value=  TextSubstitution(text="/camera/color/image_raw")
)
depth_topic_relay_arg = DeclareLaunchArgument(
    "depth_topic_relay",
    default_value= TextSubstitution(text="/camera/aligned_depth_to_color/image_raw")
)
left_image_topic_relay_arg = DeclareLaunchArgument(
    "left_image_topic_relay",
    default_value= TextSubstitution(text="/stereo_camera/left/image_rect_color")
)    
right_image_topic_relay_arg = DeclareLaunchArgument(
    "right_image_topic_relay",
    default_value= TextSubstitution(text="/stereo_camera/right/image_rect_color")
)
rgbd_topic_relay_arg = DeclareLaunchArgument(
    "rgbd_topic_relay",
    default_value= TextSubstitution(text="/rgbd_image_relay")
)

# arguments for robot_localization
frequency_arg = DeclareLaunchArgument(
    "frequency",
    default_value=TextSubstitution(text="300")
)

base_link_frame_arg = DeclareLaunchArgument(
    "base_link_frame",
    default_value=TextSubstitution(text="camera_link")
)

odom0_arg = DeclareLaunchArgument(
    "odom0",
    default_value=TextSubstitution(text="rtabmap/odom")
)

odom0_config_arg = DeclareLaunchArgument(
    "odom0_config",
    default_value=TextSubstitution(text="true,true,true,true,true,true,true,true,true,true,true,true,true,true,true")
)

odom0_relative_arg = DeclareLaunchArgument(
    "odom0_relative",
    default_value=TextSubstitution(text="true")    
)

odom0_pose_rejection_threshold_arg = DeclareLaunchArgument( 
    "odom0_pose_rejection_threshold",
    default_value=TextSubstitution(text="10000000")
)

odom0_twist_rejection_threshold_arg = DeclareLaunchArgument(    
    "odom0_twist_rejection_threshold",
    default_value=TextSubstitution(text="10000000")
)

imu0_arg = DeclareLaunchArgument(
    "imu0",
    default_value=TextSubstitution(text="/imu/data")
)   

imu0_config_arg = DeclareLaunchArgument(
    "imu0_config",
    default_value=TextSubstitution(text="false,false,false,true,true,true,true,true,true,true,true,true,true,true,true")

)   

imu0_differential_arg = DeclareLaunchArgument(
    "imu0_differential",
    default_value=TextSubstitution(text="true")
)

imu0_relative_arg = DeclareLaunchArgument(
    "imu0_relative",
    default_value=TextSubstitution(text="false")
)       

use_control_arg = DeclareLaunchArgument(
    "use_control",
    default_value=TextSubstitution(text="false")
)

rs2_camera_launch_include = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(           
        [ThisLaunchFileDir(), '/rs_launch.py']),
    launch_arguments={
        #'buffer_length': LaunchConfiguration('buffer_length'),
        'emitter_enabled': LaunchConfiguration('emitter_enabled'),
        'enable_infra1': LaunchConfiguration('enable_infra1'),
        'enable_infra2': LaunchConfiguration('enable_infra2'),
        'remappings': str(remappings),
        'enable_accel': LaunchConfiguration('enable_accel'),
        'enable_gyro': LaunchConfiguration('enable_gyro'),
        'enable_rgbd': LaunchConfiguration('enable_rgbd'),
        'align_depth.enable': LaunchConfiguration('align_depth.enable'),
        'enable_sync': LaunchConfiguration('enable_sync'),
        'enable_color': LaunchConfiguration('enable_color'),
        'enable_depth': LaunchConfiguration('enable_depth'), 
        'linear_accel_cov': LaunchConfiguration('linear_accel_cov'),
        'unite_imu_method': LaunchConfiguration('unite_imu_method')}.items()            
)

imu_filter_madgwick_node = Node(
        package='imu_filter_madgwick',
        executable='imu_filter_madgwick_node',
        name='ImuFilter',
        remappings=[
            ('/imu/data_raw', '/camera/imu')
        ],
        parameters=[{
            'use_mag': False,
            'publish_tf': False,
            'world_frame': 'enu',
        }]
    )

rtabmap_lite_launch_include = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
        os.path.join(
            get_package_share_directory('rtabmap_lite'),
            'launch',
            'rtabmap_lite.launch.py')),
            launch_arguments={                
                'frame_id': LaunchConfiguration('frame_id'),
                'odom_frame_id': LaunchConfiguration('odom_frame_id'),
                'approx_sync': LaunchConfiguration('approx_sync'),
                'approx_sync_max_interval': LaunchConfiguration('approx_sync_max_interval'),
            }.items()
)

rtabmap_ros_launch_include = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
        os.path.join(
            get_package_share_directory('rtabmap_launch'),
            'launch/rtabmap.launch.py')),
            launch_arguments={
            'args': LaunchConfiguration('args'), 
            'rgb_topic': LaunchConfiguration('rgb_topic'),
            'depth_topic': LaunchConfiguration('depth_topic'),
            'camera_info_topic': LaunchConfiguration('camera_info_topic'),
            'depth_camera_info_topic': LaunchConfiguration('depth_camera_info_topic'),
            'rtabmap_viz': LaunchConfiguration('rtabmap_viz'),
            'rviz': LaunchConfiguration('rviz'),
            'stereo': LaunchConfiguration('stereo'),
            'depth': LaunchConfiguration('depth'),
            'subscribe_rgb': LaunchConfiguration('subscribe_rgb'),
            'rtabmap_viz': LaunchConfiguration('rtabmap_viz'),
            'localization': LaunchConfiguration('localization'),
            'initial_pose': LaunchConfiguration('initial_pose'),
            'use_sim_time': LaunchConfiguration('use_sim_time'),
            'cfg': LaunchConfiguration('cfg'),
            'gui_cfg': LaunchConfiguration('gui_cfg'),
            'rviz_cfg': LaunchConfiguration('rviz_cfg'),
            'frame_id': LaunchConfiguration('frame_id'),
            'odom_frame_id': LaunchConfiguration('odom_frame_id'),
            'odom_frame_id_init': LaunchConfiguration('odom_frame_id_init'),
            'map_frame_id': LaunchConfiguration('map_frame_id'),
            'ground_truth_frame_id': LaunchConfiguration('ground_truth_frame_id'),
            'ground_truth_base_frame_id': LaunchConfiguration('ground_truth_base_frame_id'),
            'namespace': LaunchConfiguration('namespace'),
            'database_path': LaunchConfiguration('database_path'),
            'queue_size': LaunchConfiguration('queue_size'),
            'wait_for_transform': LaunchConfiguration('wait_for_transform'),
            'gdb': LaunchConfiguration('gdb'),
            'launch_prefix': LaunchConfiguration('launch_prefix'),
            'clear_params': LaunchConfiguration('clear_params'),
            'output': LaunchConfiguration('output'),
            'publish_tf_map': LaunchConfiguration('publish_tf_map'),   

            'approx_sync': LaunchConfiguration('approx_sync'),
            'approx_sync_max_interval': LaunchConfiguration('approx_sync_max_interval'),
            'rgb_topic': LaunchConfiguration('rgb_topic'),
            'depth_topic': LaunchConfiguration('depth_topic'),
            'camera_info_topic': LaunchConfiguration('camera_info_topic'),
            'depth_camera_info_topic': LaunchConfiguration('depth_camera_info_topic'),
            'stereo_namespace': LaunchConfiguration('stereo_namespace'),
            'left_image_topic': LaunchConfiguration('left_image_topic'),
            'right_image_topic': LaunchConfiguration('right_image_topic'),
            'left_camera_info_topic': LaunchConfiguration('left_camera_info_topic'),
            'right_camera_info_topic': LaunchConfiguration('right_camera_info_topic'),
            'rgbd_sync': LaunchConfiguration('rgbd_sync'),
            'approx_rgbd_sync': LaunchConfiguration('approx_rgbd_sync'),
            'subscribe_rgbd': LaunchConfiguration('subscribe_rgbd'),
            'rgbd_topic': LaunchConfiguration('rgbd_topic'),
            'depth_scale': LaunchConfiguration('depth_scale'),
            'rgbd_depth_scale': LaunchConfiguration('rgbd_depth_scale'),
            'rgbd_decimation': LaunchConfiguration('rgbd_decimation'),
            'compressed': LaunchConfiguration('compressed'),
            'rgb_image_transport': LaunchConfiguration('rgb_image_transport'),
            'depth_image_transport': LaunchConfiguration('depth_image_transport'),

            'gen_cloud': LaunchConfiguration('gen_cloud'),
            'gen_cloud_decimation': LaunchConfiguration('gen_cloud_decimation'),
            'gen_cloud_voxel': LaunchConfiguration('gen_cloud_voxel'),
            'subscribe_scan': LaunchConfiguration('subscribe_scan'),
            'scan_cloud_topic': LaunchConfiguration('scan_cloud_topic'),
            'subscribe_scan_descriptor': LaunchConfiguration('subscribe_scan_descriptor'),
            'scan_descriptor_topic': LaunchConfiguration('scan_descriptor_topic'),
            'scan_deskewing': LaunchConfiguration('scan_deskewing'),
            'scan_deskewing_slerp': LaunchConfiguration('scan_deskewing_slerp'),
            'scan_cloud_max_points': LaunchConfiguration('scan_cloud_max_points'),
            'scan_cloud_filtered': LaunchConfiguration('scan_cloud_filtered'),
            'gen_scan': LaunchConfiguration('gen_scan'),

            'gen_depth': LaunchConfiguration('gen_depth'),
            'gen_depth_decimation': LaunchConfiguration('gen_depth_decimation'),
            'gen_depth_fill_holes_size': LaunchConfiguration('gen_depth_fill_holes_size'),
            'gen_depth_fill_iterations': LaunchConfiguration('gen_depth_fill_iterations'),
            'gen_depth_fill_holes_error': LaunchConfiguration('gen_depth_fill_holes_error'),

            'visual_odometry': LaunchConfiguration('visual_odometry'),
            'icp_odometry': LaunchConfiguration('icp_odometry'),
            'odom_topic': LaunchConfiguration('odom_topic'),
            'vo_frame_id': LaunchConfiguration('vo_frame_id'),
            'publish_tf_odom': LaunchConfiguration('publish_tf_odom'),
            'odom_tf_angular_variance': LaunchConfiguration('odom_tf_angular_variance'),
            'odom_tf_linear_variance': LaunchConfiguration('odom_tf_linear_variance'),
            'odom_args': LaunchConfiguration('odom_args'),
            'odom_sensor_sync': LaunchConfiguration('odom_sensor_sync'),
            'odom_guess_frame_id': LaunchConfiguration('odom_guess_frame_id'),
            'odom_guess_min_translation': LaunchConfiguration('odom_guess_min_translation'),
            'odom_guess_min_rotation': LaunchConfiguration('odom_guess_min_rotation'),
            'odom_max_rate': LaunchConfiguration('odom_max_rate'),
            'odom_expected_rate': LaunchConfiguration('odom_expected_rate'),
            'imu_topic': LaunchConfiguration('imu_topic'),
            'wait_imu_to_init': LaunchConfiguration('wait_imu_to_init'),
            'use_odom_features': LaunchConfiguration('use_odom_features'),

            'scan_cloud_assembling': LaunchConfiguration('scan_cloud_assembling'),
            'scan_cloud_assembling_time': LaunchConfiguration('scan_cloud_assembling_time'),
            'scan_cloud_assembling_max_clouds': LaunchConfiguration('scan_cloud_assembling_max_clouds'),
            'scan_cloud_assembling_fixed_frame': LaunchConfiguration('scan_cloud_assembling_fixed_frame'),
            'scan_cloud_assembling_voxel_size': LaunchConfiguration('scan_cloud_assembling_voxel_size'),
            'scan_cloud_assembling_range_min': LaunchConfiguration('scan_cloud_assembling_range_min'),
            'scan_cloud_assembling_range_max': LaunchConfiguration('scan_cloud_assembling_range_max'),
            'scan_cloud_assembling_noise_radius': LaunchConfiguration('scan_cloud_assembling_noise_radius'),
            'scan_cloud_assembling_noise_min_neighbors': LaunchConfiguration('scan_cloud_assembling_noise_min_neighbors'),

            'subscribe_user_data': LaunchConfiguration('subscribe_user_data'),
            'user_data_topic': LaunchConfiguration('user_data_topic'),
            'user_data_async_topic': LaunchConfiguration('user_data_async_topic'),
            'gps_topic': LaunchConfiguration('gps_topic'),
            'tag_topic': LaunchConfiguration('tag_topic'),
            'tag_linear_variance': LaunchConfiguration('tag_linear_variance'),
            'tag_angular_variance': LaunchConfiguration('tag_angular_variance'),
            'fiducial_topic': LaunchConfiguration('fiducial_topic'),

            'rgb_topic_relay': LaunchConfiguration('rgb_topic_relay'),
            'depth_topic_relay': LaunchConfiguration('depth_topic_relay'),
            'left_image_topic_relay': LaunchConfiguration('left_image_topic_relay'),
            'right_image_topic_relay': LaunchConfiguration('right_image_topic_relay'),
            'rgbd_topic_relay' : LaunchConfiguration('rgbd_topic_relay'), 
            #'approx_sync': 'true',
            #'approx_sync_max_interval': '0.02'
            }.items()                               
)

robot_localization_launch_include = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
        os.path.join(
            get_package_share_directory('robot_localization'),
            'launch/ukf.launch.py'))     
)   

return LaunchDescription([
    emitter_enabled_arg,
    enable_infra1_arg,
    enable_infra2_arg,
    enable_accel_arg,
    enable_gyro_arg,
    offline_arg,
    enable_rgbd_arg,
    align_depth_arg,
    enable_sync_arg,
    enable_color_arg,
    enable_depth_arg,
    linear_accel_cov_arg,
    unite_imu_method_arg,
    use_mag_arg,
    _publish_tf_arg,
    _world_frame_arg,
    args_arg,
    rgb_topic_arg,
    depth_topic_arg,
    camera_info_topic_arg,
    depth_camera_info_topic_arg,
    rtabmap_viz_arg,
    rtabmapviz_arg,
    rviz_arg,
    stereo_arg,
    depth_arg,       
    subscribe_rgb_arg,
    localization_arg,
    initial_pose_arg,
    use_sim_time_arg,
    cfg_arg,
    gui_cfg_arg,
    rviz_cfg_arg,
    frame_id_arg,
    odom_frame_id_arg,
    odom_frame_id_init_arg,
    map_frame_id_arg,
    ground_truth_frame_id_arg,
    ground_truth_base_frame_id_arg,
    namespace_arg,
    database_path_arg,
    queue_size_arg,
    wait_for_transform_arg,
    gdb_arg,
    launch_prefix_arg,      
    clear_params_arg,
    output_arg,
    publish_tf_map_arg,
    approx_sync_arg,
    approx_sync_max_interval_arg,
    rgb_topic_arg,
    depth_topic_arg,
    camera_info_topic_arg,
    depth_camera_info_topic_arg,
    stereo_namespace_arg,
    left_image_topic_arg,
    right_image_topic_arg,
    left_camera_info_topic_arg,
    right_camera_info_topic_arg,
    rgbd_sync_arg,
    approx_rgbd_sync_arg,
    subscribe_rgbd_arg,
    rgbd_topic_arg,
    depth_scale_arg,
    rgbd_depth_scale_arg,
    rgbd_decimation_arg,
    compressed_arg,
    rgb_image_transport_arg,
    depth_image_transport_arg,
    gen_cloud_arg,
    gen_cloud_decimation_arg,
    gen_cloud_voxel_arg,
    subscribe_scan_arg,
    scan_cloud_topic_arg,
    subscribe_scan_descriptor_arg,
    scan_descriptor_topic_arg,
    scan_deskewing_arg,
    scan_deskewing_slerp_arg,
    scan_cloud_max_points_arg,
    scan_cloud_filtered_arg,
    gen_scan_arg,
    gen_depth_arg,
    gen_depth_decimation_arg,
    gen_depth_fill_holes_size_arg,
    gen_depth_fill_iterations_arg,
    gen_depth_fill_holes_error_arg,
    visual_odometry_arg,
    icp_odometry_arg,
    odom_topic_arg,
    vo_frame_id_arg,
    publish_tf_odom_arg,
    odom_tf_angular_variance_arg,
    odom_tf_linear_variance_arg,
    odom_args_arg,
    odom_sensor_sync_arg,
    odom_guess_frame_id_arg,
    odom_guess_min_translation_arg,
    odom_guess_min_rotation_arg,
    odom_max_rate_arg,
    odom_expected_rate_arg,
    imu_topic_arg,
    wait_imu_to_init_arg,
    use_odom_features_arg,
    scan_cloud_assembling_arg,
    scan_cloud_assembling_time_arg,
    scan_cloud_assembling_max_clouds_arg,
    scan_cloud_assembling_fixed_frame_arg,
    scan_cloud_assembling_voxel_size_arg,
    scan_cloud_assembling_range_min_arg,
    scan_cloud_assembling_range_max_arg,
    scan_cloud_assembling_noise_radius_arg,
    scan_cloud_assembling_noise_min_neighbors_arg,
    subscribe_user_data_arg,
    user_data_topic_arg,
    user_data_async_topic_arg,
    gps_topic_arg,
    tag_topic_arg,
    tag_linear_variance_arg,
    tag_angular_variance_arg,
    fiducial_topic_arg,
    rgb_topic_relay_arg,
    depth_topic_relay_arg,
    left_image_topic_relay_arg,
    right_image_topic_relay_arg,
    rgbd_topic_relay_arg,

    frequency_arg,
    base_link_frame_arg,
    odom0_arg,
    odom0_config_arg,
    odom0_relative_arg,
    odom0_pose_rejection_threshold_arg,
    odom0_twist_rejection_threshold_arg,
    imu0_arg,
    imu0_config_arg,
    imu0_differential_arg,
    imu0_relative_arg,
    use_control_arg,
    rs2_camera_launch_include,
    imu_filter_madgwick_node,
    #rtabmap_lite_launch_include,
    rtabmap_ros_launch_include,
    robot_localization_launch_include
])

------------------------------------------------ end ----------------------------------------------

MartyG-RealSense commented 3 weeks ago

Hi @sooyoungmoon Converting the SLAM guide for use with ROS2 is not usually done by RealSense users and so there is not a previous example available to compare your own work to in order to diagnose problems, unfortunately.

RealSense ROS2 users have typically used a combination of slam_toolbox and _depthimage_to_laserscan to implement a SLAM system, as described at https://github.com/IntelRealSense/realsense-ros/issues/3046#issuecomment-1999092540

slam_toolbox: https://github.com/SteveMacenski/slam_toolbox

depthimage_to_laserscan: https://github.com/ros-perception/depthimage_to_laserscan/tree/ros2

sooyoungmoon commented 3 weeks ago

Hi @sooyoungmoon Converting the SLAM guide for use with ROS2 is not usually done by RealSense users and so there is not a previous example available to compare your own work to in order to diagnose problems, unfortunately.

RealSense ROS2 users have typically used a combination of slam_toolbox and _depthimage_to_laserscan to implement a SLAM system, as described at #3046 (comment)

slam_toolbox: https://github.com/SteveMacenski/slam_toolbox

depthimage_to_laserscan: https://github.com/ros-perception/depthimage_to_laserscan/tree/ros2

Hi @MartyG-RealSense, thank you for your quick response to my question. I'll try a combination of slam_toolbox and depthimage_to_laserscan and share the result. :)

MartyG-RealSense commented 3 weeks ago

You are very welcome, @sooyoungmoon - I look forward to your next report. Good luck!

sooyoungmoon commented 3 weeks ago

@MartyG-RealSense

Hello, for other researchers & developers, I hope you had a good day.

I'm struggling with this 'SLAM with D435i' issue to get any map data shown on rviz2.

Instead of trying SLAM-with-D435i-wiki, I installed and tested slam_toolbox and depthimage_to_laserscan.

In short, I found that the rgb->laserscan data conversion worked well, but the map data was not published by slam_toolbox node. I will check similar issues in this and related repos, but I would be appreciated if you give me any hint to

The test procedure is as follows:

1) (Executed the realsense2 camera node) $ ros2 launch realsense2_camera rs_launch.py (with remapping config)

2) (Executed the package to convert rgb to scan) $ ros2 launch depthimage_to_laserscan depthimage_to_laserscan-launch.py

3) $ ros2 launch slam_toolbox online_async_launch.py

4) $ rviz2

5) (On rviz2) I added '/map' topic -> "No map received" message occurred

6) $ ros2 run tf2_tools view_frames -> there was no 'map' frame on the tf2 tree diagram.

7) I executed a static_tranform_publisher as below: $ ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 0 --qx 0 --qy 0 --qz 0 --qw 1 --frame-id map --child-frame-id camera_link

8) $ ros2 run tf2_tools view_frames -> 'map' frame was added on the tf2 tree diagram tf2_tree(slam_toolbox and depthimage_to_laserscan).pdf

9) When I added '/scan' , I could see the laser scan image on rviz2. 스크린샷 2024-10-31 00-12-31

Still, I couldn't see any map data on rviz2. $ ros2 topic echo /map -> returned nothing

10) It seemed that the scan data was being published well $ ros2 topic echo /scan -> scan data messages were printed on the screen

MartyG-RealSense commented 3 weeks ago

Hi @sooyoungmoon Do you experience the message [INFO] [1655749660.201251796] [slam_toolbox]: Message Filter dropping message: frame 'camera_depth_frame' like in the slam_toolbox case at https://github.com/IntelRealSense/realsense-ros/issues/2387 where a RealSense user was not receiving map data?

At the end of that case, another RealSense user suggested at https://github.com/IntelRealSense/realsense-ros/issues/2387#issuecomment-1667319135 to publish the odometry value to the /odom topic

sooyoungmoon commented 2 weeks ago

Hi @sooyoungmoon Do you experience the message [INFO] [1655749660.201251796] [slam_toolbox]: Message Filter dropping message: frame 'camera_depth_frame' like in the slam_toolbox case at #2387 where a RealSense user was not receiving map data?

At the end of that case, another RealSense user suggested at #2387 (comment) to publish the odometry value to the /odom topic

@MartyG-RealSense You are right! That's the same message except that in my case the reason is 'discarding-message-because-the-queue-is-full'. Based on the user's suggestion you mentioned and other resources such as a slam_toolbox repo, I believe that I figured it out how to solve this problem. (To create a new node to publish transform data (odom_frame -> base_frame) given imu data. I think I can solve this issue in a few days. Thank you!

MartyG-RealSense commented 2 weeks ago

You are very welcome, @sooyoungmoon - I'm pleased that I could be of help. Good luck!

sooyoungmoon commented 2 weeks ago

@MartyG-RealSense

You are very welcome, @sooyoungmoon - I'm pleased that I could be of help. Good luck!

I created a new ROS2 node which calculates the camera pose from imu data and then publishes transform data (frame_id: map, child_frame_id: odom). After that, the map topic data was published by /slam_toolbox node. But there is a new issue.

Q. Is there a better way to compute camera pose compared to my approach (to compute camera pose by using imu data only)?

At first, I tried to use the orientation within the Imu message to estimate camera pose. But it was too unstable, so I skipped this approach after a few trials.

After that, I am trying to compute the position and orientation of my camera using the linear acceleration and angular velocity values.

Until now, it is not quite successful because the camera position experiences drift, and the estimated position become farther and farther away from the real position.

Especially, the absolute value for linear acceleration (y axis) is strangely high (please refer to the topic data shown below)

$ ros2 topic echo /imu/data ... header: stamp: sec: 1730819791 nanosec: 632761600 frame_id: camera_imu_optical_frame orientation: x: -0.49896361969468156 y: 0.48194959538813315 z: -0.5038171864430749 w: 0.5147117021896622 orientation_covariance:

0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0 angular_velocity: x: -0.010471975430846214 y: -0.006981316953897476 z: 0.003490658476948738 angular_velocity_covariance:
0.01
0.0
0.0
0.0
0.01
0.0
0.0
0.0
0.01 linear_acceleration: x: 0.127486452460289 y: -9.551676750183105 z: 0.4314925968647003 linear_acceleration_covariance:
1.0
0.0
0.0
0.0
1.0
0.0
0.0
0.0
1.0

header: stamp: sec: 1730819791 nanosec: 637771520 frame_id: camera_imu_optical_frame orientation: x: -0.4990473420685022 y: 0.4815993639143508 z: -0.5041232176980946 w: 0.5146587067459129 orientation_covariance:
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0 angular_velocity: x: -0.001745329238474369 y: -0.012217304669320583 z: 0.003490658476948738 angular_velocity_covariance:
0.01
0.0
0.0
0.0
0.01
0.0
0.0
0.0
0.01 linear_acceleration: x: 0.127486452460289 y: -9.551676750183105 z: 0.4314925968647003 linear_acceleration_covariance:
1.0
0.0
0.0
0.0
1.0
0.0
0.0
0.0
1.0

header: stamp: sec: 1730819791 nanosec: 642781952 frame_id: camera_imu_optical_frame orientation: x: -0.49911623571819935 y: 0.48125715574947686 z: -0.5044428763567813 w: 0.5145987930177406 orientation_covariance:
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0 angular_velocity: x: 0.0 y: -0.003490658476948738 z: 0.005235987715423107 angular_velocity_covariance:
0.01
0.0
0.0
0.0
0.01
0.0
0.0
0.0
0.01 linear_acceleration: x: 0.127486452460289 y: -9.551676750183105 z: 0.4314925968647003 linear_acceleration_covariance:
1.0
0.0
0.0
0.0
1.0
0.0
0.0
0.0
1.0

header: stamp: sec: 1730819791 nanosec: 647792384 frame_id: camera_imu_optical_frame orientation: x: -0.49915365280729845 y: 0.4815220580607172 z: -0.50418338333173 w: 0.5145689987380432 orientation_covariance:
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0 angular_velocity: x: -0.003490658476948738 y: -0.001745329238474369 z: 0.0 angular_velocity_covariance:
0.01
0.0
0.0
0.0
0.01
0.0
0.0
0.0
0.01 linear_acceleration: x: 0.0 y: -9.532063484191895 z: 0.3040061295032501 linear_acceleration_covariance:
1.0
0.0
0.0
0.0
1.0
0.0
0.0
0.0
1.0

header: stamp: sec: 1730819791 nanosec: 652804608 frame_id: camera_imu_optical_frame orientation: x: -0.49919502532603227 y: 0.4817768005011726 z: -0.5039204720266867 w: 0.5145479560361664 orientation_covariance:
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0 angular_velocity: x: -0.001745329238474369 y: -0.006981316953897476 z: 0.0 angular_velocity_covariance:
0.01
0.0
0.0
0.0
0.01
0.0
0.0
0.0
0.01 linear_acceleration: x: 0.0 y: -9.532063484191895 z: 0.3040061295032501 linear_acceleration_covariance:
1.0
0.0
0.0
0.0
1.0
0.0
0.0
0.0
1.0

MartyG-RealSense commented 2 weeks ago

RealSense 400 Series cameras do not have the in-built ability to calculate the relative position of the camera device. The only approach that I know of (other than the ROS2 one that you created yourself) is a ROS1 user at the link below who used rtabmap to study a recorded pointcloud map file and obtain the relative position from that map.

https://shinkansan.github.io/2019-UGRP-DPoom/SLAM

MartyG-RealSense commented 1 week ago

Hi @sooyoungmoon Do you require further assistance with this case, please? Thanks!

sooyoungmoon commented 6 days ago

@MartyG-RealSense Hello, I managed to draw a map using slam_toolbox and depthimage_to_laserscan. This was possible by providing transform data (odom to camera_link). I have one more question. When I move my D435i camera to generate maps, the realsense2_camera node often disconnects the connection to the camera and tries to re-connect to it. Could you tell me under which condition does it happen? Thanks. ![Uploading 20241116_115249.jpg…]()

MartyG-RealSense commented 6 days ago

I'm pleased to hear that you succeeded in drawing a map!

In regard to your new question, Intel's D435i SLAM guide for ROS1 provides the following guidance

The built-in IMU can only keep track for a very short time. Moving or turning too quickly will break the sequence of successful point cloud matches and will result in the system losing track. It could happen that the system will recover immediately if stopped moving but if not, the longer the time passed since the break, the farther away it will drift from the correct position. The odds for recovery get very slim, very quickly

SLAM tends to work better in scenes that are not too sparse and have objects in them for the camera to analyze.

IntelRealSense / realsense-ros

[SLAM with D435i] TF errors (TF_NAN_INPUT, TF_DENORMALIZED_QUATERNION) and incorrect 'camera_link' frame data #3235

Issue Description

1.0

1.0

1.0

1.0

1.0