<?xml version="1.0"?>

<robot name="quad" xmlns:xacro="http://ros.org/wiki/xacro">
  <!--xacro:include filename="$(find qdrone_description)/urdf/component_snippets.xacro" /-->

  

  <!-- Instantiate a controller. -->
  <xacro:controller_plugin_macro
    namespace="${namespace}"
    imu_sub_topic="imu"
    >
  </xacro:controller_plugin_macro>

  <!-- Mount an ADIS16448 IMU. -->
  <xacro:imu_plugin_macro
    namespace="${namespace}"
    imu_suffix=""
    parent_link="base_link"
    imu_topic="imu"
    mass_imu_sensor="0.015"
    gyroscope_noise_density="0.0003394"
    gyroscopoe_random_walk="0.000038785"
    gyroscope_bias_correlation_time="1000.0"
    gyroscope_turn_on_bias_sigma="0.0087"
    accelerometer_noise_density="0.004"
    accelerometer_random_walk="0.006"
    accelerometer_bias_correlation_time="300.0"
    accelerometer_turn_on_bias_sigma="0.1960"
  >
    <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </xacro:imu_plugin_macro>

<!--Mount a basic camera -->
  <xacro:if value="$(arg enable_camera)">      
    <xacro:camera_macro
      namespace="${namespace}"
      parent_link="base_link"
      camera_suffix=""
      frame_rate="30"
      horizontal_fov="2"
      image_width="1280"
      image_height="720"
      image_format="R8G8B8"
      min_distance="0.02"
      max_distance="300"
      noise_mean="0.0"
      noise_stddev="0.007"
      enable_visual="true"
    >
      <box size="0.2 0.2 0.2" />
      <origin xyz="0.035 -0.035 0.028" rpy="0 0 0" />
    </xacro:camera_macro>
  </xacro:if>


<!--Mount a VI sensor (Kinect) -->
  <xacro:if value="$(arg enable_VI_sensor)">      
    <xacro:vi_sensor_depth_macro
      namespace="${namespace}"
      parent_link="base_link"
      camera_suffix=""
      frame_rate="30"      
    >
      <origin xyz="0.2 0.0 -0.04" rpy="0 0 0" />
    </xacro:vi_sensor_depth_macro>
  </xacro:if>


  <xacro:if value="$(arg enable_ground_truth)">
    <!-- Mount an IMU providing ground truth. -->
    <xacro:imu_plugin_macro
      namespace="${namespace}"
      imu_suffix="gt"
      parent_link="base_link"
      imu_topic="ground_truth/imu"
      mass_imu_sensor="0.00001"
      gyroscope_noise_density="0.0"
      gyroscopoe_random_walk="0.0"
      gyroscope_bias_correlation_time="1000.0"
      gyroscope_turn_on_bias_sigma="0.0"
      accelerometer_noise_density="0.0"
      accelerometer_random_walk="0.0"
      accelerometer_bias_correlation_time="300.0"
      accelerometer_turn_on_bias_sigma="0.0"
    >
      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
      <origin xyz="0 0 0" rpy="0 0 0" />
    </xacro:imu_plugin_macro>

    <!-- Mount a generic odometry sensor providing ground truth. -->
    <xacro:odometry_plugin_macro
      namespace="${namespace}/ground_truth"
      odometry_sensor_suffix="gt"
      parent_link="base_link"
      pose_topic="pose"
      pose_with_covariance_topic="pose_with_covariance"
      position_topic="position"
      transform_topic="transform"
      odometry_topic="odometry"
      parent_frame_id="world"
      mass_odometry_sensor="0.00001"
      measurement_divisor="1"
      measurement_delay="0"
      unknown_delay="0.0"
      noise_normal_position="0 0 0"
      noise_normal_quaternion="0 0 0"
      noise_normal_linear_velocity="0 0 0"
      noise_normal_angular_velocity="0 0 0"
      noise_uniform_position="0 0 0"
      noise_uniform_quaternion="0 0 0"
      noise_uniform_linear_velocity="0 0 0"
      noise_uniform_angular_velocity="0 0 0"
      enable_odometry_map="false"
      odometry_map=""
      image_scale=""
    >
      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" /> <!-- [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] -->
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
    </xacro:odometry_plugin_macro>
  </xacro:if>



  <xacro:if value="$(arg enable_logging)">
    <!-- Instantiate a logger -->
    <xacro:bag_plugin_macro
      namespace="${namespace}"
      bag_file="$(arg log_file)"
      rotor_velocity_slowdown_sim="${rotor_velocity_slowdown_sim}"
    >
    </xacro:bag_plugin_macro>
  </xacro:if>

</robot>

