i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixing topic sync lagging and delay issues, more examples (depthai, zed) (#1206)

Browse Source 
* Fixed odometry latency

* Fixed node ID=0 issues as msgs may not have seq. (#1202)

(cherry picked from commit 097cab0667)

* Odom: removed long processing from ros callbacks (decreasing delay, also making delay independent of the message filters topic_queue_size and sync_queue_size parameters)

* Changed a log from info->debug

* Changed a log from info->debug

* Updated default topic and sync queue_size inside nodes. Exposing topic and sync queue size params in warning when cannot synchronize. Added zed and depthai examples. Odom: Fixed imu callback group, added multi-threaded executors for all odometry nodes.

* fixed merge

* Include everything needed in example launch files for simple launch. Small fixes.

---------

Co-authored-by: Borong Yuan <[email protected]>
This commit is contained in:
matlabbe
2024-09-04 00:46:44 -07:00
committed by GitHub
co-authored by Borong Yuan
parent 268609b0b7
commit c956e3780f
27 changed files with 465 additions and 180 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_examples/launch/depthai.launch.py
+72
View File
@@ -0,0 +1,72 @@
# Requirements:
# A OAK-D camera
# Install depthai-ros package (https://github.com/luxonis/depthai-ros)
# Example:
# $ ros2 launch rtabmap_examples depthai.launch.py camera_model:=OAK-D
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
parameters=[{'frame_id':'oak-d-base-frame',
'subscribe_rgbd':True,
'subscribe_odom_info':True,
'approx_sync':False,
'wait_imu_to_init':True}]
remappings=[('imu', '/imu/data')]
return LaunchDescription([
# Launch camera driver
IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('depthai_examples'), 'launch'),
'/stereo_inertial_node.launch.py']),
launch_arguments={'depth_aligned': 'false',
'enableRviz': 'false',
'monoResolution': '400p'}.items(),
),
# Sync right/depth/camera_info together
Node(
package='rtabmap_sync', executable='rgbd_sync', output='screen',
parameters=parameters,
remappings=[('rgb/image', '/right/image_rect'),
('rgb/camera_info', '/right/camera_info'),
('depth/image', '/stereo/depth')]),
# 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', '/imu')]),
# Visual odometry
Node(
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
parameters=parameters,
remappings=remappings),
# VSLAM
Node(
package='rtabmap_slam', executable='rtabmap', output='screen',
parameters=parameters,
remappings=remappings,
arguments=['-d']),
# Visualization
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
parameters=parameters,
remappings=remappings)
])
rtabmap_examples/launch/realsense_d400.launch.py
+19 -8
View File
@@ -2,16 +2,16 @@
# A realsense D400 series
# Install realsense2 ros2 package (make sure you have this patch: https://github.com/IntelRealSense/realsense-ros/issues/2564#issuecomment-1336288238)
# Example:
# $ ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true
#
# $ ros2 launch rtabmap_examples realsense_d400.launch.py
# OR
# $ ros2 launch rtabmap_launch rtabmap.launch.py frame_id:=camera_link args:="-d" rgb_topic:=/camera/color/image_raw depth_topic:=/camera/aligned_depth_to_color/image_raw camera_info_topic:=/camera/color/camera_info approx_sync:=false
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.actions import Node, SetParameter
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
parameters=[{
@@ -27,7 +27,18 @@ def generate_launch_description():
return LaunchDescription([
# Nodes to launch
# Make sure IR emitter is enabled
SetParameter(name='depth_module.emitter_enabled', value=1),
# Launch camera driver
IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('realsense2_camera'), 'launch'),
'/rs_launch.py']),
launch_arguments={'align_depth.enable': 'true',
'rgb_camera.profile': '640x360x30'}.items(),
),
Node(
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
parameters=parameters,
rtabmap_examples/launch/realsense_d435i_color.launch.py
+25 -25
View File
@@ -6,10 +6,14 @@
#
# $ ros2 launch rtabmap_examples realsense_d435i_color.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.actions import Node, SetParameter
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
parameters=[{
@@ -23,11 +27,26 @@ def generate_launch_description():
('imu', '/imu/data'),
('rgb/image', '/camera/color/image_raw'),
('rgb/camera_info', '/camera/color/camera_info'),
('depth/image', '/camera/realigned_depth_to_color/image_raw')]
('depth/image', '/camera/aligned_depth_to_color/image_raw')]
return LaunchDescription([
# Nodes to launch
# Make sure IR emitter is enabled
SetParameter(name='depth_module.emitter_enabled', value=1),
# Launch camera driver
IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('realsense2_camera'), 'launch'),
'/rs_launch.py']),
launch_arguments={'enable_gyro': 'true',
'enable_accel': 'true',
'unite_imu_method': '1',
'align_depth.enable': 'true',
'enable_sync': 'true',
'rgb_camera.profile': '640x360x30'}.items(),
),
Node(
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
parameters=parameters,
@@ -43,26 +62,7 @@ def generate_launch_description():
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
parameters=parameters,
remappings=remappings),
# Because of this issue: https://github.com/IntelRealSense/realsense-ros/issues/2564
# Generate point cloud from not aligned depth
Node(
package='rtabmap_util', executable='point_cloud_xyz', output='screen',
parameters=[{'approx_sync':False}],
remappings=[('depth/image', '/camera/depth/image_rect_raw'),
('depth/camera_info', '/camera/depth/camera_info'),
('cloud', '/camera/cloud_from_depth')]),
# Generate aligned depth to color camera from the point cloud above
Node(
package='rtabmap_util', executable='pointcloud_to_depthimage', output='screen',
parameters=[{ 'decimation':2,
'fixed_frame_id':'camera_link',
'fill_holes_size':1}],
remappings=[('camera_info', '/camera/color/camera_info'),
('cloud', '/camera/cloud_from_depth'),
('image_raw', '/camera/realigned_depth_to_color/image_raw')]),
# Compute quaternion of the IMU
Node(
package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
rtabmap_examples/launch/realsense_d435i_infra.launch.py
+23 -7
View File
@@ -2,15 +2,16 @@
# A realsense D435i
# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
# Example:
# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true
# $ ros2 param set /camera/camera depth_module.emitter_enabled 0
#
# $ ros2 launch rtabmap_examples realsense_d435i_infra.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.actions import Node, SetParameter
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
parameters=[{
@@ -28,7 +29,22 @@ def generate_launch_description():
return LaunchDescription([
# Nodes to launch
#Hack to disable IR emitter
SetParameter(name='depth_module.emitter_enabled', value=0),
# Launch camera driver
IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('realsense2_camera'), 'launch'),
'/rs_launch.py']),
launch_arguments={'enable_gyro': 'true',
'enable_accel': 'true',
'unite_imu_method': '1',
'enable_infra1': 'true',
'enable_infra2': 'true',
'enable_sync': 'true'}.items(),
),
Node(
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
parameters=parameters,
rtabmap_examples/launch/realsense_d435i_stereo.launch.py
+23 -7
View File
@@ -2,15 +2,16 @@
# A realsense D435i
# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
# Example:
# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true
# $ ros2 param set /camera/camera depth_module.emitter_enabled 0
#
# $ ros2 launch rtabmap_examples realsense_d435i_stereo.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.actions import Node, SetParameter
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
parameters=[{
@@ -28,7 +29,22 @@ def generate_launch_description():
return LaunchDescription([
# Nodes to launch
#Hack to disable IR emitter
SetParameter(name='depth_module.emitter_enabled', value=0),
# Launch camera driver
IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('realsense2_camera'), 'launch'),
'/rs_launch.py']),
launch_arguments={'enable_gyro': 'true',
'enable_accel': 'true',
'unite_imu_method': '1',
'enable_infra1': 'true',
'enable_infra2': 'true',
'enable_sync': 'true'}.items(),
),
Node(
package='rtabmap_odom', executable='stereo_odometry', output='screen',
parameters=parameters,
rtabmap_examples/launch/vlp16.launch.py
+18 -17
View File
@@ -1,15 +1,16 @@
# Example:
# $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py
# $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py
#
# SLAM:
# $ ros2 launch rtabmap_examples vlp16.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
@@ -28,6 +29,17 @@ def generate_launch_description():
description='Enable lidar deskewing'),
# Nodes to launch
IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('velodyne_driver'), 'launch'),
'/velodyne_driver_node-VLP16-launch.py']),
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('velodyne_pointcloud'), 'launch'),
'/velodyne_transform_node-VLP16-launch.py']),
),
Node(
package='rtabmap_odom', executable='icp_odometry', output='screen',
parameters=[{
@@ -57,18 +69,7 @@ def generate_launch_description():
remappings=[
('scan_cloud', '/velodyne_points')
]),
Node(
package='rtabmap_util', executable='point_cloud_assembler', output='screen',
parameters=[{
'max_clouds':10,
'fixed_frame_id':'',
'use_sim_time':use_sim_time,
}],
remappings=[
('cloud', 'odom_filtered_input_scan')
]),
Node(
package='rtabmap_slam', executable='rtabmap', output='screen',
parameters=[{
@@ -102,7 +103,7 @@ def generate_launch_description():
'Icp/CorrespondenceRatio': '0.2'
}],
remappings=[
('scan_cloud', 'assembled_cloud')
('scan_cloud', 'odom_filtered_input_scan')
],
arguments=[
'-d' # This will delete the previous database (~/.ros/rtabmap.db)
rtabmap_examples/launch/zed.launch.py
+76
View File
@@ -0,0 +1,76 @@
# Requirements:
# A ZED camera
# Install zed ros2 wrapper package (https://github.com/stereolabs/zed-ros2-wrapper)
# Example:
# $ ros2 launch rtabmap_examples zed.launch.py camera_model:=zed2i
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
import tempfile
def generate_launch_description():
# Hack to override grab_resolution parameter without changing any files
with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file:
zed_override_file.write("---\n"+
"/**:\n"+
" ros__parameters:\n"+
" general:\n"+
" grab_resolution: 'VGA'")
camera_model = LaunchConfiguration('camera_model'),
parameters=[{'frame_id':'zed_camera_link',
'subscribe_rgbd':True,
'subscribe_odom_info':True,
'approx_sync':False,
'wait_imu_to_init':True}]
remappings=[('imu', '/zed/zed_node/imu/data')]
return LaunchDescription([
# Launch camera driver
IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('zed_wrapper'), 'launch'),
'/zed_camera.launch.py']),
launch_arguments={'camera_model': camera_model,
'ros_params_override_path': zed_override_file.name}.items(),
),
# Sync right/depth/camera_info together
Node(
package='rtabmap_sync', executable='rgbd_sync', output='screen',
parameters=parameters,
remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'),
('rgb/camera_info', '/zed/zed_node/rgb/camera_info'),
('depth/image', '/zed/zed_node/depth/depth_registered')]),
# Visual odometry
Node(
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
parameters=parameters,
remappings=remappings),
# VSLAM
Node(
package='rtabmap_slam', executable='rtabmap', output='screen',
parameters=parameters,
remappings=remappings,
arguments=['-d']),
# Visualization
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
parameters=parameters,
remappings=remappings)
])