<?xml version="1.0"?>
<robot name="quad" xmlns:xacro="http://ros.org/wiki/xacro">
 
<xacro:property name="PI" value="3.1415926535897931"/>
<xacro:property name="cameraSize" value="0.05"/>
<xacro:property name="cameraMass" value="0.1"/>

<xacro:property name="mesh_file" value="hummingbird.dae" />
<xacro:property name="namespace" value="quad" />
<xacro:property name="body_width" value="0.3" /> <!-- [m] -->
<xacro:property name="body_height" value="0.17" /> <!-- [m] -->
<xacro:property name="mesh_scale" value="1 1 1"/>
<xacro:property name="mass" value="1.40" /> <!-- [kg] -->
<xacro:property name="motor_constant" value="2.39e-05" /> <!-- [kg.m/s^2] -->
<xacro:property name="moment_constant" value="0.016" /> <!-- [m] -->
<xacro:property name="mass_rotor" value="0.005" /> <!-- [kg] -->
<xacro:property name="radius_rotor" value="0.14" /> <!-- [m] -->
<xacro:property name="time_constant_up" value="0.15" /> <!-- [s] -->
<xacro:property name="time_constant_down" value="0.15" /> <!-- [s] -->
<xacro:property name="max_rot_velocity" value="770" /> <!-- [rad/s] -->
<xacro:property name="rolling_moment_coefficient" value="0.000001" />
<xacro:property name="mesh_scale_prop" value=".12 .12 .12"/>
<xacro:property name="arm_length" value="0.17" /> <!-- [m] -->
<xacro:property name="rotor_offset_top" value="0.01" /> <!-- [m] --> <!--0.91-->
<xacro:property name="rotor_drag_coefficient" value="1.3858e-06" />
<xacro:property name="rotor_velocity_slowdown_sim" value="10" />
<xacro:property name="color" value="blue" />

<xacro:include filename="$(find qdrone_description)/urdf/qdrone.gazebo" />
<xacro:include filename="$(find qdrone_description)/urdf/materials.xacro" />
<xacro:include filename="$(find qdrone_description)/urdf/macros.xacro" />
<xacro:include filename="$(find qdrone_description)/urdf/component_snippets.xacro" />
<xacro:include filename="$(find qdrone_description)/urdf/qdrone_base.xacro" />



<!--xacro:include filename="$(find awc_description)/urdf/sensors/kinect.urdf.xacro"/-->


<!--xacro:include filename="$(find qdrone_description)/urdf/sensors/asus_xtion_pro.urdf.xacro"/-->
<xacro:include filename="$(find zed_wrapper)/urdf/zed_test.xacro"/>


<!-- Property Blocks -->
  <xacro:property name="body_inertia">
    <inertia ixx="0.05" ixy="0.0" ixz="0.0" iyy="0.05" iyz="0.0" izz="0.24" /> <!-- [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] -->
  </xacro:property>


<!-- inertia of a single rotor, assuming it is a cuboid. Height=3mm, width=15mm -->
  <xacro:property name="rotor_inertia">
    <inertia
    ixx="${1/12 * mass_rotor * (0.015 * 0.015 + 0.003 * 0.003) * rotor_velocity_slowdown_sim}"
    iyy="${1/12 * mass_rotor * (4 * radius_rotor * radius_rotor + 0.003 * 0.003) * rotor_velocity_slowdown_sim}"
    izz="${1/12 * mass_rotor * (4 * radius_rotor * radius_rotor + 0.015 * 0.015) * rotor_velocity_slowdown_sim}"
    ixy="0.0" ixz="0.0"  iyz="0.0" /> <!-- [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] -->
  </xacro:property>

 <!-- Main multirotor link -->
<!--link name="base_link"></link-->

    <!--joint name="base_joint" type="fixed">
      <origin xyz="0 0 0" rpy="0 0 0" />
      <parent link="base_link" />
      <child link="footprint" />
    </joint-->

    <link name="base_link">
      <inertial>
        <mass value="${mass}" />  <!-- [kg] -->
        <origin xyz="0 0 -1.57" />
        <xacro:insert_block name="body_inertia" />
      </inertial>

      <visual>
        <origin xyz="0 0 0" rpy="0 0 -1.57" />
        <geometry>
          <mesh filename="package://qdrone_description/meshes/dronebase.dae"
            scale="${mesh_scale}" />
        </geometry>
        <material name = "blue"/>
      </visual>

      <collision>
        <origin xyz="0 0 0" rpy="0 0 -1.57" />
        <geometry>
         <mesh filename="package://qdrone_description/meshes/dronebase.dae"
            scale="${mesh_scale}" />
        </geometry>
      </collision>
    </link>
    <!-- attach multirotor_base_plugin to the base_link -->
    <gazebo>
      <plugin filename="librotors_gazebo_multirotor_base_plugin.so" name="rosbag">
        <robotNamespace>${namespace}</robotNamespace>
        <linkName>base_link</linkName>
        <rotorVelocitySlowdownSim>${rotor_velocity_slowdown_sim}</rotorVelocitySlowdownSim>
      </plugin>
    </gazebo>

    <gazebo reference="base_link">
      <material>Gazebo/${color}</material>
    </gazebo>

