<?xml version="1.0"?>
<robot name="sensor_rgb" xmlns:xacro="http://ros.org/wiki/xacro">

    <!-- Xacro properties -->
    <xacro:property name="astra_cam_py" value="-0.0125"/>
    <xacro:property name="astra_depth_rel_rgb_py" value="0.0250"/>
    <xacro:property name="astra_cam_rel_rgb_py" value="-0.0125"/>
    <xacro:property name="astra_dae_display_scale" value="0.1"/>
    <!-- Parameterised in part by the values in turtlebot_properties.urdf.xacro -->
    <xacro:macro name="sensor_rgb" params="parent prefix *origin scale">
        <joint name="${prefix}_joint" type="fixed">
            <xacro:insert_block name="origin"/>
            <parent link="${parent}"/>
            <child link="${prefix}_link"/>
        </joint>
        <link name="${prefix}_link">
            <visual>
                <origin xyz="-0.03 -0.06 0" rpy="${pi} 0 ${pi/2}"/>
                <geometry>
                    <mesh filename="package://larcc_description/meshes/sensors/astra2.dae" scale="${scale}"/>
                </geometry>
            </visual>
            <!--      <collision>-->
            <!--        &lt;!&ndash; <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> &ndash;&gt;-->
            <!--        <origin xyz="-0.01 -0.06 0" rpy="0 0 0"/>-->
            <!--        <geometry>-->
            <!--        <box size="0.0400 0.1650 0.0300"/>-->
            <!--      </geometry>-->
            <!--      </collision>-->
            <inertial>
                <mass value="0.564"/>
                <origin xyz="0 0 0"/>
                <inertia ixx="0.003881243" ixy="0.0" ixz="0.0"
                         iyy="0.000498940" iyz="0.0"
                         izz="0.003879257"/>
            </inertial>
        </link>
        <gazebo reference="${prefix}_link">
            <selfCollide>false</selfCollide>
        </gazebo>

        <joint name="${prefix}_rgb_joint" type="fixed">
            <origin xyz="0 -0.045 0" rpy="0 0 0"/>
            <parent link="${prefix}_link"/>
            <child link="${prefix}_rgb_frame"/>
        </joint>
        <link name="${prefix}_rgb_frame"/>

        <joint name="${prefix}_rgb_optical_joint" type="fixed">
            <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
            <parent link="${prefix}_rgb_frame"/>
            <child link="${prefix}_rgb_optical_frame"/>
        </joint>
        <link name="${prefix}_rgb_optical_frame"/>

        <joint name="${prefix}_depth_joint" type="fixed">
            <origin xyz="0 -0.02 0" rpy="0 0 0"/>
            <parent link="${prefix}_link"/>
            <child link="${prefix}_depth_frame"/>
        </joint>
        <link name="${prefix}_depth_frame"/>

        <joint name="${prefix}_depth_optical_joint" type="fixed">
            <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
            <parent link="${prefix}_depth_frame"/>
            <child link="${prefix}_depth_optical_frame"/>
        </joint>
        <link name="${prefix}_depth_optical_frame"/>


        <gazebo reference="${prefix}_rgb_optical_frame">
            <sensor type="camera" name="${prefix}">
                <update_rate>20.0</update_rate>
                <camera name="head">
                    <horizontal_fov>${60.0*pi/180.0}</horizontal_fov>
                    <image>
                        <width>1200</width>
                        <height>800</height>
                        <format>R8G8B8</format>
                    </image>
                    <clip>
                        <near>0.1</near>
                        <far>8.0</far>
                    </clip>
                    <noise>
                        <type>gaussian</type>
                        <!-- Noise is sampled independently per pixel on each frame.
                             That pixel's noise value is added to each of its color
                             channels, which at that point lie in the range [0,1]. -->
                        <mean>0.0</mean>
                        <stddev>0.007</stddev>
                    </noise>
                </camera>
                <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
                    <alwaysOn>true</alwaysOn>
                    <updateRate>0.0</updateRate>
                    <cameraName>${prefix}</cameraName>
                    <imageTopicName>rgb/image_raw</imageTopicName>
                    <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
                    <frameName>${prefix}_rgb_optical_frame</frameName>
                    <hackBaseline>0.0</hackBaseline>
                    <distortionK1>0.0</distortionK1>
                    <distortionK2>0.0</distortionK2>
                    <distortionK3>0.0</distortionK3>
                    <distortionT1>0.0</distortionT1>
                    <distortionT2>0.0</distortionT2>
                </plugin>
            </sensor>
        </gazebo>

        <!--     <gazebo reference="${prefix}_depth_frame">-->
        <!--          <sensor type="depth" name="camera">-->
        <!--              <always_on>true</always_on>-->
        <!--              <update_rate>20</update_rate>-->
        <!--              <camera>-->
        <!--                  <horizontal_fov>${60.0*pi/180.0}</horizontal_fov>-->
        <!--                  <image>-->
        <!--                      <format>B8G8R8</format>-->
        <!--&lt;!&ndash;                      <format>RGB_INT8</format>&ndash;&gt;-->
        <!--                      <width>1280</width>-->
        <!--                      <height>800</height>-->
        <!--                  </image>-->
        <!--                  <clip>-->
        <!--                      <near>0.10</near>-->
        <!--                      <far>8.0</far>-->
        <!--                  </clip>-->
        <!--              </camera>-->
        <!--              <plugin name="${prefix}_camera_controller" filename="libgazebo_ros_openni_kinect.so">-->
        <!--                  <cameraName>${prefix}</cameraName>-->
        <!--                  <alwaysOn>true</alwaysOn>-->
        <!--                  <updateRate>0</updateRate>-->
        <!--                  <imageTopicName>rgb/image_raw</imageTopicName>-->
        <!--                  <depthImageTopicName>depth/image_raw</depthImageTopicName>-->
        <!--                  <pointCloudTopicName>depth/points</pointCloudTopicName>-->
        <!--                  <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>-->
        <!--                  <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>-->
        <!--&lt;!&ndash;                  <frameName>${prefix}_depth_optical_frame</frameName>&ndash;&gt;-->
        <!--                  <frameName>${prefix}_rgb_optical_frame</frameName>-->
        <!--&lt;!&ndash;                  <frameName>${prefix}_link</frameName>&ndash;&gt;-->
        <!--                  <baseline>0.1</baseline>-->
        <!--                  <distortion_k1>0.0</distortion_k1>-->
        <!--                  <distortion_k2>0.0</distortion_k2>-->
        <!--                  <distortion_k3>0.0</distortion_k3>-->
        <!--                  <distortion_t1>0.0</distortion_t1>-->
        <!--                  <distortion_t2>0.0</distortion_t2>-->
        <!--                  <pointCloudCutoff>0.4</pointCloudCutoff>-->
        <!--              </plugin>-->
        <!--          </sensor>-->
        <!--      </gazebo>-->
    </xacro:macro>
</robot>
