<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="block">
  <xacro:include filename="$(find gazebo_world_manager)/scene/grasp-box/models/apriltag/urdf/apriltag.xacro"/>

  <!-- ===== 参数定义 ===== -->
  <xacro:arg name="box_length"   default="0.5"/>  <!-- x -->
  <xacro:arg name="box_width"    default="0.3"/>  <!-- y -->
  <xacro:arg name="box_height"   default="0.2"/>  <!-- z -->
  <xacro:arg name="box_mass"     default="1.0"/>
  <xacro:arg name="tag_id"     default="0"/>

  <!-- ===== 派生参数计算 ===== -->
  <!-- 从参数创建属性 -->
  <xacro:property name="box_length" value="$(arg box_length)"/>
  <xacro:property name="box_width" value="$(arg box_width)"/>
  <xacro:property name="box_height" value="$(arg box_height)"/>
  <xacro:property name="box_mass" value="$(arg box_mass)"/>

  <!-- 耳朵长度：箱子宽度 - 0.05 -->
  <xacro:property name="ear_length" value="${box_width - 0.05}"/>
  <!-- 耳朵偏移：箱子长度的一半 + 0.025 -->
  <xacro:property name="ear_offset" value="${box_length/2 + 0.025}"/>
  <xacro:property name="ear_width" value="0.1"/>
  <xacro:property name="ear_thickness" value="0.025"/>

  <!-- 惯性矩阵计算 -->
  <xacro:property name="ixx" value="${(1.0/12.0) * box_mass * (box_height*box_height + box_width*box_width)}"/>
  <xacro:property name="iyy" value="${(1.0/12.0) * box_mass * (box_length*box_length + box_height*box_height)}"/>
  <xacro:property name="izz" value="${(1.0/12.0) * box_mass * (box_length*box_length + box_width*box_width)}"/>

  <!-- ===== 耳朵宏定义 ===== -->
  <!-- 参数：
       side_offset：左右位置（正值右耳，负值左耳）
  -->
  <xacro:macro name="ear" params="side_offset">
    <visual>
      <geometry>
        <box size="${ear_width} ${ear_length} ${ear_thickness}"/>
      </geometry>
      <origin xyz="${side_offset} 0.0 ${box_height/2 - ear_thickness}" rpy="0 0 0"/>
      <material name="red"/>
    </visual>
    <collision>
      <geometry>
        <box size="${ear_width} ${ear_length} ${ear_thickness}"/>
      </geometry>
      <origin xyz="${side_offset} 0.0 ${box_height/2 - ear_thickness}" rpy="0 0 0"/>
      <surface>
        <contact>
          <max_contacts>10</max_contacts>
          <ode>
            <kp>1e6</kp>
            <kd>100</kd>
          </ode>
        </contact>
      </surface>
    </collision>
  </xacro:macro>

  <!-- 主体 -->
  <link name="base_link">
    <!-- 主体视觉 -->
    <visual>
      <geometry>
        <box size="${box_length} ${box_width} ${box_height}"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="red">
        <color rgba="1 0 0 1"/>
      </material>
    </visual>
    <!-- 主体碰撞 -->
    <collision>
      <geometry>
        <box size="${box_length} ${box_width} ${box_height}"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <surface>
        <contact>
          <max_contacts>10</max_contacts>
          <ode>
            <kp>1e6</kp>
            <kd>100</kd>
          </ode>
        </contact>
      </surface>
    </collision>

    <!-- 左耳（调用宏） -->
    <xacro:ear side_offset="-${ear_offset}"/>
    <!-- 右耳（调用宏） -->
    <xacro:ear side_offset="${ear_offset}"/>

    <!-- 质量惯性（自动计算） -->
    <inertial>
      <mass value="${box_mass}"/>
      <inertia ixx="${ixx}" iyy="${iyy}" izz="${izz}" ixy="0" iyz="0" ixz="0"/>
    </inertial>
  </link>

<xacro:apriltag_marker parent_link="base_link" id="$(arg tag_id)">
    <origin xyz="0 -${box_width/2 + 0.001} 0" rpy="-${pi/2} 0 0"/>
</xacro:apriltag_marker>

</robot>
