<?xml version="1.0" ?>
<!--
Ignition Gazebo Trigger Publisher demo

Send the command:
  ign topic -t "/start" -m ignition.msgs.Empty -p " "

The vehicle should start moving while the two boxes remain floating. When the
vehicle crosses the line on the ground, the first box should start falling. The
moment the box hits the ground, the second box should start falling.

-->
<sdf version="1.6">
  <world name="triggered_publisher">

    <physics name="1ms" type="ignored">
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1.0</real_time_factor>
    </physics>
    <plugin
      filename="ignition-gazebo-physics-system"
      name="ignition::gazebo::systems::Physics">
    </plugin>
    <plugin
      filename="ignition-gazebo-contact-system"
      name="ignition::gazebo::systems::Contact">
    </plugin>
    <plugin
      filename="ignition-gazebo-user-commands-system"
      name="ignition::gazebo::systems::UserCommands">
    </plugin>
    <plugin
      filename="ignition-gazebo-scene-broadcaster-system"
      name="ignition::gazebo::systems::SceneBroadcaster">
    </plugin>
    <plugin
      filename="ignition-gazebo-altimeter-system"
      name="ignition::gazebo::systems::Altimeter">
    </plugin>

    <gui fullscreen="0">

      <!-- 3D scene -->
      <plugin filename="GzScene3D" name="3D View">
        <ignition-gui>
          <title>3D View</title>
          <property type="bool" key="showTitleBar">false</property>
          <property type="string" key="state">docked</property>
        </ignition-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>3 -9 9 0 0.6 1.3</camera_pose>
      </plugin>

      <!-- World control -->
      <plugin filename="WorldControl" name="World control">
        <ignition-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">121</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>
        </ignition-gui>

        <play_pause>true</play_pause>
        <step>true</step>
        <start_paused>true</start_paused>

      </plugin>

      <!-- World statistics -->
      <plugin filename="WorldStats" name="World stats">
        <ignition-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>
        </ignition-gui>

        <sim_time>true</sim_time>
        <real_time>true</real_time>
        <real_time_factor>true</real_time_factor>
        <iterations>true</iterations>

      </plugin>

    </gui>

    <light type="directional" name="sun">
      <cast_shadows>true</cast_shadows>
      <pose>0 0 10 0 0 0</pose>
      <diffuse>1 1 1 1</diffuse>
      <specular>0.5 0.5 0.5 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>0.05 -0.1 -0.9</direction>
    </light>

    <model name="ground_plane">
      <static>true</static>
      <link name="link">
        <collision name="collision">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.8 0.8 0.8 1</specular>
          </material>
        </visual>
      </link>
    </model>

    <model name='vehicle_blue'>
      <pose>0 0 0.325 0 -0 0</pose>

      <link name='chassis'>
        <pose>-0.151427 -0 0.175 0 -0 0</pose>
        <inertial>
          <mass>1.14395</mass>
          <inertia>
            <ixx>0.126164</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.416519</iyy>
            <iyz>0</iyz>
            <izz>0.481014</izz>
          </inertia>
        </inertial>
        <visual name='visual'>
          <geometry>
            <box>
              <size>2.01142 1 0.568726</size>
            </box>
          </geometry>
          <material>
            <ambient>0.5 0.5 1.0 1</ambient>
            <diffuse>0.5 0.5 1.0 1</diffuse>
            <specular>0.0 0.0 1.0 1</specular>
          </material>
        </visual>
        <collision name='collision'>
          <geometry>
            <box>
              <size>2.01142 1 0.568726</size>
            </box>
          </geometry>
        </collision>
      </link>

      <link name='front_left_wheel'>
        <pose>0.554283 0.625029 -0.025 -1.5707 0 0</pose>
        <inertial>
          <mass>2</mass>
          <inertia>
            <ixx>0.145833</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.145833</iyy>
            <iyz>0</iyz>
            <izz>0.125</izz>
          </inertia>
        </inertial>
        <visual name='visual'>
          <geometry>
            <sphere>
              <radius>0.3</radius>
            </sphere>
          </geometry>
          <material>
            <ambient>0.2 0.2 0.2 1</ambient>
            <diffuse>0.2 0.2 0.2 1</diffuse>
            <specular>0.2 0.2 0.2 1</specular>
          </material>
        </visual>
        <collision name='collision'>
          <geometry>
            <sphere>
              <radius>0.3</radius>
            </sphere>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1</mu>
                <mu2>1</mu2>
                <slip1>0.035</slip1>
                <slip2>0</slip2>
                <fdir1>0 0 1</fdir1>
              </ode>
            </friction>
          </surface>
        </collision>
      </link>

      <link name='rear_left_wheel'>
        <pose>-0.957138 0.625029 -0.025 -1.5707 0 0</pose>
        <inertial>
          <mass>2</mass>
          <inertia>
            <ixx>0.145833</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.145833</iyy>
            <iyz>0</iyz>
            <izz>0.125</izz>
          </inertia>
        </inertial>
        <visual name='visual'>
          <geometry>
            <sphere>
              <radius>0.3</radius>
            </sphere>
          </geometry>
          <material>
            <ambient>0.2 0.2 0.2 1</ambient>
            <diffuse>0.2 0.2 0.2 1</diffuse>
            <specular>0.2 0.2 0.2 1</specular>
          </material>
        </visual>
        <collision name='collision'>
          <geometry>
            <sphere>
              <radius>0.3</radius>
            </sphere>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1</mu>
                <mu2>1</mu2>
                <slip1>0.035</slip1>
                <slip2>0</slip2>
                <fdir1>0 0 1</fdir1>
              </ode>
            </friction>
          </surface>
        </collision>
      </link>

      <link name='front_right_wheel'>
        <pose>0.554282 -0.625029 -0.025 -1.5707 0 0</pose>
        <inertial>
          <mass>2</mass>
          <inertia>
            <ixx>0.145833</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.145833</iyy>
            <iyz>0</iyz>
            <izz>0.125</izz>
          </inertia>
        </inertial>
        <visual name='visual'>
          <geometry>
            <sphere>
              <radius>0.3</radius>
            </sphere>
          </geometry>
          <material>
            <ambient>0.2 0.2 0.2 1</ambient>
            <diffuse>0.2 0.2 0.2 1</diffuse>
            <specular>0.2 0.2 0.2 1</specular>
          </material>
        </visual>
        <collision name='collision'>
          <geometry>
            <sphere>
              <radius>0.3</radius>
            </sphere>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1</mu>
                <mu2>1</mu2>
                <slip1>0.035</slip1>
                <slip2>0</slip2>
                <fdir1>0 0 1</fdir1>
              </ode>
            </friction>
          </surface>
        </collision>
      </link>

      <link name='rear_right_wheel'>
        <pose>-0.957138 -0.625029 -0.025 -1.5707 0 0</pose>
        <inertial>
          <mass>2</mass>
          <inertia>
            <ixx>0.145833</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.145833</iyy>
            <iyz>0</iyz>
            <izz>0.125</izz>
          </inertia>
        </inertial>
        <visual name='visual'>
          <geometry>
            <sphere>
              <radius>0.3</radius>
            </sphere>
          </geometry>
          <material>
            <ambient>0.2 0.2 0.2 1</ambient>
            <diffuse>0.2 0.2 0.2 1</diffuse>
            <specular>0.2 0.2 0.2 1</specular>
          </material>
        </visual>
        <collision name='collision'>
          <geometry>
            <sphere>
              <radius>0.3</radius>
            </sphere>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1</mu>
                <mu2>1</mu2>
                <slip1>0.035</slip1>
                <slip2>0</slip2>
                <fdir1>0 0 1</fdir1>
              </ode>
            </friction>
          </surface>
        </collision>
      </link>


      <joint name='front_left_wheel_joint' type='revolute'>
        <parent>chassis</parent>
        <child>front_left_wheel</child>
        <axis>
          <xyz>0 0 1</xyz>
          <limit>
            <lower>-1.79769e+308</lower>
            <upper>1.79769e+308</upper>
          </limit>
        </axis>
      </joint>

      <joint name='front_right_wheel_joint' type='revolute'>
        <parent>chassis</parent>
        <child>front_right_wheel</child>
        <axis>
          <xyz>0 0 1</xyz>
          <limit>
            <lower>-1.79769e+308</lower>
            <upper>1.79769e+308</upper>
          </limit>
        </axis>
      </joint>

      <joint name='rear_left_wheel_joint' type='revolute'>
        <parent>chassis</parent>
        <child>rear_left_wheel</child>
        <axis>
          <xyz>0 0 1</xyz>
          <limit>
            <lower>-1.79769e+308</lower>
            <upper>1.79769e+308</upper>
          </limit>
        </axis>
      </joint>

      <joint name='rear_right_wheel_joint' type='revolute'>
        <parent>chassis</parent>
        <child>rear_right_wheel</child>
        <axis>
          <xyz>0 0 1</xyz>
          <limit>
            <lower>-1.79769e+308</lower>
            <upper>1.79769e+308</upper>
          </limit>
        </axis>
      </joint>

      <plugin
        filename="ignition-gazebo-diff-drive-system"
        name="ignition::gazebo::systems::DiffDrive">
        <left_joint>front_left_wheel_joint</left_joint>
        <left_joint>rear_left_wheel_joint</left_joint>
        <right_joint>front_right_wheel_joint</right_joint>
        <right_joint>rear_right_wheel_joint</right_joint>
        <wheel_separation>1.25</wheel_separation>
        <wheel_radius>0.3</wheel_radius>
        <topic>cmd_vel</topic>
      </plugin>
    </model>

    <model name="trigger">
      <pose>3 0 0 0 0 0</pose>
      <static>true</static>
      <link name="body">
        <visual name="v1">
          <geometry>
            <box><size>0.1 10 0.01</size></box>
          </geometry>
        </visual>
        <collision name="c1">
          <geometry>
            <box><size>0.1 10 0.01</size></box>
          </geometry>
        </collision>
        <sensor name='sensor_contact' type='contact'>
          <contact>
            <collision>c1</collision>
          </contact>
        </sensor>
      </link>
      <plugin
        filename="ignition-gazebo-touchplugin-system"
        name="ignition::gazebo::systems::TouchPlugin">
        <target>vehicle_blue</target>
        <namespace>trigger</namespace>
        <time>0.001</time>
        <enabled>true</enabled>
      </plugin>
      <plugin filename="ignition-gazebo-detachable-joint-system" name="ignition::gazebo::systems::DetachableJoint">
        <parent_link>body</parent_link>
        <child_model>box1</child_model>
        <child_link>box_body</child_link>
        <topic>/box1/detach</topic>
      </plugin>
      <plugin filename="ignition-gazebo-detachable-joint-system" name="ignition::gazebo::systems::DetachableJoint">
        <parent_link>body</parent_link>
        <child_model>box2</child_model>
        <child_link>box_body</child_link>
        <topic>/box2/detach</topic>
      </plugin>
    </model>

    <model name="box1">
      <pose>3 0 8 0 0 0</pose>
      <link name="box_body">
        <visual name="v1">
          <geometry>
            <box><size>1 1 1</size></box>
          </geometry>
          <material>
            <diffuse>0.8 0.2 0.2 1</diffuse>
            <specular>1.0 0 0 1</specular>
          </material>
        </visual>
        <collision name="c1">
          <geometry>
            <box><size>1 1 1</size></box>
          </geometry>
        </collision>

        <sensor name="altimeter_sensor" type="altimeter">
          <topic>/altimeter</topic>
          <always_on>1</always_on>
          <update_rate>30</update_rate>
          <visualize>true</visualize>
        </sensor>
      </link>
    </model>

    <model name="box2">
      <pose>8 0 5 0 0 0</pose>
      <link name="box_body">
        <visual name="v1">
          <geometry>
            <box><size>1 1 1</size></box>
          </geometry>
          <material>
            <diffuse>0.2 0.8 0.2 1</diffuse>
            <specular>1.0 0 0 1</specular>
          </material>
        </visual>
        <collision name="c1">
          <geometry>
            <box><size>1 1 1</size></box>
          </geometry>
        </collision>
      </link>
    </model>

    <plugin filename="ignition-gazebo-triggered-publisher-system" name="ignition::gazebo::systems::TriggeredPublisher">
      <input type="ignition.msgs.Empty" topic="/start"/>
      <output type="ignition.msgs.Twist" topic="/cmd_vel">
          linear: {x: 3}
      </output>
    </plugin>
    <plugin filename="ignition-gazebo-triggered-publisher-system" name="ignition::gazebo::systems::TriggeredPublisher">
      <input type="ignition.msgs.Boolean" topic="/trigger/touched">
        <match>data: true</match>
      </input>
      <output type="ignition.msgs.Empty" topic="/box1/detach"/>
    </plugin>
    <plugin filename="ignition-gazebo-triggered-publisher-system" name="ignition::gazebo::systems::TriggeredPublisher">
      <input type="ignition.msgs.Altimeter" topic="/altimeter">
        <match field="vertical_position" tol="0.2">-7.5</match>
      </input>
      <output type="ignition.msgs.Empty" topic="/box2/detach"/>
    </plugin>
</world>
</sdf>
