<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="standalone_no_apriltag_table">
  <xacro:arg name="x" default="0"/>
  <xacro:arg name="y" default="0"/>
  <xacro:arg name="height" default="0"/>

  <!-- 定义参数 -->
  <xacro:property name="table_height" value="$(arg height)" />  <!-- 桌子高度参数 -->
  <xacro:property name="table_thickness" value="0.04" />  <!-- 桌面厚度 -->
  <xacro:property name="leg_radius" value="0.02" />  <!-- 桌腿半径 -->
  
  <!-- 第一个桌子位置参数 -->
  <xacro:property name="table1_x" value="$(arg x)" />
  <xacro:property name="table1_y" value="$(arg y)" />
  <xacro:property name="table1_z" value="${table_height}" />
  
  <!-- 立方体位置参数/参数相对于桌子来说 -->
  <xacro:property name="cube_x" value="0.0" />
  <xacro:property name="cube_y" value="0.0" />
  <xacro:property name="cube_z" value="${table_thickness/2 + 0.05}" />
  
  <!-- 引入apriltag -->
  <xacro:include filename="$(find gazebo_world_manager)/scene/grasp-box/models/apriltag/urdf/apriltag.xacro"/>
  
  <!-- 添加一个世界链接作为根链接 -->
  <link name="world"/>
  
  <!-- 创建第一个桌子链接 -->
  <link name="table1_link">
    <visual>
      <geometry>
        <box size="0.4 0.4 ${table_thickness}"/>
      </geometry>
      <material name="wood">
        <color rgba="0.8 0.6 0.4 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.4 0.4 ${table_thickness}"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="5.0"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
  </link>
  
  <!-- 添加Gazebo物理属性 -->
  <gazebo reference="table1_link">
    <material>Gazebo/Wood</material>
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <minDepth>0.001</minDepth>
    <maxVel>1.0</maxVel>
    <maxContacts>20</maxContacts>
  </gazebo>
  
  <!-- 将世界链接与第一个桌子连接 -->
  <joint name="world_to_table1_joint" type="fixed">
    <parent link="world"/>
    <child link="table1_link"/>
    <origin xyz="${table1_x} ${table1_y} ${table1_z}" rpy="0.0 0.0 1.5707963267948966"/>
  </joint>
  
  <!-- 创建第一个桌子的四条桌腿 -->
  <xacro:macro name="table_leg" params="name x y height">
    <link name="${name}">
      <visual>
        <geometry>
          <cylinder radius="${leg_radius}" length="${height}"/>
        </geometry>
        <material name="wood">
          <color rgba="0.7 0.5 0.3 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <cylinder radius="${leg_radius}" length="${height}"/>
        </geometry>
      </collision>
      <inertial>
        <mass value="0.5"/>
        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.001"/>
      </inertial>
    </link>
    
    <gazebo reference="${name}">
      <material>Gazebo/Wood</material>
      <mu1>0.5</mu1>
      <mu2>0.5</mu2>
    </gazebo>
  </xacro:macro>
  
  <!-- 创建第一个桌子的桌腿 -->
  <xacro:table_leg name="table1_leg1" x="0.15" y="0.15" height="${table_height}"/>
  <xacro:table_leg name="table1_leg2" x="-0.15" y="0.15" height="${table_height}"/>
  <xacro:table_leg name="table1_leg3" x="0.15" y="-0.15" height="${table_height}"/>
  <xacro:table_leg name="table1_leg4" x="-0.15" y="-0.15" height="${table_height}"/>
  
  <!-- 连接第一个桌子的桌腿 -->
  <joint name="table1_to_leg1_joint" type="fixed">
    <parent link="table1_link"/>
    <child link="table1_leg1"/>
    <origin xyz="0.15 0.15 ${-table_height/2}" rpy="0 0 0"/>
  </joint>
  
  <joint name="table1_to_leg2_joint" type="fixed">
    <parent link="table1_link"/>
    <child link="table1_leg2"/>
    <origin xyz="-0.15 0.15 ${-table_height/2}" rpy="0 0 0"/>
  </joint>
  
  <joint name="table1_to_leg3_joint" type="fixed">
    <parent link="table1_link"/>
    <child link="table1_leg3"/>
    <origin xyz="0.15 -0.15 ${-table_height/2}" rpy="0 0 0"/>
  </joint>
  
  <joint name="table1_to_leg4_joint" type="fixed">
    <parent link="table1_link"/>
    <child link="table1_leg4"/>
    <origin xyz="-0.15 -0.15 ${-table_height/2}" rpy="0 0 0"/>
  </joint>
  
</robot>