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

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


    <xacro:property name="chassis_length" value="0.600"/>
    <xacro:property name="chassis_width" value="0.330"/>
    <xacro:property name="chassis_height" value="0.155"/>
    <xacro:property name="chassis_mass" value="15.0"/>
    <xacro:property name="wheel_radius" value="0.135"/>
    <xacro:property name="wheel_thickness" value="0.080"/>
    <xacro:property name="wheel_mass" value="4.0"/>
    <xacro:property name="wheel_offset_x" value="0.285748"/>
    <xacro:property name="wheel_offset_y" value="0.33675"/>
    <xacro:property name="wheel_offset_z" value="-0.035"/>
    <xacro:property name="caster_wheel_radius" value="0.01"/>
    <xacro:property name="caster_wheel_mass" value="0.01"/>
    <xacro:property name="caster_wheel_offset_x" value="0.075"/>
    <xacro:property name="caster_wheel_offset_z" value="${wheel_offset_z - wheel_radius + caster_wheel_radius}"/>

    <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>

    
    <material name="green">
        <color rgba="0.0 0.8 0.0 1.0"/>
    </material>
    
    <material name="grey">
        <color rgba="0.6 0.6 0.6 1.0"/>
    </material>
    
    <material name="grey2">
        <color rgba="0.9 0.9 0.9 1.0"/>
    </material>

    <material name="lightGrey">
        <color rgba="0.6 0.6 0.6 1.0"/>
    </material>

    <!-- BASE LINK -->

    <link name="base_link">
    </link>

    <!-- BASE_FOOTPRINT LINK -->

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

    <link name="base_footprint">
    </link>


    <!-- CHASSIS LINK -->

    <joint name="chassis_joint" type="fixed">
        <parent link="base_link"/>
        <child link="chassis"/>
        <origin xyz="${-chassis_length/2 + 0.075} 0 ${wheel_radius - wheel_offset_z}"/>
    </joint>

    <link name="chassis">
        <visual>
            <origin xyz="0 0 0"/>
            <geometry>
                <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
            </geometry>
            <material name="orange"/>
        </visual>
        <collision>
            <origin xyz="0 0 0"/>
            <geometry>
                <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
            </geometry>
        </collision>
        <xacro:inertial_box mass="0.5" x="${chassis_length}" y="${chassis_width}" z="${chassis_height}">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_box>
    </link>

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

    <!-- LEFT WHEEL LINK -->

    <joint name="left_wheel_joint" type="continuous">
        <parent link="chassis"/>
        <child link="left_wheel"/>
        <origin xyz="${chassis_length/2 - 0.075} ${wheel_offset_y/2 + wheel_thickness/2 + 0.01} ${wheel_offset_z}" rpy="-${pi/2} 0 0" />
        <axis xyz="0 0 1"/>
    </joint>

    <link name="left_wheel">
        <visual>
            <geometry>
                <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <geometry>
                <sphere radius="${wheel_radius}"/>
            </geometry>
        </collision>
        <xacro:inertial_cylinder mass="${wheel_mass}" length="${wheel_thickness}" radius="${wheel_radius}">
            <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="chassis"/>
        <child link="right_wheel"/>
        <origin xyz="${chassis_length/2 - 0.075} -${wheel_offset_y/2 + wheel_thickness/2 + 0.01} ${wheel_offset_z}" rpy="${pi/2} 0 0" />
        <axis xyz="0 0 -1"/>
    </joint>

    <link name="right_wheel">
        <visual>
            <geometry>
                <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <geometry>
                <sphere radius="${wheel_radius}"/>
            </geometry>
        </collision>
        <xacro:inertial_cylinder mass="${wheel_mass}" length="${wheel_thickness}" radius="${wheel_radius}">
            <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="-${caster_wheel_offset_x*2} 0 ${caster_wheel_offset_z}"/>
    </joint>


    <link name="caster_wheel">
        <visual>
            <geometry>
                <sphere radius="${caster_wheel_radius}"/>
            </geometry>
            <material name="white"/>
        </visual>
        <collision>
            <geometry>
                <sphere radius="${caster_wheel_radius}"/>
            </geometry>
        </collision>
        <xacro:inertial_sphere mass="${caster_wheel_mass}" radius="${caster_wheel_radius}">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_sphere>
    </link>

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

    <!-- Lidar LINK -->

    <!-- <joint name="lidar_joint" type="fixed">
        <parent link="chassis"/>
        <child link="laser_frame"/>
        <origin xyz="${chassis_length/2 -0.150} 0 ${chassis_height + 0.030}" rpy="0 0 0"/>
    </joint>


    <link name="laser_frame">
        <visual>
            <geometry>
                <sphere radius="${caster_wheel_radius}"/>
            </geometry>
            <material name="white"/>
        </visual>
        <collision>
            <geometry>
                <sphere radius="${caster_wheel_radius}"/>
            </geometry>
        </collision>
        <xacro:inertial_sphere mass="${caster_wheel_mass}" radius="${caster_wheel_radius}">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_sphere>
    </link> -->

    <joint name="laser_joint" type="fixed">
        <parent link="chassis"/>
        <child link="laser_frame"/>
        <origin xyz="${chassis_length/2 - 0.15285} 0 ${chassis_height/2 + 0.030}" rpy="0 0 ${pi}"/>
    </joint>


    <link name="laser_frame">
        <visual>
            <geometry>
                <sphere radius="${caster_wheel_radius}"/>
            </geometry>
            <material name="white"/>
        </visual>
        <collision>
            <geometry>
                <sphere radius="${caster_wheel_radius}"/>
            </geometry>
        </collision>
        <xacro:inertial_sphere mass="${caster_wheel_mass}" radius="${caster_wheel_radius}">
            <origin xyz="0 0 0" rpy="0 0 0"/>
        </xacro:inertial_sphere>
    </link>

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

</robot>