feat: let Gazebo spawn MKMini

2026-08-06 14:07:45 +09:00
parent 71b8cd7dc0
commit 93fb5f87ba
4 changed files with 98 additions and 24 deletions
@@ -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()
@@ -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,38 +248,12 @@ def launch_gazebo(context):
            '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),
+    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',
@@ -299,6 +274,54 @@ def generate_launch_description():
                    '--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={
+                'world': world_path,
+                'extra_gazebo_args': '-slibgazebo_ros_state.so',
+            }.items(),
+        ),
+        Node(
+            package='gazebo_ros',
+            executable='spawn_entity.py',
+            arguments=[
+                '-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,
+    ]
+
+
+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('platform', default_value='turtlebot3'),
+        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')),
@@ -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>
@@ -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>