<?xml version="1.0"?>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="imu">
    <xacro:macro name="IMU" params="connected_to imu_name xyz:='0 0 0' rpy:='0 0 0' ">
        <!-- Imu is fixed to the base link -->
        <joint name="${imu_name}_joint" type="fixed">
            <origin xyz="${xyz}" rpy="${rpy}"/>
            <parent link="${connected_to}"/>
            <child link="${imu_name}"/>
        </joint>
        <!-- Imu link -->
        <link name="${imu_name}">
            <inertial>
                <mass value="0.01"/>
                <origin rpy="0 0 0" xyz="0 0 0"/>
                <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
            </inertial>
            <visual>
                <origin rpy="0 0 0" xyz="0 0 0"/>
                <geometry>
                    <box size="0.015 0.015 0.004"/>
                </geometry>
            </visual>
            <material name="orange">
                <color rgba="255 108 10 255"/>
            </material>
        </link>
        <gazebo reference="${imu_name}">
            <sensor name="imu_sensor" type="imu">
                <always_on>1</always_on>
                <update_rate>1000</update_rate>
                <visualize>true</visualize>
                <topic>imu</topic>
                <imu>
                    <angular_velocity>
                        <x>
                            <noise type="gaussian">
                                <mean>0.0</mean>
                                <stddev>2e-4</stddev>
                                <bias_mean>0.0000075</bias_mean>
                                <bias_stddev>0.0000008</bias_stddev>
                            </noise>
                        </x>
                        <y>
                            <noise type="gaussian">
                                <mean>0.0</mean>
                                <stddev>2e-4</stddev>
                                <bias_mean>0.0000075</bias_mean>
                                <bias_stddev>0.0000008</bias_stddev>
                            </noise>
                        </y>
                        <z>
                            <noise type="gaussian">
                                <mean>0.0</mean>
                                <stddev>2e-4</stddev>
                                <bias_mean>0.0000075</bias_mean>
                                <bias_stddev>0.0000008</bias_stddev>
                            </noise>
                        </z>
                    </angular_velocity>
                    <linear_acceleration>
                        <x>
<!--                            <noise type="gaussian">-->
<!--                                <mean>0.0</mean>-->
<!--                                <stddev>1.7e-2</stddev>-->
<!--                                <bias_mean>0.1</bias_mean>-->
<!--                                <bias_stddev>0.001</bias_stddev>-->
<!--                            </noise>-->
                        </x>
                        <y>
<!--                            <noise type="gaussian">-->
<!--                                <mean>0.0</mean>-->
<!--                                <stddev>1.7e-2</stddev>-->
<!--                                <bias_mean>0.1</bias_mean>-->
<!--                                <bias_stddev>0.001</bias_stddev>-->
<!--                            </noise>-->
                        </y>
                        <z>
<!--                            <noise type="gaussian">-->
<!--                                <mean>0.0</mean>-->
<!--                                <stddev>1.7e-2</stddev>-->
<!--                                <bias_mean>0.1</bias_mean>-->
<!--                                <bias_stddev>0.001</bias_stddev>-->
<!--                            </noise>-->
                        </z>
                    </linear_acceleration>
                </imu>
            </sensor>
        </gazebo>
        <gazebo reference="${imu_name}_joint">
            <disableFixedJointLumping>true</disableFixedJointLumping>
        </gazebo>
    </xacro:macro>
</robot>
