<?xml version="1.0"?>

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

    <xacro:arg name="robot_namespace" default="/" />
    <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>

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

    <link name="inertial_link">
        <inertial>
            <mass value="40" />
            <!-- <mass value="50" /> -->
            <!-- <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" /> -->
            <origin xyz="0.0 0.0 0.0" />
            <inertia ixx="2.288641" ixy="0" ixz="0" iyy="5.103976" iyz="0" izz="3.431465" />
        </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>

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

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

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

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