search

introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
960 stars 558 forks source link

For Realsense D435i #318

Closed Garand0o0 closed 10 months ago

Garand0o0 commented 5 years ago

Hi, roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" depth_topic:=/camera/aligned_depth_to_color/image_raw rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info approx_sync:=false Is the image_raw pre-calibration of the d435i?

roslaunch realsense2_camera rs_camera.launch align_depth:=true Does the d435i only send pre-calibration data?

how can I use the calibrated date of d435i?

thanks

matlabbe commented 5 years ago

It depends on what means "image_raw" for the realsense driver. Is there an image_rect topic? If so, make sure the registered depth image matches the image_rect topic, and you can use it. What are the topics available when you launch realsense2 with:

$roslaunch realsense2_camera rs_camera.launch align_depth:=true

?

$ rostopic list

In this example, we assume that raw RGB image has low distortions. RTAB-Map requires that input images are already rectified.

cheers, Mathieu

Garand0o0 commented 5 years ago

It depends on what means "image_raw" for the realsense driver. Is there an image_rect topic? If so, make sure the registered depth image matches the image_rect topic, and you can use it. What are the topics available when you launch realsense2 with:

$roslaunch realsense2_camera rs_camera.launch align_depth:=true

?

$ rostopic list

In this example, we assume that raw RGB image has low distortions. RTAB-Map requires that input images are already rectified.

cheers, Mathieu

Thank you for your reply.

ahar commented 3 years ago

Hi Matheu, I have a doubt related to this issue. I am using D435 Model in a Gazebo simulation. The list of topics published in the beginning after the camera is up, are given below.

/camera/color/camera_info /camera/color/image_raw /camera/color/image_raw/compressed /camera/color/image_raw/compressed/parameter_descriptions /camera/color/image_raw/compressed/parameter_updates /camera/color/image_raw/compressedDepth /camera/color/image_raw/compressedDepth/parameter_descriptions /camera/color/image_raw/compressedDepth/parameter_updates /camera/color/image_raw/theora /camera/color/image_raw/theora/parameter_descriptions /camera/color/image_raw/theora/parameter_updates /camera/depth/camera_info /camera/depth/color/points /camera/depth/image_raw /camera/depth/image_raw/compressed /camera/depth/image_raw/compressed/parameter_descriptions /camera/depth/image_raw/compressed/parameter_updates /camera/depth/image_raw/compressedDepth /camera/depth/image_raw/compressedDepth/parameter_descriptions /camera/depth/image_raw/compressedDepth/parameter_updates /camera/depth/image_raw/theora /camera/depth/image_raw/theora/parameter_descriptions /camera/depth/image_raw/theora/parameter_updates /camera/infra1/camera_info /camera/infra1/image_raw /camera/infra1/image_raw/compressed /camera/infra1/image_raw/compressed/parameter_descriptions /camera/infra1/image_raw/compressed/parameter_updates /camera/infra1/image_raw/compressedDepth /camera/infra1/image_raw/compressedDepth/parameter_descriptions /camera/infra1/image_raw/compressedDepth/parameter_updates /camera/infra1/image_raw/theora /camera/infra1/image_raw/theora/parameter_descriptions /camera/infra1/image_raw/theora/parameter_updates /camera/infra2/camera_info /camera/infra2/image_raw /camera/infra2/image_raw/compressed /camera/infra2/image_raw/compressed/parameter_descriptions /camera/infra2/image_raw/compressed/parameter_updates /camera/infra2/image_raw/compressedDepth /camera/infra2/image_raw/compressedDepth/parameter_descriptions /camera/infra2/image_raw/compressedDepth/parameter_updates /camera/infra2/image_raw/theora /camera/infra2/image_raw/theora/parameter_descriptions /camera/infra2/image_raw/theora/parameter_updates

Now, as registered depth images are required for working with various rtabmap nodes, I executed rs_rgbd.launch. However, I am stuck at the following remapping:

    <group if="$(eval depth_registered_processing and sw_registered_processing)">
      <node pkg="nodelet" type="nodelet" name="register_depth"
            args="load depth_image_proc/register $(arg manager) $(arg bond)" respawn="$(arg respawn)">
        <remap from="rgb/camera_info"             to="camera/$(arg rgb)/camera_info" />
        <remap from="depth/camera_info"           to="camera/$(arg depth)/camera_info" />
        <remap from="depth/image_rect"            to="camera/$(arg depth)/image_rect_raw" />
        <remap from="depth_registered/image_rect" to="camera/$(arg depth_registered)/sw_registered/image_rect_raw" />
      </node>

