feat: widen MKMini Gazebo world

src/nav2_turtlebot_simulator/launch/simulator.launch.py

@@ -33,6 +33,8 @@ LOCAL_SUN_LIGHT = '''    <light type="directional" name="sun">
       <cast_shadows>false</cast_shadows><pose>0 0 10 0 0 0</pose><diffuse>0.8 0.8 0.8 1</diffuse><specular>0.2 0.2 0.2 1</specular><direction>-0.5 0.1 -0.9</direction>
     </light>'''
 
+MKMINI_WORLD_SPACING_SCALE = 1.6
+
 
 def replace_house_visual_meshes_with_boxes(house_model):
     """Keep physical mesh collisions, but render furniture as inexpensive boxes."""
@@ -174,6 +176,53 @@ def create_flat_house_world(gazebo_dir):
     return flat_world_path
 
 
+def create_mkmini_world(gazebo_dir):
+    """Expand the TurtleBot3 obstacle layout without changing obstacle sizes."""
+    source_model_path = os.path.join(gazebo_dir, 'models', 'turtlebot3_world', 'model.sdf')
+    models_dir = os.path.join(tempfile.gettempdir(), 'nav2_mkmini_models')
+    model_dir = os.path.join(models_dir, 'turtlebot3_world_mkmini')
+    os.makedirs(model_dir, exist_ok=True)
+
+    model_tree = ET.parse(source_model_path)
+    model_sdf = model_tree.getroot()
+    model = model_sdf.find('model')
+    if model is None:
+        raise RuntimeError('turtlebot3_world model is missing from model.sdf')
+    model.attrib['name'] = 'turtlebot3_world_mkmini'
+
+    # Only object positions change. Keeping their geometry unchanged widens the
+    # driving lanes for the larger MKMini collision footprint.
+    for pose in model.findall('.//pose'):
+        values = pose.text.split()
+        if len(values) != 6:
+            continue
+        try:
+            values[0] = str(float(values[0]) * MKMINI_WORLD_SPACING_SCALE)
+            values[1] = str(float(values[1]) * MKMINI_WORLD_SPACING_SCALE)
+        except ValueError:
+            continue
+        pose.text = ' '.join(values)
+
+    model_tree.write(
+        os.path.join(model_dir, 'model.sdf'),
+        encoding='utf-8',
+        xml_declaration=True,
+    )
+    with open(os.path.join(model_dir, 'model.config'), 'w', encoding='utf-8') as config:
+        config.write(
+            '<model><name>turtlebot3_world_mkmini</name>'
+            '<version>1.0</version><sdf version="1.6">model.sdf</sdf></model>'
+        )
+
+    source_world_path = os.path.join(gazebo_dir, 'worlds', 'turtlebot3_world.world')
+    with open(source_world_path, 'r', encoding='utf-8') as source:
+        world = source.read().replace('model://turtlebot3_world', 'model://turtlebot3_world_mkmini')
+    world_path = os.path.join(tempfile.gettempdir(), 'nav2_mkmini_world.world')
+    with open(world_path, 'w', encoding='utf-8') as destination:
+        destination.write(world)
+    return world_path, models_dir
+
+
 def create_custom_layout_world():
     """Build a Gazebo world from the editor's persisted 2D layout."""
     layout_path = os.environ.get('WORLD_LAYOUT_PATH', '/modules/world-layout.json')
@@ -234,6 +283,7 @@ def launch_gazebo(context):
     world = LaunchConfiguration('world').perform(context)
     render_mode = LaunchConfiguration('render_mode').perform(context).strip().lower()
     platform = LaunchConfiguration('platform').perform(context).strip().lower()
+    extra_model_path = ''
 
     if render_mode not in {'flat', 'full'}:
         raise RuntimeError('render_mode must be either flat or full')
@@ -242,6 +292,8 @@ def launch_gazebo(context):
         world_path = create_custom_layout_world()
     elif world == 'turtlebot3_house.world' and render_mode == 'flat':
         world_path = create_flat_house_world(gazebo_dir)
+    elif world == 'turtlebot3_world.world' and platform in {'mkmini', 'mk-mini'}:
+        world_path, extra_model_path = create_mkmini_world(gazebo_dir)
     else:
         world_path = os.path.join(
             simulator_dir if world == 'empty_world.world' else gazebo_dir,
@@ -287,7 +339,7 @@ def launch_gazebo(context):
     return [
         SetEnvironmentVariable(
             name='GAZEBO_MODEL_PATH',
-            value=os.pathsep.join(part for part in (model_directory, existing_model_path) if part),
+            value=os.pathsep.join(part for part in (extra_model_path, model_directory, existing_model_path) if part),
         ),
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(os.path.join(gazebo_ros_dir, 'launch', 'gzserver.launch.py')),