<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >

    <xacro:include filename="inertial.xacro"/>

    <!-- COLOR -->
    <material name="white">
        <color rgba="1 1 1 1" />
    </material>

    <material name="orange">
        <color rgba="1 0.3 0.1 1"/>
    </material>

    <material name="blue">
        <color rgba="0.2 0.2 1 1"/>
    </material>

    <material name="black">
        <color rgba="0 0 0 1"/>
    </material>

    <material name="red">
        <color rgba="1 0 0 1"/>
    </material>

    <!-- BASE LINK -->

    <link name="base_footprint"/>

    <joint name="base_joint" type="fixed">
        <parent link="base_footprint"/>
        <child link="base_link"/>
    </joint>

    <link name="base_link"/>


    <!-- CHASSIS LINK -->

    <joint name="chassis_joint" type="fixed">
        <parent link="base_link"/>
        <child link="chassis"/>
        <origin xyz="0.1 0 -0.01"/>
    </joint>

    <link name="chassis">
        <visual>
            <origin xyz="-0.2 0.2 -0.07" rpy="1.5708 0 0"/>
            <geometry>
                <mesh filename="package://agv_description/meshes/bases/kubot_base.STL" scale="0.001 0.001 0.001"/>
            </geometry>
            <material name="white"/>
        </visual>
        <collision>
            <origin xyz="-0.064 0 0.06"/>
            <geometry>
                <box size="0.266 0.266 0.055"/>
            </geometry>
        </collision>
        <xacro:inertial_box mass="1.6329594" x="0.266" y="0.266" z="0.094">
            <origin xyz="-0.064 0 0.047" rpy="0 0 0"/>
        </xacro:inertial_box>
    </link>

    <gazebo reference="chassis">
        <material>Gazebo/White</material>
    </gazebo>

    <!-- LEFT WHEEL LINK -->

    <joint name="left_wheel_joint" type="continuous">
        <parent link="base_link"/>
        <child link="left_wheel"/>
        <origin xyz="0.0 0.144 0.023" rpy="0 0 0" />
        <axis xyz="0 1 0"/>
    </joint>

    <link name="left_wheel">
        <origin xyz="0.0 0.0 0.0" rpy="0 0.0 0.0"/>
        <visual>
            <geometry>
                <mesh filename="package://agv_description/meshes/wheels/left_tire.stl" scale="0.001 0.001 0.001"/>
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 0.0" rpy="1.57 0.0 0.0"/>
            <geometry>
                <cylinder radius="0.033" length="0.018"/>
            </geometry>
        </collision>
        <xacro:inertial_cylinder mass="2.8498940e-02" length="0.033" radius="0.018">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_cylinder>
    </link>

    <gazebo reference="left_wheel">
        <material>Gazebo/Blue</material>
    </gazebo>

    <!-- RIGHT WHEEL LINK -->

    <joint name="right_wheel_joint" type="continuous">
        <parent link="base_link"/>
        <child link="right_wheel"/>
        <origin xyz="0.0 -0.144 0.023" rpy="0 0 0" />
        <axis xyz="0 1 0"/>
    </joint>

    <link name="right_wheel">
        <origin xyz="0.0 0.0 0.0" rpy="0 0.0 0.0"/>
        <visual>
            <geometry>
                <mesh filename="package://agv_description/meshes/wheels/right_tire.stl" scale="0.001 0.001 0.001"/>
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 0.0" rpy="1.57 0.0 0.0"/>
            <geometry>
                <cylinder radius="0.033" length="0.018"/>
            </geometry>
        </collision>
        <xacro:inertial_cylinder mass="2.8498940e-02" length="0.18" radius="0.033">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_cylinder>
    </link>

    <gazebo reference="right_wheel">
        <material>Gazebo/Blue</material>
    </gazebo>


    <!-- CASTER WHEEL LINK -->

    <joint name="caster_wheel_joint" type="fixed">
        <parent link="chassis"/>
        <child link="caster_wheel"/>
        <origin xyz="0.06 0 0.033" rpy="-1.57 0 0"/>
    </joint>

    <link name="caster_wheel">
        <collision>
           <origin xyz="0.0 0.004 0.0" rpy="0.0 0.0 0.0"/>
            <geometry>
                <sphere radius="0.033"/>
            </geometry>
        </collision>
        <xacro:inertial_sphere mass="0.1" radius="0.033">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_sphere>
    </link>

    <gazebo reference="caster_wheel">
        <material>Gazebo/Black</material>
        <mu1 value="0.001"/>
        <mu2 value="0.001"/>
    </gazebo>

    <!-- LIDAR -->
    
    <joint name="laser_joint" type="fixed">
        <parent link="chassis"/>
        <child link="laser_frame"/>
        <origin xyz="-0.024 0 0.122" rpy="0 0 0"/> 
    </joint>

    <link name="laser_frame">
        <visual>
            <origin xyz="0 0 0.0" rpy="0 0 0"/>
            <geometry>
                <mesh filename="package://agv_description/meshes/sensors/lds.stl" scale="0.001 0.001 0.001"/>
            </geometry>
            <material name="black"/>
        </visual>

        <collision>
            <origin xyz="0.015 0 -0.0065" rpy="0 0 0"/>
            <geometry>
                <cylinder length="0.0315" radius="0.045"/>
            </geometry>
        </collision>

        <inertial>
            <mass value="0.114" />
            <origin xyz="0 0 0" />
            <inertia ixx="0.001" ixy="0.0" ixz="0.0"
               iyy="0.001" iyz="0.0"
               izz="0.001" />
        </inertial>
    </link>

</robot>