competition.ign

<?xml version="1.0"?>
<!-- Usage: ign launch competition.ign circuit:=<circuitName> worldName:=<worldName>
             [robotName1:=<robotName> robotConfig1:=<robotConfig>
             robotName2:=<robotName> robotConfig2:=<robotConfig>
             marsupial1:=<robotName>:<robotName>
             ... ]

    circuitName should be one of {cave, tunnel, urban}
    The [robotNameX] command line argument is optional, where X can be 1 to 20

    Example that loads cave circuit world with an X2 robot with configuration 3

      ign launch competition.ign circuit:=cave worldName:=simple_cave_01
          robotName1:=X2_3 robotConfig1:=X2_SENSOR_CONFIG_3
-->

<%
  # Needed for adding marsupial robots
  require "rexml/document"
%>

<%
  # When 'enableGroundTruth = true' absolute poses of vehicles will be published.
  # This is useful for debugging purposes, but will not be available during
  # competition scoring.

  # Check if enableGroundTruth is not defined or is empty/nil
  if !defined?(enableGroundTruth) || enableGroundTruth == nil || enableGroundTruth.empty?
    $enableGroundTruth = false
  else
    $enableGroundTruth = enableGroundTruth
  end

  # Check if circuit is not defined or is empty/nil
  # Also make sure the given circuit name is valid
  validCircuits = ["cave", "tunnel", "urban", "finals"]
  if !defined?(circuit) || circuit == nil || circuit.empty?
    abort("circuit was not defined, exiting now")
  elsif validCircuits.include?(circuit)
    $circuit = circuit
  else
    abort("circuit given is not valid. Please choose from #{validCircuits}")
  end
%>

<%

  Vector3d = Struct.new(:x, :y, :z)
  AngularVector3d = Struct.new(:roll, :pitch, :yaw)

  class Robot
    attr_accessor :name, :config, :type, :configNumber, :pos, :rot
    def initialize(name, config)
      @name = name
      @config = config
      match = config.match(/(.*)_SENSOR_CONFIG/)
      if match.nil?
        @type = config
      else
        @type = match[1] unless match.nil?
      end
      @configNumber = config[-1]
      @pos = Vector3d.new(0, 0, 0)
      @rot = AngularVector3d.new(0, 0, 0)
    end
  end

  # CommsBrokerPlugin parameters
  # (default parameters can be found in subt_rf_model.h)
  $commsFadingExponent = 1.5
  $commsScalingFactor = 0.55
  $commsRangePerHop = 2.0

  # spawn world offset
  if $circuit == "urban"
    spawnWorldXPos = 6
    spawnWorldYPos = 27
    spawnWorldZPos = 7.5
    spawnWorldYaw = 0
    guiCameraPose = "-2.0 27 15 0 0.868 0"
  elsif $circuit == "finals"
    spawnWorldXPos = -18
    spawnWorldYPos = -1
    spawnWorldZPos = 0
    spawnWorldYaw = 0
    guiCameraPose = "-20 0 2.5 0 0.268 0"
  else
    spawnWorldXPos = 0
    spawnWorldYPos = 0
    spawnWorldZPos = 0
    spawnWorldYaw = 0
    guiCameraPose = "-6.3 -4.2 3.6 0 0.268 0.304"
  end

  teamBaseSpawned = false

  # Check if robotNameX and robotConfigX exists
  spawnRowSize = 4
  spawnColSize = 5
  spawnGridSize = 2
  maxRobotCount = spawnRowSize * spawnColSize
  spawnColOffset = spawnColSize * spawnGridSize / 2
  spawnRowOffset = spawnRowSize * spawnGridSize / 2
  robots = Hash.new
  for i in 1..maxRobotCount do
    if (local_variables.include?(:"robotName#{i}") &&
        local_variables.include?(:"robotConfig#{i}"))
      name=eval "robotName#{i}"
      config=eval "robotConfig#{i}"
      if name != nil && !name.empty?
        raise "Duplicate robot name #{name}" if robots.has_key? name
        robots[name] = Robot.new(name, config)
      end
    end
  end

  MARSUPIAL_PARENT_LINK_NAMES = {
     "X1" => "base_link",
     "MARBLE_HUSKY" => "base_link",
     "EXPLORER_X1" => "base_link",
     "EXPLORER_R2" => "Rear_Rocker_Link",
     "CSIRO_DATA61_OZBOT_ATR" => "base_link",
     "CORO_ALLIE" => "base_link",
     "CORO_JEANINE" => "base_link",
     "CORO_KAREN" => "base_link",
     "CORO_MIKE" => "base_link",
   }

  MARSUPIAL_VALID_ROBOT_PAIRS = {
    "X1" => ["X3", "MARBLE_QAV500", "CERBERUS_GAGARIN", "CERBERUS_RMF", "COSTAR_SHAFTER", "CORO_PAM", "CTU_CRAS_NORLAB_X500", "CORO_CRYSTAL"],
    "MARBLE_HUSKY" =>["X3", "MARBLE_QAV500", "CERBERUS_GAGARIN", "CERBERUS_RMF", "COSTAR_SHAFTER", "CORO_PAM", "CTU_CRAS_NORLAB_X500", "CORO_CRYSTAL"],
    "CORO_ALLIE" => ["CERBERUS_RMF", "CORO_PAM", "CORO_CRYSTAL"],
    "CORO_JEANINE" => ["CERBERUS_RMF", "CORO_PAM", "CORO_CRYSTAL"],
    "CORO_KAREN" => ["CERBERUS_RMF", "CORO_PAM", "CORO_CRYSTAL"],
    "CORO_MIKE" => ["CERBERUS_RMF", "CORO_PAM", "CORO_CRYSTAL"],
    "EXPLORER_X1" =>["X3", "MARBLE_QAV500", "CERBERUS_GAGARIN", "CERBERUS_RMF", "COSTAR_SHAFTER", "CORO_PAM", "CTU_CRAS_NORLAB_X500", "CORO_CRYSTAL"],
    "EXPLORER_R2" =>["X3", "X4", "SSCI_X4", "SOPHISTICATED_ENGINEERING_X4", "EXPLORER_DS1", "MARBLE_QAV500", "CERBERUS_M100", "CERBERUS_GAGARIN", "CERBERUS_RMF", "COSTAR_SHAFTER", "CORO_PAM", "CTU_CRAS_NORLAB_X500", "CORO_CRYSTAL", "EMESENT_HOVERMAP"],
    "CSIRO_DATA61_OZBOT_ATR" =>["X3", "X4", "SSCI_X4", "SOPHISTICATED_ENGINEERING_X4","EXPLORER_DS1", "MARBLE_QAV500", "CERBERUS_M100", "CERBERUS_GAGARIN", "CERBERUS_RMF", "COSTAR_SHAFTER", "CORO_PAM", "CTU_CRAS_NORLAB_X500", "CORO_CRYSTAL", "EMESENT_HOVERMAP"]
  }

  MARSUPIAL_PARENT_ROBOT_POSITION_OFFSETS = {
    "X1" => Vector3d.new(0, 0, 0.717),
    "MARBLE_HUSKY" => Vector3d.new(-0.06, 0, 0.43),
    "EXPLORER_X1" => Vector3d.new(0, 0, 0.717),
    "EXPLORER_R2" => Vector3d.new(-0.402375, 0, 0.6874),
    "CSIRO_DATA61_OZBOT_ATR" => Vector3d.new(-0.15, 0, 0.528),
    "CORO_ALLIE" => Vector3d.new(0.13, 0, 0.66),
    "CORO_JEANINE" => Vector3d.new(0, 0, 0.58),
    "CORO_KAREN" => Vector3d.new(0.37, 0, 0.86),
    "CORO_MIKE" => Vector3d.new(0.345, -0.125, 0.92)
  }

  MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS = {
    "X1" => AngularVector3d.new(0, 0, 0),
    "MARBLE_HUSKY" => AngularVector3d.new(0, 0, 0),
    "EXPLORER_X1" => AngularVector3d.new(0, 0, 0),
    "EXPLORER_R2" => AngularVector3d.new(0, 0, -3.1416),
    "CSIRO_DATA61_OZBOT_ATR" => AngularVector3d.new(0, 0, -3.1416),
    "CORO_ALLIE" => AngularVector3d.new(0, 0, 0),
    "CORO_JEANINE" => AngularVector3d.new(0, 0, 0),
    "CORO_KAREN" => AngularVector3d.new(0, 0, 0),
    "CORO_MIKE" => AngularVector3d.new(0, 0, 0)
  }

  allExecutables = ""

  # key: parent, value: child
  marsupialParents = Hash.new

  # The logic for validating marsupial robots is:
  # * Check that parent names are unique
  # * Check that child names are unique
  # * Check that each parent and child robot is in the list of robots to be spawned
  # * Check that parent robots are in the MARSUPIAL_VALID_ROBOT_PAIRS list
  # * Check that child robots are in the MARSUPIAL_VALID_ROBOT_PAIRS list
  # These criteria ensure that the robots specified in the "marsupial" variables are valid
  # and that there is a one-to-one relationship between parent and child robots

  # Handle marsupial robots
  for i in 1..maxRobotCount do
    if local_variables.include?(:"marsupial#{i}")
      parent, child = (eval "marsupial#{i}").split(':')

      # Check that parent names are unique
      if marsupialParents.has_key?(parent)
        raise "Invalid marsupial configuration: The parent robot [#{parent}] cannot be used more than once"
      end

      # Check that child names are unique
      if marsupialParents.has_value?(child)
        raise "Invalid marsupial configuration: The child robot [#{child}] cannot be used more than once"
      end

      # Check that each parent and child robot is in the list of robots to be spawned
      unless robots.has_key?(parent)
        raise "Invalid marsupial configuration: The parent robot [#{parent}] is not in the list of robots to be spawned"
      end
      unless robots.has_key?(child)
        raise "Invalid marsupial configuration: The child robot [#{child}] is not in the list of robots to be spawned"
      end

      parentType = robots[parent].type
      childType = robots[child].type

      # Check that parent robots are in the MARSUPIAL_VALID_ROBOT_PAIRS hash
      unless MARSUPIAL_VALID_ROBOT_PAIRS.key?(parentType)
        raise "Invalid marsupial configuration: The parent robot [#{parent}] with type [#{parentType}] is not in the list of robots allowed to be marsupial parents. The list is #{MARSUPIAL_VALID_ROBOT_PAIRS}"
      end
      # Check that child robots are in the MARSUPIAL_VALID_ROBOT_PARIS hash
      unless MARSUPIAL_VALID_ROBOT_PAIRS[parentType].include?(childType)
        raise "Invalid marsupial configuration: The child robot [#{child}] with type [#{childType}] is not in the list of robots allowed to be marsupial children: The list is #{MARSUPIAL_VALID_ROBOT_PAIRS}"
      end

      marsupialParents[parent] = child
    end
  end
