<?xml version="1.0" ?>
<sdf version="1.9">
  <world name="iris_warehouse">
    <physics name="1ms" type="ignore">
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1.0</real_time_factor>
    </physics>

    <plugin
      filename="gz-sim-physics-system"
      name="gz::sim::systems::Physics">
    </plugin>
    <plugin
      filename="gz-sim-sensors-system"
      name="gz::sim::systems::Sensors">
      <render_engine>ogre2</render_engine>
    </plugin>
    <plugin
      filename="gz-sim-user-commands-system"
      name="gz::sim::systems::UserCommands">
    </plugin>
    <plugin
      filename="gz-sim-scene-broadcaster-system"
      name="gz::sim::systems::SceneBroadcaster">
    </plugin>
    <plugin
      filename="gz-sim-imu-system"
      name="gz::sim::systems::Imu">
    </plugin>

    <scene>
      <grid>false</grid>
    </scene>

    <gui fullscreen='0'>
      <plugin filename="MinimalScene" name="3D View">
        <gz-gui>
          <title>3D View</title>
          <property type="bool" key="showTitleBar">false</property>
          <property type="string" key="state">docked</property>
        </gz-gui>
        <engine>ogre2</engine>
        <scene>scene</scene>
        <ambient_light>0.4 0.4 0.4</ambient_light>
        <background_color>0.8 0.8 0.8</background_color>
        <camera_pose>6 0 6 0 0.5 3.14</camera_pose>
      </plugin>
      <plugin filename="WorldControl" name="World control">
        <gz-gui>
          <title>World control</title>
          <property type="bool" key="showTitleBar">false</property>
          <property type="bool" key="resizable">false</property>
          <property type="double" key="height">72</property>
          <property type="double" key="width">145</property>
          <property type="double" key="z">1</property>
          <property type="string" key="state">floating</property>
          <anchors target="3D View">
            <line own="left" target="left"/>
            <line own="bottom" target="bottom"/>
          </anchors>
        </gz-gui>
        <play_pause>true</play_pause>
        <step>true</step>
        <start_paused>true</start_paused>
      </plugin>
      <plugin filename="EntityContextMenuPlugin" name="Entity context menu">
        <gz-gui>
          <property key="state" type="string">floating</property>
          <property key="width" type="double">5</property>
          <property key="height" type="double">5</property>
          <property key="showTitleBar" type="bool">false</property>
        </gz-gui>
      </plugin>
      <plugin filename="GzSceneManager" name="Scene Manager">
        <gz-gui>
          <property key="resizable" type="bool">false</property>
          <property key="width" type="double">5</property>
          <property key="height" type="double">5</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
        </gz-gui>
      </plugin>
      <plugin filename="InteractiveViewControl" name="Interactive view control">
        <gz-gui>
          <property key="resizable" type="bool">false</property>
          <property key="width" type="double">5</property>
          <property key="height" type="double">5</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
        </gz-gui>
      </plugin>
      <plugin filename="CameraTracking" name="Camera Tracking">
        <gz-gui>
          <property key="resizable" type="bool">false</property>
          <property key="width" type="double">5</property>
          <property key="height" type="double">5</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
        </gz-gui>
      </plugin>
      <plugin filename="MarkerManager" name="Marker manager">
        <gz-gui>
          <property key="resizable" type="bool">false</property>
          <property key="width" type="double">5</property>
          <property key="height" type="double">5</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
        </gz-gui>
      </plugin>
      <plugin filename="SelectEntities" name="Select Entities">
        <gz-gui>
          <anchors target="Select entities">
            <line own="right" target="right"/>
            <line own="top" target="top"/>
          </anchors>
          <property key="resizable" type="bool">false</property>
          <property key="width" type="double">5</property>
          <property key="height" type="double">5</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
        </gz-gui>
      </plugin>
      <plugin filename="VisualizationCapabilities" name="Visualization Capabilities">
        <gz-gui>
          <property key="resizable" type="bool">false</property>
          <property key="width" type="double">5</property>
          <property key="height" type="double">5</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
        </gz-gui>
      </plugin>
      <plugin filename="Spawn" name="Spawn Entities">
        <gz-gui>
          <anchors target="Select entities">
            <line own="right" target="right"/>
            <line own="top" target="top"/>
          </anchors>
          <property key="resizable" type="bool">false</property>
          <property key="width" type="double">5</property>
          <property key="height" type="double">5</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
        </gz-gui>
      </plugin>
      <plugin filename="WorldStats" name="World stats">
        <gz-gui>
          <title>World stats</title>
          <property type="bool" key="showTitleBar">false</property>
          <property type="bool" key="resizable">false</property>
          <property type="double" key="height">110</property>
          <property type="double" key="width">290</property>
          <property type="double" key="z">1</property>
          <property type="string" key="state">floating</property>
          <anchors target="3D View">
            <line own="right" target="right"/>
            <line own="bottom" target="bottom"/>
          </anchors>
        </gz-gui>
        <sim_time>true</sim_time>
        <real_time>true</real_time>
        <real_time_factor>true</real_time_factor>
        <iterations>true</iterations>
      </plugin>
      <plugin filename="Shapes" name="Shapes">
        <gz-gui>
          <property key="resizable" type="bool">false</property>
          <property key="x" type="double">0</property>
          <property key="y" type="double">0</property>
          <property key="width" type="double">250</property>
          <property key="height" type="double">50</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
          <property key="cardBackground" type="string">#666666</property>
        </gz-gui>
      </plugin>
      <plugin filename="Lights" name="Lights">
        <gz-gui>
          <property key="resizable" type="bool">false</property>
          <property key="x" type="double">250</property>
          <property key="y" type="double">0</property>
          <property key="width" type="double">150</property>
          <property key="height" type="double">50</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
          <property key="cardBackground" type="string">#666666</property>
        </gz-gui>
      </plugin>
      <plugin filename="TransformControl" name="Transform control">
        <gz-gui>
          <property key="resizable" type="bool">false</property>
          <property key="x" type="double">0</property>
          <property key="y" type="double">50</property>
          <property key="width" type="double">250</property>
          <property key="height" type="double">50</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
          <property key="cardBackground" type="string">#777777</property>
        </gz-gui>
      </plugin>
      <plugin filename="Screenshot" name="Screenshot">
        <gz-gui>
          <property key="resizable" type="bool">false</property>
          <property key="x" type="double">250</property>
          <property key="y" type="double">50</property>
          <property key="width" type="double">50</property>
          <property key="height" type="double">50</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
          <property key="cardBackground" type="string">#777777</property>
        </gz-gui>
      </plugin>
      <plugin filename="ComponentInspector" name="Component inspector">
        <gz-gui>
          <property type="bool" key="showTitleBar">true</property>
          <property type="string" key="state">docked</property>
        </gz-gui>
      </plugin>
      <plugin filename="EntityTree" name="Entity tree">
        <gz-gui>
          <property type="bool" key="showTitleBar">true</property>
          <property type="string" key="state">docked</property>
        </gz-gui>
      </plugin>
      <plugin filename="ImageDisplay" name="ImageDisplay">
        <gz-gui>
          <property type="string" key="state">docked_collapsed</property>
        </gz-gui>
      </plugin>
      <!-- missing table (see below)
      <plugin filename="JointPositionController" name="JointPositionController">
        <gz-gui>
          <property type="string" key="state">docked_collapsed</property>
        </gz-gui>
        <model_name>Panda</model_name>
      </plugin> -->
      <plugin filename="KeyPublisher" name="Key publisher">
        <gz-gui>
          <anchors target="3D View">
            <line own="right" target="right"/>
            <line own="top" target="top"/>
          </anchors>
          <property key="resizable" type="bool">false</property>
          <property key="width" type="double">5</property>
          <property key="height" type="double">5</property>
          <property key="state" type="string">floating</property>
          <property key="showTitleBar" type="bool">false</property>
        </gz-gui>
      </plugin>
    </gui>

    <include>
        <pose>-6 0 0.25 0 0 0</pose>
        <uri>model://iris_with_gimbal</uri>
    </include>

    <include>
        <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Depot</uri>
    </include>

    <include>
        <pose>-9.4 1.6 0 0 0 -0.5</pose>
        <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift</uri>
    </include>

    <include>
        <pose>-13.2 -6.0 0.15 0 0 0</pose>
        <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/fog emitter</uri>
    </include>

    <!-- missing table (see below) -->
    <!-- <include>
        <pose>-9.0 -1.8 1.038 0 0 0</pose>
        <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Panda with Ignition position controller model</uri>
    </include> -->

    <!-- model has incorrect fuel server details -->
    <!-- <include>
        <pose degrees="true">-9.0 -2.4 0 0 0 90</pose>
        <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Reflective table</uri>
    </include> -->

    <!-- model has incorrect fuel server details -->
    <!-- <include>
        <pose degrees="true">-13.2 -6.0 0 0 0 90</pose>
        <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Rescue Randy Sitting</uri>
    </include> -->

    <include>
        <pose degrees="true">-9.0 -6.21 5.0 0 51.5 -180</pose>
        <static>true</static>
        <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/16-Bit Thermal Camera</uri>
    </include>

    <include>
        <pose degrees="true">-9.0 -5.95 5.0 0 51.5 -180</pose>
        <static>true</static>
        <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/8-Bit Thermal Camera</uri>
    </include>

    <include>
        <pose degrees="true">-9.075 -6.15 4.995 0 51.5 -180</pose>
        <static>true</static>
        <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Camera</uri>
    </include>

    <include degrees="true">
        <pose>-9.04 -6.04 4.97 0 51.5 -180</pose>
        <static>true</static>
        <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Depth Camera</uri>
    </include>

    <!-- uniform temperature object for comparison against rescue randy's heat signature -->
    <model name="uniform_temp_box">
      <pose>-13.2 -5.3 0.2 0 0 0</pose>
      <link name="box_link">
        <inertial>
          <inertia>
            <ixx>1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>1</iyy>
            <iyz>0</iyz>
            <izz>1</izz>
          </inertia>
          <mass>1.0</mass>
        </inertial>
        <collision name="box_collision">
          <geometry>
            <box>
              <size>0.4 0.4 0.4</size>
            </box>
          </geometry>
        </collision>

        <visual name="box_visual">
          <geometry>
            <box>
              <size>0.4 0.4 0.4</size>
            </box>
          </geometry>
          <material>
            <ambient>1 0 0 1</ambient>
            <diffuse>1 0 0 1</diffuse>
            <specular>1 0 0 1</specular>
          </material>
          <plugin
            filename="gz-sim-thermal-system"
            name="gz::sim::systems::Thermal">
            <temperature>285.0</temperature>
          </plugin>
        </visual>
      </link>
    </model>

    <!-- Mecanum drive teleop -->

    <!-- Moving Forward: w -->
    <plugin filename="gz-sim-triggered-publisher-system"
      name="gz::sim::systems::TriggeredPublisher">
        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
            <match field="data">87</match>
        </input>
        <output type="gz.msgs.Twist" topic="/model/Mecanum_lift/cmd_vel">
                linear: {x: 1.0, y: 0.0}, angular: {z: 0.0}
        </output>
    </plugin>

    <!-- Moving Backward: x -->
    <plugin filename="gz-sim-triggered-publisher-system"
      name="gz::sim::systems::TriggeredPublisher">
        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
            <match field="data">88</match>
        </input>
        <output type="gz.msgs.Twist" topic="/model/Mecanum_lift/cmd_vel">
                linear: {x: -1.0, y: 0.0}, angular: {z: 0.0}
        </output>
    </plugin>

    <!-- Moving right: d -->
    <plugin filename="gz-sim-triggered-publisher-system"
      name="gz::sim::systems::TriggeredPublisher">
        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
            <match field="data">68</match>
        </input>
        <output type="gz.msgs.Twist" topic="/model/Mecanum_lift/cmd_vel">
                linear: {x: 0.0, y: -1.0}, angular: {z: 0.0}
        </output>
    </plugin>

    <!--Moving left: a -->
    <plugin filename="gz-sim-triggered-publisher-system"
      name="gz::sim::systems::TriggeredPublisher">
        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
            <match field="data">65</match>
        </input>
        <output type="gz.msgs.Twist" topic="/model/Mecanum_lift/cmd_vel">
                linear: {x: 0.0, y: 1.0}, angular: {z: 0.0}
        </output>
    </plugin>

    <!-- Rotating right: e -->
    <plugin filename="gz-sim-triggered-publisher-system"
      name="gz::sim::systems::TriggeredPublisher">
        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
            <match field="data">69</match>
        </input>
        <output type="gz.msgs.Twist" topic="/model/Mecanum_lift/cmd_vel">
                linear: {x: 0.0, y: 0.0}, angular: {z: -1.0}
        </output>
    </plugin>

    <!--Rotating left: q -->
    <plugin filename="gz-sim-triggered-publisher-system"
      name="gz::sim::systems::TriggeredPublisher">
        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
            <match field="data">81</match>
        </input>
        <output type="gz.msgs.Twist" topic="/model/Mecanum_lift/cmd_vel">
                linear: {x: 0.0, y: 0.0}, angular: {z: 1.0}
        </output>
    </plugin>

    <!--Stop: s -->
    <plugin filename="gz-sim-triggered-publisher-system"
      name="gz::sim::systems::TriggeredPublisher">
        <input type="gz.msgs.Int32" topic="/keyboard/keypress">
            <match field="data">83</match>
        </input>
        <output type="gz.msgs.Twist" topic="/model/Mecanum_lift/cmd_vel">
                linear: {x: 0.0, y: 0.0}, angular: {z: 0.0}
        </output>
    </plugin>

  </world>
</sdf>
