diff --git a/README.md b/README.md index 44dfd05..fe4526b 100644 --- a/README.md +++ b/README.md @@ -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에서 설치 -> 빌드 -> 실행합니다. diff --git a/src/yahboom_vehicle/launch/vehicle.launch.py b/src/yahboom_vehicle/launch/vehicle.launch.py index 3efe3d4..242411f 100644 --- a/src/yahboom_vehicle/launch/vehicle.launch.py +++ b/src/yahboom_vehicle/launch/vehicle.launch.py @@ -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), ]) diff --git a/src/yahboom_vehicle/urdf/simulation.py b/src/yahboom_vehicle/urdf/simulation.py new file mode 100644 index 0000000..70a3388 --- /dev/null +++ b/src/yahboom_vehicle/urdf/simulation.py @@ -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")