%>

<%
  # Check if worldName is not defined or is empty/nil
  if !defined?(worldName) || worldName == nil || worldName.empty?
    abort("worldName was not defined, exiting now")
  else
    $worldName = worldName
  end

  if !defined?(headless) || headless == nil || headless.empty?
    $headless = false
  elsif headless == 'true' || headless == 'True' || headless == '1'
    $headless = true
  end

  # Check if durationSec is not defined or is empty/nil. In this case, set
  # durationSec to zero, which equats to an infinite run time.
  if !defined?(durationSec) || durationSec == nil || durationSec.empty?
    $durationSec = 0
  else
    $durationSec = durationSec
  end

  worldNumber = $worldName.split('_').last

  # If localModel is set, the model URI will use a local path to the model with
  # the assumption that the model is installed in the same workspace as the
  # launch file. This only applies to models submitted by teams.
  if !defined?(localModel) || localModel == nil || localModel.empty?
    $localModel = false
  else
    $localModel = localModel
  end
%>

<%
  # disable levels for simple cave worlds
  $levels = "true"
  if $worldName.include?('simple_cave_') || $worldName.include?('cave_qual')
    $levels = "false"
  end

  if defined?(levels) && levels != nil && !levels.empty?
    $levels = levels.downcase == "true"
  end
%>

