<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="saltybot">
  <!--
    SaltyBot URDF Description

    Robot Configuration:
    - Base: Wheeled differential drive platform
    - Footprint: ~0.4m x 0.4m (square base)
    - Height: ~0.3m to sensor payload
    - Sensors:
      * RPLIDAR A1M8 (360° scanning LiDAR)
      * RealSense D435i (RGB-D camera + IMU)
      * BNO055 (9-DOF IMU, ESP32-S3 FC)
    - Actuators:
      * 2x differential drive motors
      * Pan/Tilt servos for camera
  -->

  <!-- Constants -->
  <xacro:property name="PI" value="3.14159265359" />
  <xacro:property name="base_mass" value="2.5" />
  <xacro:property name="wheel_radius" value="0.0475" />
  <xacro:property name="wheel_separation" value="0.380" />
  <xacro:property name="footprint_x" value="0.40" />
  <xacro:property name="footprint_y" value="0.40" />
  <xacro:property name="footprint_z" value="0.15" />

  <!-- Base Footprint -->
  <link name="base_footprint" />

  <!-- Base Link (Robot Center) -->
  <link name="base_link">
    <inertial>
      <origin xyz="0 0 0.08" rpy="0 0 0" />
      <mass value="${base_mass}" />
      <!-- Rough rectangular inertia approximation -->
      <inertia ixx="0.020" ixy="0.0" ixz="0.0" iyy="0.020" iyz="0.0" izz="0.030" />
    </inertial>
    <visual>
      <origin xyz="0 0 0.075" rpy="0 0 0" />
      <geometry>
        <box size="${footprint_x} ${footprint_y} ${footprint_z}" />
      </geometry>
      <material name="dark_gray">
        <color rgba="0.25 0.25 0.25 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0.075" rpy="0 0 0" />
      <geometry>
        <box size="${footprint_x} ${footprint_y} ${footprint_z}" />
      </geometry>
    </collision>
  </link>

  <!-- Footprint to Base Transform (Z-only, footprint at ground level) -->
  <joint name="base_footprint_joint" type="fixed">
    <origin xyz="0 0 ${footprint_z}" rpy="0 0 0" />
    <parent link="base_footprint" />
    <child link="base_link" />
  </joint>

  <!-- Left Wheel -->
  <link name="left_wheel_link">
    <inertial>
      <mass value="0.2" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0" />
      <geometry>
        <cylinder radius="${wheel_radius}" length="0.03" />
      </geometry>
      <material name="black">
        <color rgba="0.1 0.1 0.1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0" />
      <geometry>
        <cylinder radius="${wheel_radius}" length="0.03" />
      </geometry>
    </collision>
  </link>

  <joint name="left_wheel_joint" type="continuous">
    <origin xyz="0 ${wheel_separation/2} 0.0475" rpy="0 0 0" />
    <axis xyz="0 1 0" />
    <parent link="base_link" />
    <child link="left_wheel_link" />
  </joint>

  <!-- Right Wheel -->
  <link name="right_wheel_link">
    <inertial>
      <mass value="0.2" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0" />
      <geometry>
        <cylinder radius="${wheel_radius}" length="0.03" />
      </geometry>
      <material name="black">
        <color rgba="0.1 0.1 0.1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="${PI/2} 0 0" />
      <geometry>
        <cylinder radius="${wheel_radius}" length="0.03" />
      </geometry>
    </collision>
  </link>

  <joint name="right_wheel_joint" type="continuous">
    <origin xyz="0 ${-wheel_separation/2} 0.0475" rpy="0 0 0" />
    <axis xyz="0 1 0" />
    <parent link="base_link" />
    <child link="right_wheel_link" />
  </joint>

  <!-- IMU Link (ESP32-S3 FC BNO055, mounted on main board) -->
  <link name="imu_link">
    <inertial>
      <mass value="0.01" />
      <inertia ixx="0.000001" ixy="0.0" ixz="0.0" iyy="0.000001" iyz="0.0" izz="0.000001" />
    </inertial>
  </link>

  <joint name="imu_joint" type="fixed">
    <origin xyz="0 0 0.05" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="imu_link" />
  </joint>

  <!-- RPLIDAR A1M8 Frame (360° LiDAR, mounted on top of robot) -->
  <link name="laser_frame">
    <inertial>
      <mass value="0.15" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <cylinder radius="0.035" length="0.02" />
      </geometry>
      <material name="gray">
        <color rgba="0.5 0.5 0.5 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <cylinder radius="0.035" length="0.02" />
      </geometry>
    </collision>
  </link>

  <joint name="laser_joint" type="fixed">
    <!-- RPLIDAR mounted on top, center of robot, ~0.15m above base -->
    <origin xyz="0 0 0.15" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="laser_frame" />
  </joint>

  <!-- RealSense D435i Camera Frame (Mounted forward-facing on pan/tilt) -->
  <link name="camera_link">
    <inertial>
      <mass value="0.1" />
      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="0.09 0.025 0.024" />
      </geometry>
      <material name="black">
        <color rgba="0.1 0.1 0.1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="0.09 0.025 0.024" />
      </geometry>
    </collision>
  </link>

  <!-- RealSense Optical Frame (Z-forward for optical convention) -->
  <link name="camera_optical_frame" />

  <!-- Pan Servo Frame (Base of pan-tilt mechanism) -->
  <link name="pan_servo">
    <inertial>
      <mass value="0.05" />
      <inertia ixx="0.000005" ixy="0.0" ixz="0.0" iyy="0.000005" iyz="0.0" izz="0.000005" />
    </inertial>
  </link>

  <joint name="pan_servo_joint" type="fixed">
    <!-- Pan servo mounted at front, ~0.12m forward and 0.10m up from base center -->
    <origin xyz="0.12 0 0.10" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="pan_servo" />
  </joint>

  <!-- Tilt Servo Frame (Mounted on pan servo) -->
  <link name="tilt_servo">
    <inertial>
      <mass value="0.05" />
      <inertia ixx="0.000005" ixy="0.0" ixz="0.0" iyy="0.000005" iyz="0.0" izz="0.000005" />
    </inertial>
  </link>

  <joint name="tilt_servo_joint" type="fixed">
    <!-- Tilt servo mounted on pan servo, offset for mechanical linkage -->
    <origin xyz="0 0 0.02" rpy="0 0 0" />
    <parent link="pan_servo" />
    <child link="tilt_servo" />
  </joint>

  <!-- Camera mounted on tilt servo -->
  <joint name="camera_joint" type="fixed">
    <!-- Camera offset from tilt servo (forward and up for good viewing angle) -->
    <origin xyz="0.04 0 0.02" rpy="0 0 0" />
    <parent link="tilt_servo" />
    <child link="camera_link" />
  </joint>

  <!-- Camera optical frame (ROS convention: Z-forward, X-right, Y-down) -->
  <joint name="camera_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="${-PI/2} 0 ${-PI/2}" />
    <parent link="camera_link" />
    <child link="camera_optical_frame" />
  </joint>

  <!-- Materials -->
  <material name="dark_gray">
    <color rgba="0.25 0.25 0.25 1" />
  </material>
  <material name="gray">
    <color rgba="0.5 0.5 0.5 1" />
  </material>
  <material name="black">
    <color rgba="0.1 0.1 0.1 1" />
  </material>

</robot>
