feat: add opt-in Gazebo mode for MK-mini

2026-08-06 13:25:06 +09:00
parent 705113708e
commit 6c4594b21f
11 changed files with 164 additions and 4 deletions
@@ -26,6 +26,12 @@ platform:
  sim_sensor_driver:      carla_lidar      # sim_mode=true
  sim_lidar:              vanjee           # CARLA 스폰 blueprint 정체성 (차량↔라이다 고정 페어링)

+  # Gazebo is an opt-in simulation backend (simulator:=gazebo).  Keep the
+  # existing CARLA selections above as the default so existing runs are not
+  # changed.
+  gazebo_vehicle_driver:  gazebo_mkmini_passthrough
+  gazebo_sensor_driver:   gazebo_mkmini_lidar
+
  # cross-workspace 규칙 키 (기존 {robot} 컨벤션 보존 — 다운스트림 무수정)
  perception_profile: mkmini       # → perception_{...}.yaml, tf_{...}.yaml, control_*_{...}.yaml
  scan_topic:  /scan
@@ -0,0 +1,14 @@
+# Gazebo publishes the simulated 3D point cloud directly to the topic used by
+# the current MK-mini perception and control configurations.
+sensor_driver:
+  name: gazebo_mkmini_lidar
+  display_name: Gazebo MK-mini 3D LiDAR
+  dimension: 3d
+  modes: [sim]
+  compatible_platforms: [mkmini]
+  scan_adapter: pointcloud_passthrough
+  driver_launch:
+    package: ""
+    launch: ""
+  speed_msg: none
+  bridge: {}
@@ -0,0 +1,10 @@
+# Gazebo MK-mini consumes Nav2 Twist messages directly through the Ackermann
+# plugin. This profile intentionally has no hardware driver or adapter.
+vehicle_driver:
+  name: gazebo_mkmini_passthrough
+  display_name: Gazebo MK-mini Twist passthrough
+  adapter: ""
+  driver_launch:
+    package: ""
+    launch: ""
+  adapter_params: {}
@@ -0,0 +1,35 @@
+"""Opt-in Gazebo backend for the MK-mini vehicle profile."""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import EnvironmentVariable, LaunchConfiguration
+
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory('vehicle_pkg')
+    gazebo_share = get_package_share_directory('gazebo_ros')
+    model_dir = os.path.join(pkg_share, 'models')
+    world_path = os.path.join(pkg_share, 'worlds', 'mkmini_gazebo.world')
+    gui = LaunchConfiguration('gui')
+
+    return LaunchDescription([
+        DeclareLaunchArgument('gui', default_value='true'),
+        SetEnvironmentVariable(
+            name='GAZEBO_MODEL_PATH',
+            value=[model_dir, os.pathsep, EnvironmentVariable('GAZEBO_MODEL_PATH', default_value='')],
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(gazebo_share, 'launch', 'gazebo.launch.py')),
+            launch_arguments={
+                'world': world_path,
+                'gui': gui,
+                'verbose': 'false',
+                'pause': 'false',
+            }.items(),
+        ),
+    ])
@@ -24,6 +24,10 @@ from vehicle_pkg.profiles import resolve_plan

 ARGUMENTS = [
    DeclareLaunchArgument('sim_mode', default_value='false'),
+    # Empty keeps the existing platform profile behavior (MK-mini sim = CARLA).
+    # gazebo is opt-in so the real vehicle and current CARLA flows are unchanged.
+    DeclareLaunchArgument('simulator', default_value=''),
+    DeclareLaunchArgument('gui', default_value='true'),
    DeclareLaunchArgument('lidar', default_value='ouster'),                  # carla spawn 용 (choices 완화)
    DeclareLaunchArgument('level', default_value='None', choices=['None', '2d', '3d']),
    DeclareLaunchArgument('platform', default_value='mkmini'),
@@ -46,6 +50,8 @@ ARGUMENTS = [

 def build(context, *args, **kwargs):
    sim_mode = LaunchConfiguration('sim_mode').perform(context)
+    simulator = LaunchConfiguration('simulator').perform(context)
+    gui = LaunchConfiguration('gui').perform(context)
    lidar = LaunchConfiguration('lidar').perform(context)
    level = LaunchConfiguration('level').perform(context)
    platform = LaunchConfiguration('platform').perform(context)
@@ -69,7 +75,8 @@ def build(context, *args, **kwargs):
        pkg_share, platform_name, sim_mode, vehicle_driver, sensor_driver,
        extra_config_dirs=extra_config_dirs,
        sensor_driver_params=sensor_driver_params,
-        vehicle_driver_params=vehicle_driver_params)
+        vehicle_driver_params=vehicle_driver_params,
+        simulator=simulator)

    sim = (sim_mode == 'true')
    actions = []
