<?xml version="1.0"?>

<robot name="vision_box_plugins" xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
  
  <xacro:macro name="vision_box_common_gazebo_plugins">
    
    <gazebo>
      <plugin name="camera_servo_controller" filename="libhector_servo_plugin.so">
        <alwaysOn>true</alwaysOn>
        <controlRate>50.0</controlRate>
        <updateRate>0.0</updateRate>
        <firstServoName>camera/z</firstServoName>
        <firstServoAxis>0 0 1</firstServoAxis>
        <secondServoName>camera/y</secondServoName>
        <secondServoAxis>0 1 0</secondServoAxis>
        <torque>0.5</torque>
        <robotNamespace>/</robotNamespace>
        <topicName>camera/command</topicName>
        <jointStateName>joint_states</jointStateName>
        <proportionalControllerGain>8.0</proportionalControllerGain>
        <derivativeControllerGain>0.0</derivativeControllerGain>
        <maxVelocity>2.0</maxVelocity>
      </plugin>
    
      <plugin name="laser1_servo_controller" filename="libhector_servo_plugin.so">
        <alwaysOn>true</alwaysOn>
        <controlRate>50.0</controlRate>
        <updateRate>0.0</updateRate>
        <firstServoName>laser1/x</firstServoName>
        <firstServoAxis>1 0 0</firstServoAxis>
        <secondServoName>laser1/y</secondServoName>
        <secondServoAxis>0 1 0</secondServoAxis>
        <torque>0.5</torque>
        <robotNamespace>/</robotNamespace>
        <topicName>laser1/command</topicName>
        <jointStateName>joint_states</jointStateName>
        <proportionalControllerGain>8.0</proportionalControllerGain>
        <derivativeControllerGain>0.0</derivativeControllerGain>
        <maxVelocity>0.0</maxVelocity>
      </plugin>
    </gazebo>
    
    <gazebo reference="openni_depth_frame">
      <sensor type="depth" name="kinect_camera">
        <update_rate>10</update_rate>
        
        <camera>
          <horizontal_fov>${60.0*M_PI/180.0}</horizontal_fov>
          <image>
            <format>R8G8B8</format>
            <width>320</width>
            <height>240</height>
          </image>
          <clip>
            <near>0.05</near>
            <far>3</far>
          </clip>
        </camera>
        
        <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
          <alwaysOn>true</alwaysOn>
          <updateRate>10</updateRate>
          <imageTopicName>openni/rgb/image_color</imageTopicName>
          <cameraInfoTopicName>openni/camera_info</cameraInfoTopicName>
          <depthImageTopicName>openni/depth/image_raw</depthImageTopicName>
          <depthImageCameraInfoTopicName>openni/depth/camera_info</depthImageCameraInfoTopicName>
          <pointCloudTopicName>openni/depth/points</pointCloudTopicName>
          <frameName>openni_depth_optical_frame</frameName>
          <pointCloudCutoff>0.5</pointCloudCutoff>
          <distortionK1>0</distortionK1>
          <distortionK2>0</distortionK2>
          <distortionK3>0</distortionK3>
          <distortionT1>0</distortionT1>
          <distortionT2>0</distortionT2>
          <CxPrime>0</CxPrime>
          <Cx>0</Cx>
          <Cy>0</Cy>
          <focalLength>0</focalLength>
          <hackBaseline>0</hackBaseline>
        </plugin>
      </sensor>
    </gazebo>
    
    <gazebo reference="infrared_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>/thermal/image</imageTopicName>
          <cameraInfoTopicName>/thermal/camera_info</cameraInfoTopicName>
          <frameName>infrared_optical_frame</frameName>
        </plugin>
      </sensor>
    </gazebo>
    
  </xacro:macro>
</robot>
