feat: add lms-nav2-gazebo-simulator module

2026-07-28 10:35:22 +09:00
commit 0db91a16b8
160 changed files with 275895 additions and 0 deletions
@@ -0,0 +1,133 @@
+import os
+import tempfile
+import xml.etree.ElementTree as ET
+
+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 Node
+
+
+def create_flat_house_world(gazebo_dir):
+    """Create a texture-free copy of the house model while retaining its mesh hierarchy."""
+    world_path = os.path.join(gazebo_dir, 'worlds', 'turtlebot3_house.world')
+    model_path = os.path.join(gazebo_dir, 'models', 'turtlebot3_house', 'model.sdf')
+    flat_models_dir = os.path.join(tempfile.gettempdir(), 'nav2_flat_models')
+    flat_model_dir = os.path.join(flat_models_dir, 'turtlebot3_house_flat')
+    os.makedirs(flat_model_dir, exist_ok=True)
+
+    house_model_tree = ET.parse(model_path)
+    house_model = house_model_tree.getroot()
+    house_model.attrib['name'] = 'turtlebot3_house_flat'
+
+    for visual in house_model.findall('.//visual'):
+        material = visual.find('material')
+        if material is None:
+            material = ET.SubElement(visual, 'material')
+
+        for script in material.findall('script'):
+            material.remove(script)
+
+        for name, value in (
+            ('ambient', '0.62 0.67 0.76 1'),
+            ('diffuse', '0.62 0.67 0.76 1'),
+            ('specular', '0.05 0.05 0.05 1'),
+        ):
+            color = material.find(name)
+            if color is None:
+                color = ET.SubElement(material, name)
+            color.text = value
+
+        shadows = visual.find('cast_shadows')
+        if shadows is None:
+            shadows = ET.SubElement(visual, 'cast_shadows')
+        shadows.text = 'false'
+
+    # ElementTree serializes Gazebo's frame='' as frame="", which is invalid SDF.
+    for element in house_model.iter():
+        if element.attrib.get('frame') == '':
+            del element.attrib['frame']
+
+    house_model_tree.write(
+        os.path.join(flat_model_dir, 'model.sdf'),
+        encoding='utf-8',
+        xml_declaration=True,
+    )
+    with open(os.path.join(flat_model_dir, 'model.config'), 'w', encoding='utf-8') as config:
+        config.write(
+            '<model><name>turtlebot3_house_flat</name>'
+            '<version>1.0</version><sdf version="1.6">model.sdf</sdf></model>'
+        )
+
+    with open(world_path, 'r', encoding='utf-8') as source:
+        flat_world = source.read().replace('model://turtlebot3_house', 'model://turtlebot3_house_flat')
+
+    flat_world_path = os.path.join(tempfile.gettempdir(), 'nav2_turtlebot3_house_flat.world')
+    with open(flat_world_path, 'w', encoding='utf-8') as destination:
+        destination.write(flat_world)
+
+    existing_model_path = os.environ.get('GAZEBO_MODEL_PATH', '')
+    os.environ['GAZEBO_MODEL_PATH'] = os.pathsep.join(
+        part for part in (flat_models_dir, existing_model_path) if part
+    )
+    return flat_world_path
+
+
+def launch_gazebo(context):
+    gazebo_dir = get_package_share_directory('turtlebot3_gazebo')
+    simulator_dir = get_package_share_directory('nav2_turtlebot_simulator')
+    gazebo_ros_dir = get_package_share_directory('gazebo_ros')
+    world = LaunchConfiguration('world').perform(context)
+    render_mode = LaunchConfiguration('render_mode').perform(context).strip().lower()
+
+    if render_mode not in {'flat', 'full'}:
+        raise RuntimeError('render_mode must be either flat or full')
+
+    if world == 'turtlebot3_house.world' and render_mode == 'flat':
+        world_path = create_flat_house_world(gazebo_dir)
+    else:
+        world_path = os.path.join(
+            simulator_dir if world == 'empty_world.world' else gazebo_dir,
+            'worlds',
+            world,
+        )
+    return [
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(gazebo_ros_dir, 'launch', 'gzserver.launch.py')),
+            launch_arguments={
+                'world': world_path,
+                'extra_gazebo_args': '-slibgazebo_ros_state.so',
+            }.items(),
+        ),
+        Node(
+            package='gazebo_ros',
+            executable='spawn_entity.py',
+            arguments=[
+                '-entity', 'turtlebot3_burger_3d_lidar',
+                '-file', os.path.join(gazebo_dir, 'models', 'turtlebot3_burger', 'model.sdf'),
+                '-x', LaunchConfiguration('x_pose').perform(context),
+                '-y', LaunchConfiguration('y_pose').perform(context),
+                '-z', '0.01',
+            ],
+            output='screen',
+        ),
+    ]
+
+
+def generate_launch_description():
+    gazebo_ros_dir = get_package_share_directory('gazebo_ros')
+    return LaunchDescription([
+        DeclareLaunchArgument('gui', default_value='false'),
+        DeclareLaunchArgument('world', default_value='turtlebot3_world.world'),
+        DeclareLaunchArgument('render_mode', default_value='flat'),
+        DeclareLaunchArgument('x_pose', default_value='-2.0'),
+        DeclareLaunchArgument('y_pose', default_value='-0.5'),
+        OpaqueFunction(function=launch_gazebo),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(gazebo_ros_dir, 'launch', 'gzclient.launch.py')),
+            condition=IfCondition(LaunchConfiguration('gui')),
+        ),
+    ])