<ignition version='1.0'>
  <env>
    <name>IGN_GAZEBO_SYSTEM_PLUGIN_PATH</name>
    <value>$LD_LIBRARY_PATH</value>
  </env>

  <env>
    <name>IGN_TRANSPORT_TOPIC_STATISTICS</name>
    <value>1</value>
  </env>

  <!-- Start ROS first. This is a bit hacky for now. -->
  <!-- Make sure to source /opt/ros/melodic/setup.bash -->
  <executable name='ros'>
    <command>roslaunch subt_ros competition_init.launch world_name:=<%=$worldName%> vehicle_topics:=<%=(robots.empty?)?"1":"0"%> enable_ground_truth:=<%=($enableGroundTruth)?"1":"0"%> robot_names:=<%= robots.keys.join(",") %></command>
  </executable>

  <plugin name="ignition::launch::GazeboServer"
          filename="libignition-launch-gazebo.so">
    <% if $worldName.include?('tunnel_circuit_') &&
          !$worldName.include?('practice') %>
      <world_file>tunnel_circuit/<%= worldNumber %>/<%= $worldName %>.sdf</world_file>
    <% elsif $worldName.include?('urban_circuit_') &&
          !$worldName.include?('practice') %>
      <world_file>urban_circuit/<%= worldNumber %>/<%= $worldName %>.sdf</world_file>
    <% elsif $worldName.include?('cave_circuit_') &&
          !$worldName.include?('practice') %>
      <world_file>cave_circuit/<%= worldNumber %>/<%= $worldName %>.sdf</world_file>
    <% elsif $worldName.include?('final_event') &&
          !$worldName.include?('practice') %>
      <world_file>final_event/<%= worldNumber %>/<%= $worldName %>.sdf</world_file>
    <% elsif $worldName.include?('final_prelim') &&
          !$worldName.include?('practice') %>
      <world_file>final_prelim/<%= worldNumber %>/<%= $worldName %>.sdf</world_file>
    <% else %>
      <world_file><%= $worldName %>.sdf</world_file>
    <% end %>
    <%if defined?(updateRate) && updateRate != nil && !updateRate.empty?%>
    <update_rate><%= updateRate %></update_rate>
    <%end%>
    <run>true</run>
    <levels><%= $levels %></levels>
    <record>
      <enabled>true</enabled>
      <!-- This path is used by cloudsim, please do not change -->
      <path>/tmp/ign/logs/gazebo</path>
      <overwrite>true</overwrite>
    </record>
    <%if defined?(seed) && seed != nil && !seed.empty?%>
    <seed><%= seed %></seed>
    <%end%>

    <plugin entity_name="<%= $worldName %>"
            entity_type="world"
            filename="libignition-gazebo-physics-system.so"
            name="ignition::gazebo::systems::Physics">
    </plugin>
    <plugin entity_name="<%= $worldName %>"
            entity_type="world"
            filename="libignition-gazebo-sensors-system.so"
            name="ignition::gazebo::systems::Sensors">
            <render_engine>ogre2</render_engine>
    </plugin>
    <plugin entity_name="<%= $worldName %>"
            entity_type="world"
            filename="libignition-gazebo-user-commands-system.so"
            name="ignition::gazebo::systems::UserCommands">
    </plugin>
    <plugin entity_name="<%= $worldName %>"
            entity_type="world"
            filename="libignition-gazebo-scene-broadcaster-system.so"
            name="ignition::gazebo::systems::SceneBroadcaster">
    </plugin>

    <plugin entity_name="<%= $worldName %>"
            entity_type="world"
            filename="libignition-gazebo-imu-system.so"
            name="ignition::gazebo::systems::Imu">
    </plugin>

    <plugin entity_name="<%= $worldName %>"
            entity_type="world"
            filename="libignition-gazebo-magnetometer-system.so"
            name="ignition::gazebo::systems::Magnetometer">
    </plugin>

    <plugin entity_name="<%= $worldName %>"
            entity_type="world"
            filename="libignition-gazebo-air-pressure-system.so"
            name="ignition::gazebo::systems::AirPressure">
    </plugin>

    <plugin entity_name="<%= $worldName %>"
            entity_type="world"
            filename="ignition-gazebo-contact-system"
            name="ignition::gazebo::systems::Contact">
    </plugin>

    <plugin entity_name="<%= $worldName %>"
            entity_type="world"
            filename="ignition-gazebo-particle-emitter2-system"
            name="ignition::gazebo::systems::ParticleEmitter2">
    </plugin>

    <!-- The SubT challenge logic plugin -->
    <plugin entity_name="<%= $worldName %>"
            entity_type="world"
            filename="libGameLogicPlugin.so"
            name="subt::GameLogicPlugin">
      <!-- The collection of artifacts to locate -->
      <world_name><%= $worldName %></world_name>
      <ros>false</ros>
      <ke_height>0.24</ke_height>

      <duration_seconds><%= $durationSec %></duration_seconds>

      <logging>
        <!-- Use the <path> element to control where to record the log file.
             The HOME path is used by default -->
        <path>/tmp/ign/logs</path>
        <filename_prefix>subt_<%= $circuit %></filename_prefix>
        <elevation_step_size>5</elevation_step_size>
      </logging>
    </plugin>
  </plugin>

  <%if !$headless %>
  <executable_wrapper>
    <plugin name="ignition::launch::GazeboGui"
          filename="libignition-launch-gazebogui.so">
      <world_name><%= $worldName %></world_name>
      <window_title>SubT Simulator</window_title>
      <window_icon><%= ENV['SUBT_IMAGES_PATH'] %>/SubT_logo.svg</window_icon>
      <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.2 0.2 0.2</ambient_light>
        <background_color>0.8 0.8 0.8</background_color>
        <camera_pose><%= guiCameraPose %></camera_pose>
      </plugin>
      <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>
        <service>/world/<%= $worldName %>/control</service>
        <stats_topic>/world/<%= $worldName %>/stats</stats_topic>

      </plugin>

      <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>
        <topic>/world/<%= $worldName %>/stats</topic>
      </plugin>
      <!-- Entity tree -->
      <!-- <plugin filename="EntityTree" name="Entity tree">
        <ignition-gui>
          <title>Entity tree</title>
        </ignition-gui>
      </plugin> -->

      <!-- Transform Control -->
      <!-- <plugin filename="TransformControl" name="Transform Control">
        <service>/world/<%= $worldName %>/gui/transform_mode</service>
      </plugin> -->
    </plugin>
  </executable_wrapper>
  <%end%>

  <!-- The SubT challenge comms broker plugin -->
  <plugin entity_name="<%= $worldName %>"
          entity_type="world"
          name="subt::CommsBrokerPlugin"
          filename="libCommsBrokerPlugin.so">
    <generate_table>true</generate_table>
    <world_name><%= $worldName %></world_name>
    <comms_model>
      <comms_model_type>visibility_range</comms_model_type>

      <range_config>
        <max_range>500.0</max_range>
        <fading_exponent><%= $commsFadingExponent %></fading_exponent>
        <L0>40</L0>
        <sigma>10.0</sigma>
        <scaling_factor><%= $commsScalingFactor %></scaling_factor>
        <range_per_hop><%= $commsRangePerHop %></range_per_hop>
      </range_config>

      <visibility_config>
        <visibility_cost_to_fading_exponent>0.2</visibility_cost_to_fading_exponent>
        <comms_cost_max>10</comms_cost_max>
      </visibility_config>

      <radio_config>
        <capacity>1000000</capacity>
        <tx_power>20</tx_power>
        <noise_floor>-90</noise_floor>
        <modulation>QPSK</modulation>
      </radio_config>
    </comms_model>
  </plugin>

  <!-- The SubT challenge base station plugin -->
  <plugin entity_name="base_station"
          entity_type="model"
          name="subt::BaseStationPlugin"
          filename="libBaseStationPlugin.so">
  </plugin>


