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

    <material name="mecanum_wheel_material">
        <ambient>0.2 0.2 0.2 1</ambient>
        <diffuse>0.2 0.2 0.2 1</diffuse>
        <specular>0.2 0.2 0.2 1</specular>
        <emissive>0 0 0 0</emissive>
        <color rgba="0.2 0.2 0.2 1"/> 
    </material>

    <xacro:macro name="mecanum_wheel" params="name parent xyz rpy direction radius mass">

        <!-- get left or right wheel -->
        <xacro:if value="${direction == 'l'}">
            <xacro:property name="mesh" value="package://sherpa/urdf/meshes//mecanum_wheel_left.STL"/>
            <xacro:property name="friction_dir" value="-1"/>
        </xacro:if>
        <xacro:if value="${direction == 'r'}">
            <xacro:property name="mesh" value="package://sherpa/urdf/meshes//mecanum_wheel_right.STL"/>
            <xacro:property name="friction_dir" value="1"/>
        </xacro:if>

        <xacro:property name="mesh_scale" value="${0.006 * (radius / 0.3)}"/>

        <link name='${name}_link'>
            <inertial>
                <mass value="${mass}"/>
                <inertia 
                    ixx="${(13.0/48.0) * mass * radius * radius}"
                    ixy="0.0"
                    ixz="0.0"
                    iyy="${0.5 * mass * radius * radius}"
                    iyz="0.0"
                    izz="${(13.0/48.0) * mass * radius * radius}"
                />
            </inertial>
            <visual name='visual'>
                <geometry>
                    <mesh
                        filename="${mesh}"
                        scale="${mesh_scale} ${mesh_scale} ${mesh_scale}"
                    />
                </geometry>
                <material name="mecanum_wheel_material"/>
            </visual>
            <collision name='collision'>
                <origin xyz="0 0 0" rpy="-1.5707 0 0"/>
                <geometry>
                    <cylinder radius="${radius}" length="${radius}"/>
                </geometry>
            </collision>
        </link>
        <xacro:element xacro:name="gazebo" reference="${name}_link">
            <surface>
                <friction>
                <ode>
                    <mu>1.0</mu>
                    <mu2>0.0</mu2>
                    <fdir1>1 ${friction_dir} 0</fdir1>
                </ode>
                </friction>
            </surface>
        </xacro:element>
        <joint name='${name}_joint' type='continuous'>
            <parent link="${parent}_link"/>
            <child link="${name}_link"/>
            <axis xyz="0 1 0"/>
            <limit 
                velocity="35"
                effort="350"
            />
            <origin xyz="${xyz}" rpy="${rpy}"/>
        </joint>
    </xacro:macro>
</robot>