From 93fb5f87ba7283a6a494f6bea310f5b40e1508f9 Mon Sep 17 00:00:00 2001
From: chyoo <chyoo@futuredrive.net>
Date: Thu, 6 Aug 2026 14:07:45 +0900
Subject: [PATCH] feat: let Gazebo spawn MKMini

---
 src/nav2_turtlebot_simulator/CMakeLists.txt   |  2 +-
 .../launch/simulator.launch.py                | 69 ++++++++++++-------
 .../models/mkmini_gazebo/model.config         |  7 ++
 .../models/mkmini_gazebo/model.sdf            | 44 ++++++++++++
 4 files changed, 98 insertions(+), 24 deletions(-)
 create mode 100644 src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.config
 create mode 100644 src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf

diff --git a/src/nav2_turtlebot_simulator/CMakeLists.txt b/src/nav2_turtlebot_simulator/CMakeLists.txt
index 4c70be9..a11cfc3 100644
--- a/src/nav2_turtlebot_simulator/CMakeLists.txt
+++ b/src/nav2_turtlebot_simulator/CMakeLists.txt
@@ -3,6 +3,6 @@ project(nav2_turtlebot_simulator)
 
 find_package(ament_cmake REQUIRED)
 
-install(DIRECTORY launch worlds DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY launch worlds models DESTINATION share/${PROJECT_NAME})
 
 ament_package()
diff --git a/src/nav2_turtlebot_simulator/launch/simulator.launch.py b/src/nav2_turtlebot_simulator/launch/simulator.launch.py
index 9c2e5d8..2fcb20d 100644
--- a/src/nav2_turtlebot_simulator/launch/simulator.launch.py
+++ b/src/nav2_turtlebot_simulator/launch/simulator.launch.py
@@ -5,7 +5,7 @@ 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.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, SetEnvironmentVariable
 from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
@@ -233,6 +233,7 @@ def launch_gazebo(context):
     gazebo_ros_dir = get_package_share_directory('gazebo_ros')
     world = LaunchConfiguration('world').perform(context)
     render_mode = LaunchConfiguration('render_mode').perform(context).strip().lower()
+    platform = LaunchConfiguration('platform').perform(context).strip().lower()
 
     if render_mode not in {'flat', 'full'}:
         raise RuntimeError('render_mode must be either flat or full')
@@ -247,7 +248,47 @@ def launch_gazebo(context):
             'worlds',
             world,
         )
+
+    if platform in {'turtlebot3', 'turtlebot3_burger', 'burger'}:
+        entity_name = 'turtlebot3_burger_3d_lidar'
+        model_path = os.path.join(gazebo_dir, 'models', 'turtlebot3_burger', 'model.sdf')
+        model_directory = os.path.join(gazebo_dir, 'models')
+        static_transforms = [
+            Node(
+                package='tf2_ros',
+                executable='static_transform_publisher',
+                name='turtlebot3_base_link_tf',
+                arguments=[
+                    '--x', '0', '--y', '0', '--z', '0.01',
+                    '--roll', '0', '--pitch', '0', '--yaw', '0',
+                    '--frame-id', 'base_footprint', '--child-frame-id', 'base_link',
+                ],
+            ),
+            Node(
+                package='tf2_ros',
+                executable='static_transform_publisher',
+                name='turtlebot3_laser_tf',
+                arguments=[
+                    '--x', '-0.032', '--y', '0', '--z', '0.171',
+                    '--roll', '0', '--pitch', '0', '--yaw', '0',
+                    '--frame-id', 'base_link', '--child-frame-id', 'base_scan',
+                ],
+            ),
+        ]
+    elif platform in {'mkmini', 'mk-mini'}:
+        entity_name = 'mkmini_gazebo'
+        model_directory = os.path.join(simulator_dir, 'models')
+        model_path = os.path.join(model_directory, 'mkmini_gazebo', 'model.sdf')
+        static_transforms = []
+    else:
+        raise RuntimeError('platform must be turtlebot3 or mkmini')
+
+    existing_model_path = os.environ.get('GAZEBO_MODEL_PATH', '')
     return [
+        SetEnvironmentVariable(
+            name='GAZEBO_MODEL_PATH',
+            value=os.pathsep.join(part for part in (model_directory, existing_model_path) if part),
+        ),
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(os.path.join(gazebo_ros_dir, 'launch', 'gzserver.launch.py')),
             launch_arguments={
@@ -259,14 +300,15 @@ def launch_gazebo(context):
             package='gazebo_ros',
             executable='spawn_entity.py',
             arguments=[
-                '-entity', 'turtlebot3_burger_3d_lidar',
-                '-file', os.path.join(gazebo_dir, 'models', 'turtlebot3_burger', 'model.sdf'),
+                '-entity', entity_name,
+                '-file', model_path,
                 '-x', LaunchConfiguration('x_pose').perform(context),
                 '-y', LaunchConfiguration('y_pose').perform(context),
                 '-z', '0.01',
             ],
             output='screen',
         ),
+        *static_transforms,
     ]
 
 