@@ -90,6 +97,12 @@ def build(context, *args, **kwargs):
                ('spawn_imu', spawn_imu),
            ],
        ))
+    elif sim and plan['simulator'] == 'gazebo':
+        gazebo_launch = os.path.join(pkg_share, 'launch', 'mkmini_gazebo.launch.py')
+        actions.append(IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(gazebo_launch),
+            launch_arguments=[('gui', gui)],
+        ))
    elif not sim:
        actions.append(Node(package='vehicle_pkg', executable='pub_clock',
                            name='pub_clock', output='screen'))
@@ -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>
@@ -13,6 +13,7 @@
  <exec_depend>yhs_can_interfaces</exec_depend>
  <exec_depend>yhs_can_control</exec_depend>
  <exec_depend>vanjee_lidar_sdk</exec_depend>
+  <exec_depend>gazebo_ros</exec_depend>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
@@ -20,6 +20,8 @@ setup(
        (os.path.join('share', package_name, 'config', 'platforms'), glob('config/platforms/*.yaml')),
        (os.path.join('share', package_name, 'config', 'vehicle_drivers'), glob('config/vehicle_drivers/*.yaml')),
        (os.path.join('share', package_name, 'config', 'sensor_drivers'), glob('config/sensor_drivers/*.yaml')),
+        (os.path.join('share', package_name, 'models', 'mkmini_gazebo'), glob('models/mkmini_gazebo/*')),
+        (os.path.join('share', package_name, 'worlds'), glob('worlds/*.world')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
@@ -99,7 +99,7 @@ def _merge_params(prof, kind, raw_b64):
 def resolve_plan(pkg_share, platform_name, sim_mode,
                 vehicle_driver_arg='', sensor_driver_arg='',
                 extra_config_dirs='', sensor_driver_params='',
-                 vehicle_driver_params=''):
+                 vehicle_driver_params='', simulator=''):
    """platform/sim_mode/override 로 launch 플랜(dict) 해소. ROS 비의존.

    sim_mode 는 'true'/'false' 문자열. override 가 빈 문자열이면 platform 기본값 사용.
@@ -113,7 +113,14 @@ def resolve_plan(pkg_share, platform_name, sim_mode,
    extra_dirs = [d for d in str(extra_config_dirs).split(':') if d]

    # 드라이버 이름 선택 (선택 source = platform 프로파일 데이터)
-    if sim:
+    simulator_name = str(simulator).strip()
+    if simulator_name and not sim:
+        raise ValueError('[profiles] simulator override requires sim_mode:=true')
+
+    if sim and simulator_name == 'gazebo':
+        vd_name = P.get('gazebo_vehicle_driver', P['sim_vehicle_driver'])
+        sd_name = P.get('gazebo_sensor_driver', P['sim_sensor_driver'])
+    elif sim:
        vd_name = P['sim_vehicle_driver']
        sd_name = P['sim_sensor_driver']
    else:
@@ -149,7 +156,7 @@ def resolve_plan(pkg_share, platform_name, sim_mode,

    return {
        'platform': P['name'],
-        'simulator': P.get('simulator', 'carla' if sim else 'real'),
+        'simulator': simulator_name or P.get('simulator', 'carla' if sim else 'real'),
        'drive_type': P['drive_type'],
        'perception_profile': P.get('perception_profile', P['name']),
        'lidar_frame': P.get('lidar_frame'),
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <world name="mkmini_gazebo">
+    <gravity>0 0 -9.8</gravity>
+    <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>
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision"><geometry><plane><normal>0 0 1</normal><size>100 100</size></plane></geometry></collision>
+        <visual name="visual"><geometry><plane><normal>0 0 1</normal><size>100 100</size></plane></geometry><material><ambient>0.22 0.25 0.29 1</ambient><diffuse>0.22 0.25 0.29 1</diffuse></material></visual>
+      </link>
+    </model>
+    <include><uri>model://mkmini_gazebo</uri></include>
+  </world>
+</sdf>