cloudsim_bridge.ign

<?xml version="1.0"?>
<!-- This launch file is used by CloudSim to start a ROS master, and handle
     bridging of data to/from Ignition Gazebo to a competitor's docker image.
     It is possible, and safe, to run this launch file locally during testing
     development.

     For more information, please refer to
         https://github.com/osrf/subt/wiki/Cloudsim

    Usage: ign launch cloudsim_bridge.ign
            [worldName:=<worldName>
             robotNameX:=<robotName> robotConfigX:=<robotConfig>
             marsupial:=true
             ... ]

    The [worldName] command line argument is optional,
          defaults to tunnel_circuit_practice_01 if not specified
    The [robotNameX] command line argument is required, where X can be 1 to 20

    Note that each robot requires its own bridge, that is, only one pair of
    robotNameX and robotConfigX arguments can be passed. So if two robots named
    X1 and X2 have been spawned, cloudsim_bridge.ign must be launched two
    times.

    Example that loads tunnel circuit practice world with an X2 robot with configuration 3

      ign launch tunnel_circuit_practice.ign worldName:=tunnel_circuit_practice_01
          robotName1:=X2_3 robotConfig1:=X2_SENSOR_CONFIG_3

    If [marsupial] set set to true, this robot will be a marsupial child robot
-->

<%
  # 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 enableGroundTruith is not defined or is empty/nil
  if !defined?(enableGroundTruth) || enableGroundTruth == nil || enableGroundTruth.empty?
    $enableGroundTruth = false
  else
    $enableGroundTruth = enableGroundTruth
  end
%>

<%
  teamBaseSpawned = false

  # Check if robotNameX and robotConfigX exists
  maxRobotCount = 20
  robotNames = []
  robotConfigs = []
  for i in 1..maxRobotCount do
    if (local_variables.include?(:"robotName#{i}") &&
        local_variables.include?(:"robotConfig#{i}"))
      robotName=eval "robotName#{i}"
      robotConfig=eval "robotConfig#{i}"
      robotNames.append(robotName)
      robotConfigs.append(robotConfig)
    end
  end
  fail ArgumentError, "missing robotNameX argument" if robotNames.nil? or robotNames.empty?
  fail ArgumentError, "missing robotConfigX argument" if robotConfigs.nil? or robotConfigs.empty?

%>

<%
  # Check if worldName is not defined or is empty/nil
  if !defined?(worldName) || worldName == nil || worldName.empty?
    $worldName = 'tunnel_circuit_practice_01'
  else
    $worldName = worldName
  end

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


<ignition version='1.0'>
  <!-- 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%> enable_ground_truth:=<%=($enableGroundTruth)?"1":"0"%> robot_names:=<%= robotNames.join(",") %></command>
  </executable>
<%
  def spawnX1(_name, _config)
    uav=0
    laserScan=0
    stereoCam=0
    lidar3d=0
    breadcrumbs=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
    if _config == "7" or _config == "8"
      breadcrumbs = 1
    end

    "<executable name='x1_description'>\n"\
    "  <command>roslaunch --wait subt_ros x1_description.launch name:=#{_name}</command>\n"\
    "</executable>\n"\
    "<executable name='x1_ros_ign_bridge'>\n"\
      "  <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:=#{breadcrumbs}</command>\n"\
    "</executable>\n"
  end

  def spawnX2(_name, _config)
    uav=0
    laserScan=0
    stereoCam=0
    lidar3d=0
    breadcrumbs=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
    if _config == "8" or _config == "9"
      breadcrumbs=1
    end

    "<executable name='x2_description'>\n"\
    "  <command>roslaunch --wait subt_ros x2_description.launch name:=#{_name}</command>\n"\
    "</executable>\n"\
    "<executable name='x2_ros_ign_bridge'>\n"\
    "  <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:=#{breadcrumbs}</command>\n"\
    "</executable>\n"
  end

  def spawnX3(_name, _config)
    uav=1
    laserScan=0
    stereoCam=0
    rgbdCam=0
    if _config == "3" || _config == "4"
      rgbdCam=1
    end
    if _config == "5"
      stereoCam=1
    end
    "<executable name='x3_description'>\n"\
    "  <command>roslaunch --wait subt_ros x3_description.launch name:=#{_name}</command>\n"\
    "</executable>\n"\
    "<executable name='x3_ros_ign_bridge'>\n"\
      "  <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>\n"\
    "</executable>\n"
  end

  def spawnX4(_name, _config)
    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
    "<executable name='x4_description'>\n"\
    "  <command>roslaunch --wait subt_ros x4_description.launch name:=#{_name}</command>\n"\
    "</executable>\n"\
    "<executable name='x4_ros_ign_bridge'>\n"\
    "  <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>\n"\
    "</executable>\n"
  end

  def spawnTeamBase(_name)
    "<executable name='teambase_description'>\n"\
      "  <command>roslaunch --wait subt_ros teambase_description.launch world_name:=#{$worldName} name:=#{_name}</command>\n"\
    "</executable>\n"\
    "<executable name='teambase_ros_ign_bridge'>\n"\
      "  <command>roslaunch --wait subt_ros teambase_topics.launch world_name:=#{$worldName} name:=#{_name}</command>\n"\
    "</executable>\n"
  end
%>

<%
  robotNames.each_with_index do |name, i|
    config = robotConfigs[i]
    robotConfigN = config[-1]
    if robotConfigs[i][1] == "1" and (robotConfigN == "1" or robotConfigN == "2" or robotConfigN == "3" or robotConfigN == "4" or robotConfigN == "5" or robotConfigN == "7" or robotConfigN == "8")
%>
<%=   spawnX1(robotNames[i], robotConfigs[i][-1]) %>
<%  elsif robotConfigs[i][1] == "2" and (robotConfigN == "1" or robotConfigN == "2" or robotConfigN == "3" or robotConfigN == "4" or robotConfigN == "5" or robotConfigN == "6" or robotConfigN == "8" or robotConfigN == "9") %>
<%=   spawnX2(robotNames[i], robotConfigs[i][-1]) %>
<%  elsif robotConfigs[i][1] == "3" and (robotConfigN == "1" or robotConfigN == "2" or robotConfigN == "3" or robotConfigN == "4") %>
<%=   spawnX3(robotNames[i], robotConfigs[i][-1]) %>
<%  elsif robotConfigs[i][1] == "4" and (robotConfigN == "1" or robotConfigN == "2" or robotConfigN == "3" or robotConfigN == "4" or robotConfigN == "5") %>
<%=   spawnX4(robotNames[i],  robotConfigs[i][-1]) %>
<%
    elsif config.upcase == "TEAMBASE"
      if !teamBaseSpawned
        teamBaseSpawned = true
%>
<%=     spawnTeamBase(robotNames[i]) %>
<%    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
%>
<%=     rosExecutables(name, $worldName) %>
<%
      end
    end

    if defined?(marsupial) and marsupial.downcase.strip == 'true'
%>
<executable name="<%=name%>_marsupial">
  <command>roslaunch --wait subt_ros marsupial_topics.launch name:=<%=name%></command>
</executable>
<%
    end
  end
%>

</ignition>