INo topics are published with the topic camera/$(arg depth)/image_rect_raw. I believe the reason may be that I do not have a rectified depth image publisher working. Also, who is publishing on depth_registered/image_rect? I don't see that. Would you please look into this issue?

matlabbe commented 3 years ago

You may have to use image_raw instead of image_rect_raw. With the simulator, the _raw topics are generally already rectified.

yuan-0816 commented 10 months ago

I've encountered a similar issue. I want to simulate the RealSense D435i in Gazebo and use RTAB-Map for localization. Here are my xacro and launch files.

<?xml version="1.0"?>
<!--
Aknolegment: This file was originally copied from the realsense repository of
pal-robotics-forks( https://github.com/pal-robotics-forks/realsense/ )
and then modified.

License: Apache 2.0. See LICENSE file in root directory.
Copyright(c) 2017 PAL Robotics, S.L. All Rights Reserved

This is the Gazebo URDF model for the Intel RealSense D435i camera
-->
<robot xmlns:xacro="http://ros.org/wiki/xacro">
    <xacro:macro name="gazebo_d435i" params=" reference_link
                                            camera_name:=camera
                                            topics_ns:=camera

                                            depth_optical_frame
                                            color_optical_frame
                                            infrared1_optical_frame
                                            infrared2_optical_frame
                                            accel_optical_frame
                                            gyro_optical_frame

                                            visualize:=false
                                            align_depth:=false
                                            enable_pointCloud:=false

                                            unite_imu_method:=false
                                            accel_fps:=250
                                            gyro_fps:=400

                                            clip_distance:=-1.0
                                            depth_width:=1280
                                            depth_height:=720
                                            depth_fps:=30

                                            infra_width:=640
                                            infra_height:=480
                                            infra_fps:=30

                                            color_width:=1920
                                            color_height:=1080
                                            color_fps:=30
                                            ">
        <!-- Load parameters to model's main link-->
        <xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
        <gazebo reference="${reference_link}">
            <self_collide>0</self_collide>
            <enable_wind>0</enable_wind>
            <kinematic>0</kinematic>
            <!-- <gravity>1</gravity> -->
            <!--<mu>1</mu>-->
            <mu2>1</mu2>
            <fdir1>0 0 0</fdir1>
            <!--<slip1>0</slip1>
      <slip2>0</slip2>-->
            <kp>1e+13</kp>
            <kd>1</kd>
            <!--<max_vel>0.01</max_vel>
      <min_depth>0</min_depth>-->
            <sensor name="${camera_name}color" type="camera">
                <camera name="${camera_name}">
                    <horizontal_fov>${69.4*deg_to_rad}</horizontal_fov>
                    <image>
                        <width>${color_width}</width>
                        <height>${color_height}</height>
                        <format>RGB_INT8</format>
                    </image>
                    <clip>
                        <near>0.1</near>
                        <far>100</far>
                    </clip>
                    <noise>
                        <type>gaussian</type>
                        <mean>0.0</mean>
                        <stddev>0.007</stddev>
                    </noise>
                </camera>
                <always_on>1</always_on>
                <update_rate>${color_fps}</update_rate>
                <visualize>${visualize}</visualize>
            </sensor>
            <sensor name="${camera_name}ired1" type="camera">
                <camera name="${camera_name}">
                    <horizontal_fov>${85.2*deg_to_rad}</horizontal_fov>
                    <image>
                        <width>${infra_width}</width>
                        <height>${infra_height}</height>
                        <format>L_INT8</format>
                    </image>
                    <clip>
                        <near>0.1</near>
                        <far>100</far>
                    </clip>
                    <noise>
                        <type>gaussian</type>
                        <mean>0.0</mean>
                        <stddev>0.05</stddev>
                    </noise>
                </camera>
                <always_on>1</always_on>
                <update_rate>${infra_fps}</update_rate>
                <visualize>false</visualize>
            </sensor>
            <sensor name="${camera_name}ired2" type="camera">
                <camera name="${camera_name}">
                    <horizontal_fov>${85.2*deg_to_rad}</horizontal_fov>
                    <image>
                        <width>${infra_width}</width>
                        <height>${infra_height}</height>
                        <format>L_INT8</format>
                    </image>
                    <clip>
                        <near>0.1</near>
                        <far>100</far>
                    </clip>
                    <noise>
                        <type>gaussian</type>
                        <mean>0.0</mean>
                        <stddev>0.05</stddev>
                    </noise>
                </camera>
                <always_on>1</always_on>
                <update_rate>${infra_fps}</update_rate>
                <visualize>false</visualize>
            </sensor>
            <sensor name="${camera_name}depth" type="depth">
                <camera name="${camera_name}">
                    <!-- align-depth settings -->
                    <xacro:unless value="${align_depth}">
                        <horizontal_fov>${85.2*deg_to_rad}</horizontal_fov>
                        <image>
                            <width>${depth_width}</width>
                            <height>${depth_height}</height>
                        </image>
                    </xacro:unless>
                    <xacro:if value="${align_depth}">
                        <horizontal_fov>${69.4*deg_to_rad}</horizontal_fov>
                        <image>
                            <width>${color_width}</width>
                            <height>${color_height}</height>
                        </image>
                    </xacro:if>
                    <clip>
                        <near>0.1</near>
                        <xacro:unless value="${clip_distance > 0.0}">
                            <far>100</far>
                        </xacro:unless>
                        <xacro:if value="${clip_distance > 0.0}">
                            <far>${clip_distance}</far>
                        </xacro:if>
                    </clip>
                    <noise>
                        <type>gaussian</type>
                        <mean>0.0</mean>
                        <stddev>0.100</stddev>
                    </noise>
                </camera>
                <always_on>1</always_on>
                <xacro:unless value="${align_depth}">
                    <update_rate>${depth_fps}</update_rate>
                </xacro:unless>
                <xacro:if value="${align_depth}">
                    <update_rate>${color_fps}</update_rate>
                </xacro:if>
                <visualize>false</visualize>
            </sensor>
            <!-- IMU -->
            <sensor name="${camera_name}imu" type="imu">
                <always_on>true</always_on>
                <update_rate>${gyro_fps}</update_rate>
                <!-- <topic>${camera_name}/imu/sample</topic> -->
                <topic>__default_topic__</topic>
                <plugin name="${camera_name}imu" filename="libgazebo_ros_imu_sensor.so">
                    <ros>
                        <!-- Will publish to /camera/imu -->
                        <!-- <namespace>${camera_name}/imu</namespace> -->
                        <namespace>/camera</namespace>
                        <remapping>~/out:=imu</remapping>
                    </ros>
                    <bodyName>${camera_name}_link</bodyName>
                    <frameName>${gyro_optical_frame}</frameName>
                    <updateRateHZ>${gyro_fps}</updateRateHZ>
                    <gaussianNoise>0.1</gaussianNoise>
                    <xyzOffset>0 0 0</xyzOffset>
                    <rpyOffset>0 0 0</rpyOffset>
                </plugin>
            </sensor>
        </gazebo>
        <gazebo>
            <plugin name="${topics_ns}" filename="librealsense_gazebo_plugin.so">
                <prefix>${camera_name}</prefix>
                <!-- Color camera settings -->
                <colorUpdateRate>${color_fps}</colorUpdateRate>
                <colorTopicName>camera/color/image_raw</colorTopicName>
                <colorOpticalframeName>${color_optical_frame}</colorOpticalframeName>
                <colorCameraInfoTopicName>color/camera_info</colorCameraInfoTopicName>
                <!-- Infrared camera settings -->
                <infraredUpdateRate>${infra_fps}</infraredUpdateRate>
                <infrared1TopicName>camera/infra1/image_rect_raw</infrared1TopicName>
                <infrared2TopicName>camera/infra2/image_rect_raw</infrared2TopicName>
                <infrared1CameraInfoTopicName>camera/infra1/camera_info</infrared1CameraInfoTopicName>
                <infrared2CameraInfoTopicName>camera/infra2/camera_info</infrared2CameraInfoTopicName>
                <infrared1OpticalframeName>${infrared1_optical_frame}</infrared1OpticalframeName>
                <infrared2OpticalframeName>${infrared2_optical_frame}</infrared2OpticalframeName>
                <!-- Depth camera settings -->
                <rangeMinDepth>0.2</rangeMinDepth>
                <xacro:unless value="${clip_distance > 0.0}">
                    <rangeMaxDepth>10.0</rangeMaxDepth>
                </xacro:unless>
                <xacro:if value="${clip_distance > 0.0}">
                    <rangeMaxDepth>${clip_distance}</rangeMaxDepth>
                </xacro:if>
                <xacro:unless value="${align_depth}">
                    <depthUpdateRate>${depth_fps}</depthUpdateRate>
                    <depthTopicName>camera/depth/image_rect_raw</depthTopicName>
                    <depthCameraInfoTopicName>camera/depth/camera_info</depthCameraInfoTopicName>
                    <depthOpticalframeName>${depth_optical_frame}</depthOpticalframeName>
                </xacro:unless>
                <xacro:if value="${align_depth}">
                    <depthUpdateRate>${color_fps}</depthUpdateRate>
                    <depthTopicName>camera/aligned_depth_to_color/image_raw</depthTopicName>
                    <depthCameraInfoTopicName>camera/aligned_depth_to_color/camera_info</depthCameraInfoTopicName>
                    <depthOpticalframeName>${color_optical_frame}</depthOpticalframeName>
                </xacro:if>
                <!-- Pointlcloud settings -->
                <pointCloud>${enable_pointCloud}</pointCloud>
                <pointCloudTopicName>depth/color/points</pointCloudTopicName>
                <pointCloudCutoff>0.5</pointCloudCutoff>
                <!-- <ros>
                    <namespace>/camera</namespace>
                </ros> -->
            </plugin>
        </gazebo>
    </xacro:macro>
</robot>
# Requirements:
#   A realsense D435i
#   Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
#
#   RTAB-Map in mapping mode

import os

from launch import LaunchDescription, Substitution, LaunchContext
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, LogInfo, OpaqueFunction
from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PythonExpression
from launch.conditions import IfCondition, UnlessCondition
from launch_ros.actions import Node
from launch_ros.actions import SetParameter
from typing import Text
from ament_index_python.packages import get_package_share_directory

class ConditionalText(Substitution):
    def __init__(self, text_if, text_else, condition):
        self.text_if = text_if
        self.text_else = text_else
        self.condition = condition

    def perform(self, context: 'LaunchContext') -> Text:
        if self.condition == True or self.condition == 'true' or self.condition == 'True':
            return self.text_if
        else:
            return self.text_else

def launch_setup(context, *args, **kwargs):     

    use_sim_time = LaunchConfiguration('use_sim_time')
    qos = LaunchConfiguration('qos')
    localization = LaunchConfiguration('localization')

    parameters={
    'frame_id':'camera_link',
    # 'frame_id':'camera_color_optical_frame',
    'use_sim_time':use_sim_time,
    'subscribe_depth':True,
    'subscribe_odom_info':True,
    'approx_sync':False,
    'qos_image':qos,
    'qos_imu':qos,
    'wait_imu_to_init':True
    }

    # /camera/infra1/image_rect_raw,
    # /camera/depth/image_rect_raw,
    # /camera/infra1/camera_info

    remappings=[
          ('imu', '/imu/data'),
          ('rgb/image', '/camera/infra1/image_rect_raw'),
          ('rgb/camera_info', '/camera/infra1/camera_info'),
          ('depth/image', '/camera/depth/image_rect_raw')] 

    # remappings=[
    #     ('rgb/image', '/camera/camera/image_raw'),
    #     ('rgb/camera_info', '/camera/camera/camera_info'),
    #     ('depth/image', '/camera/camera/depth/image_raw')]

    return [

        DeclareLaunchArgument('args',  default_value=LaunchConfiguration('rtabmap_args'), 
                              description='Can be used to pass RTAB-Map\'s parameters or other flags like --udebug and --delete_db_on_start/-d'),

        Node(
            package='rtabmap_odom', executable='rgbd_odometry', output='screen',
            parameters=[parameters],
            remappings=remappings),

        # SLAM mode:
        Node(
            condition=UnlessCondition(localization),
            package='rtabmap_slam', executable='rtabmap', output='screen',
            parameters=[parameters],
            remappings=remappings,
            arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db)

        # 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],
            condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
            remappings=remappings),

        Node(
            package='rviz2', executable='rviz2', output='screen',
            condition=IfCondition(LaunchConfiguration("rviz")),
            arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]
            ),

        Node(
            package='rtabmap_util', executable='point_cloud_xyzrgb', output='screen',
            condition=IfCondition(LaunchConfiguration("rviz")),
            parameters=[{
                "decimation": 4,
                "voxel_size": 0.0,
                "approx_sync": LaunchConfiguration('approx_sync'),
                "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval')
            }],
            remappings=remappings),

        # Compute quaternion of the IMU
        Node(
            package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
            parameters=[{'use_mag': False, 
                         'world_frame':'enu', 
                         'publish_tf':False}],
            remappings=[('imu/data_raw', '/camera/imu')]),

        # The IMU frame is missing in TF tree, add it:
        # Node(
        #     package='tf2_ros', executable='static_transform_publisher', output='screen',
        #     arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']),
    ]

