<?xml version="1.0"?>



<robot name="quad" xmlns:xacro="http://ros.org/wiki/xacro">
  
  <!-- Rotor joint and link -->
  <xacro:macro name="vertical_rotor"
    params="robot_namespace suffix direction motor_constant moment_constant parent mass_rotor radius_rotor time_constant_up time_constant_down max_rot_velocity motor_number rotor_drag_coefficient rolling_moment_coefficient color mesh mesh_scale *origin *inertia">
    <joint name="rotor_${motor_number}_joint" type="continuous">
      <xacro:insert_block name="origin" />
      <axis xyz="0 0 1" />
      <!-- TODO(ff): not currently set because it's not yet supported -->
      <!-- <limit effort="2000" velocity="${max_rot_velocity}" /> -->
      <parent link="${parent}" />
      <child link="rotor_${motor_number}" />

    </joint>
    <!-- TODO(ff): not currently set because it's not yet supported -->
    <!-- <gazebo reference="rotor_${motor_number}_joint"> <axis> <xyz>0 0 1</xyz>
      <limit> <velocity> ${max_rot_velocity} </velocity> </limit> </axis> </gazebo> -->
    <link name="rotor_${motor_number}">
      <inertial>
        <mass value="${mass_rotor}" /> <!-- [kg] -->
        <xacro:insert_block name="inertia" />
      </inertial>
      <visual>
        <geometry>
          <!-- <cylinder length="0.005" radius="${radius_rotor}"/> --> <!-- [m] -->
          <!-- <mesh filename="package://rotors_description/meshes/propeller_${direction}.dae" -->
          <mesh filename="package://rotors_description/meshes/${mesh}_${direction}.dae"
            scale="${mesh_scale}" />
          <!-- <box size="${2*radius_rotor} 0.01 0.005"/> -->
        </geometry>
      </visual>
      <collision>
        <geometry>
          <cylinder length="0.005" radius="${radius_rotor}" /> <!-- [m] -->
        </geometry>
      </collision>
    </link>
    <gazebo>
      <plugin name="${suffix}_motor_model" filename="librotors_gazebo_motor_model.so">
        <robotNamespace>${robot_namespace}</robotNamespace>
        <jointName>rotor_${motor_number}_joint</jointName>
        <linkName>rotor_${motor_number}</linkName>
        <turningDirection>${direction}</turningDirection>
        <timeConstantUp>${time_constant_up}</timeConstantUp>
        <timeConstantDown>${time_constant_down}</timeConstantDown>
        <maxRotVelocity>${max_rot_velocity}</maxRotVelocity>
        <motorConstant>${motor_constant}</motorConstant>
        <momentConstant>${moment_constant}</momentConstant>
        <commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
        <motorNumber>${motor_number}</motorNumber>
        <rotorDragCoefficient>${rotor_drag_coefficient}</rotorDragCoefficient>
        <rollingMomentCoefficient>${rolling_moment_coefficient}</rollingMomentCoefficient>
        <motorSpeedPubTopic>motor_speed/${motor_number}</motorSpeedPubTopic>
        <rotorVelocitySlowdownSim>${rotor_velocity_slowdown_sim}</rotorVelocitySlowdownSim>
      </plugin>
    </gazebo>
    <gazebo reference="rotor_${motor_number}">
      <material>Gazebo/${color}</material>
    </gazebo>
  </xacro:macro>
</robot>

