<?xml version="1.0"?>

<robot xmlns:xacro="http://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 *origin">
    <joint name="${name}_joint" type="fixed">
      <xacro:insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_frame"/>
    </joint>
    
    <link name="${name}_frame">
      <!--        <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/thermaleye_camera/thermaleye_camera_hector_v2.dae"/>
      </geometry>
      </visual>
      <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
      <mesh filename="package://hector_sensors_description/meshes/thermaleye_camera/thermaleye_camera_hector_v2.stl"/>
      </geometry>
      </collision>-->
    </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>160</width>
            <height>120</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</imageTopicName>
          <cameraInfoTopicName>/${name}/camera_info</cameraInfoTopicName>
          <frameName>${name}_optical_frame</frameName>
        </plugin>
      </sensor>
    </gazebo>
    
  </xacro:macro>
</robot>

