<?xml version="1.0"?>
<robot name="box_bot" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <!-- 参数-->
    <xacro:property name="pi" value="3.14159265359" />
    <!-- 车子长度 -->
    <xacro:property name="base_l" value="0.40" />
    <!-- 车子宽度 -->
    <xacro:property name="base_w" value="0.30" />
    <!-- 车子高度 -->
    <xacro:property name="base_h" value="0.12" />
    <!-- 车子质量 -->
    <xacro:property name="base_mass" value="5.0" />

    <!-- 轮子半径 -->
    <xacro:property name="wheel_r" value="0.05" />
    <!-- 轮子宽度 -->
    <xacro:property name="wheel_w" value="0.03" />
    <!-- 轮子质量 -->
    <xacro:property name="wheel_mass" value="0.3" />

    <!-- base_link-->
    <link name="base_link">
        <visual>
            <origin xyz="0.0 0.0 ${base_h/2}" rpy="0.0 0.0 0.0" />
            <geometry>
                <box size="${base_l} ${base_w} ${base_h}" />
            </geometry>
            <material name="Gray">
                <color rgba="0.5 0.5 0.5 1.0" />
            </material>
        </visual>

        <collision>
            <origin xyz="0.0 0.0 ${base_h/2}" rpy="0.0 0.0 0.0" />
            <geometry>
                <box size="${base_l} ${base_w} ${base_h}" />
            </geometry>
        </collision>

        <!-- 简化惯性 -->
        <inertial>
            <origin xyz="0 0 ${base_h/2}" rpy="0 0 0" />
            <mass value="${base_mass}" />
            <!-- 盒子惯性：Ixx = 1/12 m (y^2 + z^2) 等 -->
            <inertia
                ixx="${(1.0/12.0)*base_mass*(base_w*base_w + base_h*base_h)}"
                iyy="${(1.0/12.0)*base_mass*(base_l*base_l + base_h*base_h)}"
                izz="${(1.0/12.0)*base_mass*(base_l*base_l + base_w*base_w)}"
                ixy="0" ixz="0" iyz="0" />
        </inertial>
    </link>

    <!-- 轮子宏,生成wheel_link和wheel_joint -->
    <xacro:macro name="wheel" params="name x y z">
        <link name="${name}_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="1.5708 0.0 0.0" />
                <geometry>
                    <cylinder radius="${wheel_r}" length="${wheel_w}" />
                </geometry>
                <material name="Black">
                    <color rgba="0.1 0.1 0.1 1.0" />
                </material>
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
                <geometry>
                    <cylinder radius="${wheel_r}" length="${wheel_w}" />
                </geometry>
            </collision>

            <inertial>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <mass value="${wheel_mass}" />
                <!-- 圆柱体惯性：Ixx = Iyy = (1/12) m (3r^2 + h^2) 等 -->
                <inertia ixx="0.001" iyy="0.001" izz="0.001" ixy="0" ixz="0" iyz="0" />
            </inertial>
        </link>

        <joint name="${name}_joint" type="continuous">
            <parent link="base_link" />
            <child link="${name}_link" />
            <origin xyz="${x} ${y} ${z}" rpy="0.0 0.0 0.0" />
            <!-- 轮子绕y轴旋转 -->
            <axis xyz="0.0 1.0 0.0" />
        </joint>
    </xacro:macro>

    <!-- 生成四个轮子 -->
    <xacro:wheel name="front_left_wheel" x="${base_l/2 - wheel_r}" y="${base_w/2}" z="${wheel_r}" />
    <xacro:wheel name="front_right_wheel" x="${base_l/2 - wheel_r}" y="-${base_w/2}" z="${wheel_r}" />
    <xacro:wheel name="rear_left_wheel" x="-${base_l/2 + wheel_r}" y="${base_w/2}" z="${wheel_r}" />
    <xacro:wheel name="rear_right_wheel" x="-${base_l/2 + wheel_r}" y="-${base_w/2}" z="${wheel_r}" />


    <!-- 雷达支架 -->
    <!-- 通常放到轮心高度 -->
    <link name="lidar_mount_link">
        <visual>
            <origin xyz="0.0 0.0 ${base_h+0.05}" rpy="0.0 0.0 0.0" />
            <geometry>
                <box size="0.05 0.05 0.10" />
            </geometry>

            <material name="Blue">
                <color rgba="0.2 0.2 0.8 1.0" />
            </material>
        </visual>

        <collision>
            <origin xyz="0.0 0.0 ${base_h+0.05}" rpy="0.0 0.0 0.0" />
            <geometry>
                <box size="0.05 0.05 0.10" />
            </geometry>
        </collision>

        <!-- 简化惯性 -->
        <inertial>
            <origin xyz="0 0 ${base_h + 0.05}" rpy="0 0 0" />
            <mass value="0.2" />
            <inertia ixx="0.0001" iyy="0.0001" izz="0.0001" ixy="0" ixz="0" iyz="0" />
        </inertial>
    </link>

    <joint name="lidar_mount_joint" type="fixed">
        <parent link="base_link" />
        <child link="lidar_mount_link" />
        <!-- 前方: x正方向 -->
        <origin xyz="${base_l/2 - 0.05} 0 0" rpy="0.0 0.0 0.0" />
    </joint>

    <!-- 四轮驱动插件(skid steer) -->
    <gazebo>
        <plugin name="skid_steer_drive" filename="libgazebo_ros_skid_steer_drive.so">
            <ros>
                <namespace>/</namespace>
            </ros>

            <!-- cmd_vel 输入 -->
            <commandTopic>cmd_vel</commandTopic>

            <!-- 里程计 -->
            <odometryTopic>odom</odometryTopic>
            <odometryFrame>odom</odometryFrame>
            <robotBaseFrame>base_link</robotBaseFrame>

            <!-- 四个轮子关节命名 -->
            <leftFrontWheelJoint>front_left_wheel_joint</leftFrontWheelJoint>
            <rightFrontWheelJoint>front_right_wheel_joint</rightFrontWheelJoint>
            <leftRearWheelJoint>rear_left_wheel_joint</leftRearWheelJoint>
            <rightRearWheelJoint>rear_right_wheel_joint</rightRearWheelJoint>

            <!-- 几何参数 -->
            <wheelSeparation>${base_w}</wheelSeparation>
            <wheelDiameter>${2*wheel_r}</wheelDiameter>

            <!-- 可调 -->
            <wheelTorque>5.0</wheelTorque>
            <updateRate>50</updateRate>
        </plugin>
    </gazebo>

</robot>