@@ -276,29 +318,10 @@ def generate_launch_description():
         DeclareLaunchArgument('gui', default_value='false'),
         DeclareLaunchArgument('world', default_value='turtlebot3_world.world'),
         DeclareLaunchArgument('render_mode', default_value='flat'),
+        DeclareLaunchArgument('platform', default_value='turtlebot3'),
         DeclareLaunchArgument('x_pose', default_value='-2.0'),
         DeclareLaunchArgument('y_pose', default_value='-0.5'),
         OpaqueFunction(function=launch_gazebo),
-        Node(
-            package='tf2_ros',
-            executable='static_transform_publisher',
-            name='turtlebot3_base_link_tf',
-            arguments=[
-                '--x', '0', '--y', '0', '--z', '0.01',
-                '--roll', '0', '--pitch', '0', '--yaw', '0',
-                '--frame-id', 'base_footprint', '--child-frame-id', 'base_link',
-            ],
-        ),
-        Node(
-            package='tf2_ros',
-            executable='static_transform_publisher',
-            name='turtlebot3_laser_tf',
-            arguments=[
-                '--x', '-0.032', '--y', '0', '--z', '0.171',
-                '--roll', '0', '--pitch', '0', '--yaw', '0',
-                '--frame-id', 'base_link', '--child-frame-id', 'base_scan',
-            ],
-        ),
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(os.path.join(gazebo_ros_dir, 'launch', 'gzclient.launch.py')),
             condition=IfCondition(LaunchConfiguration('gui')),