<!-- kinect/asus -->
<!--sensor_kinect parent="seat"/-->
<!--sensor_asus_xtion_pro parent="base_link"/-->
<sensor_zed_camera parent="base_link"/>


<!-- Hokuyo Laser -->

  <!--link name="laser">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://qdrone_description/meshes/hokuyo.dae"/>
      </geometry>
    <material name="red" />
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
    <box size="0.1 0.1 0.1"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1e-5" />
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
    </inertial>
  </link-->

<!--joint name="hokuyo_joint" type="fixed">
    <axis xyz="0 1 0" />
    <origin xyz="0.0 0 -0.09" rpy="3.1416 0 0"/>
    <parent link="base_link"/>
    <child link="laser"/>
  </joint-->

 <!-- Instantiate rotors -->
  <xacro:vertical_rotor
    robot_namespace="${namespace}"
    suffix="front"
    direction="ccw"
    motor_constant="${motor_constant}"
    moment_constant="${moment_constant}"
    parent="base_link"
    mass_rotor="${mass_rotor}"
    radius_rotor="${radius_rotor}"
    time_constant_up="${time_constant_up}"
    time_constant_down="${time_constant_down}"
    max_rot_velocity="${max_rot_velocity}"
    motor_number="0"
    rotor_drag_coefficient="${rotor_drag_coefficient}"
    rolling_moment_coefficient="${rolling_moment_coefficient}"
    mesh ="propeller"
    mesh_scale="${mesh_scale_prop}"
    color="Blue">
    <origin xyz="${arm_length} 0 ${rotor_offset_top}" rpy="0 0 0" />
    <xacro:insert_block name="rotor_inertia" />
  </xacro:vertical_rotor>

  <xacro:vertical_rotor 
    robot_namespace="${namespace}"
    suffix="right"
    direction="cw"
    motor_constant="${motor_constant}"
    moment_constant="${moment_constant}"
    parent="base_link"
    mass_rotor="${mass_rotor}"
    radius_rotor="${radius_rotor}"
    time_constant_up="${time_constant_up}"
    time_constant_down="${time_constant_down}"
    max_rot_velocity="${max_rot_velocity}"
    motor_number="1"
    rotor_drag_coefficient="${rotor_drag_coefficient}"
    rolling_moment_coefficient="${rolling_moment_coefficient}"
    mesh ="propeller"
    mesh_scale="${mesh_scale_prop}"
    color="Grey">
    <origin xyz="0 -${arm_length} ${rotor_offset_top}" rpy="0 0 0" />
    <xacro:insert_block name="rotor_inertia" />
  </xacro:vertical_rotor>

  <xacro:vertical_rotor 
    robot_namespace="${namespace}"
    suffix="back"
    direction="ccw"
    motor_constant="${motor_constant}"
    moment_constant="${moment_constant}"
    parent="base_link"
    mass_rotor="${mass_rotor}"
    radius_rotor="${radius_rotor}"
    time_constant_up="${time_constant_up}"
    time_constant_down="${time_constant_down}"
    max_rot_velocity="${max_rot_velocity}"
    motor_number="2"
    rotor_drag_coefficient="${rotor_drag_coefficient}"
    rolling_moment_coefficient="${rolling_moment_coefficient}"
    mesh ="propeller"
    mesh_scale="${mesh_scale_prop}"
    color="Grey">
    <origin xyz="-${arm_length} 0 ${rotor_offset_top}" rpy="0 0 0" />
    <xacro:insert_block name="rotor_inertia" />
  </xacro:vertical_rotor>

  <xacro:vertical_rotor
    robot_namespace="${namespace}"
    suffix="left"
    direction="cw"
    motor_constant="${motor_constant}"
    moment_constant="${moment_constant}"
    parent="base_link"
    mass_rotor="${mass_rotor}"
    radius_rotor="${radius_rotor}"
    time_constant_up="${time_constant_up}"
    time_constant_down="${time_constant_down}"
    max_rot_velocity="${max_rot_velocity}"
    motor_number="3"
    rotor_drag_coefficient="${rotor_drag_coefficient}"
    rolling_moment_coefficient="${rolling_moment_coefficient}"
    mesh ="propeller"
    mesh_scale="${mesh_scale_prop}"
    color="Grey">
    <origin xyz="0 ${arm_length} ${rotor_offset_top}" rpy="0 0 0" />
    <xacro:insert_block name="rotor_inertia" />
  </xacro:vertical_rotor>







</robot>
