<?xml version="1.0"?>
<robot
    name="wheel_macro"
    xmlns:xacro="http://www.ros.org/wiki/xacro"
    xmlns:gz="http://gazebosim.org/schema">
    <!--
    AGV Pro wheel macro.

    Builds a continuous-rotation wheel (link + joint) from a mesh, mass,
    inertia tensor and mounting origin.

    Params:
      prefix               base name, e.g. 'left_front'
      mesh                 STL filename in agv_pro_description/meshes
      reflect              reflect the model and the physics
      wheel_radius          
      wheel_height
      wheel_mass           link mass (kg)
      origin_xyz           joint origin relative to base_link
  -->
    <xacro:macro
        name="agv_pro_wheel"
        params="prefix
            parent
            origin_xyz
            reflect
            mesh
            wheel_radius:=0.072
            wheel_height:=0.04
            wheel_mass:=0.25
            gazebo_ignition:=false">

        <xacro:macro name="cylinder_inertia" params="m r h">
            <inertia
                ixx="${m*(3*r*r+h*h)/12}"
                ixy="0"
                ixz="0"
                iyy="${m*r*r/2}"
                iyz="0"
                izz="${m*(3*r*r+h*h)/12}" />
        </xacro:macro>

        <joint name="${prefix}wheel_joint" type="continuous">
            <origin xyz="${origin_xyz}" rpy="0 0 0" />
            <parent link="${parent}" />
            <child link="${prefix}wheel_link" />
            <axis xyz="0 1 0" />
            <limit effort="100000" velocity="100" />
        </joint>


        <link name="${prefix}wheel_link">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh
                        filename="package://agv_pro_description/meshes/wheels/${mesh}" />
                </geometry>
                <material name="darkgrey">
                    <color rgba="0.2 0.2 0.2 1" />
                </material>
            </visual>

            <collision>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <sphere radius="${wheel_radius}" />
                </geometry>
            </collision>

            <inertial>
                <mass value="${wheel_mass}" />
                <origin xyz="0 0 0" />
                <xacro:cylinder_inertia
                    m="${wheel_mass}"
                    r="${wheel_radius}"
                    h="${wheel_height}" />
            </inertial>

        </link>


        <xacro:if value="${gazebo_ignition}">
            <gazebo reference="${prefix}wheel_link">
                <collision>
                    <surface>
                        <friction>
                            <ode>
                                <mu>1.0</mu>
                                <mu2>0.0</mu2>
                                <xacro:if value="${reflect}">
                                    <fdir1
                                        gz:expressed_in="$(arg prefix)base_footprint">1 -1 0</fdir1>
                                </xacro:if>
                                <xacro:unless value="${reflect}">
                                    <fdir1
                                        gz:expressed_in="$(arg prefix)base_footprint">1 1 0</fdir1>
                                </xacro:unless>
                            </ode>
                        </friction>
                        <contact>
                            <ode>
                                <kp>100000.0</kp>
                                <kd>10.0</kd>
                            </ode>
                        </contact>
                    </surface>
                </collision>
            </gazebo>
        </xacro:if>

    </xacro:macro>
</robot>