def generate_launch_description():

    config_rviz = os.path.join(
        get_package_share_directory('AirLab_in_gazebo'), 'configs', 'sim.rviz')   

    return LaunchDescription([

        # Declare Launch Argument
        DeclareLaunchArgument('approx_sync', default_value='false', 
                              description='If timestamps of the input topics should be synchronized using approximate or exact time policy.'),

        DeclareLaunchArgument('approx_sync_max_interval', default_value='0.0', 
                              description='(sec) 0 means infinite interval duration (used with approx_sync=true)'),

        DeclareLaunchArgument('database_path', default_value='~/ros2_ws/src/drone_vslam/map_database/rtabmap.db',  
                              description='Where is the map saved/loaded.'),

        DeclareLaunchArgument('localization', default_value='false', 
                              description='Launch in localization mode.'),

        DeclareLaunchArgument('rtabmap_viz', default_value='false',  
                              description='Launch RTAB-Map UI (optional).'),

        DeclareLaunchArgument('rviz', default_value='true', 
                              description='Launch RVIZ (optional).'),

        DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, 
                              description='Configuration path of rviz2.'),

        DeclareLaunchArgument('initial_pose', default_value='', 
                              description='Set an initial pose (only in localization mode). Format: "x y z roll pitch yaw" or "x y z qx qy qz qw". Default: see "RGBD/StartAtOrigin" doc'),

        DeclareLaunchArgument('rtabmap_args',   default_value='',                   
                              description='Backward compatibility, use "args" instead.'),

        DeclareLaunchArgument('use_sim_time', default_value='true',
                              description='Use simulation (Gazebo) clock if true'),

        DeclareLaunchArgument('qos', default_value='2',
                              description='QoS used for input sensor topics'),

        OpaqueFunction(function=launch_setup)

    ])

