rtabmap_ros/rtabmap_demos/launch/netherdrone_lidar3d_demo.launch.py

# Description:
#   3D lidar SLAM with an Ouster OS1-32 turning on a motorized mast (a dynamixel
#   joint), carried by a drone frame. The mast rotation sweeps the lidar's 32 beams
#   over the whole sphere, but it also moves the lidar by a few degrees during each
#   sweep: scans are deskewed point by point with TF, using the IMU orientation for
#   the base and the joint states for the mast. This is rtabmap_examples'
#   lidar3d_assemble.launch.py, set up for the bag: scans are assembled over half a
#   turn of the mast, so each node of the map holds a full sphere.
#
#   Note: the camera's rotation relative to the lidar (box_link -> camera_link in the
#   bags' TF) was calibrated with rtabmap's rtabmap-lidarCameraCalibration tool; its
#   translation is as measured.
#
# Requirements:
#   Download one or more rosbags, the consecutive minutes of the same run:
#    * netherdrone_ouster_vertige_bag_0.zip: https://github.com/introlab/rtabmap_ros/releases/download/0.23.13/netherdrone_ouster_vertige_bag_0.zip
#    * netherdrone_ouster_vertige_bag_1.zip: https://github.com/introlab/rtabmap_ros/releases/download/0.23.13/netherdrone_ouster_vertige_bag_1.zip
#    * netherdrone_ouster_vertige_bag_2.zip: https://github.com/introlab/rtabmap_ros/releases/download/0.23.13/netherdrone_ouster_vertige_bag_2.zip
#    * netherdrone_ouster_vertige_bag_3.zip: https://github.com/introlab/rtabmap_ros/releases/download/0.23.13/netherdrone_ouster_vertige_bag_3.zip
#    * netherdrone_ouster_vertige_bag_4.zip: https://github.com/introlab/rtabmap_ros/releases/download/0.23.13/netherdrone_ouster_vertige_bag_4.zip
#    * netherdrone_ouster_vertige_bag_5.zip: https://github.com/introlab/rtabmap_ros/releases/download/0.23.13/netherdrone_ouster_vertige_bag_5.zip
#
# Example:
#
#   SLAM:
#     $ ros2 launch rtabmap_demos netherdrone_lidar3d_demo.launch.py
#
#   Rosbag:
#     $ ros2 bag play netherdrone_ouster_vertige_bag_0 --clock
#     or, to play the bags one after the other:
#     $ ros2 bag play netherdrone_ouster_vertige_bag_0 --clock && \
#       ros2 bag play netherdrone_ouster_vertige_bag_1 --clock && \
#       ros2 bag play netherdrone_ouster_vertige_bag_2 --clock && \
#       ros2 bag play netherdrone_ouster_vertige_bag_3 --clock && \
#       ros2 bag play netherdrone_ouster_vertige_bag_4 --clock && \
#       ros2 bag play netherdrone_ouster_vertige_bag_5 --clock
#

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node


