mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-11 20:19:50 +08:00
matlabbe
82f0754bf7
* rtabmap_demos tests and docs * added bag testing * added rtabmap_examples launch tests * updating demo bag download paths * added netherdrone demo * Fixed rgb-only callback with lidar rejected. Updated lidar params * Added back OrbitOriented rviz view to ros2, with optional octomap wll clipping * fixing ci * lidar demo added intermediate_nodes option * added netherdrone as demo test * running rtabmap_demos tests on ci * added rtabmap_launch tests, fixed ground_truth_base_frame_id usage * ficing rolling * updating demo test harnest * densify golden trajectories to avoid tf missing * fixing tf steps * updated min icp ratio for netherdrone demo * fixing image_transport arg->params * export pose opt=0 * lets process all frames * updated netherdrone golden * fixing publishers queue size just for tests * added playdback demo doc * Adding more logs to debug ci * Fixing QOS for CI to reliable, added find-object demo test * name threads * fixing camera info expected transient on lyrical/rolling. Fixing find_object not appearing idle * 30 Hz polling backward comp * fixing test tf sim lock * fixing lyrical qos bag parsing * faster replay * lockstep * fixing clock deadlock * Added test on shutdown * updated netherdrone golden poses * Extended stereo outdoor test * updated shutdown test * updated test * multi-thread flaky test * adding backtrace when test fails * increased closure slack for netherdrone * g2o gauss newton on stereo * adjusted maximum optimizer iterations * updated default iterations * Added netherdrone in list of demos
113 lines
5.1 KiB
Python
113 lines
5.1 KiB
Python
# Requirements:
|
|
# Download rosbag:
|
|
# * demo_mapping_bag.zip: https://github.com/introlab/rtabmap_ros/releases/download/0.23.13/demo_mapping_bag.zip
|
|
#
|
|
# Example:
|
|
#
|
|
# SLAM:
|
|
# $ ros2 launch rtabmap_demos robot_mapping_demo.launch.py rviz:=true rtabmap_viz:=true
|
|
#
|
|
# Rosbag:
|
|
# $ ros2 bag play demo_mapping_bag --clock
|
|
#
|
|
|
|
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch.conditions import IfCondition, UnlessCondition
|
|
from launch_ros.actions import Node
|
|
from launch_ros.actions import SetParameter
|
|
import os
|
|
from ament_index_python.packages import get_package_share_directory
|
|
|
|
def generate_launch_description():
|
|
|
|
localization = LaunchConfiguration('localization')
|
|
|
|
parameters={
|
|
'frame_id':'base_footprint',
|
|
'odom_frame_id':'odom',
|
|
'odom_tf_linear_variance':0.001,
|
|
'odom_tf_angular_variance':0.001,
|
|
'subscribe_rgbd':True,
|
|
'subscribe_scan':True,
|
|
'approx_sync':True,
|
|
'sync_queue_size': 10,
|
|
# RTAB-Map's internal parameters should be strings
|
|
'RGBD/NeighborLinkRefining': 'true', # Do odometry correction with consecutive laser scans
|
|
'RGBD/ProximityBySpace': 'true', # Local loop closure detection (using estimated position) with locations in WM
|
|
'RGBD/ProximityByTime': 'false', # Local loop closure detection with locations in STM
|
|
'RGBD/ProximityPathMaxNeighbors': '10', # Do also proximity detection by space by merging close scans together.
|
|
'Reg/Strategy': '1', # 0=Visual, 1=ICP, 2=Visual+ICP
|
|
'Vis/MinInliers': '12', # 3D visual words minimum inliers to accept loop closure
|
|
'RGBD/OptimizeFromGraphEnd': 'false', # Optimize graph from initial node so /map -> /odom transform will be generated
|
|
'RGBD/OptimizeMaxError': '4', # Reject any loop closure causing large errors (>3x link's covariance) in the map
|
|
'Reg/Force3DoF': 'true', # 2D SLAM
|
|
'Grid/Sensor': '0', # Create 2D occupancy grid from laser scan
|
|
'Mem/STMSize': '30', # increased to 30 to avoid adding too many loop closures on just seen locations
|
|
'RGBD/LocalRadius': '5', # limit length of proximity detections
|
|
'Icp/CorrespondenceRatio': '0.2', # minimum scan overlap to accept loop closure
|
|
'Icp/Strategy': '0', # 0=PCL, 1=libpointmatcher
|
|
'Icp/PointToPlane': 'false',
|
|
'Icp/MaxCorrespondenceDistance': '0.15',
|
|
'Icp/VoxelSize': '0.05'
|
|
}
|
|
|
|
remappings=[
|
|
('rgb/image', '/data_throttled_image'),
|
|
('depth/image', '/data_throttled_image_depth'),
|
|
('rgb/camera_info', '/data_throttled_camera_info'),
|
|
('scan', '/jn0/base_scan')]
|
|
|
|
config_rviz = os.path.join(
|
|
get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz'
|
|
)
|
|
|
|
return LaunchDescription([
|
|
|
|
# Launch arguments
|
|
DeclareLaunchArgument('rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'),
|
|
DeclareLaunchArgument('rviz', default_value='false', description='Launch RVIZ (optional).'),
|
|
DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'),
|
|
DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'),
|
|
|
|
SetParameter(name='use_sim_time', value=True),
|
|
|
|
# Nodes to launch
|
|
Node(
|
|
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
|
parameters=[parameters,
|
|
{'rgb_image_transport':'compressed',
|
|
'depth_image_transport':'compressedDepth',
|
|
'approx_sync_max_interval': 0.02}],
|
|
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),
|
|
|
|
# Visualization:
|
|
Node(
|
|
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
|
condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
|
|
parameters=[parameters],
|
|
remappings=remappings),
|
|
Node(
|
|
package='rviz2', executable='rviz2', name="rviz2", output='screen',
|
|
condition=IfCondition(LaunchConfiguration("rviz")),
|
|
arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]),
|
|
])
|