<?xml version="1.0"?>
<robot name="my_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <!-- 定义常量 -->
  <xacro:property name="M_PI" value="3.141592653589793"/>
  
  <!-- 机器人底座 -->
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.1" radius="0.2"/>
      </geometry>
      <material name="blue">
        <color rgba="0 0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.1" radius="0.2"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="10.0"/>
      <inertia ixx="0.2" ixy="0.0" ixz="0.0" iyy="0.2" iyz="0.0" izz="0.2"/>
    </inertial>
  </link>
  
  <!-- 轮子 -->
  <xacro:macro name="wheel" params="prefix x y z">
    <link name="${prefix}_wheel_link">
      <visual>
        <geometry>
          <cylinder length="0.05" radius="0.075"/>
        </geometry>
        <material name="gray">
          <color rgba="0.5 0.5 0.5 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <cylinder length="0.05" radius="0.075"/>
        </geometry>
      </collision>
      <inertial>
        <mass value="1.0"/>
        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.002"/>
      </inertial>
    </link>
    
    <joint name="${prefix}_wheel_joint" type="continuous">
      <origin xyz="${x} ${y} ${z}" rpy="0 ${M_PI/2} 0"/>
      <parent link="base_link"/>
      <child link="${prefix}_wheel_link"/>
      <axis xyz="0 1 0"/>
      <dynamics damping="0.1" friction="0.01"/>
    </joint>
  </xacro:macro>
  
  <!-- 创建四个轮子 -->
  <xacro:wheel prefix="front_left" x="0.15" y="0.2" z="-0.05"/>
  <xacro:wheel prefix="front_right" x="0.15" y="-0.2" z="-0.05"/>
  <xacro:wheel prefix="rear_left" x="-0.15" y="0.2" z="-0.05"/>
  <xacro:wheel prefix="rear_right" x="-0.15" y="-0.2" z="-0.05"/>
  
  <!-- 激光雷达 -->
  <link name="laser_link">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.05"/>
      </geometry>
      <material name="yellow">
        <color rgba="1.0 1.0 0 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.05"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0002"/>
    </inertial>
  </link>
  
  <joint name="laser_joint" type="fixed">
    <origin xyz="0 0 0.15" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="laser_link"/>
  </joint>
  
  <!-- Gazebo插件 -->
  <!-- 差速驱动插件 -->
  <gazebo>
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <ros>
        <namespace>/</namespace>
        <param name="cmd_vel_topic">cmd_vel</param>
        <param name="odom_topic">odom</param>
        <param name="odom_frame">odom</param>
        <param name="base_frame">base_link</param>
      </ros>
      <wheel_separation>0.4</wheel_separation>
      <wheel_diameter>0.15</wheel_diameter>
      <update_rate>100.0</update_rate>
      <publish_tf>1</publish_tf>
      <odometry_frame>odom</odometry_frame>
      <odometry_topic>odom</odometry_topic>
      <robot_base_frame>base_link</robot_base_frame>
    </plugin>
  </gazebo>
  
  <!-- 激光雷达插件 -->
  <gazebo reference="laser_link">
    <sensor type="ray" name="laser_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>true</visualize>
      <update_rate>20.0</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>360</samples>
            <resolution>1</resolution>
            <min_angle>-3.141592653589793</min_angle>
            <max_angle>3.141592653589793</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.1</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_laser" filename="libgazebo_ros_laser.so">
        <ros>
          <namespace>/</namespace>
          <param name="topic">scan</param>
        </ros>
        <frameName>laser_link</frameName>
        <topicName>scan</topicName>
        <visualize>true</visualize>
        <updateRateHZ>20.0</updateRateHZ>
      </plugin>
    </sensor>
  </gazebo>
</robot>