<?xml version="1.0"?>
<!-- 
  Robot model adapted from https://github.com/CIR-KIT/steer_drive_ros/blob/kinetic-devel/steer_drive_controller/test/common/urdf/wheel.xacro

  Modifications:
    <transmissions> elements have been updated and added to 'front_steer'.
    Update inertials.
    Remove dependency on externally defined parameters.
-->
<robot name="wheel" xmlns:xacro="http://wiki.ros.org/xacro">
  <!-- Include xacro for materials and inertials -->

  <!-- Properties -->
  <xacro:property name="steer_effort" value="100.0"/>
  <xacro:property name="steer_velocity" value="20.0"/>
  <xacro:property name="steer_limit_deg" value="45.0"/>

  <!-- Bicycle model front steering link (required for steer_drive_controller) -->
  <xacro:macro name="front_steer"
    params="
      name
      parent
      steer_radius
      steer_thickness
      steer_mass
      base_length
      base_width
      axle_offset
      steer_height">
    <link name="${name}_steer_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${steer_radius}" length="${steer_thickness}"/>
        </geometry>
        <material name="yellow" />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${steer_radius}" length="${steer_thickness}"/>
        </geometry>
      </collision>
      <xacro:solid_cylinder_inertial 
          rpy="0 0 0" xyz="0 0 0"
          mass="${steer_mass}"
          radius="${steer_radius}" length="${steer_thickness}" />
    </link>

    <joint name="${name}_steer_joint" type="revolute">
      <parent link="${parent}_link"/>
      <child link="${name}_steer_link"/>
      <origin xyz="${base_length/2-axle_offset} 0 ${steer_height}" rpy="0 0 0"/>
      <axis xyz="0 0 1"/>
      <limit effort="${steer_effort}"
             lower="${-180.0 * deg_to_rad}" upper="${180.0 * deg_to_rad}"
             velocity="${steer_velocity}"/>
    </joint>

    <transmission name="${name}_steer_joint_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <actuator name="${name}_steer_joint_motor">
        <mechanicalReduction>1</mechanicalReduction>
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      </actuator>
      <joint name="${name}_steer_joint">
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      </joint>
    </transmission>

    <gazebo reference="${name}_steer_link">
      <material>Gazebo/Yellow</material>
    </gazebo>
  </xacro:macro>

  <!-- Car steering model front (steerable) wheel links (left/right) -->
  <xacro:macro name="front_wheel_lr"
    params="
      name
      parent
      reflect
      wheel_radius
      wheel_thickness
      wheel_mass
      steer_radius
      steer_thickness
      steer_mass
      base_length
      base_width
      axle_offset
      steer_height">
    <link name="${name}_steer_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${steer_radius}" length="${steer_thickness}"/>
        </geometry>
        <material name="blue" />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${steer_radius}" length="${wheel_thickness}"/>
        </geometry>
      </collision>
      <xacro:solid_cylinder_inertial 
          rpy="0 0 0" xyz="0 0 0"
          mass="${steer_mass}"
          radius="${steer_radius}" length="${steer_thickness}" />
    </link>

    <joint name="${name}_steer_joint" type="revolute">
      <parent link="${parent}_link"/>
      <child link="${name}_steer_link"/>
      <origin xyz="${base_length/2-axle_offset} ${reflect*(base_width/2+axle_offset)} ${steer_height}" rpy="0 0 0"/>
      <axis xyz="0 0 1"/>
      <limit effort="${steer_effort}"
             lower="${steer_limit_deg * deg_to_rad * -1.0}" upper="${steer_limit_deg * deg_to_rad}"
             velocity="${steer_velocity}"/>
    </joint>

    <link name="${name}_wheel_link">
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
        </geometry>
      </collision>
      <visual>
        <origin xyz="0 0 0" rpy="${-pi/2} ${(reflect * pi/2) + pi/2} 0"/>
        <geometry>
          <mesh filename="file://$(find dcaiti_control)/description/assets/ft24_wheel_simplified.dae" />
        </geometry>
      </visual>
      <xacro:solid_cylinder_inertial 
          rpy="0 0 0" xyz="0 0 0"
          mass="${wheel_mass}"
          radius="${wheel_radius}" length="${wheel_thickness}" />
    </link>

    <joint name="${name}_wheel_joint" type="continuous">
      <parent link="${name}_steer_link"/>
      <child link="${name}_wheel_link"/>
      <origin xyz="0 0 ${-steer_height}" rpy="${-90 * deg_to_rad} 0 0"/>
      <axis xyz="0 0 1"/>
    </joint>

    <transmission name="${name}_steer_joint_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <actuator name="${name}_steer_joint_motor">
        <mechanicalReduction>1</mechanicalReduction>
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      </actuator>
      <joint name="${name}_steer_joint">
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      </joint>
    </transmission>

    <transmission name="${name}_wheel_joint_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <actuator name="${name}_wheel_joint_motor">
        <mechanicalReduction>1</mechanicalReduction>
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      </actuator>
      <joint name="${name}_wheel_joint">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      </joint>
    </transmission>

    <gazebo reference="${name}_steer_link">
      <material>Gazebo/Blue</material>
    </gazebo>
    
    <gazebo reference="${name}_wheel_link">
      <material>Gazebo/Red</material>
    </gazebo>
  </xacro:macro>

  <!-- Bicycle model rear wheel link (required for steer_drive_controller) -->
  <xacro:macro name="rear_wheel"
    params="
      name
      parent
      wheel_radius
      wheel_thickness
      wheel_mass
      *origin">
    <link name="${name}_wheel_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
        </geometry>
        <material name="yellow" />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
        </geometry>
      </collision>
      <xacro:solid_cylinder_inertial 
          rpy="0 0 0" xyz="0 0 0"
          mass="${wheel_mass}"
          radius="${wheel_radius}" length="${wheel_thickness}" />
    </link>

    <joint name="${name}_wheel_joint" type="continuous">
      <parent link="${parent}_link"/>
      <child link="${name}_wheel_link"/>
      <xacro:insert_block name="origin"/>
      <axis xyz="0 0 1"/>
    </joint>

    <transmission name="${name}_wheel_joint_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <actuator name="${name}_wheel_joint_motor">
        <mechanicalReduction>1</mechanicalReduction>
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      </actuator>
      <joint name="${name}_wheel_joint">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      </joint>
    </transmission>

    <gazebo reference="${name}_wheel_link">
      <material>Gazebo/Yellow</material>
    </gazebo>
  </xacro:macro>

  <!-- Bicycle model front wheel link (passive - no transmission) -->
  <xacro:macro name="front_wheel"
    params="
      name
      parent
      wheel_radius
      wheel_thickness
      wheel_mass
      *origin">
    <link name="${name}_wheel_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
        </geometry>
        <material name="yellow" />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
        </geometry>
      </collision>
      <xacro:solid_cylinder_inertial 
          rpy="0 0 0" xyz="0 0 0"
          mass="${wheel_mass}"
          radius="${wheel_radius}" length="${wheel_thickness}" />
    </link>

    <joint name="${name}_wheel_joint" type="continuous">
      <parent link="${parent}_link"/>
      <child link="${name}_wheel_link"/>
      <xacro:insert_block name="origin"/>
      <axis xyz="0 0 1"/>
    </joint>

    <gazebo reference="${name}_wheel_link">
      <material>Gazebo/Yellow</material>
    </gazebo>

    <gazebo reference="${name}_wheel_joint">
      <provideFeedback>true</provideFeedback>
    </gazebo>
  </xacro:macro>

  <!-- Car steering model rear wheel links (left/right) -->
  <xacro:macro name="rear_wheel_lr"
    params="
      name
      parent
      reflect
      wheel_radius
      wheel_thickness
      wheel_mass
      *origin">
    <link name="${name}_wheel_link">
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
        </geometry>
      </collision>
      <visual>
        <origin xyz="0 0 0" rpy="${-pi/2} ${(reflect * pi/2) + pi/2} 0"/>
        <geometry>
          <mesh filename="file://$(find dcaiti_control)/description/assets/ft24_wheel_simplified.dae" />
        </geometry>
      </visual>
      <xacro:solid_cylinder_inertial 
          rpy="0 0 0" xyz="0 0 0"
          mass="${wheel_mass}"
          radius="${wheel_radius}" length="${wheel_thickness}" />
    </link>

    <joint name="${name}_wheel_joint" type="continuous">
      <parent link="${parent}_link"/>
      <child link="${name}_wheel_link"/>
      <xacro:insert_block name="origin"/>
      <axis xyz="0 0 1"/>
    </joint>

    <transmission name="${name}_wheel_joint_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <actuator name="${name}_wheel_joint_motor">
        <mechanicalReduction>1</mechanicalReduction>
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      </actuator>
      <joint name="${name}_wheel_joint">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      </joint>
    </transmission>

    <gazebo reference="${name}_wheel_link">
      <material>Gazebo/Red</material>
    </gazebo>
  </xacro:macro>
</robot>