<?xml version="1.0"?>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:include filename="$(find hector_xacro_tools)/urdf/inertia_tensors.urdf.xacro"/>
    <xacro:property name="M_PI" value="3.1415926535897931" />

    <xacro:macro name="kinect_camera_model" params="name parent *origin">
      <joint name="${name}_joint" type="fixed">
        <xacro:insert_block name="origin" />
        <parent link="${parent}"/>
        <child link="${name}_link"/>
      </joint>

      <link name="${name}_link">
        <xacro:inertial_sphere mass="0.01" diameter="0.07" />
        <visual>
          <origin xyz="0 0 0" rpy="0 0 0" />
          <geometry>
            <mesh filename="package://hector_sensors_description/meshes/kinect_camera/kinect_camera_simple.dae"/>
          </geometry>
        </visual>
        <collision>
          <origin xyz="0 0 0" rpy="0 0 0" />
          <geometry>
            <mesh filename="package://hector_sensors_description/meshes/kinect_camera/kinect_camera_simple.stl"/>
          </geometry>
        </collision>
      </link>

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

      <link name="${name}_depth_frame"/>

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

      <link name="${name}_depth_optical_frame"/>

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

      <link name="${name}_rgb_frame"/>

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

      <link name="${name}_rgb_optical_frame"/>

    </xacro:macro>

    <xacro:macro name="kinect_camera" params="name parent *origin">
      <xacro:kinect_camera_model name="${name}" parent="${parent}">
        <xacro:insert_block name="origin" />
      </xacro:kinect_camera_model>

      <gazebo reference="${name}_depth_frame">
        <sensor type="depth" name="${name}">
          <update_rate>20</update_rate>
          <camera>
            <horizontal_fov>${60 * M_PI/180.0}</horizontal_fov>
            <image>
              <format>B8G8R8</format>
              <width>640</width>
              <height>480</height>
            </image>
            <clip>
              <near>0.10</near>
              <far>8.0</far>
            </clip>
          </camera>
          <plugin name="${name}_camera_controller" filename="libgazebo_ros_openni_kinect.so">
            <imageTopicName>null</imageTopicName>
            <cameraInfoTopicName>null/camera_info</cameraInfoTopicName>
            <depthImageTopicName>${name}/depth/image_raw</depthImageTopicName>
            <depthImageCameraInfoTopicName>${name}/depth/camera_info</depthImageCameraInfoTopicName>
            <pointCloudTopicName>${name}/depth/points</pointCloudTopicName>
            <frameName>${name}_depth_optical_frame</frameName>
          </plugin>
        </sensor>
      </gazebo>



    <gazebo reference="${name}_rgb_frame">
            <sensor type="camera" name="${name}">
                <update_rate>20.0</update_rate>
                <camera>
                    <!--                    <pose>0 0 0 ${M_PI/2} ${-M_PI/2} 0</pose>-->
                    <horizontal_fov>${60.0*pi/180.0}</horizontal_fov>
                    <image>
                        <format>B8G8R8</format>
                        <!--                      <format>RGB_INT8</format>-->
                        <width>640</width>
                        <height>480</height>
                    </image>
                    <clip>
                        <near>0.10</near>
                        <far>8.0</far>
                    </clip>
                </camera>
                <plugin name="${name}}_rgb_camera_controller" filename="libgazebo_ros_camera.so">
                    <!-- <cameraName>${name}</cameraName> -->
                    <alwaysOn>true</alwaysOn>
                    <updateRate>0</updateRate>
                    <imageTopicName>${name}/rgb/image_raw</imageTopicName>
                    <!--                    <depthImageTopicName>depth/image_raw</depthImageTopicName>-->
                    <!--                    <pointCloudTopicName>depth/points</pointCloudTopicName>-->
                    <cameraInfoTopicName>${name}/rgb/camera_info</cameraInfoTopicName>
                    <!--                    <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>-->
                    <!--                    <frameName>${prefix}_depth_optical_frame</frameName>-->
                    <frameName>${name}_rgb_optical_frame</frameName>
                    <!--                  <frameName>${prefix}_link</frameName>-->
                    <!-- Apparently this is not used-->
                    <!-- <baseline>0.1</baseline> -->
                    <!--                  <hackBaseline>2</hackBaseline>-->
                    <!-- <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>