<?xml version="1.0"?>

<robot xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:macro name="sonar_sensor" params="name parent *origin ros_topic update_rate min_range max_range field_of_view ray_count">
    <joint name="${name}_joint" type="fixed">
      <xacro:insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_link"/>
    </joint>

    <link name="${name}_link">
      <inertial>
        <mass value="0.001" />
        <origin xyz="0 0 0" rpy="0 0 0" />
        <inertia ixx="0.000000017" ixy="0" ixz="0" iyy="0.000000017" iyz="0" izz="0.000000017" />
      </inertial>
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <!--<box size="0.01 0.01 0.01" /> -->
          <mesh filename="package://hector_sensors_description/meshes/sonar_sensor/max_sonar_ez4.dae"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.01 0.01 0.01" />
        </geometry>
      </collision>
    </link>

    <gazebo reference="${name}_link">
      <sensor type="ray" name="${name}">
        <always_on>true</always_on>
        <update_rate>${update_rate}</update_rate>
        <pose>0 0 0 0 0 0</pose>
        <visualize>false</visualize>
        <ray>
          <scan>
            <horizontal>
              <samples>${ray_count}</samples>
              <resolution>1</resolution>
              <min_angle>-${field_of_view/2}</min_angle>
              <max_angle> ${field_of_view/2}</max_angle>
            </horizontal>
            <vertical>
              <samples>${ray_count}</samples>
              <resolution>1</resolution>
              <min_angle>-${field_of_view/2}</min_angle>
              <max_angle> ${field_of_view/2}</max_angle>
            </vertical>
          </scan>
          <range>
            <min>${min_range}</min>
            <max>${max_range}</max>
            <resolution>0.01</resolution>
          </range>
        </ray>

        <plugin name="gazebo_ros_${name}_controller" filename="libhector_gazebo_ros_sonar.so">
          <gaussianNoise>0.005</gaussianNoise>
          <topicName>${ros_topic}</topicName>
          <frameId>${name}_link</frameId>
        </plugin>
      </sensor>
    </gazebo>
  </xacro:macro>
</robot>
