<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >

    <joint name="camera_joint" type="fixed">
        <parent link="chassis"/>
        <child link="camera_link"/>
        <origin xyz="-0.996 0.0696 1.04" rpy="${11.8*deg_to_rad} ${20.2*deg_to_rad} ${22.5*deg_to_rad}"/>
    </joint>

    <link name="camera_link">
        <visual>
            <geometry>
                <box size="0.03 0.03 0.03"/>
            </geometry>
            <material name="black"/>
        </visual>
    </link>


    <joint name="camera_optical_joint" type="fixed">
        <parent link="camera_link"/>
        <child link="camera_link_optical"/>
        <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
    </joint>

    <link name="camera_link_optical"></link>



    <gazebo reference="camera_link">
        <material>Gazebo/Black</material>
        <sensor name="segmentation_camera" type="segmentation_camera">
          <ignition_frame_id>camera_link_optical</ignition_frame_id>
          <topic>semantic</topic>
          <camera>
            <horizontal_fov>${84.7*deg_to_rad}</horizontal_fov>
            <vertical_fov>${66.1*deg_to_rad}</vertical_fov>
            <image>
              <width>640</width>
              <height>480</height>
            </image>
            <clip>
              <near>0.01</near>
              <far>60</far>
            </clip>
          </camera>
          <always_on>1</always_on>
          <update_rate>20</update_rate>
          <visualize>false</visualize>
        </sensor>
        <sensor name="boundingbox_camera" type="boundingbox_camera">
          <ignition_frame_id>camera_link_optical</ignition_frame_id>
          <topic>box</topic>
          <box_type>2d</box_type>
          <camera>
            <horizontal_fov>${84.7*deg_to_rad}</horizontal_fov>
            <vertical_fov>${66.1*deg_to_rad}</vertical_fov>
            <image>
              <width>640</width>
              <height>480</height>
            </image>
            <clip>
              <near>0.01</near>
              <far>60</far>
            </clip>
          </camera>
          <always_on>1</always_on>
          <update_rate>20</update_rate>
          <visualize>false</visualize>
        </sensor>
    </gazebo>

</robot>