<%
  def spawnX1(_name, _config, _x, _y, _z, _yaw)
    uav=0
    laserScan=0
    stereoCam=0
    lidar3d=0
    if _config == "1" or _config == "2"
      laserScan=1
    end
    if _config == "3" or _config == "4" or _config == "7" or _config == "8"
      lidar3d=1
    end
    if _config == "5"
      stereoCam=1
    end

    max_breadcrumbs = 0
    if _config == "7" or _config == "8"
      max_breadcrumbs = 12
    end

    z = _z + 0.2

    spawn = <<-HEREDOC
      <spawn name='#{_name}'>
        <name>#{_name}</name>
        <allow_renaming>false</allow_renaming>
        <pose>#{_x} #{_y} #{z} 0 0 #{_yaw}</pose>
        <world>#{$worldName}</world>
        <is_performer>true</is_performer>
        <sdf version='1.6'>
        <include>
          <name>#{_name}</name>
          <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X1 Config #{_config}</uri>
          <!-- Diff drive -->
          <plugin filename="libignition-gazebo-diff-drive-system.so"
                  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>#{0.45649 * 1.5}</wheel_separation>
            <wheel_radius>0.1651</wheel_radius>
            <topic>/model/#{_name}/cmd_vel_relay</topic>
            <min_velocity>-1</min_velocity>
            <max_velocity>1</max_velocity>
            <min_acceleration>-3</min_acceleration>
            <max_acceleration>3</max_acceleration>
          </plugin>
          <!-- Publish robot state information -->
          <plugin filename="libignition-gazebo-pose-publisher-system.so"
            name="ignition::gazebo::systems::PosePublisher">
            <publish_link_pose>true</publish_link_pose>
            <publish_sensor_pose>true</publish_sensor_pose>
            <publish_collision_pose>false</publish_collision_pose>
            <publish_visual_pose>false</publish_visual_pose>
            <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
            <use_pose_vector_msg>true</use_pose_vector_msg>
            <static_publisher>true</static_publisher>
            <static_update_frequency>1</static_update_frequency>
          </plugin>
          <!-- Battery plugin -->
          <plugin filename="libignition-gazebo-linearbatteryplugin-system.so"
            name="ignition::gazebo::systems::LinearBatteryPlugin">
            <battery_name>linear_battery</battery_name>
            <voltage>12.694</voltage>
            <open_circuit_voltage_constant_coef>12.694</open_circuit_voltage_constant_coef>
            <open_circuit_voltage_linear_coef>-3.1424</open_circuit_voltage_linear_coef>
            <initial_charge>78.4</initial_charge>
            <capacity>78.4</capacity>
            <resistance>0.061523</resistance>
            <smooth_current_tau>1.9499</smooth_current_tau>
            <power_load>6.6</power_load>
            <start_on_motion>true</start_on_motion>
          </plugin>
          <!-- Gas Sensor plugin -->
          <plugin filename="libGasEmitterDetectorPlugin.so"
            name="subt::GasDetector">
            <topic>/model/#{_name}/gas_detected</topic>
            <update_rate>10</update_rate>
            <type>gas</type>
          </plugin>
          <plugin filename="libignition-gazebo-breadcrumbs-system.so"
            name="ignition::gazebo::systems::Breadcrumbs">
            <topic>/model/#{_name}/breadcrumb/deploy</topic>
            <max_deployments>#{max_breadcrumbs}</max_deployments>
            <disable_physics_time>3.0</disable_physics_time>
            <topic_statistics>true</topic_statistics>
            <breadcrumb>
              <sdf version="1.6">
                <model name="#{_name}__breadcrumb__">
                  <pose>-0.45 0 0 0 0 0</pose>
                  <include>
                    <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Breadcrumb Node</uri>
                  </include>
                </model>
              </sdf>
            </breadcrumb>
          </plugin>
          <!-- Wheel plugin -->
          <plugin filename="libignition-gazebo-wheel-slip-system.so"
            name="ignition::gazebo::systems::WheelSlip">
            <wheel link_name="front_left_wheel">
              <slip_compliance_lateral>0.172</slip_compliance_lateral>
              <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
              <wheel_normal_force>138.767</wheel_normal_force>
              <wheel_radius>0.1651</wheel_radius>
            </wheel>
            <wheel link_name="rear_left_wheel">
              <slip_compliance_lateral>0.172</slip_compliance_lateral>
              <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
              <wheel_normal_force>138.767</wheel_normal_force>
              <wheel_radius>0.1651</wheel_radius>
            </wheel>
            <wheel link_name="front_right_wheel">
              <slip_compliance_lateral>0.172</slip_compliance_lateral>
              <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
              <wheel_normal_force>138.767</wheel_normal_force>
              <wheel_radius>0.1651</wheel_radius>
            </wheel>
            <wheel link_name="rear_right_wheel">
              <slip_compliance_lateral>0.172</slip_compliance_lateral>
              <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
              <wheel_normal_force>138.767</wheel_normal_force>
              <wheel_radius>0.1651</wheel_radius>
            </wheel>
          </plugin>
        </include>
        </sdf>
      </spawn>
    HEREDOC

    exec = <<-HEREDOC
      <executable name='x1_description'>
        <command>roslaunch --wait subt_ros x1_description.launch world_name:=#{$worldName} name:=#{_name}</command>
      </executable>
      <executable name='x1_ros_ign_bridge'>
        <command>roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} lidar_3d:=#{lidar3d} breadcrumbs:=#{max_breadcrumbs>0?1:0}</command>
      </executable>
    HEREDOC

    return spawn, exec
  end

  def spawnX2(_name, _config, _x, _y, _z, _yaw)
    uav=0
    laserScan=0
    stereoCam=0
    lidar3d=0
    if _config == "1" or _config == "2" or _config == "3" or _config == "4" or _config == "9"
      laserScan=1
    end
    if _config == "5"
      stereoCam=1
    end
    if _config == "6" or _config == "8"
      lidar3d=1
    end

    max_breadcrumbs = 0
    if _config == "8" or _config == "9"
      max_breadcrumbs = 6
    end

    z = _z + 0.063494

    spawn = <<-HEREDOC
      <spawn name='#{_name}'>
        <name>#{_name}</name>
        <allow_renaming>false</allow_renaming>
        <pose>#{_x} #{_y} #{z} 0 0 #{_yaw}</pose>
        <world>#{$worldName}</world>
        <is_performer>true</is_performer>
        <sdf version='1.6'>
        <include>
          <name>#{_name}</name>
          <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X2 Config #{_config}</uri>
          <!-- Diff drive -->
          <plugin filename="libignition-gazebo-diff-drive-system.so"
                  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>#{0.33559 * 1.23}</wheel_separation>
            <wheel_radius>0.098</wheel_radius>
            <topic>/model/#{_name}/cmd_vel_relay</topic>
            <min_velocity>-2</min_velocity>
            <max_velocity>2</max_velocity>
            <min_acceleration>-6</min_acceleration>
            <max_acceleration>6</max_acceleration>
          </plugin>
          <!-- Publish robot state information -->
          <plugin filename="libignition-gazebo-pose-publisher-system.so"
            name="ignition::gazebo::systems::PosePublisher">
            <publish_link_pose>true</publish_link_pose>
            <publish_sensor_pose>true</publish_sensor_pose>
            <publish_collision_pose>false</publish_collision_pose>
            <publish_visual_pose>false</publish_visual_pose>
            <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
            <use_pose_vector_msg>true</use_pose_vector_msg>
            <static_publisher>true</static_publisher>
            <static_update_frequency>1</static_update_frequency>
          </plugin>
          <!-- Battery plugin -->
          <plugin filename="libignition-gazebo-linearbatteryplugin-system.so"
            name="ignition::gazebo::systems::LinearBatteryPlugin">
            <battery_name>linear_battery</battery_name>
            <voltage>12.694</voltage>
            <open_circuit_voltage_constant_coef>12.694</open_circuit_voltage_constant_coef>
            <open_circuit_voltage_linear_coef>-3.1424</open_circuit_voltage_linear_coef>
            <initial_charge>78.4</initial_charge>
            <capacity>78.4</capacity>
            <resistance>0.061523</resistance>
            <smooth_current_tau>1.9499</smooth_current_tau>
            <power_load>6.6</power_load>
            <start_on_motion>true</start_on_motion>
          </plugin>
          <!-- Gas Sensor plugin -->
          <plugin filename="libGasEmitterDetectorPlugin.so"
            name="subt::GasDetector">
            <topic>/model/#{_name}/gas_detected</topic>
            <update_rate>10</update_rate>
            <type>gas</type>
          </plugin>
          <plugin filename="libignition-gazebo-breadcrumbs-system.so"
            name="ignition::gazebo::systems::Breadcrumbs">
            <topic>/model/#{_name}/breadcrumb/deploy</topic>
            <max_deployments>#{max_breadcrumbs}</max_deployments>
            <disable_physics_time>3.0</disable_physics_time>
            <topic_statistics>true</topic_statistics>
            <breadcrumb>
              <sdf version="1.6">
                <model name="#{_name}__breadcrumb__">
                  <pose>-0.24 0 0 0 0 0</pose>
                  <include>
                    <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Breadcrumb Node</uri>
                  </include>
                </model>
              </sdf>
            </breadcrumb>
          </plugin>
          <!-- Wheel plugin -->
          <plugin filename="libignition-gazebo-wheel-slip-system.so"
            name="ignition::gazebo::systems::WheelSlip">
            <wheel link_name="front_left_wheel">
              <slip_compliance_lateral>0.116</slip_compliance_lateral>
              <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
              <wheel_normal_force>45.20448</wheel_normal_force>
              <wheel_radius>0.098</wheel_radius>
            </wheel>
            <wheel link_name="rear_left_wheel">
              <slip_compliance_lateral>0.116</slip_compliance_lateral>
              <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
              <wheel_normal_force>45.20448</wheel_normal_force>
              <wheel_radius>0.098</wheel_radius>
            </wheel>
            <wheel link_name="front_right_wheel">
              <slip_compliance_lateral>0.116</slip_compliance_lateral>
              <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
              <wheel_normal_force>45.20448</wheel_normal_force>
              <wheel_radius>0.098</wheel_radius>
            </wheel>
            <wheel link_name="rear_right_wheel">
              <slip_compliance_lateral>0.116</slip_compliance_lateral>
              <slip_compliance_longitudinal>0</slip_compliance_longitudinal>
              <wheel_normal_force>45.20448</wheel_normal_force>
              <wheel_radius>0.098</wheel_radius>
            </wheel>
          </plugin>
        </include>
        </sdf>
      </spawn>
    HEREDOC

    exec = <<-HEREDOC
      <executable name='x2_description'>
        <command>roslaunch --wait subt_ros x2_description.launch world_name:=#{$worldName} name:=#{_name}</command>
      </executable>
      <executable name='x2_ros_ign_bridge'>
        <command>roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} lidar_3d:=#{lidar3d} breadcrumbs:=#{max_breadcrumbs>0?1:0}</command>
      </executable>
    HEREDOC

    return spawn, exec
  end

  def spawnX3(_name, _config, _x, _y, _z, _yaw)
    uav=1
    laserScan=0
    stereoCam=0
    rgbdCam=0
    if _config == "3" || _config == "4"
      rgbdCam=1
    end
    if _config == "5"
      stereoCam=1
    end

    z = _z + 0.053302

    spawn = <<-HEREDOC
    <spawn name='#{_name}'>
      <name>#{_name}</name>
      <allow_renaming>false</allow_renaming>
      <pose>#{_x} #{_y} #{z} 0 0 #{_yaw}</pose>
      <world>#{$worldName}</world>
      <is_performer>true</is_performer>
      <sdf version='1.6'>
      <include>
        <name>#{_name}</name>
        <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X3 UAV Config #{_config}</uri>
        <!-- Publish robot state information -->
        <plugin filename="libignition-gazebo-pose-publisher-system.so"
          name="ignition::gazebo::systems::PosePublisher">
          <publish_link_pose>true</publish_link_pose>
          <publish_sensor_pose>true</publish_sensor_pose>
          <publish_collision_pose>false</publish_collision_pose>
          <publish_visual_pose>false</publish_visual_pose>
          <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
          <use_pose_vector_msg>true</use_pose_vector_msg>
          <static_publisher>true</static_publisher>
          <static_update_frequency>1</static_update_frequency>
        </plugin>
        <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
          name="ignition::gazebo::systems::MulticopterMotorModel">
          <robotNamespace>model/#{_name}</robotNamespace>
          <jointName>rotor_0_joint</jointName>
          <linkName>rotor_0</linkName>
          <turningDirection>ccw</turningDirection>
          <timeConstantUp>0.0125</timeConstantUp>
          <timeConstantDown>0.025</timeConstantDown>
          <maxRotVelocity>800.0</maxRotVelocity>
          <motorConstant>8.54858e-06</motorConstant>
          <momentConstant>0.016</momentConstant>
          <commandSubTopic>command/motor_speed</commandSubTopic>
          <motorNumber>0</motorNumber>
          <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
          <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
          <motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
          <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
          <motorType>velocity</motorType>
        </plugin>
        <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
          name="ignition::gazebo::systems::MulticopterMotorModel">
          <robotNamespace>model/#{_name}</robotNamespace>
          <jointName>rotor_1_joint</jointName>
          <linkName>rotor_1</linkName>
          <turningDirection>ccw</turningDirection>
          <timeConstantUp>0.0125</timeConstantUp>
          <timeConstantDown>0.025</timeConstantDown>
          <maxRotVelocity>800.0</maxRotVelocity>
          <motorConstant>8.54858e-06</motorConstant>
          <momentConstant>0.016</momentConstant>
          <commandSubTopic>command/motor_speed</commandSubTopic>
          <motorNumber>1</motorNumber>
          <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
          <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
          <motorSpeedPubTopic>motor_speed/1</motorSpeedPubTopic>
          <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
          <motorType>velocity</motorType>
        </plugin>
        <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
          name="ignition::gazebo::systems::MulticopterMotorModel">
          <robotNamespace>model/#{_name}</robotNamespace>
          <jointName>rotor_2_joint</jointName>
          <linkName>rotor_2</linkName>
          <turningDirection>cw</turningDirection>
          <timeConstantUp>0.0125</timeConstantUp>
          <timeConstantDown>0.025</timeConstantDown>
          <maxRotVelocity>800.0</maxRotVelocity>
          <motorConstant>8.54858e-06</motorConstant>
          <momentConstant>0.016</momentConstant>
          <commandSubTopic>command/motor_speed</commandSubTopic>
          <motorNumber>2</motorNumber>
          <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
          <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
          <motorSpeedPubTopic>motor_speed/2</motorSpeedPubTopic>
          <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
          <motorType>velocity</motorType>
        </plugin>
        <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
          name="ignition::gazebo::systems::MulticopterMotorModel">
          <robotNamespace>model/#{_name}</robotNamespace>
          <jointName>rotor_3_joint</jointName>
          <linkName>rotor_3</linkName>
          <turningDirection>cw</turningDirection>
          <timeConstantUp>0.0125</timeConstantUp>
          <timeConstantDown>0.025</timeConstantDown>
          <maxRotVelocity>800.0</maxRotVelocity>
          <motorConstant>8.54858e-06</motorConstant>
          <momentConstant>0.016</momentConstant>
          <commandSubTopic>command/motor_speed</commandSubTopic>
          <motorNumber>3</motorNumber>
          <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
          <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
          <motorSpeedPubTopic>motor_speed/3</motorSpeedPubTopic>
          <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
          <motorType>velocity</motorType>
        </plugin>
        <!--Multicopter velocity controller-->
        <plugin
          filename="libignition-gazebo-multicopter-control-system.so"
          name="ignition::gazebo::systems::MulticopterVelocityControl">
          <robotNamespace>model/#{_name}</robotNamespace>
          <commandSubTopic>cmd_vel</commandSubTopic>
          <motorControlPubTopic>command/motor_speed</motorControlPubTopic>
          <enableSubTopic>velocity_controller/enable</enableSubTopic>
          <comLinkName>base_link</comLinkName>
          <velocityGain>2.7 2.7 2.7</velocityGain>
          <attitudeGain>2 3 0.15</attitudeGain>
          <angularRateGain>0.4 0.52 0.18</angularRateGain>
          <maximumLinearAcceleration>1 1 2</maximumLinearAcceleration>
          <maximumLinearVelocity>5 5 5</maximumLinearVelocity>
          <maximumAngularVelocity>3 3 3</maximumAngularVelocity>
          <linearVelocityNoiseMean>0 0 0.05</linearVelocityNoiseMean>
          <!-- linearVelocityNoiseStdDev based on error values reported in the paper Shen et. al., -->
          <!-- Vision-Based State Estimation and Trajectory Control Towards High-Speed Flight with a Quadrotor -->
          <!-- http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.490.7958&rep=rep1&type=pdf -->
          <linearVelocityNoiseStdDev>0.1105 0.1261 0.00947</linearVelocityNoiseStdDev>
          <angularVelocityNoiseMean>0 0 0</angularVelocityNoiseMean>
          <!-- angularVelocityNoiseStdDev values based on ADIS16448's Rate Noise Density with a sample  -->
          <!-- time of 0.004 ms. -->
          <angularVelocityNoiseStdDev>0.004 0.004 0.004</angularVelocityNoiseStdDev>
          <rotorConfiguration>
            <rotor>
              <jointName>rotor_0_joint</jointName>
              <forceConstant>8.54858e-06</forceConstant>
              <momentConstant>0.016</momentConstant>
              <direction>1</direction>
            </rotor>
            <rotor>
              <jointName>rotor_1_joint</jointName>
              <forceConstant>8.54858e-06</forceConstant>
              <momentConstant>0.016</momentConstant>
              <direction>1</direction>
            </rotor>
            <rotor>
              <jointName>rotor_2_joint</jointName>
              <forceConstant>8.54858e-06</forceConstant>
              <momentConstant>0.016</momentConstant>
              <direction>-1</direction>
            </rotor>
            <rotor>
              <jointName>rotor_3_joint</jointName>
              <forceConstant>8.54858e-06</forceConstant>
              <momentConstant>0.016</momentConstant>
              <direction>-1</direction>
            </rotor>
          </rotorConfiguration>
        </plugin>
        <!-- Battery plugin -->
        <plugin filename="libignition-gazebo-linearbatteryplugin-system.so"
          name="ignition::gazebo::systems::LinearBatteryPlugin">
          <battery_name>linear_battery</battery_name>
          <voltage>12.694</voltage>
          <open_circuit_voltage_constant_coef>12.694</open_circuit_voltage_constant_coef>
          <open_circuit_voltage_linear_coef>-3.1424</open_circuit_voltage_linear_coef>
          <initial_charge>18.0</initial_charge>
          <capacity>18.0</capacity>
          <resistance>0.061523</resistance>
          <smooth_current_tau>1.9499</smooth_current_tau>
          <power_load>6.6</power_load>
          <start_on_motion>true</start_on_motion>
        </plugin>
        <!-- Gas Sensor plugin -->
        <plugin filename="libGasEmitterDetectorPlugin.so"
          name="subt::GasDetector">
          <topic>/model/#{_name}/gas_detected</topic>
          <update_rate>10</update_rate>
          <type>gas</type>
        </plugin>
      </include>
      </sdf>
    </spawn>
    HEREDOC

    exec = <<-HEREDOC
      <executable name='x3_description'>
        <command>roslaunch --wait subt_ros x3_description.launch world_name:=#{$worldName} name:=#{_name}</command>
      </executable>
      <executable name='x3_ros_ign_bridge'>
        <command>roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} rgbd_cam:=#{rgbdCam} breadcrumbs:=0</command>
      </executable>
    HEREDOC

    return spawn, exec
  end

  def spawnX4(_name, _config, _x, _y, _z, _yaw)
    uav=1
    laserScan=0
    stereoCam=0
    rgbdCam=0
    if _config == "2"
      rgbdCam=1
    end
    if _config == "3" or _config == "4"
      laserScan=1
    end
    if _config == "5"
      stereoCam=1
    end

    z = _z + 0.125

    spawn = <<-HEREDOC
    <spawn name='#{_name}'>
      <name>#{_name}</name>
      <allow_renaming>false</allow_renaming>
      <pose>#{_x} #{_y} #{z} 0 0 #{_yaw}</pose>
      <world>#{$worldName}</world>
      <is_performer>true</is_performer>
      <sdf version='1.6'>
      <include>
        <name>#{_name}</name>
        <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X4 UAV Config #{_config}</uri>
        <!-- Publish robot state information -->
        <plugin filename="libignition-gazebo-pose-publisher-system.so"
          name="ignition::gazebo::systems::PosePublisher">
          <publish_link_pose>true</publish_link_pose>
          <publish_sensor_pose>true</publish_sensor_pose>
          <publish_collision_pose>false</publish_collision_pose>
          <publish_visual_pose>false</publish_visual_pose>
          <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
          <use_pose_vector_msg>true</use_pose_vector_msg>
          <static_publisher>true</static_publisher>
          <static_update_frequency>1</static_update_frequency>
        </plugin>
        <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
          name="ignition::gazebo::systems::MulticopterMotorModel">
          <robotNamespace>model/#{_name}</robotNamespace>
          <jointName>rotor_0_joint</jointName>
          <linkName>rotor_0</linkName>
          <turningDirection>ccw</turningDirection>
          <timeConstantUp>0.0182</timeConstantUp>
          <timeConstantDown>0.0182</timeConstantDown>
          <maxRotVelocity>1000.0</maxRotVelocity>
          <motorConstant>1.269e-05</motorConstant>
          <momentConstant>0.016754</momentConstant>
          <commandSubTopic>command/motor_speed</commandSubTopic>
          <motorNumber>0</motorNumber>
          <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
          <rollingMomentCoefficient>0</rollingMomentCoefficient>
          <motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
          <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
          <motorType>velocity</motorType>
        </plugin>
        <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
          name="ignition::gazebo::systems::MulticopterMotorModel">
          <robotNamespace>model/#{_name}</robotNamespace>
          <jointName>rotor_1_joint</jointName>
          <linkName>rotor_1</linkName>
          <turningDirection>cw</turningDirection>
          <timeConstantUp>0.0182</timeConstantUp>
          <timeConstantDown>0.0182</timeConstantDown>
          <maxRotVelocity>1000.0</maxRotVelocity>
          <motorConstant>1.269e-05</motorConstant>
          <momentConstant>0.016754</momentConstant>
          <commandSubTopic>command/motor_speed</commandSubTopic>
          <motorNumber>1</motorNumber>
          <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
          <rollingMomentCoefficient>0</rollingMomentCoefficient>
          <motorSpeedPubTopic>motor_speed/1</motorSpeedPubTopic>
          <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
          <motorType>velocity</motorType>
        </plugin>
        <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
          name="ignition::gazebo::systems::MulticopterMotorModel">
          <robotNamespace>model/#{_name}</robotNamespace>
          <jointName>rotor_2_joint</jointName>
          <linkName>rotor_2</linkName>
          <turningDirection>ccw</turningDirection>
          <timeConstantUp>0.0182</timeConstantUp>
          <timeConstantDown>0.0182</timeConstantDown>
          <maxRotVelocity>1000.0</maxRotVelocity>
          <motorConstant>1.269e-05</motorConstant>
          <momentConstant>0.016754</momentConstant>
          <commandSubTopic>command/motor_speed</commandSubTopic>
          <motorNumber>2</motorNumber>
          <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
          <rollingMomentCoefficient>0</rollingMomentCoefficient>
          <motorSpeedPubTopic>motor_speed/2</motorSpeedPubTopic>
          <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
          <motorType>velocity</motorType>
        </plugin>
        <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
          name="ignition::gazebo::systems::MulticopterMotorModel">
          <robotNamespace>model/#{_name}</robotNamespace>
          <jointName>rotor_3_joint</jointName>
          <linkName>rotor_3</linkName>
          <turningDirection>cw</turningDirection>
          <timeConstantUp>0.0182</timeConstantUp>
          <timeConstantDown>0.0182</timeConstantDown>
          <maxRotVelocity>1000.0</maxRotVelocity>
          <motorConstant>1.269e-05</motorConstant>
          <momentConstant>0.016754</momentConstant>
          <commandSubTopic>command/motor_speed</commandSubTopic>
          <motorNumber>3</motorNumber>
          <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
          <rollingMomentCoefficient>0</rollingMomentCoefficient>
          <motorSpeedPubTopic>motor_speed/3</motorSpeedPubTopic>
          <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
          <motorType>velocity</motorType>
        </plugin>
        <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
          name="ignition::gazebo::systems::MulticopterMotorModel">
          <robotNamespace>model/#{_name}</robotNamespace>
          <jointName>rotor_4_joint</jointName>
          <linkName>rotor_4</linkName>
          <turningDirection>ccw</turningDirection>
          <timeConstantUp>0.0182</timeConstantUp>
          <timeConstantDown>0.0182</timeConstantDown>
          <maxRotVelocity>1000.0</maxRotVelocity>
          <motorConstant>1.269e-05</motorConstant>
          <momentConstant>0.016754</momentConstant>
          <commandSubTopic>command/motor_speed</commandSubTopic>
          <motorNumber>4</motorNumber>
          <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
          <rollingMomentCoefficient>0</rollingMomentCoefficient>
          <motorSpeedPubTopic>motor_speed/4</motorSpeedPubTopic>
          <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
          <motorType>velocity</motorType>
        </plugin>
        <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
          name="ignition::gazebo::systems::MulticopterMotorModel">
          <robotNamespace>model/#{_name}</robotNamespace>
          <jointName>rotor_5_joint</jointName>
          <linkName>rotor_5</linkName>
          <turningDirection>cw</turningDirection>
          <timeConstantUp>0.0182</timeConstantUp>
          <timeConstantDown>0.0182</timeConstantDown>
          <maxRotVelocity>1000.0</maxRotVelocity>
          <motorConstant>1.269e-05</motorConstant>
          <momentConstant>0.016754</momentConstant>
          <commandSubTopic>command/motor_speed</commandSubTopic>
          <motorNumber>5</motorNumber>
          <rotorDragCoefficient>2.0673e-04</rotorDragCoefficient>
          <rollingMomentCoefficient>0</rollingMomentCoefficient>
          <motorSpeedPubTopic>motor_speed/5</motorSpeedPubTopic>
          <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
          <motorType>velocity</motorType>
        </plugin>
        <!--Multicopter velocity controller-->
        <plugin
          filename="libignition-gazebo-multicopter-control-system.so"
          name="ignition::gazebo::systems::MulticopterVelocityControl">
          <robotNamespace>model/#{_name}</robotNamespace>
          <commandSubTopic>cmd_vel</commandSubTopic>
          <motorControlPubTopic>command/motor_speed</motorControlPubTopic>
          <enableSubTopic>velocity_controller/enable</enableSubTopic>
          <comLinkName>base_link</comLinkName>
          <velocityGain>6 6 10</velocityGain>
          <attitudeGain>4 4 2</attitudeGain>
          <angularRateGain>0.7 0.7 0.7</angularRateGain>
          <maximumLinearAcceleration>1 1 2</maximumLinearAcceleration>
          <maximumLinearVelocity>5 5 5</maximumLinearVelocity>
          <maximumAngularVelocity>3 3 3</maximumAngularVelocity>
          <linearVelocityNoiseMean>0 0 0</linearVelocityNoiseMean>
          <!-- linearVelocityNoiseStdDev based on error values reported in the paper Shen et. al., -->
          <!-- Vision-Based State Estimation and Trajectory Control Towards High-Speed Flight with a Quadrotor -->
          <!-- http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.490.7958&rep=rep1&type=pdf -->
          <linearVelocityNoiseStdDev>0.1105 0.1261 0.0947</linearVelocityNoiseStdDev>
          <angularVelocityNoiseMean>0 0 0</angularVelocityNoiseMean>
          <!-- angularVelocityNoiseStdDev values based on ADIS16448's Rate Noise Density with a sample  -->
          <!-- time of 0.004 ms. -->
          <angularVelocityNoiseStdDev>0.004 0.004 0.004</angularVelocityNoiseStdDev>
          <rotorConfiguration>
            <rotor>
              <jointName>rotor_0_joint</jointName>
              <forceConstant>1.269e-05</forceConstant>
              <momentConstant>1.6754e-2</momentConstant>
              <direction>1</direction>
            </rotor>
            <rotor>
              <jointName>rotor_1_joint</jointName>
              <forceConstant>1.269e-05</forceConstant>
              <momentConstant>1.6754e-2</momentConstant>
              <direction>-1</direction>
            </rotor>
            <rotor>
              <jointName>rotor_2_joint</jointName>
              <forceConstant>1.269e-05</forceConstant>
              <momentConstant>1.6754e-2</momentConstant>
              <direction>1</direction>
            </rotor>
            <rotor>
              <jointName>rotor_3_joint</jointName>
              <forceConstant>1.269e-05</forceConstant>
              <momentConstant>1.6754e-2</momentConstant>
              <direction>-1</direction>
            </rotor>
            <rotor>
              <jointName>rotor_4_joint</jointName>
              <forceConstant>1.269e-05</forceConstant>
              <momentConstant>1.6754e-2</momentConstant>
              <direction>1</direction>
            </rotor>
            <rotor>
              <jointName>rotor_5_joint</jointName>
              <forceConstant>1.269e-05</forceConstant>
              <momentConstant>1.6754e-2</momentConstant>
              <direction>-1</direction>
            </rotor>
          </rotorConfiguration>
        </plugin>
        <!-- Battery plugin -->
        <plugin filename="libignition-gazebo-linearbatteryplugin-system.so"
          name="ignition::gazebo::systems::LinearBatteryPlugin">
          <battery_name>linear_battery</battery_name>
          <voltage>12.694</voltage>
          <open_circuit_voltage_constant_coef>12.694</open_circuit_voltage_constant_coef>
          <open_circuit_voltage_linear_coef>-3.1424</open_circuit_voltage_linear_coef>
          <initial_charge>18.0</initial_charge>
          <capacity>18.0</capacity>
          <resistance>0.061523</resistance>
          <smooth_current_tau>1.9499</smooth_current_tau>
          <power_load>6.6</power_load>
          <start_on_motion>true</start_on_motion>
        </plugin>
        <!-- Gas Sensor plugin -->
        <plugin filename="libGasEmitterDetectorPlugin.so"
          name="subt::GasDetector">
          <topic>/model/#{_name}/gas_detected</topic>
          <update_rate>10</update_rate>
          <type>gas</type>
        </plugin>
        </include>
      </sdf>
    </spawn>
    HEREDOC

    exec = <<-HEREDOC
    <executable name='x4_description'>
      <command>roslaunch --wait subt_ros x4_description.launch world_name:=#{$worldName} name:=#{_name}</command>
    </executable>
    <executable name='x4_ros_ign_bridge'>
      <command>roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} rgbd_cam:=#{rgbdCam} breadcrumbs:=0</command>
    </executable>
    HEREDOC

    return spawn, exec
  end

  def spawnTeamBase(_name, _x, _y, _z, _yaw)
    spawn = <<-HEREDOC
    <spawn name='#{_name}'>
      <name>#{_name}</name>
      <allow_renaming>false</allow_renaming>
      <world>#{$worldName}</world>
      <is_performer>false</is_performer>
      <sdf version='1.6'>
      <model name='#{_name}'>
        <pose>#{_x} #{_y} #{_z} 0 0 #{_yaw}</pose>
        <static>true</static>
        <link name='teambase_link'>
          <pose>0 0 0.05 0 0 0</pose>
          <visual name='visual'>
            <geometry>
              <box>
                <size>.1 .1 .1</size>
              </box>
            </geometry>
          </visual>
        </link>
      </model>
      </sdf>
    </spawn>
    HEREDOC

    exec = <<-HEREDOC
    <executable name='teambase_description'>
      <command>roslaunch --wait subt_ros teambase_description.launch world_name:=#{$worldName} name:=#{_name}</command>
    </executable>
    <executable name='teambase_ros_ign_bridge'>
      <command>roslaunch --wait subt_ros teambase_topics.launch world_name:=#{$worldName} name:=#{_name}</command>
    </executable>
    HEREDOC

    return spawn, exec
  end
