<?xml version="1.0"?>
<robot name="sensor_imu" xmlns:xacro="http://ros.org/wiki/xacro"
                         xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
                         xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">

  <xacro:macro name="sensor_imu" params="name parent *origin size">
    <joint name="${name}_joint" type="fixed">
        <xacro:insert_block name="origin"/>
        <axis xyz="1 0 0"/>
        <parent link="${parent}"/>
        <child link="${name}_link"/>
    </joint>
    
    <link name="${name}_link">
        <inertial>
          <mass value="0.001"/>
          <origin rpy="0 0 0" xyz="0 0 0"/>
          <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
        </inertial>
        <visual>
          <origin rpy="0 0 0" xyz="0 0 0"/>
          <geometry>
            <box size="${size}"/>
          </geometry>
        </visual>
        <collision>
          <origin rpy="0 0 0" xyz="0 0 0"/>
          <geometry>
            <box size="${size}"/>
          </geometry>
        </collision>
    </link>
    
    <gazebo reference="${name}_link">
      <sensor type="imu" name="imu">
        <always_on>true</always_on>
        <topic>imu</topic>
        <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
          <bodyName>${name}_link</bodyName> <!-- bodyName always set base_link by URDF parser -->
          <frameName>${name}_link</frameName>
          <topicName>imu</topicName>
          <updateRateHZ>10.0</updateRateHZ>
          <gaussianNoise>0.001</gaussianNoise>
          <xyzOffset>0 0 0</xyzOffset>
          <rpyOffset>0 0 0</rpyOffset>
        </plugin>
      </sensor>
      <material>Gazebo/White</material>
    </gazebo>
  </xacro:macro>

</robot>
