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

  <!-- 基础参数 - 使用更合理的尺寸 -->
  <xacro:property name="base_length" value="0.5" />
  <xacro:property name="base_width" value="0.3" />
  <xacro:property name="base_height" value="0.1" />
  <xacro:property name="wheel_radius" value="0.08" />
  <xacro:property name="wheel_thickness" value="0.05" />
  <xacro:property name="wheel_mass" value="1.0" />
  <xacro:property name="base_mass" value="10.0" />

  <!-- 底盘链接 -->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
      <material name="blue">
        <color rgba="0 0 1 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="${base_mass}"/>
      <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
    </inertial>
  </link>

  <!-- 轮子宏定义 - 经过验证的正确配置 -->
  <xacro:macro name="wheel" params="prefix x y">
    <link name="${prefix}_wheel">
      <visual>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
        </geometry>
        <material name="black">
          <color rgba="0.1 0.1 0.1 1"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
        </geometry>
      </collision>
      <inertial>
        <mass value="${wheel_mass}"/>
        <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.05" iyz="0" izz="0.05"/>
      </inertial>
    </link>
    
    <joint name="${prefix}_wheel_joint" type="continuous">
      <parent link="base_link"/>
      <child link="${prefix}_wheel"/>
      <!-- 
        经过验证的正确配置：
        - 轮子位置：在车身外侧，底部接触地面
        - 旋转：轮子围绕平行于Y轴的轴线旋转
        - 方向：圆柱体侧面接触地面（rpy="1.57 0 0" 让圆柱体躺倒）
      -->
      <origin xyz="${x} ${y} -${(base_height/2)}" rpy="1.57 0 0"/>
      <axis xyz="0 0 1"/>
    </joint>
  </xacro:macro>

  <!-- 创建四个车轮 -->
  <xacro:wheel prefix="left_front" x="${base_length/4}" y="${base_width/2 + wheel_thickness/2}"/>
  <xacro:wheel prefix="right_front" x="${base_length/4}" y="-${base_width/2 + wheel_thickness/2}"/>
  <xacro:wheel prefix="left_rear" x="-${base_length/4}" y="${base_width/2 + wheel_thickness/2}"/>
  <xacro:wheel prefix="right_rear" x="-${base_length/4}" y="-${base_width/2 + wheel_thickness/2}"/>

  <!-- IMU传感器 -->
  <link name="imu_link">
    <visual>
      <geometry>
        <box size="0.02 0.02 0.02"/>
      </geometry>
      <material name="red">
        <color rgba="1 0 0 1"/>
      </material>
    </visual>
    <inertial>
      <mass value="0.01"/>
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
    </inertial>
  </link>

  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin xyz="0 0 ${base_height/2}" rpy="0 0 0"/>
  </joint>

  <!-- Gazebo插件 -->
  <gazebo reference="base_link">
    <material>Gazebo/Blue</material>
  </gazebo>

  <gazebo reference="left_front_wheel">
    <material>Gazebo/Black</material>
  </gazebo>
  <gazebo reference="right_front_wheel">
    <material>Gazebo/Black</material>
  </gazebo>
  <gazebo reference="left_rear_wheel">
    <material>Gazebo/Black</material>
  </gazebo>
  <gazebo reference="right_rear_wheel">
    <material>Gazebo/Black</material>
  </gazebo>

  <gazebo reference="imu_link">
    <material>Gazebo/Red</material>
  </gazebo>

  <!-- Gazebo ROS控制插件 -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/</robotNamespace>
    </plugin>
  </gazebo>

  <!-- 差分驱动控制器 - 使用所有四个轮子 -->
  <gazebo>
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <robotNamespace>/</robotNamespace>
      <alwaysOn>true</alwaysOn>
      <updateRate>50</updateRate>
      <leftJoint>left_front_wheel_joint</leftJoint>
      <rightJoint>right_front_wheel_joint</rightJoint>
      <wheelSeparation>${base_width + wheel_thickness}</wheelSeparation>
      <wheelDiameter>${2*wheel_radius}</wheelDiameter>
      <wheelTorque>20</wheelTorque>
      <commandTopic>cmd_vel</commandTopic>
      <odometryTopic>odom</odometryTopic>
      <odometryFrame>odom</odometryFrame>
      <robotBaseFrame>base_link</robotBaseFrame>
      <publishOdomTF>true</publishOdomTF>
      <publishWheelTF>true</publishWheelTF>
      <publishWheelJointState>true</publishWheelJointState>
    </plugin>
  </gazebo>

  <!-- IMU Gazebo插件 -->
  <gazebo reference="imu_link">
    <sensor name="imu_sensor" type="imu">
      <always_on>true</always_on>
      <update_rate>100</update_rate>
      <visualize>true</visualize>
      <topic>imu/data</topic>
      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
        <robotNamespace>/</robotNamespace>
        <topicName>imu/data</topicName>
        <bodyName>imu_link</bodyName>
        <updateRateHZ>100.0</updateRateHZ>
        <gaussianNoise>0.0001</gaussianNoise>
        <xyzOffset>0 0 0</xyzOffset>
        <rpyOffset>0 0 0</rpyOffset>
        <frameName>imu_link</frameName>
      </plugin>
    </sensor>
  </gazebo>

</robot>