<?xml version="1.0" encoding="UTF-8"?>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    
  <xacro:include filename="$(find hector_xacro_tools)/urdf/sensor_macros.urdf.xacro" />
  
  <xacro:macro name="seek_thermal_compact_camera_model" params="name parent *origin add_geometry">
      
    <joint name="${name}_joint" type="fixed">
      <xacro:insert_block name="origin" />
      <parent link="${parent}" />
      <child link="${name}_frame" />
    </joint>
    
    <link name="${name}_frame">
      <xacro:inertial_sphere mass="0.03" diameter="0.02" />
      
      <xacro:if value="${add_geometry}">

        <visual>
            <origin xyz="0 0.0 -0.025" rpy="0 0 0" />
            <geometry>
            <box size="0.02 0.03 0.0375" />
            </geometry>
            <material name="DarkGrey">
            <color rgba="0.3 0.3 0.3 1" />
            </material>
        </visual>
        <visual>
            <origin xyz="0 0 0" rpy="0 ${M_PI/2} 0" />
            <geometry>
            <cylinder length="0.02" radius="0.017" />
            </geometry>
            <material name="DarkGrey" />
        </visual>
        <visual>
            <origin xyz="0.01 0 0" rpy="0 ${M_PI/2} 0" />
            <geometry>
            <cylinder length="0.018" radius="0.011" />
            </geometry>
            <material name="DarkGrey" />
        </visual>
        
        <collision>
            <origin xyz="0.0055 0 -0.013375" rpy="0 0 0" />
            <geometry>
            <box size="0.031 0.034 0.06075" />
            </geometry>
        </collision>
                
        <xacro:if value="$(arg add_raycast_self_filter_geom)">
            <collision>
            <origin xyz="0.0055 0 -0.013375" rpy="0 0 0" />
            <geometry>
                <cylinder length="0.06075" radius="0.023"/>
            </geometry>
            </collision>
        </xacro:if>
      </xacro:if>

    </link>
    
    <xacro:add_optical_frame name="${name}" />
    
    <gazebo reference="${name}_frame">
      <material>Gazebo/Grey</material>
    </gazebo>
    
  </xacro:macro>
  
  <xacro:macro name="seek_thermal_compact_gazebo_plugin_macro" params="name parent *origin">

    <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>206</width>
            <height>156</height>
          </image>
          <clip>
            <near>0.01</near>
            <far>10</far>
          </clip>
        </camera>
        <plugin name="thermal_camera_controller" filename="libgazebo_ros_thermal_camera.so">
          <alwaysOn>true</alwaysOn>
          <updateRate>10</updateRate>
          <cameraName>${name}</cameraName>
          <imageTopicName>image_raw</imageTopicName>
          <cameraInfoTopicName>camera_info</cameraInfoTopicName>
          <frameName>${name}_optical_frame</frameName>
        </plugin>
      </sensor>
    </gazebo>
    
  </xacro:macro>
  
  <xacro:macro name="seek_thermal_compact_camera" params="name parent *origin">
      
    <xacro:seek_thermal_compact_camera_model name="${name}" parent="${parent}" add_geometry="true">
      <xacro:insert_block name="origin" />
    </xacro:seek_thermal_compact_camera_model>
    
    <xacro:seek_thermal_compact_gazebo_plugin_macro name="${name}" parent="${parent}">
      <xacro:insert_block name="origin" />
    </xacro:seek_thermal_compact_gazebo_plugin_macro>
    
  </xacro:macro>
  
  <xacro:macro name="seek_thermal_compact_camera_no_geom" params="name parent *origin">
      
    <xacro:seek_thermal_compact_camera_model name="${name}" parent="${parent}" add_geometry="false">
      <xacro:insert_block name="origin" />
    </xacro:seek_thermal_compact_camera_model>
    
    <xacro:seek_thermal_compact_gazebo_plugin_macro name="${name}" parent="${parent}">
      <xacro:insert_block name="origin" />
    </xacro:seek_thermal_compact_gazebo_plugin_macro>
    
  </xacro:macro>
</robot>
