feat: support Yahboom local Gazebo simulation

2026-09-22 10:28:54 +09:00
parent fbdcc5528e
commit 155a6ff215
3 changed files with 93 additions and 8 deletions
@@ -4,7 +4,11 @@ LMS 전용 템플릿 저장소입니다. 설치 폴더: `[lms-nav2-yahboom-vehic

 - 패키지: `yahboom_vehicle`
 - 런치: `vehicle.launch.py`
- 이미지: `future-nav2:humble-yahboom`
+- 이미지: `future-nav2:humble-yahboom` (실차 기본)
+- 로컬 시뮬레이션: 빌드 전에 `sim_mode:=true`를 설정하면 LMS가
+  `future-nav2-gazebo:humble`을 사용합니다. 기본값은 `false`입니다.
+- 시뮬레이션에서는 기존 NAV2-Gazebo에 `platform:=yahboom`을 설정하고
+  NAV2-Yahboom 차량 모듈을 함께 실행합니다. 실차 Agent/Camera는 실행하지 않습니다.
 - 앱의 야붐 실행기 지원이 반영되어야 합니다.
 - 서버 과정의 야붐 전용 환경에 이 모듈과 이미지 아티팩트를 등록한 후
  LMS에서 설치 -> 빌드 -> 실행합니다.
@@ -1,22 +1,36 @@
 import os
+import runpy
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node

 from launch_ros.parameter_descriptions import ParameterValue

-def generate_launch_description():
+def launch_vehicle(context):
+    simulation = LaunchConfiguration("sim_mode").perform(context).lower() == "true"
    share = get_package_share_directory("yahboom_vehicle")
    with open(os.path.join(share, "urdf", "yahboom.urdf"), encoding="utf-8") as stream:
        description = stream.read()
-    return LaunchDescription([
-        DeclareLaunchArgument("publish_odom_tf", default_value="true"),
+    if simulation:
+        description = runpy.run_path(os.path.join(share, "urdf", "simulation.py"))["simulation_description"](share)
+    actions = [
        Node(package="robot_state_publisher", executable="robot_state_publisher",
-             parameters=[{"robot_description": description, "use_sim_time": False}],
+             parameters=[{"robot_description": description, "use_sim_time": simulation}],
+             remappings=[("robot_description", "/yahboom/robot_description")] if simulation else [],
             output="screen"),
-        Node(package="yahboom_vehicle", executable="odom_bridge.py", output="screen",
+    ]
+    if not simulation:
+        actions.append(Node(package="yahboom_vehicle", executable="odom_bridge.py", output="screen",
             parameters=[{"publish_odom_tf": ParameterValue(
-                 LaunchConfiguration("publish_odom_tf"), value_type=bool)}]),
+                 LaunchConfiguration("publish_odom_tf"), value_type=bool)}]))
+    return actions
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument("sim_mode", default_value="false", choices=["true", "false"]),
+        DeclareLaunchArgument("publish_odom_tf", default_value="true"),
+        OpaqueFunction(function=launch_vehicle),
    ])
@@ -0,0 +1,67 @@
+"""Gazebo differential-drive approximation using the supplied vehicle geometry."""
+from pathlib import Path
+import xml.etree.ElementTree as ET
+
+
+def simulation_description(share):
+    root = ET.parse(Path(share) / "urdf" / "yahboom.urdf").getroot()
+    for mesh in root.findall(".//mesh"):
+        mesh.set("filename", (Path(share) / mesh.get("filename").split("package://yahboom_vehicle/")[1]).as_uri())
+    for joint in root.findall("joint"):
+        if joint.find("child").get("link") == "base_link":
+            joint.find("origin").set("xyz", "0 0 0.04525")
+    wheels = []
+    for link_name in ("zq_Link", "zh_Link", "yq_Link", "yh_Link"):
+        joint = next(j for j in root.findall("joint") if j.find("child").get("link") == link_name)
+        joint.set("type", "continuous")
+        axis = joint.find("axis")
+        if axis is None:
+            axis = ET.SubElement(joint, "axis")
+        axis.set("xyz", "0 1 0")
+        ET.SubElement(joint, "dynamics", damping="0.01", friction="0.0")
+        wheels.append(joint.get("name"))
+        link = root.find(f"link[@name='{link_name}']")
+        for collision in link.findall("collision"):
+            link.remove(collision)
+        collision = ET.SubElement(link, "collision")
+        ET.SubElement(collision, "origin", xyz="0 0 0", rpy="1.57079632679 0 0")
+        ET.SubElement(ET.SubElement(collision, "geometry"), "cylinder", radius="0.024", length="0.0215")
+        gazebo = ET.SubElement(root, "gazebo", reference=link_name)
+        for key, value in {"mu1": "1.0", "mu2": "0.1", "fdir1": "1 0 0"}.items():
+            ET.SubElement(gazebo, key).text = value
+    gazebo = ET.SubElement(root, "gazebo")
+    drive = ET.SubElement(gazebo, "plugin", name="yahboom_drive", filename="libgazebo_ros_diff_drive.so")
+    ros = ET.SubElement(drive, "ros")
+    for remapping in ("cmd_vel:=/cmd_vel", "odom:=/odom"):
+        ET.SubElement(ros, "remapping").text = remapping
+    for key, value in {
+        "num_wheel_pairs": "2", "update_rate": "50", "max_wheel_torque": "2",
+        "max_wheel_acceleration": "1", "publish_odom": "true", "publish_odom_tf": "true",
+        "publish_wheel_tf": "false", "odometry_frame": "odom",
+        "robot_base_frame": "base_footprint", "odometry_source": "1",
+    }.items():
+        ET.SubElement(drive, key).text = value
+    for left, right in zip(wheels[:2], wheels[2:]):
+        for key, value in {"left_joint": left, "right_joint": right,
+                           "wheel_separation": "0.135", "wheel_diameter": "0.048"}.items():
+            ET.SubElement(drive, key).text = value
+    state = ET.SubElement(gazebo, "plugin", name="yahboom_joints", filename="libgazebo_ros_joint_state_publisher.so")
+    ET.SubElement(state, "update_rate").text = "30"
+    for joint in wheels:
+        ET.SubElement(state, "joint_name").text = joint
+    laser = ET.SubElement(root, "gazebo", reference="laser_frame")
+    sensor = ET.SubElement(laser, "sensor", name="yahboom_lidar", type="ray")
+    ET.SubElement(sensor, "always_on").text = "true"
+    ET.SubElement(sensor, "update_rate").text = "10"
+    ray = ET.SubElement(sensor, "ray")
+    horizontal = ET.SubElement(ET.SubElement(ray, "scan"), "horizontal")
+    for key, value in {"samples": "720", "resolution": "1", "min_angle": "-3.14159265", "max_angle": "3.14159265"}.items():
+        ET.SubElement(horizontal, key).text = value
+    distance = ET.SubElement(ray, "range")
+    for key, value in {"min": "0.12", "max": "8.0", "resolution": "0.01"}.items():
+        ET.SubElement(distance, key).text = value
+    plugin = ET.SubElement(sensor, "plugin", name="yahboom_scan", filename="libgazebo_ros_ray_sensor.so")
+    ET.SubElement(ET.SubElement(plugin, "ros"), "remapping").text = "~/out:=/scan"
+    ET.SubElement(plugin, "output_type").text = "sensor_msgs/LaserScan"
+    ET.SubElement(plugin, "frame_name").text = "laser_frame"
+    return ET.tostring(root, encoding="unicode")