Fixing turtlebot3 demos on Jazzy (#1422)

* Fixing turtlebot3 demos on Jazzy

* Updated humble

* Working turtlebot3 demos on humble and jazzy

rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py

@@ -20,11 +20,13 @@ from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
 from launch.substitutions import LaunchConfiguration
 from launch.conditions import IfCondition, UnlessCondition
 from launch_ros.actions import Node
+from launch.actions import OpaqueFunction
 
-def generate_launch_description():
+def launch_setup(context, *args, **kwargs):
 
     use_sim_time = LaunchConfiguration('use_sim_time')
     localization = LaunchConfiguration('localization')
+    max_ground_height = LaunchConfiguration('max_ground_height').perform(context)
 
     parameters={
           'frame_id':'base_footprint',
@@ -36,7 +38,7 @@ def generate_launch_description():
           'Grid/3D':'false', # Use 2D occupancy
           'Grid/RangeMax':'3',
           'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles
-          'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles
+          'Grid/MaxGroundHeight': str(max_ground_height), # All points above 5 cm are obstacles
           'Grid/MaxObstacleHeight':'0.4',  # All points over 1 meter are ignored
           'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
     }
@@ -46,17 +48,7 @@ def generate_launch_description():
           ('rgb/camera_info', '/camera/camera_info'),
           ('depth/image', '/camera/depth/image_raw')]
 
-    return LaunchDescription([
-
-        # Launch arguments
-        DeclareLaunchArgument(
-            'use_sim_time', default_value='true',
-            description='Use simulation (Gazebo) clock if true'),
-        
-        DeclareLaunchArgument(
-            'localization', default_value='false',
-            description='Launch in localization mode.'),
-
+    return [
         # Nodes to launch
         
         # SLAM mode:
@@ -98,4 +90,23 @@ def generate_launch_description():
             remappings=[('cloud', '/camera/cloud'),
                         ('obstacles', '/camera/obstacles'),
                         ('ground', '/camera/ground')]),
-    ])
+    ]
+
+def generate_launch_description():
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'use_sim_time', default_value='true',
+            description='Use simulation (Gazebo) clock if true'),
+        
+        DeclareLaunchArgument(
+            'localization', default_value='false',
+            description='Launch in localization mode.'),
+        
+        DeclareLaunchArgument(
+            'max_ground_height', default_value='0.05',
+            description='Maximum ground height, everything above is obstacle'),
+
+        OpaqueFunction(function=launch_setup)
+    ])

rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_scan.launch.py

@@ -20,12 +20,13 @@ from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
 from launch.substitutions import LaunchConfiguration
 from launch.conditions import IfCondition, UnlessCondition
 from launch_ros.actions import Node
+from launch.actions import OpaqueFunction
 
-
-def generate_launch_description():
+def launch_setup(context, *args, **kwargs):
 
     use_sim_time = LaunchConfiguration('use_sim_time')
     localization = LaunchConfiguration('localization')
+    max_ground_height = LaunchConfiguration('max_ground_height').perform(context)
 
     parameters={
           'frame_id':'base_footprint',
@@ -42,7 +43,7 @@ def generate_launch_description():
           'Grid/RangeMax':'3',
           'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles
           'Grid/Sensor':'2', # Use both laser scan and camera for obstacle detection in global map
-          'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles
+          'Grid/MaxGroundHeight': str(max_ground_height), # All points above are obstacles
           'Grid/MaxObstacleHeight':'0.4',  # All points over 1 meter are ignored
           'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself
           'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
@@ -53,17 +54,7 @@ def generate_launch_description():
           ('rgb/camera_info', '/camera/camera_info'),
           ('depth/image', '/camera/depth/image_raw')]
 
