ros2 humble realsense d435i in gazebo · Issue 1070 · introlabrtabmap_ros
我想在 Gazebo 中模拟 RealSense D435i，并使用 RTAB-Map 进行定位 。
但是 RTAB-Map 无法接收摄像头主题。
这是我的 xacro 和启动文件。
<?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)
])
我可以使用 ros2 主题列表看到 RealSense D435i 的主题，但 RTAB-Map 显示错误。
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]
“我在 Ubuntu 22.04 上使用 ROS2 Humble。”