def generate_launch_description():

    lidar3d_assemble_launch = os.path.join(
        get_package_share_directory('rtabmap_examples'), 'launch', 'lidar3d_assemble.launch.py')
    config_rviz = os.path.join(
        get_package_share_directory('rtabmap_demos'), 'config', 'demo_lidar3d.rviz')
    config_rtabmap_viz = os.path.join(
        get_package_share_directory('rtabmap_demos'), 'config', 'demo_lidar3d_gui.ini')

    camera = LaunchConfiguration('camera')
    rgbd_image_topic = PythonExpression(
        ["'/camera/rgbd_image' if '", camera, "'.lower() == 'true' else ''"])


    return LaunchDescription([

        # Launch arguments
        DeclareLaunchArgument('rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'),
        DeclareLaunchArgument('rtabmap_viz_cfg', default_value=config_rtabmap_viz, description='Configuration path of rtabmap_viz.'),
        DeclareLaunchArgument('rviz', default_value='true', description='Launch RVIZ (optional).'),
        DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'),
        DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'),
        DeclareLaunchArgument('voxel_size', default_value='0.3',
                              description='Voxel size (m) of the downsampled lidar point cloud. In this bag, the median range is 5-7 m and the farthest points are ~23 m.'),
        DeclareLaunchArgument('camera', default_value='true',
                              description='Add the camera images to the map, with a depth made from the lidar, for visual loop closure detection (bag-of-words). Registration stays lidar only.'),
        DeclareLaunchArgument('lidar_range_min', default_value='0.84',
                              description='Lidar points closer than this (m) are ignored. About 6% of each scan hits the drone itself, all within 0.6 m; the surroundings start at ~0.9 m.'),
        DeclareLaunchArgument('assembler_voxel_size', default_value='0.05',
                              description='Voxel size (m) of the assembled clouds added to the map; 0 keeps every point (larger database, e.g., for camera-lidar calibration).'),
        DeclareLaunchArgument('assembling_time', default_value='4.3',
                              description='How long (s) scans are assembled before being added to the map. The mast turns at 42 deg/s: 4.3 s is half a turn, in which the lidar\'s scanning plane sweeps the whole sphere.'),
        DeclareLaunchArgument('intermediate_nodes', default_value='false',
                              description='Also save every odometry pose (10 Hz) between the map\'s nodes in the database.'),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(lidar3d_assemble_launch),
            launch_arguments={
                'use_sim_time': 'true',
                'frame_id': 'base_link',
                'lidar_topic': '/os_cloud_node/points',
                # The fixed frame for deskewing (base_link_stabilized) is made from its
                # orientation.
                'imu_topic': '/imu/data_filtered',
                'voxel_size': LaunchConfiguration('voxel_size'),
                'max_correspondence_distance': '1.0',
                'icp_outlier_ratio': '0.3',
                'odom_key_frame_threshold': '0.9',
                'min_loop_closure_overlap': '0.1',
                'odom_local_map_size': '20000',
                'assembler_voxel_size': LaunchConfiguration('assembler_voxel_size'),
                'rgbd_image_topic': rgbd_image_topic,
                # The images are raw (k1=-0.34): rectify them, so that the lidar projected
                # into them as their depth (gen_depth) lines up.
                'rectify_images': 'true',
                'gen_depth': camera,
                'lidar_range_min': LaunchConfiguration('lidar_range_min'),
                'expected_update_rate': '15.0',  # the Ouster runs at 10 Hz
                'assembling_time': LaunchConfiguration('assembling_time'),
                # Look TF up at every point's own time ("t" field) rather than
                # interpolating between the first and last points of the sweep: exact,
                # for more TF lookups.
                'deskewing_slerp': 'false',
                'rtabmap_viz': LaunchConfiguration('rtabmap_viz'),
                'rtabmap_viz_cfg': LaunchConfiguration('rtabmap_viz_cfg'),
                'localization': LaunchConfiguration('localization'),
                'intermediate_nodes': LaunchConfiguration('intermediate_nodes'),
            }.items()),

        # Camera image and calibration, together in one topic for rtabmap. Both have
        # the same stamps. The camera has no depth: rtabmap makes it from the lidar.
        Node(
            condition=IfCondition(camera),
            package='rtabmap_sync', executable='rgb_sync', output='screen',
            parameters=[{
                'use_sim_time': True,
                'approx_sync': False,
                'image_transport': 'compressed'}],
            remappings=[('rgb/image', '/camera/image_raw'),
                        ('rgb/camera_info', '/camera/camera_info'),
                        ('rgbd_image', '/camera/rgbd_image')]),

        # Orientation from the gyro and accelerometer of /imu/data_raw. Its own
        # orientation (despite the name, it has one) is off from the accelerometer by a
        # constant ~2 deg in roll and ~4 deg in pitch, so it is not used.
        Node(
            package='imu_complementary_filter', executable='complementary_filter_node', output='screen',
            parameters=[{
                'use_sim_time': True,
                'use_mag': False,
                'publish_tf': False,
                'do_bias_estimation': True,
                'do_adaptive_gain': True}],
            remappings=[('imu/data_raw', '/imu/data_raw'),
                        ('imu/data', '/imu/data_filtered')]),

        Node(
            package='rviz2', executable='rviz2', name='rviz2', output='screen',
            condition=IfCondition(LaunchConfiguration('rviz')),
            parameters=[{'use_sim_time': True}],
            arguments=['-d', LaunchConfiguration('rviz_cfg')]),
    ])