<?xml version="1.0"?>
<!-- Revolute-Revolute Manipulator -->
<robot name="orange2022" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Constants for robot dimensions -->
  <xacro:property name="PI" value="3.1415926535897931"/>
  
  <!-- Macro -->
  <xacro:macro name="box_inertia" params="m x y z">
    <inertia ixx="${m*(y*y+z*z)/12.0}" ixy = "0.0" ixz = "0.0"
             iyy="${m*(z*z+x*x)/12.0}" iyz = "0.0"
             izz="${m*(x*x+y*y)/12.0}" />
  </xacro:macro>
  <xacro:macro name="cylinder_inertia" params="m r h">
    <inertia ixx="${m*(3*r*r+h*h)/12.0}" ixy = "0.0" ixz = "0.0"
             iyy="${m*(3*r*r+h*h)/12.0}" iyz = "0.0"
             izz="${m*r*r/2.0}" />
  </xacro:macro>

  <!-- Import gazebo reference -->
  <xacro:include filename="$(find tsukuba2022)/urdf/orange2022.gazebo"/>
  
  <!-- Import 2D LiDAR model -->
  <xacro:include filename="$(find tsukuba2022)/urdf/sensors/hokuyo.urdf.xacro"/>
  <xacro:sensor_hokuyo name="hokuyo" parent="base_link" min_angle="-1.22" max_angle="1.22" samples="720">
    <origin xyz="0.1075 0.0 0.1085" rpy="0.0 0.0 0.0"/>
  </xacro:sensor_hokuyo>
  
  <!-- Import 3D LiDAR model -->
  <xacro:include filename="$(find tsukuba2022)/urdf/sensors/velodyne.urdf.xacro"/>
  <xacro:sensor_velodyne name="velodyne" parent="base_link" min_angle="-2.35619" max_angle="2.35619" samples="720">
    <origin xyz="-0.0185 0.0 0.8985" rpy="0.0 0.0 0.0"/>
  </xacro:sensor_velodyne>
  
  <!-- Import imu model -->
  <xacro:include filename="$(find tsukuba2022)/urdf/sensors/imu.urdf.xacro"/>
  <xacro:sensor_imu name="imu" parent="base_link" size="0.05 0.05 0.05">
    <origin xyz="-0.253 0.0 0.068" rpy="0.0 0.0 0.0"/>
  </xacro:sensor_imu>
  
  <!-- Base Footprint -->
  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
  </joint>
  
  <link name="base_footprint"/>
  
  <!-- Base Link -->
  <link name="base_link">
    <collision>
      <origin xyz="-0.1995 0.0 0.341" rpy="0.0 0.0 0.0"/>
      <geometry>
        <box size="0.549 0.5055 0.603"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0.0775 0.25275 0.6425" rpy="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://tsukuba2022/meshes/chassis.dae"/>
      </geometry>
    </visual>

    <inertial>
       <mass value="27.1" />
       <origin xyz="-0.1995 0.0 0.19" rpy="0.0 0.0 0.0"/>
       <xacro:box_inertia m="27.1" x="0.549" y="0.5055" z="0.3"/>
    </inertial>
  </link>
  
  <!-- Back wheels -->
  <joint name="left_caster_hinge" type ="fixed">
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
    <child link="left_caster_link"/>
    <parent link="base_link"/>
  </joint>
  
  <joint name="right_caster_hinge" type ="fixed">
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
    <child link="right_caster_link"/>
    <parent link="base_link"/>
  </joint>
  
  <link name="left_caster_link">
    <collision>
      <origin xyz="-0.4815 0.145 -0.039" rpy="0.0 ${PI/2} ${PI/2}"/>
      <geometry>
        <sphere radius="0.0625"/>
      </geometry>
    </collision>
    
    <visual>
      <origin xyz="-0.4815 0.145 -0.039" rpy="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://tsukuba2022/meshes/left_back_wheel.dae"/>
      </geometry>
    </visual>
  </link>
  
  <link name="right_caster_link">
    <collision>
      <origin xyz="-0.4815 -0.145 -0.039" rpy="0.0 ${PI/2} ${PI/2}"/>
      <geometry>
        <sphere radius="0.0625"/>
      </geometry>
    </collision>
    
    <visual>
      <origin xyz="-0.4815 -0.145 -0.039" rpy="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://tsukuba2022/meshes/right_back_wheel.dae"/>
        <sphere radius="0.0625"/>
      </geometry>
    </visual>
  </link>
  
  <!-- Front wheels -->
  <joint type="revolute" name="left_wheel_hinge">
    <origin xyz="0.0 0.2834 0.0" rpy="0.0 0.0 0.0"/>
    <child link="left_wheel">left_wheel</child>
    <parent link="base_link">base_link</parent>
    <axis xyz="0 1 0"/>
    <limit effort="100" velocity="100.0"  lower="-5000" upper="5000" />
  </joint>
  
  <joint type="revolute" name="right_wheel_hinge">
    <origin xyz="0.0 -0.2834 0.0" rpy="0.0 0.0 0.0"/>
    <child link="right_wheel">right_wheel</child>
    <parent link="base_link">base_link</parent>
    <axis xyz="0 1 0"/>
    <limit effort="100" velocity="100.0"  lower="-5000" upper="5000" />
  </joint>
  
  <link name="left_wheel">
    <collision name="collision">
      <origin xyz="0.0 0.0 0.0" rpy="0.0 ${PI/2} ${PI/2}"/>
      <geometry>
        <cylinder length="0.047" radius="0.1015"/>
      </geometry>
    </collision>
    
    <visual name="visual">
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://tsukuba2022/meshes/front_wheel.dae"/>
      </geometry>
    </visual>
    
    <inertial>
       <mass value="3.4" />
       <origin xyz="0.0 0.0 0.0" rpy="0.0 ${PI/2} ${PI/2}"/>
       <xacro:cylinder_inertia m="3.4" r="0.1015" h="0.047"/>
    </inertial>
  </link>
  
  <link name="right_wheel">
    <collision name="collision">
      <origin xyz="0.0 0.0 0.0" rpy="0.0 ${PI/2} ${PI/2}"/>
      <geometry>
        <cylinder length="0.047" radius="0.1015"/>
      </geometry>
    </collision>
    
    <visual name="visual">
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://tsukuba2022/meshes/front_wheel.dae"/>
      </geometry>
    </visual>
    
    <inertial>
       <mass value="3.4" />
       <origin xyz="0.0 0.0 0.0" rpy="0.0 ${PI/2} ${PI/2}"/>
       <xacro:cylinder_inertia m="3.4" r="0.1015" h="0.047"/>
    </inertial>
  </link>
</robot>