diff --git a/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.config b/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.config
new file mode 100644
index 0000000..f50fbe0
--- /dev/null
+++ b/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.config
@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+<model>
+  <name>mkmini_gazebo</name>
+  <version>1.0</version>
+  <sdf version="1.6">model.sdf</sdf>
+  <description>Lightweight Ackermann MK-mini model for Gazebo learning runs.</description>
+</model>
diff --git a/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf b/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf
new file mode 100644
index 0000000..5d59150
--- /dev/null
+++ b/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf
@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <!-- Lightweight MK-mini approximation. The body dimensions and wheelbase
+       match the platform profile; Gazebo's tricycle plugin supplies stable
+       non-holonomic Ackermann-like motion from the standard /cmd_vel topic. -->
+  <model name="mkmini_gazebo">
+    <pose>0 0 0.16 0 0 0</pose>
+
+    <link name="base_link">
+      <inertial><mass>38.0</mass><inertia><ixx>2.0</ixx><iyy>2.6</iyy><izz>3.2</izz></inertia></inertial>
+      <collision name="chassis_collision"><pose>0 0 0.16 0 0 0</pose><geometry><box><size>1.05 0.70 0.32</size></box></geometry></collision>
+      <visual name="chassis_visual"><pose>0 0 0.16 0 0 0</pose><geometry><box><size>1.05 0.70 0.32</size></box></geometry><material><ambient>0.12 0.20 0.25 1</ambient><diffuse>0.12 0.35 0.42 1</diffuse></material></visual>
+      <visual name="front_marker"><pose>0.56 0 0.23 0 0 0</pose><geometry><box><size>0.06 0.42 0.10</size></box></geometry><material><ambient>0.95 0.65 0.08 1</ambient><diffuse>0.95 0.65 0.08 1</diffuse></material></visual>
+    </link>
+
+    <link name="front_fork"><pose>0.30 0 0 0 0 0</pose><inertial><mass>1.0</mass><inertia><ixx>0.01</ixx><iyy>0.01</iyy><izz>0.01</izz></inertia></inertial></link>
+    <joint name="front_steering_joint" type="revolute"><parent>base_link</parent><child>front_fork</child><axis><xyz>0 0 1</xyz><limit><lower>-0.5</lower><upper>0.5</upper></limit></axis></joint>
+
+    <link name="front_drive_wheel"><pose>0.30 0 0 0 0 0</pose><inertial><mass>1.2</mass><inertia><ixx>0.02</ixx><iyy>0.01</iyy><izz>0.02</izz></inertia></inertial><collision name="collision"><pose>0 0 0 1.5708 0 0</pose><geometry><cylinder><radius>0.14</radius><length>0.18</length></cylinder></geometry></collision><visual name="visual"><pose>0 0 0 1.5708 0 0</pose><geometry><cylinder><radius>0.14</radius><length>0.18</length></cylinder></geometry><material><ambient>0.04 0.04 0.04 1</ambient><diffuse>0.04 0.04 0.04 1</diffuse></material></visual></link>
+    <joint name="front_drive_joint" type="revolute"><parent>front_fork</parent><child>front_drive_wheel</child><axis><xyz>0 1 0</xyz></axis></joint>
+
+    <link name="rear_left_wheel"><pose>-0.30 0.36 0 0 0 0</pose><inertial><mass>1.0</mass><inertia><ixx>0.02</ixx><iyy>0.01</iyy><izz>0.02</izz></inertia></inertial><collision name="collision"><pose>0 0 0 1.5708 0 0</pose><geometry><cylinder><radius>0.14</radius><length>0.10</length></cylinder></geometry></collision><visual name="visual"><pose>0 0 0 1.5708 0 0</pose><geometry><cylinder><radius>0.14</radius><length>0.10</length></cylinder></geometry><material><ambient>0.04 0.04 0.04 1</ambient><diffuse>0.04 0.04 0.04 1</diffuse></material></visual></link>
+    <joint name="rear_left_joint" type="revolute"><parent>base_link</parent><child>rear_left_wheel</child><axis><xyz>0 1 0</xyz></axis></joint>
+
+    <link name="rear_right_wheel"><pose>-0.30 -0.36 0 0 0 0</pose><inertial><mass>1.0</mass><inertia><ixx>0.02</ixx><iyy>0.01</iyy><izz>0.02</izz></inertia></inertial><collision name="collision"><pose>0 0 0 1.5708 0 0</pose><geometry><cylinder><radius>0.14</radius><length>0.10</length></cylinder></geometry></collision><visual name="visual"><pose>0 0 0 1.5708 0 0</pose><geometry><cylinder><radius>0.14</radius><length>0.10</length></cylinder></geometry><material><ambient>0.04 0.04 0.04 1</ambient><diffuse>0.04 0.04 0.04 1</diffuse></material></visual></link>
+    <joint name="rear_right_joint" type="revolute"><parent>base_link</parent><child>rear_right_wheel</child><axis><xyz>0 1 0</xyz></axis></joint>
+
+    <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>
+    </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>
+      <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>
+      <max_wheel_torque>120.0</max_wheel_torque><max_wheel_acceleration>1.5</max_wheel_acceleration><max_steering_speed>0.8</max_steering_speed>
+      <publish_odom>true</publish_odom><publish_wheel_tf>true</publish_wheel_tf><publish_wheel_joint_state>true</publish_wheel_joint_state>
+      <odometry_frame>odom</odometry_frame><robot_base_frame>base_link</robot_base_frame>
+    </plugin>
+  </model>
+</sdf>