[BUG] {Imu has not been utilized in rtabmap.launch.py}

[Amiraqaie]

Hi i recognized that imu data has not been utilized in rtabmap.launch.py so i did some corrections in that file and the result got such an improvement in speed of odometry calculation and stability the result is like following launch file that involves imu_filter_madgwick package:

import os

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

def launch_setup(context, *args, **kwargs): name = LaunchConfiguration('name').perform(context) depthai_prefix = get_package_share_directory("depthai_ros_driver")

params_file= LaunchConfiguration("params_file")
parameters = [
    {
        "frame_id": name,
        "wait_imu_to_init": True,
        "subscribe_rgb": True,
        "subscribe_depth": True,
        "subscribe_odom_info": True,
        "approx_sync": True,
        # "Rtabmap/DetectionRate": "1.5",
        "Grid/MapFrameProjection": "true",
        "Grid/NormalsSegmentation": "false",
        "Grid/MaxGroundHeight": "1.0",
        "Grid/MaxObstacleHeight": "1.5",
    }
]

remappings = [
    ("/imu", "/rtabmap/imu"),
    ("rgb/image", name+"/rgb/image_raw"),
    ("rgb/camera_info", name+"/rgb/camera_info"),
    ("depth/image", name+"/stereo/image_raw"),
]

return [

    IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(depthai_prefix, 'launch', 'camera.launch.py')),
        launch_arguments={"name": name,
                          "params_file": params_file}.items()),

    LoadComposableNodes(
        condition=IfCondition(LaunchConfiguration("rectify_rgb")),
        target_container=name+"_container",
        composable_node_descriptions=[
            ComposableNode(
                package="image_proc",
                plugin="image_proc::RectifyNode",
                name="rectify_color_node",
                remappings=[('image', name+'/rgb/image_raw'),
                            ('camera_info', name+'/rgb/camera_info'),
                            ('image_rect', name+'/rgb/image_rect'),
                            ('image_rect/compressed', name+'/rgb/image_rect/compressed'),
                            ('image_rect/compressedDepth', name+'/rgb/image_rect/compressedDepth'),
                            ('image_rect/theora', name+'/rgb/image_rect/theora')]
            )
        ]),

    LoadComposableNodes(
        target_container=name+"_container",
        composable_node_descriptions=[
            ComposableNode(
                package='rtabmap_odom',
                plugin='rtabmap_odom::RGBDOdometry',
                name='rgbd_odometry',
                parameters=parameters,
                remappings=remappings,
            ),
        ],
    ),

    LoadComposableNodes(
        target_container=name+"_container",
        composable_node_descriptions=[
            ComposableNode(
                package='rtabmap_slam',
                plugin='rtabmap_slam::CoreWrapper',
                name='rtabmap',
                parameters=parameters,
                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', '/oak/imu/data'),
                    ('/imu/data', '/rtabmap/imu')]),

    Node(
        package="rtabmap_viz",
        executable="rtabmap_viz",
        output="screen",
        parameters=parameters,
        remappings=remappings,
    ),
]

def generate_launch_description(): depthai_prefix = get_package_share_directory("depthai_ros_driver") declared_arguments = [ DeclareLaunchArgument("name", default_value="oak"), DeclareLaunchArgument("params_file", default_value=os.path.join(depthai_prefix, 'config', 'rgbd.yaml')), DeclareLaunchArgument("rectify_rgb", default_value="True"), ]

return LaunchDescription(
    declared_arguments + [OpaqueFunction(function=launch_setup)]
)