%>

<plugin name="ignition::launch::GazeboFactory"
        filename="libignition-launch-gazebo-factory.so">
<%
  # Reverse hash of mersupials, i.e, key: child, value: parent
  marsupialChildren = marsupialParents.invert


  # Spawn nonmarsupial and parent marsupial robots first. Then spawn child
  # marsupials since they need the positions of their parents
  spawnList = robots.select { |name| not marsupialChildren.has_key?(name) }.values +
              robots.select { |name| marsupialChildren.has_key?(name) }.values

  robotSpawned = 0
  spawnList.each do |robot|
    name = robot.name
    config = robot.config
    robotType = robot.type
    robotConfigN = robot.configNumber
    posX = -(robotSpawned / spawnColSize * spawnGridSize - spawnRowOffset)
    posY = -(robotSpawned % spawnColSize * spawnGridSize - spawnColOffset)

    # Spawn above parent position if this robot is marsupial child
    if marsupialChildren.has_key?(name)
      parentRobot = robots[marsupialChildren[name]]
      offsetX = MARSUPIAL_PARENT_ROBOT_POSITION_OFFSETS[parentRobot.type].x
      offsetY = MARSUPIAL_PARENT_ROBOT_POSITION_OFFSETS[parentRobot.type].y
      robot.pos.x = parentRobot.pos.x + Math.cos(-spawnWorldYaw) * offsetX + Math.sin(-spawnWorldYaw) * offsetY
      robot.pos.y = parentRobot.pos.y - Math.sin(-spawnWorldYaw) * offsetX + Math.cos(-spawnWorldYaw) * offsetY
      robot.pos.z = parentRobot.pos.z + MARSUPIAL_PARENT_ROBOT_POSITION_OFFSETS[parentRobot.type].z
      robot.rot.roll = parentRobot.rot.roll + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].roll
      robot.rot.pitch = parentRobot.rot.pitch + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].pitch
      robot.rot.yaw = parentRobot.rot.yaw + MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS[parentRobot.type].yaw
    else
      x = Math.cos(-spawnWorldYaw) * posX + Math.sin(-spawnWorldYaw) * posY
      y = -(Math.sin(-spawnWorldYaw)) * posX + Math.cos(-spawnWorldYaw) * posY
      robot.pos.x = x + spawnWorldXPos
      robot.pos.y = y + spawnWorldYPos
      robot.pos.z = spawnWorldZPos
      robot.rot.yaw = spawnWorldYaw
    end

    spawnStringTmp = ""
    if robotType == "X1" and (robotConfigN == "1" or robotConfigN == "2" or robotConfigN == "3" or robotConfigN == "4" or robotConfigN == "5" or robotConfigN == "7" or robotConfigN == "8")
      robotSpawned += 1
      spawnStringTmp, executables = spawnX1(name, robotConfigN, robot.pos.x, robot.pos.y, robot.pos.z, robot.rot.yaw)
    elsif robotType == "X2" and (robotConfigN == "1" or robotConfigN == "2" or robotConfigN == "3" or robotConfigN == "4" or robotConfigN == "5" or robotConfigN == "6" or robotConfigN == "8" or robotConfigN == "9")
      robotSpawned += 1
      spawnStringTmp, executables  = spawnX2(name, robotConfigN, robot.pos.x, robot.pos.y, robot.pos.z, robot.rot.yaw)
    elsif robotType == "X3" and (robotConfigN == "1" or robotConfigN == "2" or robotConfigN == "3" or robotConfigN == "4")
      robotSpawned += 1
      spawnStringTmp, executables  = spawnX3(name, robotConfigN, robot.pos.x, robot.pos.y, robot.pos.z, robot.rot.yaw)

    elsif robotType == "X4" and (robotConfigN == "1" or robotConfigN == "2" or robotConfigN == "3" or robotConfigN == "4" or robotConfigN == "5")
      robotSpawned += 1

      spawnStringTmp, executables  = spawnX4(name, robotConfigN, robot.pos.x, robot.pos.y, robot.pos.z, robot.rot.yaw)

    elsif config.upcase == "TEAMBASE"
      robotSpawned += 1
      if !teamBaseSpawned
        teamBaseSpawned = true
        spawnStringTmp, executables = spawnTeamBase(name, robot.pos.x, robot.pos.y, robot.pos.z, robot.rot.yaw)
      else
        puts "Error: Only 1 teambase is allows. Skpping[%s]." % [name]
      end
    else
      # Try a team-submitted vehicle.
      package = config.downcase
      installDir = `rospack find #{package}`.chomp
      if installDir.empty?
        raise "Unknown robot configuration #{config}. ROS package #{package} could not be found."
      end

      spawnerScript =  "#{installDir}/launch/spawner.rb"
      begin
        load spawnerScript
      rescue LoadError
        raise "Unknown robot configuration #{config}. #{spawnerScript} could not be found."
      else
        fuelPrefix = "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models"
        # We assume that the model and its supporting files can be found in
        # `installDir`. If `$localModel` is not set, the model is loaded from
        # Fuel.
        if $localModel
          modelURI = installDir
        else
          # check if robot name follows subt robot naming convention on Fuel.
          # if so, construct a name without underscores, e.g. 'X1 Config 2' or
          # 'X3 UAV Config 4', else use original name
          convention = /^x[[:digit:]]_.*[[:digit:]]/.match(package)
          if convention
            if robot.type == "X3" or robot.type == "X4"
              mUAVStr = "UAV "
            else
              mUAVStr = ""
            end
            robotName = "#{robotType} #{mUAVStr}Config #{robotConfigN}"
          else
            robotName = config
          end
          modelURI = "#{fuelPrefix}/#{robotName}"
        end
        robotSpawned += 1

        spawnStringTmp = spawner(name, modelURI, $worldName, robot.pos.x, robot.pos.y, robot.pos.z, 0, 0, robot.rot.yaw)
        executables = rosExecutables(name, $worldName)
      end
    end

    if executables != nil
      allExecutables += executables
    end

    if marsupialParents.has_key?(name)
      childModelName = marsupialParents[name]
      parentModelName = marsupialChildren[childModelName]
      detachableJointPlugin = REXML::Document.new <<-HEREDOC
         <plugin filename="libignition-gazebo-detachable-joint-system.so" name="ignition::gazebo::systems::DetachableJoint">
           <parent_link>#{MARSUPIAL_PARENT_LINK_NAMES[robots[parentModelName].type]}</parent_link>
         <child_model>#{childModelName}</child_model>
         <child_link>base_link</child_link>
         <topic>/model/#{childModelName}/detach</topic>
         <suppress_child_warning>true</suppress_child_warning>
         </plugin>
       HEREDOC

       platformJointPlugin = ""
       platform = ""

       # This will be use to attach the drone platform to the base.
       if robots[name].type.include?("X1")
         platformJointPlugin = REXML::Document.new <<-HEREDOC
           <plugin filename="libignition-gazebo-detachable-joint-system.so" name="ignition::gazebo::systems::DetachableJoint">
           <parent_link>base_link</parent_link>
           <child_model>#{name}_platform</child_model>
           <child_link>base_link</child_link>
           <suppress_child_warning>true</suppress_child_warning>
           </plugin>
         HEREDOC

         # This will be used to spawn the drone platform.
         platform = <<-HEREDOC
           <spawn name="#{name}_platform">
             <name>#{name}_platform</name>
             <allow_renaming>false</allow_renaming>
             <pose>#{robot.pos.x + 0.078} #{robot.pos.y} #{robot.pos.z-0.11} 0 0 0</pose>
             <world>#{$worldName}</world>
             <is_performer>false</is_performer>
             <sdf version='1.6'>
               <include>
                 <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/DronePlatformX1</uri>
               </include>
             </sdf>
           </spawn>
         HEREDOC
       end

       rosExecutable = <<-HEREDOC
         <executable name="#{childModelName}_marsupial">
           <command>roslaunch --wait subt_ros marsupial_topics.launch name:=#{childModelName}</command>
         </executable>
       HEREDOC
       allExecutables += rosExecutable

       # Wrap the temporary spawn string with a root element. This is needed
       # because the xml parser needs a root element. When we're adding the
       # plugin, we print only the contents of the root element.
       spawnXml = REXML::Document.new("<ignition>#{spawnStringTmp}</ignition>")
       search = "//spawn[@name='#{name}']"
       pluginElem = spawnXml.root.elements[search]
       includeElem = pluginElem.elements['sdf/include']
       includeElem.add_element(detachableJointPlugin.root)
       if platformJointPlugin != ""
         includeElem.add_element(platformJointPlugin.root)
       end

       spawnString = ""
       formatter = REXML::Formatters::Default.new
       spawnXml.root.each { |el| formatter.write(el, spawnString) }
       spawnString += platform

     else
     spawnString = spawnStringTmp
    end
%>
<%= spawnString%>
<%
  end
%>
</plugin>
<%= allExecutables %>

</ignition>