-    return LaunchDescription([
-
-        # Launch arguments
-        DeclareLaunchArgument(
-            'use_sim_time', default_value='false',
-            description='Use simulation (Gazebo) clock if true'),
-
-        DeclareLaunchArgument(
-            'localization', default_value='false',
-            description='Launch in localization mode.'),
-
+    return [
         # Nodes to launch
         Node(
             package='rtabmap_sync', executable='rgbd_sync', output='screen',
@@ -109,4 +100,23 @@ def generate_launch_description():
             remappings=[('cloud', '/camera/cloud'),
                         ('obstacles', '/camera/obstacles'),
                         ('ground', '/camera/ground')]),
-    ])
+    ]
+
+def generate_launch_description():
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'use_sim_time', default_value='true',
+            description='Use simulation (Gazebo) clock if true'),
+        
+        DeclareLaunchArgument(
+            'localization', default_value='false',
+            description='Launch in localization mode.'),
+        
+        DeclareLaunchArgument(
+            'max_ground_height', default_value='0.05',
+            description='Maximum ground height, everything above is obstacle'),
+
+        OpaqueFunction(function=launch_setup)
+    ])

rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py

@@ -13,8 +13,30 @@
 #          </joint> 
 #     3) Rename <link name="camera_rgb_frame"> to <link name="camera_rgb_optical_frame">
 #     4) Add <link name="camera_rgb_frame"/>
-#     5) Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
-#     6) Change image width/height from 1920x1080 to 640x480
+#     5) Change image width/height from 1920x1080 to 640x480
+#     6) [ROS2 HUMBLE] Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
+#     6) [ROS2 JAZZY] Change <gz_frame_id>camera_rgb_frame</gz_frame_id> to <gz_frame_id>camera_rgb_optical_frame</gz_frame_id>
+#     7) [ROS2 JAZZY] Add the following just after <sensor name="camera" ...> section
+#        <sensor name="depth" type="depth">
+#            <always_on>true</always_on>
+#            <visualize>true</visualize>
+#            <update_rate>30</update_rate>
+#            <topic>camera/depth/image_raw</topic>
+#            <gz_frame_id>camera_rgb_optical_frame</gz_frame_id>
+#            <camera name="intel_realsense_r200_depth">
+#            <camera_info_topic>camera/depth/camera_info</camera_info_topic>
+#            <horizontal_fov>1.02974</horizontal_fov>
+#            <image>
+#                <width>640</width>
+#                <height>480</height>
+#                <format>R8G8B8</format>
+#            </image>
+#            <clip>
+#                <near>0.02</near>
+#                <far>300</far>
+#            </clip>
+#            </camera>
+#        </sensor>
 # Example:
 #   $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_demo.launch.py
 #
@@ -28,9 +50,12 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, Opaq
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
 
 import os
 
+ROS_DISTRO = os.environ.get('ROS_DISTRO')
+
 def launch_setup(context, *args, **kwargs):
     if not 'TURTLEBOT3_MODEL' in os.environ:
         os.environ['TURTLEBOT3_MODEL'] = 'waffle'
@@ -45,9 +70,14 @@ def launch_setup(context, *args, **kwargs):
 
     world = LaunchConfiguration('world').perform(context)
     
