<?xml version="1.0"?>
<!--
  saltybot.urdf.xacro  —  SaltyBot robot description

  Dimensions sourced from chassis/chassis_frame.scad and
  chassis/ASSEMBLY.md (Rev A 2026-02-28).

  REP-105 conventions: x=forward, y=left, z=up.

  TF tree:
    base_footprint
    └── base_link  (z = axle_height = 0.310m)
        ├── wheel_left_link    (y = +0.300m, continuous)
        ├── wheel_right_link   (y = −0.300m, continuous)
        ├── imu_link           (FC/MPU-6000, x=+0.050m, z=+0.012m)
        ├── stem_link          (visual: 38.1mm EMT, 1.050m tall)
        └── sensor_head_link   (top of stem, z = +1.050m)
            ├── laser          (RPLIDAR A1M8, z=+0.036m — matches slam_toolbox)
            ├── camera_link    (RealSense D435i, x=+0.050m — matches realsense2_camera)
            ├── camera_front_link  (IMX219, x=+0.050m, 10° down)
            ├── camera_left_link   (IMX219, y=+0.050m, 10° down)
            ├── camera_rear_link   (IMX219, x=-0.050m, 10° down)
            └── camera_right_link  (IMX219, y=-0.050m, 10° down)

  Note: camera_front_link/left/rear/right match positions defined in
  saltybot_cameras/launch/camera_tf.launch.py — that file may be retired
  once this URDF is published by robot_state_publisher.
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="saltybot">

  <!-- ═══════════════════════════════════════════════════════════════════
       Properties  (SI: metres, kg, radians)
       Update saltybot_properties.yaml when hardware dimensions change.
       ═══════════════════════════════════════════════════════════════════ -->
  <xacro:property name="pi"  value="3.14159265358979"/>

  <!-- Drivetrain -->
  <xacro:property name="wheel_separation" value="0.600"/>   <!-- WHEELBASE 600mm -->
  <xacro:property name="wheel_radius"     value="0.1015"/>  <!-- 203mm tire ∅ / 2 -->
  <xacro:property name="wheel_width"      value="0.065"/>   <!-- hub motor width   -->

  <!-- Body -->
  <xacro:property name="axle_height"    value="0.310"/>    <!-- AXLE_HEIGHT 310mm -->
  <xacro:property name="deck_length"    value="0.640"/>
  <xacro:property name="deck_width"     value="0.220"/>
  <xacro:property name="deck_thickness" value="0.006"/>    <!-- DECK_THICKNESS 6mm -->

  <!-- Stem (38.1mm EMT, ~1050mm cut) -->
  <xacro:property name="stem_od"     value="0.0381"/>
  <xacro:property name="stem_height" value="1.050"/>

  <!-- Sensor head -->
  <xacro:property name="sensor_head_collar_h" value="0.036"/>  <!-- COL_H 36mm -->
  <xacro:property name="sensor_arm_r"         value="0.050"/>  <!-- ARM_R 50mm -->
  <xacro:property name="cam_tilt"             value="0.1745"/> <!-- 10° in rad -->

  <!-- FC / IMU offsets
       fc_x = -50mm SCAD (front = -X SCAD = +X ROS);
       z = deck_thickness/2 + mounting_pad(3mm) + standoff(6mm) = 12mm -->
  <xacro:property name="imu_x" value="0.050"/>
  <xacro:property name="imu_y" value="0.000"/>
  <xacro:property name="imu_z" value="0.012"/>

  <!-- Mass estimates -->
  <xacro:property name="chassis_mass" value="3.0"/>
  <xacro:property name="wheel_mass"   value="1.5"/>

  <!-- ═══════════════════════════════════════════════════════════════════
       Materials
       ═══════════════════════════════════════════════════════════════════ -->
  <material name="aluminum">   <color rgba="0.75 0.75 0.75 1.0"/> </material>
  <material name="black_rubber"><color rgba="0.15 0.15 0.15 1.0"/> </material>
  <material name="dark_grey">  <color rgba="0.30 0.30 0.30 1.0"/> </material>

  <!-- ═══════════════════════════════════════════════════════════════════
       Inertia macros
       ═══════════════════════════════════════════════════════════════════ -->
  <xacro:macro name="box_inertia" params="m l w h">
    <inertial>
      <mass value="${m}"/>
      <inertia
        ixx="${m*(w*w + h*h)/12}" ixy="0" ixz="0"
        iyy="${m*(l*l + h*h)/12}" iyz="0"
        izz="${m*(l*l + w*w)/12}"/>
    </inertial>
  </xacro:macro>

  <xacro:macro name="cylinder_inertia" params="m r h">
    <inertial>
      <mass value="${m}"/>
      <inertia
        ixx="${m*(3*r*r + h*h)/12}" ixy="0" ixz="0"
        iyy="${m*(3*r*r + h*h)/12}" iyz="0"
        izz="${m*r*r/2}"/>
    </inertial>
  </xacro:macro>

  <!-- ═══════════════════════════════════════════════════════════════════
       Wheel macro
         name  — link name (e.g. "wheel_left_link")
         side  — +1 = left (+Y), -1 = right (-Y)
       ═══════════════════════════════════════════════════════════════════ -->
  <xacro:macro name="wheel" params="name side">

    <link name="${name}">
      <visual>
        <!-- rpy pi/2 0 0: cylinder principal axis → y, so it rolls laterally -->
        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
        <material name="black_rubber"/>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertia m="${wheel_mass}" r="${wheel_radius}" h="${wheel_width}"/>
    </link>

    <joint name="${name}_joint" type="continuous">
      <parent link="base_link"/>
      <child  link="${name}"/>
      <!-- REP-105: left = +y (side=+1), right = -y (side=-1) -->
      <origin xyz="0 ${side * wheel_separation / 2} 0" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
      <dynamics damping="0.01" friction="0.1"/>
    </joint>

  </xacro:macro>

  <!-- ═══════════════════════════════════════════════════════════════════
       base_footprint  — virtual ground-projection frame (REP-105)
       Origin: ground plane, centred between wheels.
       ═══════════════════════════════════════════════════════════════════ -->
  <link name="base_footprint"/>

  <joint name="base_footprint_joint" type="fixed">
    <parent link="base_footprint"/>
    <child  link="base_link"/>
    <origin xyz="0 0 ${axle_height}" rpy="0 0 0"/>
  </joint>

  <!-- ═══════════════════════════════════════════════════════════════════
       base_link  — main body at axle height
       Visual: deck plate (640×220×6mm aluminium).
       ═══════════════════════════════════════════════════════════════════ -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${deck_length} ${deck_width} ${deck_thickness}"/>
      </geometry>
      <material name="aluminum"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${deck_length} ${deck_width} ${deck_thickness}"/>
      </geometry>
    </collision>
    <xacro:box_inertia m="${chassis_mass}"
                       l="${deck_length}" w="${deck_width}" h="${deck_thickness}"/>
  </link>

  <!-- ═══════════════════════════════════════════════════════════════════
       Wheels  (continuous joints; /joint_states from motor controller)
       ═══════════════════════════════════════════════════════════════════ -->
  <xacro:wheel name="wheel_left_link"  side=" 1"/>
  <xacro:wheel name="wheel_right_link" side="-1"/>

  <!-- ═══════════════════════════════════════════════════════════════════
       imu_link  — MPU-6000 on MAMBA F722S flight controller
       fc_x = -50mm SCAD = +x ROS;  z = pad + standoff above deck = 12mm
       ═══════════════════════════════════════════════════════════════════ -->
  <link name="imu_link"/>

  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child  link="imu_link"/>
    <origin xyz="${imu_x} ${imu_y} ${imu_z}" rpy="0 0 0"/>
  </joint>

  <!-- ═══════════════════════════════════════════════════════════════════
       stem_link  — 38.1mm EMT conduit, 1050mm tall (visual only)
       Frame origin at stem base (= base_link level); visual centred.
       ═══════════════════════════════════════════════════════════════════ -->
  <link name="stem_link">
    <visual>
      <origin xyz="0 0 ${stem_height/2}" rpy="0 0 0"/>
      <geometry>
        <cylinder radius="${stem_od/2}" length="${stem_height}"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
  </link>

  <joint name="stem_joint" type="fixed">
    <parent link="base_link"/>
    <child  link="stem_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>

  <!-- ═══════════════════════════════════════════════════════════════════
       sensor_head_link  — top of stem; parent for all sensors
       ═══════════════════════════════════════════════════════════════════ -->
  <link name="sensor_head_link"/>

  <joint name="sensor_head_joint" type="fixed">
    <parent link="base_link"/>
    <child  link="sensor_head_link"/>
    <origin xyz="0 0 ${stem_height}" rpy="0 0 0"/>
  </joint>

  <!-- ═══════════════════════════════════════════════════════════════════
       laser  — RPLIDAR A1M8 scan plane
       Mounted on top of sensor-head collar (COL_H = 36mm above ring).
       Frame name "laser" matches slam_toolbox config in saltybot_bringup.
       ═══════════════════════════════════════════════════════════════════ -->
  <link name="laser"/>

  <joint name="laser_joint" type="fixed">
    <parent link="sensor_head_link"/>
    <child  link="laser"/>
    <origin xyz="0 0 ${sensor_head_collar_h}" rpy="0 0 0"/>
  </joint>

  <!-- ═══════════════════════════════════════════════════════════════════
       camera_link  — RealSense D435i
       Mounted on front sensor arm (ARM_R = 50mm), 10° nose-down tilt.
       Frame name "camera_link" matches realsense2_camera launch files.
       ═══════════════════════════════════════════════════════════════════ -->
  <link name="camera_link"/>

  <joint name="camera_joint" type="fixed">
    <parent link="sensor_head_link"/>
    <child  link="camera_link"/>
    <origin xyz="${sensor_arm_r} 0 0" rpy="0 ${cam_tilt} 0"/>
  </joint>

  <!-- ═══════════════════════════════════════════════════════════════════
       IMX219 surround cameras  (4× at 90° on sensor-head ring)
       Radial offset = ARM_R = 50mm from stem CL.
       Facing direction: yaw set so x-axis of link points outward.
       10° downward pitch (cam_tilt) for ground coverage.

       Positions/yaws match saltybot_cameras/launch/camera_tf.launch.py.
       When robot_state_publisher is live, camera_tf.launch.py static TFs
       become redundant and should be removed to avoid TF conflicts.
       ═══════════════════════════════════════════════════════════════════ -->

  <!-- Front camera: faces robot +X (forward), x=+0.050m -->
  <link name="camera_front_link"/>
  <joint name="camera_front_joint" type="fixed">
    <parent link="sensor_head_link"/>
    <child  link="camera_front_link"/>
    <origin xyz="${sensor_arm_r} 0 0" rpy="0 ${cam_tilt} 0"/>
  </joint>

  <!-- Left camera: faces robot +Y (left), y=+0.050m, yaw=+90° -->
  <link name="camera_left_link"/>
  <joint name="camera_left_joint" type="fixed">
    <parent link="sensor_head_link"/>
    <child  link="camera_left_link"/>
    <origin xyz="0 ${sensor_arm_r} 0" rpy="0 ${cam_tilt} ${pi/2}"/>
  </joint>

  <!-- Rear camera: faces robot -X (rear), x=-0.050m, yaw=180° -->
  <link name="camera_rear_link"/>
  <joint name="camera_rear_joint" type="fixed">
    <parent link="sensor_head_link"/>
    <child  link="camera_rear_link"/>
    <origin xyz="${-sensor_arm_r} 0 0" rpy="0 ${cam_tilt} ${pi}"/>
  </joint>

  <!-- Right camera: faces robot -Y (right), y=-0.050m, yaw=-90° -->
  <link name="camera_right_link"/>
  <joint name="camera_right_joint" type="fixed">
    <parent link="sensor_head_link"/>
    <child  link="camera_right_link"/>
    <origin xyz="0 ${-sensor_arm_r} 0" rpy="0 ${cam_tilt} ${-pi/2}"/>
  </joint>

</robot>