I can see the topic for the RealSense D435i using ros2 topic list, but RTAB-Map displays an error.

ros2 launch AirLab_in_gazebo rtabmap.launch.py 
[INFO] [launch]: All log files can be found below /home/yuan/.ros/log/2023-11-23-23-09-47-685092-airlab-ubuntu-48020
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [rgbd_odometry-1]: process started with pid [48021]
[INFO] [rtabmap-2]: process started with pid [48023]
[INFO] [rviz2-3]: process started with pid [48025]
[INFO] [point_cloud_xyzrgb-4]: process started with pid [48027]
[INFO] [imu_filter_madgwick_node-5]: process started with pid [48029]
[rviz2-3] Warning: Ignoring XDG_SESSION_TYPE=wayland on Gnome. Use QT_QPA_PLATFORM=wayland to run on Wayland anyway.
[imu_filter_madgwick_node-5] [INFO] [1700752187.906796975] [imu_filter_madgwick]: Starting ImuFilter
[imu_filter_madgwick_node-5] [INFO] [1700752187.907461693] [imu_filter_madgwick]: Using dt computed from message headers
[imu_filter_madgwick_node-5] [INFO] [1700752187.907903148] [imu_filter_madgwick]: The gravity vector is kept in the IMU message.
[imu_filter_madgwick_node-5] [INFO] [1700752187.908067206] [imu_filter_madgwick]: Imu filter gain set to 0.100000
[imu_filter_madgwick_node-5] [INFO] [1700752187.908220104] [imu_filter_madgwick]: Gyro drift bias set to 0.000000
[imu_filter_madgwick_node-5] [INFO] [1700752187.908376629] [imu_filter_madgwick]: Magnetometer bias values: 0.000000 0.000000 0.000000
[imu_filter_madgwick_node-5] [INFO] [1700752187.951302037] [imu_filter_madgwick]: First IMU message received.
[point_cloud_xyzrgb-4] [INFO] [1700752188.383917664] [point_cloud_xyzrgb]: Approximate time sync = false
[rviz2-3] [INFO] [1700752188.410883286] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-3] [INFO] [1700752188.414315875] [rviz2]: OpenGl version: 4.3 (GLSL 4.3)
[rgbd_odometry-1] [INFO] [1700752188.414934083] [rgbd_odometry]: Odometry: frame_id               = camera_link
[rgbd_odometry-1] [INFO] [1700752188.416136494] [rgbd_odometry]: Odometry: odom_frame_id          = odom
[rgbd_odometry-1] [INFO] [1700752188.416373735] [rgbd_odometry]: Odometry: publish_tf             = true
[rgbd_odometry-1] [INFO] [1700752188.416484769] [rgbd_odometry]: Odometry: wait_for_transform     = 0.100000
[rgbd_odometry-1] [INFO] [1700752188.416590021] [rgbd_odometry]: Odometry: log_to_rosout_level    = 4
[rgbd_odometry-1] [INFO] [1700752188.416710049] [rgbd_odometry]: Odometry: initial_pose           = xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000
[rgbd_odometry-1] [INFO] [1700752188.416803205] [rgbd_odometry]: Odometry: ground_truth_frame_id  = 
[rgbd_odometry-1] [INFO] [1700752188.417031991] [rgbd_odometry]: Odometry: ground_truth_base_frame_id = camera_link
[rgbd_odometry-1] [INFO] [1700752188.417293246] [rgbd_odometry]: Odometry: config_path            = 
[rgbd_odometry-1] [INFO] [1700752188.417418053] [rgbd_odometry]: Odometry: publish_null_when_lost = true
[rgbd_odometry-1] [INFO] [1700752188.417525544] [rgbd_odometry]: Odometry: guess_frame_id         = 
[rgbd_odometry-1] [INFO] [1700752188.417676668] [rgbd_odometry]: Odometry: guess_min_translation  = 0.000000
[rgbd_odometry-1] [INFO] [1700752188.417786843] [rgbd_odometry]: Odometry: guess_min_rotation     = 0.000000
[rgbd_odometry-1] [INFO] [1700752188.417979396] [rgbd_odometry]: Odometry: guess_min_time         = 0.000000
[rgbd_odometry-1] [INFO] [1700752188.418092004] [rgbd_odometry]: Odometry: expected_update_rate   = 0.000000 Hz
[rgbd_odometry-1] [INFO] [1700752188.418207894] [rgbd_odometry]: Odometry: max_update_rate        = 0.000000 Hz
[rgbd_odometry-1] [INFO] [1700752188.418310645] [rgbd_odometry]: Odometry: min_update_rate        = 0.000000 Hz
[rgbd_odometry-1] [INFO] [1700752188.418412511] [rgbd_odometry]: Odometry: wait_imu_to_init       = true
[rgbd_odometry-1] [INFO] [1700752188.418527562] [rgbd_odometry]: Odometry: stereoParams_=0 visParams_=1 icpParams_=0
[rgbd_odometry-1] [INFO] [1700752188.517090366] [rgbd_odometry]: odometry: Subscribing to IMU topic /imu/data
[rgbd_odometry-1] [INFO] [1700752188.517746443] [rgbd_odometry]: odometry: qos_imu = 2
[rgbd_odometry-1] [INFO] [1700752188.520287700] [rgbd_odometry]: RGBDOdometry: approx_sync    = false
[rgbd_odometry-1] [INFO] [1700752188.520651535] [rgbd_odometry]: RGBDOdometry: queue_size     = 5
[rgbd_odometry-1] [INFO] [1700752188.520814326] [rgbd_odometry]: RGBDOdometry: qos            = 0
[rgbd_odometry-1] [INFO] [1700752188.520932233] [rgbd_odometry]: RGBDOdometry: qos_camera_info = 0
[rgbd_odometry-1] [INFO] [1700752188.521042385] [rgbd_odometry]: RGBDOdometry: subscribe_rgbd = false
[rgbd_odometry-1] [INFO] [1700752188.521151625] [rgbd_odometry]: RGBDOdometry: rgbd_cameras   = 1
[rgbd_odometry-1] [INFO] [1700752188.521260708] [rgbd_odometry]: RGBDOdometry: keep_color     = false
[rgbd_odometry-1] [INFO] [1700752188.525943058] [rgbd_odometry]: 
[rgbd_odometry-1] rgbd_odometry subscribed to (exact sync):
[rgbd_odometry-1]    /camera/infra1/image_rect_raw,
[rgbd_odometry-1]    /camera/depth/image_rect_raw,
[rgbd_odometry-1]    /camera/infra1/camera_info
[rviz2-3] [INFO] [1700752188.527412921] [rviz2]: Stereo is NOT SUPPORTED
[rtabmap-2] [INFO] [1700752188.582625663] [rtabmap]: rtabmap(maps): map_filter_radius          = 0.000000
[rtabmap-2] [INFO] [1700752188.582792715] [rtabmap]: rtabmap(maps): map_filter_angle           = 30.000000
[rtabmap-2] [INFO] [1700752188.582799918] [rtabmap]: rtabmap(maps): map_cleanup                = true
[rtabmap-2] [INFO] [1700752188.582802260] [rtabmap]: rtabmap(maps): map_always_update          = false
[rtabmap-2] [INFO] [1700752188.582805453] [rtabmap]: rtabmap(maps): map_empty_ray_tracing      = true
[rtabmap-2] [INFO] [1700752188.582807494] [rtabmap]: rtabmap(maps): cloud_output_voxelized     = true
[rtabmap-2] [INFO] [1700752188.582809449] [rtabmap]: rtabmap(maps): cloud_subtract_filtering   = false
[rtabmap-2] [INFO] [1700752188.582811434] [rtabmap]: rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[rtabmap-2] [INFO] [1700752188.582842066] [rtabmap]: rtabmap(maps): octomap_tree_depth         = 16
[rtabmap-2] [INFO] [1700752188.636528314] [rtabmap]: rtabmap: frame_id      = camera_link
[rtabmap-2] [INFO] [1700752188.636949700] [rtabmap]: rtabmap: map_frame_id  = map
[rtabmap-2] [INFO] [1700752188.637108394] [rtabmap]: rtabmap: log_to_rosout_level  = 4
[rtabmap-2] [INFO] [1700752188.637222744] [rtabmap]: rtabmap: initial_pose  = 
[rtabmap-2] [INFO] [1700752188.637332906] [rtabmap]: rtabmap: use_action_for_goal  = false
[rtabmap-2] [INFO] [1700752188.637567326] [rtabmap]: rtabmap: tf_delay      = 0.050000
[rtabmap-2] [INFO] [1700752188.637710874] [rtabmap]: rtabmap: tf_tolerance  = 0.100000
[rtabmap-2] [INFO] [1700752188.637820028] [rtabmap]: rtabmap: odom_sensor_sync   = false
[rtabmap-2] [INFO] [1700752188.638086276] [rtabmap]: rtabmap: gen_scan  = false
[rtabmap-2] [INFO] [1700752188.638210921] [rtabmap]: rtabmap: gen_depth  = false
[rtabmap-2] [INFO] [1700752188.757793219] [rtabmap]: RTAB-Map detection rate = 1.000000 Hz
[rtabmap-2] [INFO] [1700752188.761995410] [rtabmap]: rtabmap: Deleted database "/home/yuan/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[rtabmap-2] [INFO] [1700752188.762342135] [rtabmap]: rtabmap: Using database from "/home/yuan/.ros/rtabmap.db" (0 MB).
[rtabmap-2] [INFO] [1700752188.824142561] [rtabmap]: rtabmap: Database version = "0.21.1".
[rtabmap-2] [INFO] [1700752188.827641152] [rtabmap]: rtabmap: SLAM mode (Mem/IncrementalMemory=true)
[rtabmap-2] [INFO] [1700752188.962759328] [rtabmap]: Setup callbacks
[rtabmap-2] [INFO] [1700752188.963959838] [rtabmap]: rtabmap: subscribe_depth = true
[rtabmap-2] [INFO] [1700752188.963991957] [rtabmap]: rtabmap: subscribe_rgb = true
[rtabmap-2] [INFO] [1700752188.963995694] [rtabmap]: rtabmap: subscribe_stereo = false
[rtabmap-2] [INFO] [1700752188.963998212] [rtabmap]: rtabmap: subscribe_rgbd = false (rgbd_cameras=1)
[rtabmap-2] [INFO] [1700752188.964000885] [rtabmap]: rtabmap: subscribe_odom_info = true
[rtabmap-2] [INFO] [1700752188.964003078] [rtabmap]: rtabmap: subscribe_user_data = false
[rtabmap-2] [INFO] [1700752188.964005392] [rtabmap]: rtabmap: subscribe_scan = false
[rtabmap-2] [INFO] [1700752188.964007749] [rtabmap]: rtabmap: subscribe_scan_cloud = false
[rtabmap-2] [INFO] [1700752188.964009725] [rtabmap]: rtabmap: subscribe_scan_descriptor = false
[rtabmap-2] [INFO] [1700752188.964012034] [rtabmap]: rtabmap: queue_size      = 10
[rtabmap-2] [INFO] [1700752188.964014303] [rtabmap]: rtabmap: qos_image       = 2
[rtabmap-2] [INFO] [1700752188.964016414] [rtabmap]: rtabmap: qos_camera_info = 2
[rtabmap-2] [INFO] [1700752188.964018362] [rtabmap]: rtabmap: qos_scan        = 0
[rtabmap-2] [INFO] [1700752188.964020425] [rtabmap]: rtabmap: qos_odom        = 0
[rtabmap-2] [INFO] [1700752188.964022449] [rtabmap]: rtabmap: qos_user_data   = 0
[rtabmap-2] [INFO] [1700752188.964024395] [rtabmap]: rtabmap: approx_sync     = false
[rtabmap-2] [INFO] [1700752188.964032230] [rtabmap]: Setup depth callback
[rtabmap-2] [INFO] [1700752188.995505127] [rtabmap]: 
[rtabmap-2] rtabmap subscribed to (exact sync):
[rtabmap-2]    /odom \
[rtabmap-2]    /camera/infra1/image_rect_raw \
[rtabmap-2]    /camera/depth/image_rect_raw \
[rtabmap-2]    /camera/infra1/camera_info \
[rtabmap-2]    /odom_info
[rviz2-3] [INFO] [1700752189.378253714] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-3] [INFO] [1700752189.382556392] [rviz2]: Stereo is NOT SUPPORTED
[rgbd_odometry-1] [WARN] [1700752193.530089150] [rgbd_odometry]: rgbd_odometry: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
[rgbd_odometry-1] rgbd_odometry subscribed to (exact sync):
[rgbd_odometry-1]    /camera/infra1/image_rect_raw,
[rgbd_odometry-1]    /camera/depth/image_rect_raw,
[rgbd_odometry-1]    /camera/infra1/camera_info
[rtabmap-2] [WARN] [1700752194.019042033] [rtabmap]: rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
[rtabmap-2] rtabmap subscribed to (exact sync):
[rtabmap-2]    /odom \
[rtabmap-2]    /camera/infra1/image_rect_raw \
[rtabmap-2]    /camera/depth/image_rect_raw \
[rtabmap-2]    /camera/infra1/camera_info \
[rtabmap-2]    /odom_info
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[rgbd_odometry-1] [INFO] [1700752195.880895495] [rclcpp]: signal_handler(signum=2)
[point_cloud_xyzrgb-4] [INFO] [1700752195.880898587] [rclcpp]: signal_handler(signum=2)
[rtabmap-2] [INFO] [1700752195.881748431] [rclcpp]: signal_handler(signum=2)
[rtabmap-2] [WARN] [1700752195.882585332] [rtabmap]: rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
[rtabmap-2] rtabmap subscribed to (exact sync):
[rtabmap-2]    /odom \
[rtabmap-2]    /camera/infra1/image_rect_raw \
[rtabmap-2]    /camera/depth/image_rect_raw \
[rtabmap-2]    /camera/infra1/camera_info \
[rtabmap-2]    /odom_info
[rviz2-3] [INFO] [1700752195.882848604] [rclcpp]: signal_handler(signum=2)
[imu_filter_madgwick_node-5] [INFO] [1700752195.884572765] [rclcpp]: signal_handler(signum=2)
[rgbd_odometry-1] [WARN] [1700752195.888917444] [rgbd_odometry]: rgbd_odometry: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. 
[rgbd_odometry-1] rgbd_odometry subscribed to (exact sync):
[rgbd_odometry-1]    /camera/infra1/image_rect_raw,
[rgbd_odometry-1]    /camera/depth/image_rect_raw,
[rgbd_odometry-1]    /camera/infra1/camera_info
[rtabmap-2] [INFO] [1700752195.932880938] [rtabmap]: Parameters are not saved (No configuration file provided...)
[INFO] [imu_filter_madgwick_node-5]: process has finished cleanly [pid 48029]
[INFO] [point_cloud_xyzrgb-4]: process has finished cleanly [pid 48027]
[INFO] [rviz2-3]: process has finished cleanly [pid 48025]
[ERROR] [rtabmap-2]: process[rtabmap-2] failed to terminate '5' seconds after receiving 'SIGINT', escalating to 'SIGTERM'
[ERROR] [rgbd_odometry-1]: process[rgbd_odometry-1] failed to terminate '5' seconds after receiving 'SIGINT', escalating to 'SIGTERM'
[INFO] [rtabmap-2]: sending signal 'SIGTERM' to process[rtabmap-2]
[INFO] [rgbd_odometry-1]: sending signal 'SIGTERM' to process[rgbd_odometry-1]
[rtabmap-2] [INFO] [1700752200.887038270] [rclcpp]: signal_handler(signum=15)
[rgbd_odometry-1] [INFO] [1700752200.887479670] [rclcpp]: signal_handler(signum=15)
[rtabmap-2] rtabmap: Saving database/long-term memory... (located at /home/yuan/.ros/rtabmap.db)
[rtabmap-2] rtabmap: Saving database/long-term memory...done! (located at /home/yuan/.ros/rtabmap.db, 0 MB)
[INFO] [rtabmap-2]: process has finished cleanly [pid 48023]
[INFO] [rgbd_odometry-1]: process has finished cleanly [pid 48021]

Screenshot from 2023-11-23 23-11-30

"I'm using ROS2 Humble on Ubuntu 22.04."

matlabbe commented 10 months ago

Discussion moved to https://github.com/introlab/rtabmap_ros/issues/1070