<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="qykj_ackm">
  <!-- set param -->
  <xacro:property name="base_color" value="1 0 0 1" />
  <xacro:property name="wheel_color" value="0 0 0 1" />
  <xacro:property name="sensor_color" value="0 0 1 1" />
  <xacro:property name="camera_color" value="0 0 1 1" />
  <!-- Define meshes -->
  <xacro:property name="base_mesh" value="./meshes/base_link.STL" />
  <xacro:property name="left_wheel_mesh" value="./meshes/left_wheel.STL" />
  <xacro:property name="left_rear_wheel_mesh" value="./meshes/left_rear_wheel.STL" />
  <xacro:property name="right_wheel_mesh" value="./meshes/right_wheel.STL" />
  <xacro:property name="right_rear_wheel_mesh" value="./meshes/right_rear_wheel.STL" />
  <xacro:property name="laser_mesh" value="./meshes/laser.STL" />
  <xacro:property name="camera_mesh" value="./meshes/camera.STL" />

  <!-- base_link and base_footprint -->
  <xacro:macro name="base_link" params="mesh color">
    <link name="base_link">
      <inertial>
        <origin xyz="-0.0174221592416108 0 0.0010674027636553" rpy="0 0 0" />
        <!-- <mass value="3.18924071684633" /> -->
        <mass value="2.0946205" />
        <inertia ixx="0.00725227532564324" ixy="1.826574180869E-05" ixz="1.74522061015835E-05" iyy="0.0112392911749803" iyz="-1.16120716894918E-06" izz="0.0572118967643173" />
      </inertial>
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="${mesh}" />
        </geometry>
        <material name="base_material">
          <color rgba="${color}" />
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="${mesh}" />
        </geometry>
      </collision>
    </link>
    <!-- Base footprint -->
    <link name="base_footprint">
      <visual>
        <geometry>
          <sphere radius="0.001" />
        </geometry>
      </visual>
    </link>
    <joint name="base_link2base_footprint" type="fixed">
      <parent link="base_footprint" />
      <child link="base_link" />
      <origin xyz="0 0 0.055" rpy="0 0 0" />
    </joint>
  </xacro:macro>

  <!-- front wheel steering -->
 <xacro:macro name="steering_hinge" params="prefix color mass_center position rotation">
  <link name="${prefix}_steering_hinge">
    <inertial>
      <origin xyz="${mass_center}" rpy="0 0 0" />
      <mass value="0.085506993927856" />
      <inertia ixx="0.00342027975711424" 
               ixy="-1.72810689862337E-03" 
               ixz="2.68361824483564E-03" 
               iyy="0.00342027975711424" 
               iyz="1.65092622583439E-04" 
               izz="0.00342027975711424" />
      <!-- <inertia ixx="0.00000000342027975711424" ixy="-1.72810689862337E-07" ixz="2.68361824483564E-07" iyy="0.0000000342027975711424" iyz="1.65092622583439E-08" izz="0.0000000342027975711424" /> -->
      
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <sphere radius="0.005"/> <!-- 尝试增大半径 -->
      </geometry>
      <material name="steering_hinge_material">
        <color rgba="${color}" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <sphere radius="0.005"/> <!-- 尝试增大半径 -->
      </geometry>
    </collision>
  </link>

  <joint name="${prefix}_steering_hinge_joint" type="revolute">
    <origin xyz="${position}" rpy="${rotation}"/>
    <parent link="base_link"/>
    <child link="${prefix}_steering_hinge"/>
    <axis xyz="0 0 1" />
    <limit lower="-0.5" upper="0.5" effort="8" velocity="800" />
    <damping>0.5</damping> <!-- 增加阻尼 -->
  </joint>
 </xacro:macro> 
 
  <!-- front wheel -->
  <xacro:macro name="wheel" params="prefix mesh mass_center color position">
    <link name="${prefix}_wheel">
      <inertial>    
        <origin xyz="${mass_center}" rpy="0 0 0" />
        <mass value="0.083615304385799" />
        <!--qian <inertia ixx="0.00034504118176367" ixy="-1.05210123445392E-10" ixz="1.09117751056064E-10" iyy="0.000617238444498718" iyz="-3.52033611009814E-09" izz="0.00345048952965297" /> -->
        <!-- <inertia ixx="0.000645041181644452" ixy="-1.02836030755788E-5" ixz="1.03858733509779E-10" iyy="0.00617238444547154" iyz="-3.52038618447715E-09" izz="0.00064504895313491" /> -->
        <inertia ixx="0.00010451913048224875" 
                 ixy="-1.02836030755788E-5" 
                 ixz="1.03858733509779E-10" 
                 iyy="0.000261297820954496875" 
                 iyz="-3.52038618447715E-09" 
                 izz="0.00010451913048224875" />
      </inertial>
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="${mesh}" />
        </geometry>
        <material name="wheel_material">
          <color rgba="${color}" />
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="${mesh}"/>
        </geometry>
      </collision>
    </link>
    <joint name="${prefix}_wheel_joint" type="continuous">
      <origin xyz="${position}" rpy="0 0 0" />
      <parent link="${prefix}_steering_hinge" />
      <child link="${prefix}_wheel" />
      <axis xyz="0 1 0" />
      <limit effort="6" velocity="800" />
      <!-- <limit lower="-3.14159" upper="3.14159" effort="8" velocity="800" /> -->
    </joint> 
  </xacro:macro>

  <!-- rear wheel -->
  <xacro:macro name="rear_wheel" params="prefix mesh mass_center color position">
    <link name="${prefix}_rear_wheel">
      <inertial>
        <origin xyz="${mass_center}" rpy="0 0 0" />
        <mass value="0.083615304385799" />
        <inertia ixx="0.000645041181644452" ixy="-1.02836030755788E-5" ixz="-1.03858733509779E-10" iyy="0.00617238444547154" iyz="3.52038618447715E-09" izz="0.00064504895313491" />
      </inertial>
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="${mesh}" />
        </geometry>
        <material name="wheel_material">
          <color rgba="${color}" />
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="${mesh}" />
        </geometry>
      </collision>
    </link>
    <joint name="${prefix}_rear_wheel_joint" type="continuous">
      <origin xyz="${position}" rpy="0 0 0" />
      <parent link="base_link" />
      <child link="${prefix}_rear_wheel" />
      <axis xyz="0 1 0" /> 
      <limit lower="-3.14159" upper="3.14159" effort="10" velocity="800" />
    </joint>
  </xacro:macro> 

  <!-- laser -->
  <xacro:macro name="laser" params="name mesh color position">
    <link name="${name}">
      <inertial>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <mass value="0.136175408631878" />
        <inertia ixx="5.05126026373552E-05" ixy="1.31150574294136E-08" ixz="1.65013289478E-06" iyy="7.08926003407945E-05" iyz="2.49898507713873E-09" izz="0.00010333009695655" />
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
         <mesh filename="${mesh}" />
       </geometry>
       <material name="sensor_material">
          <color rgba="${color}" />
       </material>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="${mesh}" />
        </geometry>
      </collision>
    </link>
    <joint name="${name}_joint" type="fixed">
      <origin xyz="${position}" rpy="0 0 0" />
      <parent link="base_link" />
      <child link="${name}" />
      <axis xyz="0 0 0" />
    </joint>
  </xacro:macro>
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     
  <!-- Camera -->
  <xacro:macro name="camera" params="name mesh color position">
    <link name="${name}">
      <inertial>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <mass value="0.128875262431144" />
        <inertia ixx="0.000226098241375409" ixy="-7.63151331329159E-09" ixz="-6.02732800767945E-07" iyy="1.50549325275966E-05" iyz="-7.62743002721805E-10" izz="0.000225285047549157" />
      </inertial>
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="${mesh}" />
        </geometry>
        <material name="camera_material">
          <color rgba="${color}" />
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="${mesh}" />
        </geometry>
      </collision>
    </link>
    <joint name="${name}_joint" type="fixed">
      <origin xyz="${position}" rpy="0 0 0" />
      <parent link="base_link" />
      <child link="${name}" />
      <axis xyz="0 0 0" />
    </joint>
  </xacro:macro>

  <!--  add imu link   -->
  <xacro:macro name="imu" params="name">
    <link name="${name}">
      <visual>
        <geometry>
          <box size="0.005 0.005 0.007"/>
        </geometry>
      </visual>
    </link> 
    <joint name ="imu_joints" type="fixed">
      <origin xyz="0.0 0.0 0.0035" rpy="0.0 0.0 0.0"/>
      <parent link="base_link"/>
      <child link="${name}"/>
    </joint>  
  </xacro:macro>

  <!-- create car body -->
  <xacro:base_link mesh="${base_mesh}" color="${base_color}" />
  
  <!-- create front wheel steering -->
  <xacro:steering_hinge prefix="left" color="${wheel_color}" mass_center="0 0 0" position="0.17831 0.10435 0.0070129" rotation="0 0 0" />
  <xacro:steering_hinge prefix="right" color="${wheel_color}" mass_center="0 0 0" position="0.17831 -0.10435 0.0070129" rotation="0 0 0" />
  
  <!-- create front wheel -->
  <xacro:wheel prefix="left" mesh="${left_wheel_mesh}" mass_center="0 -0.01 0" color="${wheel_color}" position="0 0.074 0" />
  <xacro:wheel prefix="right" mesh="${right_wheel_mesh}" mass_center="0 0.01 0" color="${wheel_color}" position="0 -0.074 0" />
 
  <!-- create rear wheel -->
  <xacro:rear_wheel prefix="left" mesh="${left_rear_wheel_mesh}" mass_center="0 -0.01 0" color="${wheel_color}" position="-0.11072 0.18046 0.0099082" />
  <xacro:rear_wheel prefix="right" mesh="${right_rear_wheel_mesh}" mass_center="0 0.01 0" color="${wheel_color}" position="-0.11072 -0.18046 0.0099082" />
  
  <!-- create camera -->
  <xacro:camera name="camera" mesh="${camera_mesh}" color="${camera_color}" position="0.21669 -0.00063349 0.142" />

  <!-- create laser -->
  <xacro:laser name="laser" mesh="${laser_mesh}" color="${sensor_color}" position="0.092303 5.3876E-05 0.2031" />
  
  <!-- imu -->
  <xacro:imu name="imu"/>
   
</robot>