<?xml version="1.0"?>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:include filename="$(find hector_xacro_tools)/urdf/sensor_macros.urdf.xacro" />

  <xacro:macro name="generic_thermal_camera" params="name parent resolution_x:=320 resolution_y:=256 *origin">
    <joint name="${name}_joint" type="fixed">
      <xacro:insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_frame"/>
    </joint>

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

    <xacro:add_optical_frame name="${name}" />

    <gazebo reference="${name}_frame">
      <sensor type="camera" name="thermal_camera_sensor">
        <update_rate>10</update_rate>

        <camera>
          <horizontal_fov>${90.0*M_PI/180.0}</horizontal_fov>
          <image>
            <format>R8G8B8</format>
            <width>${resolution_x}</width>
            <height>${resolution_y}</height>
          </image>
          <clip>
            <near>0.01</near>
            <far>100</far>
          </clip>
        </camera>

        <plugin name="thermal_camera_controller" filename="libgazebo_ros_thermal_camera.so">
          <alwaysOn>true</alwaysOn>
          <updateRate>10</updateRate>
          <imageTopicName>/${name}/image_raw</imageTopicName>
          <cameraInfoTopicName>/${name}/camera_info</cameraInfoTopicName>
          <frameName>${name}_optical_frame</frameName>
        </plugin>
      </sensor>
    </gazebo>

  </xacro:macro>
</robot>
