<?xml version="1.0"?>

<robot name="scout_v2" 
    xmlns:xacro="http://ros.org/wiki/xacro">

    <xacro:arg name="urdf_extras" default="empty.urdf" />

    <xacro:include filename="$(find scout_description)/urdf/scout_wheel_type1.xacro" />
    <xacro:include filename="$(find scout_description)/urdf/scout_wheel_type2.xacro" />

    <!-- Variables -->
    <xacro:property name="M_PI" value="3.14159"/>

    <!-- Vehicle Geometries -->
    <xacro:property name="base_x_size" value="0.9250000" />
    <xacro:property name="base_y_size" value="0.380000" />
    <xacro:property name="base_z_size" value="0.210000" />

    <xacro:property name="wheelbase" value="0.498" />
    <xacro:property name="track" value="0.58306" />
    <xacro:property name="wheel_vertical_offset" value="-0.0702" />

    <xacro:property name="wheel_length" value="1.1653e-01" />
    <xacro:property name="wheel_radius" value="1.6459e-01" />

    <!-- Base link -->
    <link name="base_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="package://scout_description/meshes/base_link.dae" />
            </geometry>
        </visual>
        <collision>
            <origin xyz="0 0 0.008" rpy="0 0 0" />
            <geometry>
                <box size="${base_x_size} ${base_y_size} ${base_z_size}"/>
            </geometry>
        </collision>
        <collision>
            <origin xyz="0 0 ${base_z_size/6}" rpy="0 0 0" />
            <geometry>
                <box size="${base_x_size/6} ${base_y_size*1.65} ${base_z_size/3}"/>
            </geometry>
        </collision>
    </link>

    <!-- Base footprint: base_link projected to the gound plane -->
    <link name="base_footprint"/>

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

    <!-- Interial link -->
    <link name="inertial_link">
        <inertial>
            <mass value="46.034" />
            <origin xyz="-0.00065 -0.085 0.062" />
            <inertia ixx="0.6022" ixy="-0.02364" ixz="-0.1197" iyy="1.7386" iyz="-0.001544" izz="2.0296" />
        </inertial>
    </link>

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

    <!-- Scout wheel macros -->
    <!-- wheel labeled from 0 to 3, conter-clockwise, starting from front right wheel -->
    <!-- motor 1 and 2 (left side) are mechanically installed in a reversed direction -->
    <xacro:scout_wheel_type1 wheel_prefix="front_right">
        <origin xyz="${wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="3.14 0 0" />
    </xacro:scout_wheel_type1>
    <xacro:scout_wheel_type2 wheel_prefix="front_left">
        <origin xyz="${wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
    </xacro:scout_wheel_type2>
    <xacro:scout_wheel_type1 wheel_prefix="rear_left">
        <origin xyz="${-wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
    </xacro:scout_wheel_type1>
    <xacro:scout_wheel_type2 wheel_prefix="rear_right">
        <origin xyz="${-wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="3.14 0 0" />
    </xacro:scout_wheel_type2>

    <!-- For testing, hang the robot up in the air -->
    <link name="world" />
    <joint name="world_to_base_link=" type="fixed">
        <origin xyz="0 0 0.5" rpy="0 0 0" />
        <parent link="world"/>
        <child link="base_link"/>
    </joint>

    <!-- Additional definitions -->
    <xacro:include filename="$(arg urdf_extras)" />

    <!-- Gazebo definitions  -->
    <xacro:include filename="$(find scout_description)/urdf/scout_v2.gazebo" />
</robot>
