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')),

src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf

@@ -27,12 +27,12 @@
 
     <link name="os_lidar"><pose>0 0 0.52 0 0 0</pose><inertial><mass>0.2</mass><inertia><ixx>0.001</ixx><iyy>0.001</iyy><izz>0.001</izz></inertia></inertial>
       <visual name="visual"><geometry><cylinder><radius>0.075</radius><length>0.10</length></cylinder></geometry><material><ambient>0.85 0.12 0.10 1</ambient><diffuse>0.85 0.12 0.10 1</diffuse></material></visual>
-      <sensor name="lidar_3d" type="ray"><always_on>true</always_on><update_rate>10</update_rate><visualize>false</visualize><ray><scan><horizontal><samples>360</samples><resolution>1</resolution><min_angle>-3.14159</min_angle><max_angle>3.14159</max_angle></horizontal><vertical><samples>16</samples><resolution>1</resolution><min_angle>-0.26</min_angle><max_angle>0.26</max_angle></vertical></scan><range><min>0.20</min><max>18.0</max><resolution>0.02</resolution></range></ray><plugin name="mkmini_lidar" filename="libgazebo_ros_ray_sensor.so"><ros><remapping>~/out:=/carla/hero/lidar_filtered</remapping></ros><output_type>sensor_msgs/PointCloud2</output_type><frame_name>os_lidar</frame_name></plugin></sensor>
+      <sensor name="lidar_3d" type="ray"><always_on>true</always_on><update_rate>10</update_rate><visualize>false</visualize><ray><scan><horizontal><samples>360</samples><resolution>1</resolution><min_angle>-3.14159</min_angle><max_angle>3.14159</max_angle></horizontal><vertical><samples>16</samples><resolution>1</resolution><min_angle>-0.26</min_angle><max_angle>0.26</max_angle></vertical></scan><range><min>0.20</min><max>18.0</max><resolution>0.02</resolution></range></ray><plugin name="mkmini_lidar" filename="libgazebo_ros_ray_sensor.so"><ros><remapping>~/out:=/raw/lidar</remapping></ros><output_type>sensor_msgs/PointCloud2</output_type><frame_name>os_lidar</frame_name></plugin></sensor>
     </link>
     <joint name="os_lidar_joint" type="fixed"><parent>base_link</parent><child>os_lidar</child></joint>
 
     <plugin name="mkmini_tricycle" filename="libgazebo_ros_tricycle_drive.so">
-      <ros><remapping>cmd_vel:=/cmd_vel</remapping><remapping>odom:=/odom_custom</remapping></ros>
+      <ros><remapping>cmd_vel:=/cmd_vel</remapping><remapping>odom:=/odom</remapping></ros>
       <steering_joint>front_steering_joint</steering_joint><actuated_wheel_joint>front_drive_joint</actuated_wheel_joint>
       <encoder_wheel_left_joint>rear_left_joint</encoder_wheel_left_joint><encoder_wheel_right_joint>rear_right_joint</encoder_wheel_right_joint>
       <wheel_separation>0.72</wheel_separation><encoder_wheel_diameter>0.28</encoder_wheel_diameter><actuated_wheel_diameter>0.28</actuated_wheel_diameter>