<?xml version="1.0"?>
<robot name="fori_robot">

  <!-- 재질 정의 -->
  <material name="blue"><color rgba="0 0 0.8 1"/></material>
  <material name="black"><color rgba="0 0 0 1"/></material>
  <material name="grey"><color rgba="0.5 0.5 0.5 1"/></material>
  <material name="dark_gray"><color rgba="0.2 0.2 0.2 1.0"/></material>

  <!-- ============================================================
       Root: aft_mapped (LiDAR body frame)
       FAST-LIVO2가 camera_init → aft_mapped 를 동적으로 발행.
       ============================================================ -->
  <link name="aft_mapped"/>

  <!-- Livox MID-360 시각화 -->
  <link name="lidar_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry><box size="0.07 0.07 0.04"/></geometry>
      <material name="dark_gray"/>
    </visual>
  </link>
  <joint name="aft_mapped_to_lidar" type="fixed">
    <parent link="aft_mapped"/>
    <child  link="lidar_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>

  <!-- Hikrobot 카메라 (캘리브레이션 결과) -->
  <link name="camera_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry><box size="0.05 0.09 0.04"/></geometry>
      <material name="blue"/>
    </visual>
  </link>
  <joint name="aft_mapped_to_camera" type="fixed">
    <parent link="aft_mapped"/>
    <child  link="camera_link"/>
    <origin xyz="0.128964 -0.026998 -0.043195"
            rpy="-1.529526 -1.293497 3.122599"/>
  </joint>

  <link name="camera_optical_link"/>
  <joint name="camera_to_optical" type="fixed">
    <parent link="camera_link"/>
    <child  link="camera_optical_link"/>
    <origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
  </joint>

  <!-- ============================================================
       Base Link: 실측값 적용
       라이다(aft_mapped) 기준: x=-269.53mm, z=-412.9633mm
       rpy pitch=-0.277309rad: 라이다 pitch -15.89° 보정 → 수평
       ============================================================ -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0.143" rpy="0 0 0"/>
      <geometry><box size="0.631 0.410 0.286"/></geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <origin xyz="0 0 0.143" rpy="0 0 0"/>
      <geometry><box size="0.631 0.410 0.286"/></geometry>
    </collision>
    <inertial>
      <mass value="20.0"/>
      <inertia ixx="0.5" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1.0"/>
    </inertial>
  </link>

  <joint name="aft_mapped_to_base" type="fixed">
    <parent link="aft_mapped"/>
    <child  link="base_link"/>
    <origin xyz="-0.26953 0 -0.41296" rpy="0 -0.277309 0"/>
  </joint>

  <!-- Base Footprint: 실제 지면(Ground) 위치 -->
  <link name="base_footprint"/>
  <joint name="base_to_footprint" type="fixed">
    <parent link="base_link"/>
    <child  link="base_footprint"/>
    <origin xyz="0 0 -0.0639" rpy="0 0 0"/>
  </joint>

  <!-- ============================================================
       휠 및 차축: base_link 기준 (변경 없음)
       ============================================================ -->

  <!-- 1. Front Left -->
  <link name="front_left_axle">
    <visual>
      <origin xyz="0 0 0" rpy="1.5707 0 0"/>
      <geometry><cylinder radius="0.025" length="0.035"/></geometry>
      <material name="grey"/>
    </visual>
  </link>
  <joint name="front_left_axle_joint" type="fixed">
    <parent link="base_link"/>
    <child  link="front_left_axle"/>
    <origin xyz="0.187 0.2225 0.0631" rpy="0 0 0"/>
  </joint>

  <link name="front_left_wheel">
    <visual>
      <origin xyz="0 0 0" rpy="1.5707 0 0"/>
      <geometry><cylinder radius="0.127" length="0.08"/></geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="1.5707 0 0"/>
      <geometry><cylinder radius="0.127" length="0.08"/></geometry>
    </collision>
  </link>
  <joint name="front_left_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child  link="front_left_wheel"/>
    <origin xyz="0.187 0.28 0.0631" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

  <!-- 2. Front Right -->
  <link name="front_right_axle">
    <visual>
      <origin xyz="0 0 0" rpy="1.5707 0 0"/>
      <geometry><cylinder radius="0.025" length="0.035"/></geometry>
      <material name="grey"/>
    </visual>
  </link>
  <joint name="front_right_axle_joint" type="fixed">
    <parent link="base_link"/>
    <child  link="front_right_axle"/>
    <origin xyz="0.187 -0.2225 0.0631" rpy="0 0 0"/>
  </joint>

  <link name="front_right_wheel">
    <visual>
      <origin xyz="0 0 0" rpy="1.5707 0 0"/>
      <geometry><cylinder radius="0.127" length="0.08"/></geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="1.5707 0 0"/>
      <geometry><cylinder radius="0.127" length="0.08"/></geometry>
    </collision>
  </link>
  <joint name="front_right_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child  link="front_right_wheel"/>
    <origin xyz="0.187 -0.28 0.0631" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

  <!-- 3. Rear Left -->
  <link name="rear_left_axle">
    <visual>
      <origin xyz="0 0 0" rpy="1.5707 0 0"/>
      <geometry><cylinder radius="0.025" length="0.035"/></geometry>
      <material name="grey"/>
    </visual>
  </link>
  <joint name="rear_left_axle_joint" type="fixed">
    <parent link="base_link"/>
    <child  link="rear_left_axle"/>
    <origin xyz="-0.187 0.2225 0.0631" rpy="0 0 0"/>
  </joint>

  <link name="rear_left_wheel">
    <visual>
      <origin xyz="0 0 0" rpy="1.5707 0 0"/>
      <geometry><cylinder radius="0.127" length="0.08"/></geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="1.5707 0 0"/>
      <geometry><cylinder radius="0.127" length="0.08"/></geometry>
    </collision>
  </link>
  <joint name="rear_left_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child  link="rear_left_wheel"/>
    <origin xyz="-0.187 0.28 0.0631" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

  <!-- 4. Rear Right -->
  <link name="rear_right_axle">
    <visual>
      <origin xyz="0 0 0" rpy="1.5707 0 0"/>
      <geometry><cylinder radius="0.025" length="0.035"/></geometry>
      <material name="grey"/>
    </visual>
  </link>
  <joint name="rear_right_axle_joint" type="fixed">
    <parent link="base_link"/>
    <child  link="rear_right_axle"/>
    <origin xyz="-0.187 -0.2225 0.0631" rpy="0 0 0"/>
  </joint>

  <link name="rear_right_wheel">
    <visual>
      <origin xyz="0 0 0" rpy="1.5707 0 0"/>
      <geometry><cylinder radius="0.127" length="0.08"/></geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="1.5707 0 0"/>
      <geometry><cylinder radius="0.127" length="0.08"/></geometry>
    </collision>
  </link>
  <joint name="rear_right_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child  link="rear_right_wheel"/>
    <origin xyz="-0.187 -0.28 0.0631" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

</robot>
