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

  <!-- ============================================================
       Root: aft_mapped
       FAST-LIVO2가 camera_init → aft_mapped 를 동적으로 발행.
       이 링크 자체는 LiDAR body frame.
       ============================================================ -->
  <link name="aft_mapped"/>

  <!-- ============================================================
       Livox MID-360 시각화 (aft_mapped 원점에 위치)
       실제 크기: 직경 65mm, 높이 38mm → 박스로 근사
       ============================================================ -->
  <link name="lidar_link">
    <visual>
      <geometry>
        <box size="0.07 0.07 0.04"/>
      </geometry>
      <material name="dark_gray">
        <color rgba="0.2 0.2 0.2 1.0"/>
      </material>
    </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>

  <!-- ============================================================
       로봇 샤시 박스 (임시 크기 — 실측 후 xyz/size 수정)
       parent: aft_mapped 기준 z=-0.25m (라이다 아래 250mm, 바닥 수평)
       size: 가로 x 세로 x 높이 (m)
       ============================================================ -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.40 0.30 0.15"/>
      </geometry>
      <material name="light_gray">
        <color rgba="0.7 0.7 0.7 0.6"/>
      </material>
    </visual>
  </link>

  <joint name="aft_mapped_to_base" type="fixed">
    <parent link="aft_mapped"/>
    <child  link="base_link"/>
    <!-- 라이다 pitch -15.89° 보정 → base_link 바닥 수평
         rpy pitch = +15.89° = 0.2773 rad 반대 방향 적용 -->
    <origin xyz="0 0 -0.25" rpy="0 -0.277309 0"/>
  </joint>

  <!-- ============================================================
       Hikrobot 카메라 (캘리브레이션 결과 적용)
       camera_link: REP-103 (x=전방, y=왼쪽, z=위)
       ============================================================ -->
  <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">
        <color rgba="0.2 0.4 0.8 1.0"/>
      </material>
    </visual>
  </link>

  <joint name="aft_mapped_to_camera" type="fixed">
    <parent link="aft_mapped"/>
    <child  link="camera_link"/>
    <!-- calib.json T_lidar_camera 역변환 결과 -->
    <origin xyz="0.128964 -0.026998 -0.043195"
            rpy="-1.529526 -1.293497 3.122599"/>
  </joint>

  <!-- ============================================================
       Camera optical frame (ROS 표준)
       x=오른쪽, y=아래, z=전방
       ============================================================ -->
  <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>

</robot>