-    nav2_params_file = PathJoinSubstitution(
-        [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml']
-    )
+    if ROS_DISTRO == 'humble':
+        nav2_params_file = PathJoinSubstitution(
+            [FindPackageShare('rtabmap_demos'), 'params', 'humble', 'turtlebot3_rgbd_nav2_params.yaml']
+        )
+    else:
+        nav2_params_file = PathJoinSubstitution(
+            [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml']
+        )
 
     # Paths
     gazebo_launch = PathJoinSubstitution(
@@ -60,13 +90,22 @@ def launch_setup(context, *args, **kwargs):
         [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd.launch.py'])
 
     # Includes
-    gazebo = IncludeLaunchDescription(
+    gazebo = [IncludeLaunchDescription(
         PythonLaunchDescriptionSource([gazebo_launch]),
         launch_arguments=[
             ('x_pose', LaunchConfiguration('x_pose')),
             ('y_pose', LaunchConfiguration('y_pose'))
         ]
-    )
+    )]
+    if ROS_DISTRO != 'humble':
+        start_gazebo_ros_depth_image_bridge_cmd = Node(
+            package='ros_gz_image',
+            executable='image_bridge',
+            arguments=['/camera/depth/image_raw'],
+            output='screen',
+        )
+        gazebo.append(start_gazebo_ros_depth_image_bridge_cmd)
+    
     nav2 = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([nav2_launch]),
         launch_arguments=[
@@ -77,20 +116,25 @@ def launch_setup(context, *args, **kwargs):
     rviz = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([rviz_launch])
     )
+    
+    max_ground_height = '0.05'
+    if ROS_DISTRO == 'jazzy':
+        max_ground_height = '0.02' # for the demo, on new gazebo the depth is more accurate
+
     rtabmap = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([rtabmap_launch]),
         launch_arguments=[
             ('localization', LaunchConfiguration('localization')),
-            ('use_sim_time', 'true')
+            ('use_sim_time', 'true'),
+            ('max_ground_height', max_ground_height)
         ]
     )
     return [
         # Nodes to launch
         nav2,
         rviz,
-        rtabmap,
-        gazebo
-    ]
+        rtabmap
+    ] + gazebo
 
 def generate_launch_description():
     return LaunchDescription([

rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py

@@ -13,8 +13,30 @@
 #          </joint> 
 #     3) Rename <link name="camera_rgb_frame"> to <link name="camera_rgb_optical_frame">
 #     4) Add <link name="camera_rgb_frame"/>
-#     5) Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
-#     6) Change image width/height from 1920x1080 to 640x480
+#     5) Change image width/height from 1920x1080 to 640x480
+#     6) [ROS2 HUMBLE] Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
+#     6) [ROS2 JAZZY] Change <gz_frame_id>camera_rgb_frame</gz_frame_id> to <gz_frame_id>camera_rgb_optical_frame</gz_frame_id>
+#     7) [ROS2 JAZZY] Add the following just after <sensor name="camera" ...> section (under same link)
+#        <sensor name="depth" type="depth">
+#            <always_on>true</always_on>
+#            <visualize>true</visualize>
+#            <update_rate>30</update_rate>
+#            <topic>camera/depth/image_raw</topic>
+#            <gz_frame_id>camera_rgb_optical_frame</gz_frame_id>
+#            <camera name="intel_realsense_r200_depth">
+#            <camera_info_topic>camera/depth/camera_info</camera_info_topic>
+#            <horizontal_fov>1.02974</horizontal_fov>
+#            <image>
+#                <width>640</width>
+#                <height>480</height>
+#                <format>R8G8B8</format>
+#            </image>
+#            <clip>
+#                <near>0.02</near>
+#                <far>300</far>
+#            </clip>
+#            </camera>
+#        </sensor>
 # Example:
 #   $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_fake_scan_demo.launch.py
 #
@@ -28,9 +50,12 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, Opaq
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
 
 import os
 
+ROS_DISTRO = os.environ.get('ROS_DISTRO')
+
 def launch_setup(context, *args, **kwargs):
     if not 'TURTLEBOT3_MODEL' in os.environ:
         os.environ['TURTLEBOT3_MODEL'] = 'waffle'
@@ -45,9 +70,14 @@ def launch_setup(context, *args, **kwargs):
 
     world = LaunchConfiguration('world').perform(context)
     
-    nav2_params_file = PathJoinSubstitution(
-        [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml']
-    )
+    if ROS_DISTRO == 'humble':
+        nav2_params_file = PathJoinSubstitution(
+            [FindPackageShare('rtabmap_demos'), 'params', 'humble', 'turtlebot3_rgbd_nav2_params.yaml']
+        )
+    else:
+        nav2_params_file = PathJoinSubstitution(
+            [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml']
+        )
 
     # Paths
     gazebo_launch = PathJoinSubstitution(
@@ -60,13 +90,22 @@ def launch_setup(context, *args, **kwargs):
         [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd_fake_scan.launch.py'])
 
     # Includes
-    gazebo = IncludeLaunchDescription(
+    gazebo = [IncludeLaunchDescription(
         PythonLaunchDescriptionSource([gazebo_launch]),
         launch_arguments=[
             ('x_pose', LaunchConfiguration('x_pose')),
             ('y_pose', LaunchConfiguration('y_pose'))
         ]
-    )
+    )]
+    if ROS_DISTRO != 'humble':
+        start_gazebo_ros_depth_image_bridge_cmd = Node(
+            package='ros_gz_image',
+            executable='image_bridge',
+            arguments=['/camera/depth/image_raw'],
+            output='screen',
+        )
+        gazebo.append(start_gazebo_ros_depth_image_bridge_cmd)
+
     nav2 = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([nav2_launch]),
         launch_arguments=[
@@ -77,6 +116,7 @@ def launch_setup(context, *args, **kwargs):
     rviz = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([rviz_launch])
     )
+
     rtabmap = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([rtabmap_launch]),
         launch_arguments=[
@@ -88,9 +128,8 @@ def launch_setup(context, *args, **kwargs):
         # Nodes to launch
         nav2,
         rviz,
-        rtabmap,
-        gazebo
-    ]
+        rtabmap
+    ] + gazebo
 
 def generate_launch_description():
     return LaunchDescription([

rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py

@@ -13,8 +13,30 @@
 #          </joint> 
 #     3) Rename <link name="camera_rgb_frame"> to <link name="camera_rgb_optical_frame">
 #     4) Add <link name="camera_rgb_frame"/>
-#     5) Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
-#     6) Change image width/height from 1920x1080 to 640x480
+#     5) Change image width/height from 1920x1080 to 640x480
+#     6) [ROS2 HUMBLE] Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
+#     6) [ROS2 JAZZY] Change <gz_frame_id>camera_rgb_frame</gz_frame_id> to <gz_frame_id>camera_rgb_optical_frame</gz_frame_id>
+#     7) [ROS2 JAZZY] Add the following just after <sensor name="camera" ...> section (under same link)
+#        <sensor name="depth" type="depth">
+#            <always_on>true</always_on>
+#            <visualize>true</visualize>
+#            <update_rate>30</update_rate>
+#            <topic>camera/depth/image_raw</topic>
+#            <gz_frame_id>camera_rgb_optical_frame</gz_frame_id>
+#            <camera name="intel_realsense_r200_depth">
+#            <camera_info_topic>camera/depth/camera_info</camera_info_topic>
+#            <horizontal_fov>1.02974</horizontal_fov>
+#            <image>
+#                <width>640</width>
+#                <height>480</height>
+#                <format>R8G8B8</format>
+#            </image>
+#            <clip>
+#                <near>0.02</near>
+#                <far>300</far>
+#            </clip>
+#            </camera>
+#        </sensor>
 #     7) Note that we can increase min scan range from 0.12 to 0.2 to avoid having scans 
 #        hitting the robot itself
 # Example:
@@ -30,9 +52,12 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, Opaq
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
 
 import os
 
+ROS_DISTRO = os.environ.get('ROS_DISTRO')
+
 def launch_setup(context, *args, **kwargs):
     if not 'TURTLEBOT3_MODEL' in os.environ:
         os.environ['TURTLEBOT3_MODEL'] = 'waffle'
@@ -47,9 +72,14 @@ def launch_setup(context, *args, **kwargs):
 
     world = LaunchConfiguration('world').perform(context)
     
-    nav2_params_file = PathJoinSubstitution(
-        [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_scan_nav2_params.yaml']
-    )
+    if ROS_DISTRO == 'humble':
+        nav2_params_file = PathJoinSubstitution(
+            [FindPackageShare('rtabmap_demos'), 'params', 'humble', 'turtlebot3_rgbd_scan_nav2_params.yaml']
+        )
+    else:
+        nav2_params_file = PathJoinSubstitution(
+            [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_scan_nav2_params.yaml']
+        )
 
     # Paths
     gazebo_launch = PathJoinSubstitution(
@@ -62,13 +92,22 @@ def launch_setup(context, *args, **kwargs):
         [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd_scan.launch.py'])
 
     # Includes
-    gazebo = IncludeLaunchDescription(
+    gazebo = [IncludeLaunchDescription(
         PythonLaunchDescriptionSource([gazebo_launch]),
         launch_arguments=[
             ('x_pose', LaunchConfiguration('x_pose')),
             ('y_pose', LaunchConfiguration('y_pose'))
         ]
-    )
+    )]
+    if ROS_DISTRO != 'humble':
+        start_gazebo_ros_depth_image_bridge_cmd = Node(
+            package='ros_gz_image',
+            executable='image_bridge',
+            arguments=['/camera/depth/image_raw'],
+            output='screen',
+        )
+        gazebo.append(start_gazebo_ros_depth_image_bridge_cmd)
+
     nav2 = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([nav2_launch]),
         launch_arguments=[
@@ -79,20 +118,25 @@ def launch_setup(context, *args, **kwargs):
     rviz = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([rviz_launch])
     )
+    
+    max_ground_height = '0.05'
+    if ROS_DISTRO == 'jazzy':
+        max_ground_height = '0.02' # for the demo, on new gazebo the depth is more accurate
+    
     rtabmap = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([rtabmap_launch]),
         launch_arguments=[
             ('localization', LaunchConfiguration('localization')),
-            ('use_sim_time', 'true')
+            ('use_sim_time', 'true'),
+            ('max_ground_height', max_ground_height)
         ]
     )
     return [
         # Nodes to launch
         nav2,
         rviz,
-        rtabmap,
-        gazebo
-    ]
+        rtabmap
+    ] + gazebo
 
 def generate_launch_description():
     return LaunchDescription([

rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py

@@ -14,18 +14,22 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
+from launch.actions import AppendEnvironmentVariable, DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.substitutions import FindPackageShare
 
 import os
 
+ROS_DISTRO = os.environ.get('ROS_DISTRO')
+
 def launch_setup(context, *args, **kwargs):
     if not 'TURTLEBOT3_MODEL' in os.environ:
         os.environ['TURTLEBOT3_MODEL'] = 'waffle'
 
     # Directories
+    pkg_turtlebot3_gazebo = get_package_share_directory(
+        'turtlebot3_gazebo')
     pkg_nav2_bringup = get_package_share_directory(
         'nav2_bringup')
     pkg_rtabmap_demos = get_package_share_directory(
@@ -37,14 +41,26 @@ def launch_setup(context, *args, **kwargs):
     icp_odometry = icp_odometry == 'True' or icp_odometry == 'true'
     if icp_odometry:
         # modified nav2 params to use icp_odom instead odom frame
-        nav2_params_file = PathJoinSubstitution(
-            [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_scan_nav2_params.yaml']
-        )
+        if ROS_DISTRO == 'humble':
+            nav2_params_file = PathJoinSubstitution(
+                [FindPackageShare('rtabmap_demos'), 'params', 'humble', 'turtlebot3_scan_nav2_params.yaml']
+            )
+        else:
+            nav2_params_file = PathJoinSubstitution(
+                [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_scan_nav2_params.yaml']
+            )
     else:
-        # original nav2 params
-        nav2_params_file = PathJoinSubstitution(
-            [FindPackageShare('nav2_bringup'), 'params', 'nav2_params.yaml']
-        )
+        if ROS_DISTRO == 'humble':
+            # original nav2 params
+            nav2_params_file = PathJoinSubstitution(
+                [FindPackageShare('nav2_bringup'), 'params', 'nav2_params.yaml']
+            )
+        else:
+            # original nav2 params but with "enable_stamped_cmd_vel: True"
+            nav2_params_file = PathJoinSubstitution(
+                [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_nav2_params.yaml']
+            )
+            
 
     # Paths
     nav2_launch = PathJoinSubstitution(
@@ -53,56 +69,6 @@ def launch_setup(context, *args, **kwargs):
         [pkg_nav2_bringup, 'launch', 'rviz_launch.py'])
     rtabmap_launch = PathJoinSubstitution(
         [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_scan.launch.py'])
-
-    # To use ICP odometry, we should increase clock rate of gazebo, we copied content of
-    # turtlebot3_gazebo/launch/turtlebot3_world.launch here
-    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
-    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
-
-    world = os.path.join(
-        get_package_share_directory('turtlebot3_gazebo'),
-        'worlds',
-        f'turtlebot3_{world_name}.world'
-    )
-
-    import tempfile
-    with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as clock_override_file:
-        clock_override_file.write("---\n"+
-                  "gazebo:\n"+
-                  "    ros__parameters:\n"+
-                  "        publish_rate: 100.0")
-
-    gzserver_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
-        ),
-        launch_arguments={
-            'world': world,
-            'params_file': clock_override_file.name}.items()
-    )
-
-    gzclient_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
-        )
-    )
-
-    robot_state_publisher_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, 'robot_state_publisher.launch.py')
-        ),
-        launch_arguments={'use_sim_time': 'true'}.items()
-    )
-
-    spawn_turtlebot_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py')
-        ),
-        launch_arguments={
-            'x_pose': LaunchConfiguration('x_pose'),
-            'y_pose': LaunchConfiguration('y_pose')
-        }.items()
-    )
     
     nav2 = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([nav2_launch]),
@@ -121,16 +87,83 @@ def launch_setup(context, *args, **kwargs):
             ('use_sim_time', 'true')
         ]
     )
+        
+    # To use ICP odometry, we should increase clock rate of gazebo (humble), we copied content of
+    # turtlebot3_gazebo/launch/turtlebot3_world.launch here.
+    turtlebot3_nodes = []
+    if ROS_DISTRO == 'humble':
+        pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+
+        import tempfile
+        with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as clock_override_file:
+            clock_override_file.write("---\n"+
+                    "gazebo:\n"+
+                    "    ros__parameters:\n"+
+                    "        publish_rate: 100.0")
+
+        world = os.path.join(
+                pkg_turtlebot3_gazebo,
+                'worlds',
+                f'turtlebot3_{world_name}.world'
+        )
+
+        gzserver_cmd = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
+            ),
+            launch_arguments={
+                'world': world,
+                'params_file': clock_override_file.name}.items()
+        )
+
+        gzclient_cmd = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
+            )
+        )
+        
+        robot_state_publisher_cmd = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(pkg_turtlebot3_gazebo, 'launch', 'robot_state_publisher.launch.py')
+            ),
+            launch_arguments={'use_sim_time': 'true'}.items()
+        )
+        spawn_turtlebot_cmd = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(pkg_turtlebot3_gazebo, 'launch', 'spawn_turtlebot3.launch.py')
+            ),
+            launch_arguments={
+                'x_pose': LaunchConfiguration('x_pose'),
+                'y_pose': LaunchConfiguration('y_pose')
+            }.items()
+        )
+        
+        set_env_vars_resources = AppendEnvironmentVariable(
+                'GZ_SIM_RESOURCE_PATH',
+                os.path.join(pkg_turtlebot3_gazebo, 'models'))
+        turtlebot3_nodes = [
+            gzserver_cmd,
+            gzclient_cmd,
+            robot_state_publisher_cmd,
+            spawn_turtlebot_cmd,
+            set_env_vars_resources
+        ]
+    else:
+        gazebo_launch = PathJoinSubstitution([pkg_turtlebot3_gazebo, 'launch', f'turtlebot3_{world_name}.launch.py'])
+        gazebo = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([gazebo_launch]),
+            launch_arguments=[
+                ('x_pose', LaunchConfiguration('x_pose')),
+                ('y_pose', LaunchConfiguration('y_pose'))
+            ]
+        )
+        turtlebot3_nodes = [gazebo]
+
     return [
         # Nodes to launch
         nav2,
         rviz,
-        rtabmap,
-        gzserver_cmd,
-        gzclient_cmd,
-        robot_state_publisher_cmd,
-        spawn_turtlebot_cmd
-    ]
+        rtabmap] + turtlebot3_nodes
 
 def generate_launch_description():
     return LaunchDescription([