diff --git a/subt_ign/CMakeLists.txt b/subt_ign/CMakeLists.txt index 54342783..532aa0f7 100644 --- a/subt_ign/CMakeLists.txt +++ b/subt_ign/CMakeLists.txt @@ -132,6 +132,7 @@ list(APPEND SUBT_IGN_PLUGINS ControllerPlugin GameLogicPlugin GasEmitterDetectorPlugin + VisibilityPlugin ) foreach(PLUGIN ${SUBT_IGN_PLUGINS}) diff --git a/subt_ign/launch/virtual_stix.ign b/subt_ign/launch/virtual_stix.ign deleted file mode 100644 index 6ba2137d..00000000 --- a/subt_ign/launch/virtual_stix.ign +++ /dev/null @@ -1,1043 +0,0 @@ - - - -<% - # 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 - 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? - robots[name] = config - end - end - 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 - - - $worldName = 'virtual_stix' - - # 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 -%> - - - - IGN_GAZEBO_SYSTEM_PLUGIN_PATH - $LD_LIBRARY_PATH - - - - - - roslaunch subt_ros competition_init.launch world_name:=<%=$worldName%> robot_names:=<%= robots.keys.join(",") %> - - - - <%= $worldName %>.sdf - <%if defined?(updateRate) && updateRate != nil && !updateRate.empty?%> - <%= updateRate %> - <%end%> - true - true - - true - - /tmp/ign/logs - true - - <%if defined?(seed) && seed != nil && !seed.empty?%> - <%= seed %> - <%end%> - - - - - ogre2 - - - - - - - - - - - - - - - - - - - - <%= $worldName %> - - <%= $durationSec %> - - - - /tmp/ign/logs - subt_virtual_stix - - - - extinguisher_1 - TYPE_EXTINGUISHER - - - backpack_1 - TYPE_BACKPACK - - - phone_1 - TYPE_PHONE - - - rescue_randy_1 - TYPE_RESCUE_RANDY - - - extinguisher_2 - TYPE_EXTINGUISHER - - - drill_1 - TYPE_DRILL - - - drill_2 - TYPE_DRILL - - - drill_3 - TYPE_DRILL - - - drill_4 - TYPE_DRILL - - - phone_2 - TYPE_PHONE - - - backpack_2 - TYPE_BACKPACK - - - rescue_randy_2 - TYPE_RESCUE_RANDY - - - rescue_randy_3 - TYPE_RESCUE_RANDY - - - backpack_3 - TYPE_BACKPACK - - - phone_3 - TYPE_PHONE - - - extinguisher_3 - TYPE_EXTINGUISHER - - - - - <%if !$headless %> - - - <%= $worldName %> - SubT Simulator - <%= ENV['SUBT_IMAGES_PATH'] %>/SubT_logo.svg - - - 3D View - false - docked - - - ogre2 - scene - 0.2 0.2 0.2 - 0.8 0.8 0.8 - -6.3 -4.2 3.6 0 0.268 0.304 - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/<%= $worldName %>/control - /world/<%= $worldName %>/stats - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/<%= $worldName %>/stats - - - - - Entity tree - - - - - - /world/<%= $worldName %>/gui/transform_mode - - - - <%end%> - - - - true - <%= $worldName %> - - visibility_range - - - 500.0 - 2.5 - 40 - 10.0 - - - - 0.2 - 15 - - - - 1000000 - 20 - -90 - QPSK - - - - - - - - -<% - def spawnX1(_name, _config, _x, _y) - uav=0 - laserScan=0 - stereoCam=0 - lidar3d=0 - if _config == "1" or _config == "2" - laserScan=1 - end - if _config == "3" or _config == "4" - lidar3d=1 - end - if _config == "5" - stereoCam=1 - end - "\n"\ - " #{_name}\n"\ - " false\n"\ - " #{_x} #{_y} .2 0 0 0\n"\ - " #{$worldName}\n"\ - " true\n"\ - " \n"\ - " \n"\ - " #{_name}\n"\ - " https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X1 Config #{_config}\n"\ - " \n"\ - " \n"\ - " front_left_wheel_joint\n"\ - " rear_left_wheel_joint\n"\ - " front_right_wheel_joint\n"\ - " rear_right_wheel_joint\n"\ - " #{0.45649 * 1.5}\n"\ - " 0.1651\n"\ - " /model/#{_name}/cmd_vel_relay\n"\ - " \n"\ - " \n"\ - " \n"\ - " true\n"\ - " true\n"\ - " false\n"\ - " false\n"\ - " #{$enableGroundTruth}\n"\ - " true\n"\ - " true\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " \n"\ - " linear_battery\n"\ - " 12.694\n"\ - " 12.694\n"\ - " -3.1424\n"\ - " 78.4\n"\ - " 78.4\n"\ - " 0.061523\n"\ - " 1.9499\n"\ - " 6.6\n"\ - " true\n"\ - " \n"\ - " \n"\ - " \n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros x1_description.launch world_name:=#{$worldName} name:=#{_name}\n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} lidar_3d:=#{lidar3d}\n"\ - "\n" - end - - def spawnX2(_name, _config, _x, _y) - uav=0 - laserScan=0 - stereoCam=0 - lidar3d=0 - if _config == "1" or _config == "2" or _config == "3" or _config == "4" - laserScan=1 - end - if _config == "5" - stereoCam=1 - end - if _config == "6" - lidar3d=1 - end - "\n"\ - " #{_name}\n"\ - " false\n"\ - " #{_x} #{_y} 0.063494 0 0 0\n"\ - " #{$worldName}\n"\ - " true\n"\ - " \n"\ - " \n"\ - " #{_name}\n"\ - " https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X2 Config #{_config}\n"\ - " \n"\ - " \n"\ - " front_left_wheel_joint\n"\ - " rear_left_wheel_joint\n"\ - " front_right_wheel_joint\n"\ - " rear_right_wheel_joint\n"\ - " #{0.33559 * 1.23}\n"\ - " 0.098\n"\ - " /model/#{_name}/cmd_vel_relay\n"\ - " \n"\ - " \n"\ - " \n"\ - " true\n"\ - " true\n"\ - " false\n"\ - " false\n"\ - " #{$enableGroundTruth}\n"\ - " true\n"\ - " true\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " \n"\ - " linear_battery\n"\ - " 12.694\n"\ - " 12.694\n"\ - " -3.1424\n"\ - " 78.4\n"\ - " 78.4\n"\ - " 0.061523\n"\ - " 1.9499\n"\ - " 6.6\n"\ - " true\n"\ - " \n"\ - " \n"\ - " \n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros x2_description.launch world_name:=#{$worldName} name:=#{_name}\n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} lidar_3d:=#{lidar3d}\n"\ - "\n" - end - - def spawnX3(_name, _config, _x, _y) - uav=1 - laserScan=0 - stereoCam=0 - rgbdCam=0 - if _config == "3" || _config == "4" - rgbdCam=1 - end - if _config == "5" - stereoCam=1 - end - "\n"\ - " #{_name}\n"\ - " false\n"\ - " #{_x} #{_y} 0.2 0 0 0\n"\ - " #{$worldName}\n"\ - " true\n"\ - " \n"\ - " \n"\ - " #{_name}\n"\ - " https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X3 UAV Config #{_config}\n"\ - " \n"\ - " \n"\ - " true\n"\ - " true\n"\ - " false\n"\ - " false\n"\ - " #{$enableGroundTruth}\n"\ - " true\n"\ - " true\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_0_joint\n"\ - " rotor_0\n"\ - " ccw\n"\ - " 0.0125\n"\ - " 0.025\n"\ - " 800.0\n"\ - " 8.54858e-06\n"\ - " 0.016\n"\ - " command/motor_speed\n"\ - " 0\n"\ - " 8.06428e-05\n"\ - " 1e-06\n"\ - " motor_speed/0\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_1_joint\n"\ - " rotor_1\n"\ - " ccw\n"\ - " 0.0125\n"\ - " 0.025\n"\ - " 800.0\n"\ - " 8.54858e-06\n"\ - " 0.016\n"\ - " command/motor_speed\n"\ - " 1\n"\ - " 8.06428e-05\n"\ - " 1e-06\n"\ - " motor_speed/1\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_2_joint\n"\ - " rotor_2\n"\ - " cw\n"\ - " 0.0125\n"\ - " 0.025\n"\ - " 800.0\n"\ - " 8.54858e-06\n"\ - " 0.016\n"\ - " command/motor_speed\n"\ - " 2\n"\ - " 8.06428e-05\n"\ - " 1e-06\n"\ - " motor_speed/2\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_3_joint\n"\ - " rotor_3\n"\ - " cw\n"\ - " 0.0125\n"\ - " 0.025\n"\ - " 800.0\n"\ - " 8.54858e-06\n"\ - " 0.016\n"\ - " command/motor_speed\n"\ - " 3\n"\ - " 8.06428e-05\n"\ - " 1e-06\n"\ - " motor_speed/3\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " cmd_vel\n"\ - " command/motor_speed\n"\ - " velocity_controller/enable\n"\ - " base_link\n"\ - " 2.7 2.7 2.7\n"\ - " 2 3 0.15\n"\ - " 0.4 0.52 0.18\n"\ - " 1 1 2\n"\ - " 5 5 5\n"\ - " 3 3 3\n"\ - " 0 0 0.05\n"\ - " \n"\ - " \n"\ - " \n"\ - " 0.1105 0.1261 0.00947\n"\ - " 0 0 0\n"\ - " \n"\ - " \n"\ - " 0.004 0.004 0.004\n"\ - " \n"\ - " \n"\ - " rotor_0_joint\n"\ - " 8.54858e-06\n"\ - " 0.016\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " rotor_1_joint\n"\ - " 8.54858e-06\n"\ - " 0.016\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " rotor_2_joint\n"\ - " 8.54858e-06\n"\ - " 0.016\n"\ - " -1\n"\ - " \n"\ - " \n"\ - " rotor_3_joint\n"\ - " 8.54858e-06\n"\ - " 0.016\n"\ - " -1\n"\ - " \n"\ - " \n"\ - " \n"\ - " \n"\ - " \n"\ - " linear_battery\n"\ - " 12.694\n"\ - " 12.694\n"\ - " -3.1424\n"\ - " 18.0\n"\ - " 18.0\n"\ - " 0.061523\n"\ - " 1.9499\n"\ - " 6.6\n"\ - " true\n"\ - " \n"\ - " \n"\ - " \n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros x3_description.launch world_name:=#{$worldName} name:=#{_name}\n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} rgbd_cam:=#{rgbdCam}\n"\ - "\n" - end - - def spawnX4(_name, _config, _x, _y) - 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 - "\n"\ - " #{_name}\n"\ - " false\n"\ - " #{_x} #{_y} 0.2 0 0 0\n"\ - " #{$worldName}\n"\ - " true\n"\ - " \n"\ - " \n"\ - " #{_name}\n"\ - " https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X4 UAV Config #{_config}\n"\ - " \n"\ - " \n"\ - " true\n"\ - " true\n"\ - " false\n"\ - " false\n"\ - " #{$enableGroundTruth}\n"\ - " true\n"\ - " true\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_0_joint\n"\ - " rotor_0\n"\ - " ccw\n"\ - " 0.0182\n"\ - " 0.0182\n"\ - " 1000.0\n"\ - " 1.269e-05\n"\ - " 0.016754\n"\ - " command/motor_speed\n"\ - " 0\n"\ - " 2.0673e-04\n"\ - " 0\n"\ - " motor_speed/0\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_1_joint\n"\ - " rotor_1\n"\ - " cw\n"\ - " 0.0182\n"\ - " 0.0182\n"\ - " 1000.0\n"\ - " 1.269e-05\n"\ - " 0.016754\n"\ - " command/motor_speed\n"\ - " 1\n"\ - " 2.0673e-04\n"\ - " 0\n"\ - " motor_speed/1\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_2_joint\n"\ - " rotor_2\n"\ - " ccw\n"\ - " 0.0182\n"\ - " 0.0182\n"\ - " 1000.0\n"\ - " 1.269e-05\n"\ - " 0.016754\n"\ - " command/motor_speed\n"\ - " 2\n"\ - " 2.0673e-04\n"\ - " 0\n"\ - " motor_speed/2\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_3_joint\n"\ - " rotor_3\n"\ - " cw\n"\ - " 0.0182\n"\ - " 0.0182\n"\ - " 1000.0\n"\ - " 1.269e-05\n"\ - " 0.016754\n"\ - " command/motor_speed\n"\ - " 3\n"\ - " 2.0673e-04\n"\ - " 0\n"\ - " motor_speed/3\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_4_joint\n"\ - " rotor_4\n"\ - " ccw\n"\ - " 0.0182\n"\ - " 0.0182\n"\ - " 1000.0\n"\ - " 1.269e-05\n"\ - " 0.016754\n"\ - " command/motor_speed\n"\ - " 4\n"\ - " 2.0673e-04\n"\ - " 0\n"\ - " motor_speed/4\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_5_joint\n"\ - " rotor_5\n"\ - " cw\n"\ - " 0.0182\n"\ - " 0.0182\n"\ - " 1000.0\n"\ - " 1.269e-05\n"\ - " 0.016754\n"\ - " command/motor_speed\n"\ - " 5\n"\ - " 2.0673e-04\n"\ - " 0\n"\ - " motor_speed/5\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " cmd_vel\n"\ - " command/motor_speed\n"\ - " velocity_controller/enable\n"\ - " base_link\n"\ - " 6 6 10\n"\ - " 4 4 2\n"\ - " 0.7 0.7 0.7\n"\ - " 1 1 2\n"\ - " 5 5 5\n"\ - " 3 3 3\n"\ - " 0 0 0\n"\ - " \n"\ - " \n"\ - " \n"\ - " 0.1105 0.1261 0.0947\n"\ - " 0 0 0\n"\ - " \n"\ - " \n"\ - " 0.004 0.004 0.004\n"\ - " \n"\ - " \n"\ - " rotor_0_joint\n"\ - " 1.269e-05\n"\ - " 1.6754e-2\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " rotor_1_joint\n"\ - " 1.269e-05\n"\ - " 1.6754e-2\n"\ - " -1\n"\ - " \n"\ - " \n"\ - " rotor_2_joint\n"\ - " 1.269e-05\n"\ - " 1.6754e-2\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " rotor_3_joint\n"\ - " 1.269e-05\n"\ - " 1.6754e-2\n"\ - " -1\n"\ - " \n"\ - " \n"\ - " rotor_4_joint\n"\ - " 1.269e-05\n"\ - " 1.6754e-2\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " rotor_5_joint\n"\ - " 1.269e-05\n"\ - " 1.6754e-2\n"\ - " -1\n"\ - " \n"\ - " \n"\ - " \n"\ - " \n"\ - " \n"\ - " linear_battery\n"\ - " 12.694\n"\ - " 12.694\n"\ - " -3.1424\n"\ - " 18.0\n"\ - " 18.0\n"\ - " 0.061523\n"\ - " 1.9499\n"\ - " 6.6\n"\ - " true\n"\ - " \n"\ - " \n"\ - " \n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros x4_description.launch world_name:=#{$worldName} name:=#{_name}\n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} rgbd_cam:=#{rgbdCam}\n"\ - "\n" - end - - def spawnTeamBase(_x, _y, _z, _yaw) - "\n"\ - " TEAMBASE\n"\ - " false\n"\ - " #{$worldName}\n"\ - " false\n"\ - " \n"\ - " \n"\ - " #{_x} #{_y} #{_z} 0 0 #{_yaw}\n"\ - " true\n"\ - " \n"\ - " 0 0 0.05 0 0 0\n"\ - " \n"\ - " \n"\ - " \n"\ - " .1 .1 .1\n"\ - " \n"\ - " \n"\ - " \n"\ - " \n"\ - " \n"\ - " \n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros teambase_description.launch world_name:=#{$worldName}\n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros teambase_topics.launch world_name:=#{$worldName}\n"\ - "\n" - end -%> - -<% - robotSpawned = 0 - robots.each do |name, config| - robotType = config[1] - robotConfigN = config[-1] - posX = -(robotSpawned / spawnColSize * spawnGridSize - spawnRowOffset) - posY = -(robotSpawned % spawnColSize * spawnGridSize - spawnColOffset) - if robotType == "1" and (robotConfigN == "1" or robotConfigN == "2" or robotConfigN == "3" or robotConfigN == "4" or robotConfigN == "5") - robotSpawned += 1 -%> -<%= spawnX1(name, robotConfigN, posX, posY) %> -<% - elsif robotType == "2" and (robotConfigN == "1" or robotConfigN == "2" or robotConfigN == "3" or robotConfigN == "4" or robotConfigN == "5" or robotConfigN == "6") - robotSpawned += 1 -%> -<%= spawnX2(name, robotConfigN, posX, posY) %> -<% - elsif robotType == "3" and (robotConfigN == "1" or robotConfigN == "2" or robotConfigN == "3" or robotConfigN == "4") - robotSpawned += 1 -%> -<%= spawnX3(name, robotConfigN, posX, posY) %> -<% - elsif robotType == "4" and (robotConfigN == "1" or robotConfigN == "2" or robotConfigN == "3" or robotConfigN == "4" or robotConfigN == "5") - robotSpawned += 1 -%> -<%= spawnX4(name, robotConfigN, posX, posY) %> -<% - elsif name == "TEAMBASE" and config == "TEAMBASE" - robotSpawned += 1 - if !teamBaseSpawned - teamBaseSpawned = true -%> -<%= spawnTeamBase(posX, posY, spawnWorldZPos, spawnWorldYaw) %> -<% - 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 - robotType = config[1] - robotConfigN = config[-1] - if robotType == "3" or robotType == "4" - mUAVStr = "UAV " - else - mUAVStr = "" - end - robotName = "X#{robotType} #{mUAVStr}Config #{robotConfigN}" - else - robotName = config - end - modelURI = "#{fuelPrefix}/#{robotName}" - end - robotSpawned += 1 -%> -<%= spawner(name, modelURI, $worldName, posX, posY, 0.2, 0, 0, 0) %> -<%= rosExecutables(name, $worldName) %> -<% - end - end - end -%> - - diff --git a/subt_ign/launch/virtual_stix_headless.ign b/subt_ign/launch/virtual_stix_headless.ign deleted file mode 100644 index 2250fb1d..00000000 --- a/subt_ign/launch/virtual_stix_headless.ign +++ /dev/null @@ -1,913 +0,0 @@ - - - -<% - # 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 -%> - -<% - # 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? - robots[name] = config - end - end - 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 - - $worldName = 'virtual_stix' - - # 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 -%> - - - - IGN_GAZEBO_SYSTEM_PLUGIN_PATH - $LD_LIBRARY_PATH - - - - - - roslaunch subt_ros competition_init.launch world_name:=<%=$worldName%> robot_names:=<%= robots.keys.join(",") %> - - - - <%= $worldName %>.sdf - <%if defined?(updateRate) && updateRate != nil && !updateRate.empty?%> - <%= updateRate %> - <%end%> - true - true - - true - - /tmp/ign/logs - true - - <%if defined?(seed) && seed != nil && !seed.empty?%> - <%= seed %> - <%end%> - - - - - ogre2 - - - - - - - - - - - - - - - - - - - <%= $worldName %> - - <%= $durationSec %> - - - - /tmp/ign/logs - subt_virtual_stix - - - - extinguisher_1 - TYPE_EXTINGUISHER - - - backpack_1 - TYPE_BACKPACK - - - phone_1 - TYPE_PHONE - - - rescue_randy_1 - TYPE_RESCUE_RANDY - - - extinguisher_2 - TYPE_EXTINGUISHER - - - drill_1 - TYPE_DRILL - - - drill_2 - TYPE_DRILL - - - drill_3 - TYPE_DRILL - - - drill_4 - TYPE_DRILL - - - phone_2 - TYPE_PHONE - - - backpack_2 - TYPE_BACKPACK - - - rescue_randy_2 - TYPE_RESCUE_RANDY - - - rescue_randy_3 - TYPE_RESCUE_RANDY - - - backpack_3 - TYPE_BACKPACK - - - phone_3 - TYPE_PHONE - - - extinguisher_3 - TYPE_EXTINGUISHER - - - - - - - true - <%= $worldName %> - - visibility_range - - - 500.0 - 2.5 - 40 - 10.0 - - - - 0.2 - 15 - - - - 1000000 - 20 - -90 - QPSK - - - - - - - - - -<% - def spawnX1(_name, _config, _x, _y) - uav=0 - laserScan=0 - stereoCam=0 - lidar3d=0 - if _config == "1" or _config == "2" - laserScan=1 - end - if _config == "3" or _config == "4" - lidar3d=1 - end - if _config == "5" - stereoCam=1 - end - "\n"\ - " #{_name}\n"\ - " false\n"\ - " #{_x} #{_y} .2 0 0 0\n"\ - " #{$worldName}\n"\ - " true\n"\ - " \n"\ - " \n"\ - " #{_name}\n"\ - " https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X1 Config #{_config}\n"\ - " \n"\ - " \n"\ - " front_left_wheel_joint\n"\ - " rear_left_wheel_joint\n"\ - " front_right_wheel_joint\n"\ - " rear_right_wheel_joint\n"\ - " #{0.45649 * 1.5}\n"\ - " 0.1651\n"\ - " /model/#{_name}/cmd_vel_relay\n"\ - " \n"\ - " \n"\ - " \n"\ - " true\n"\ - " true\n"\ - " false\n"\ - " false\n"\ - " #{$enableGroundTruth}\n"\ - " true\n"\ - " true\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " \n"\ - " linear_battery\n"\ - " 12.694\n"\ - " 12.694\n"\ - " -3.1424\n"\ - " 78.4\n"\ - " 78.4\n"\ - " 0.061523\n"\ - " 1.9499\n"\ - " 6.6\n"\ - " true\n"\ - " \n"\ - " \n"\ - " \n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros x1_description.launch world_name:=#{$worldName} name:=#{_name}\n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} lidar_3d:=#{lidar3d}\n"\ - "\n" - end - - def spawnX2(_name, _config, _x, _y) - uav=0 - laserScan=0 - stereoCam=0 - lidar3d=0 - if _config == "1" or _config == "2" or _config == "3" or _config == "4" - laserScan=1 - end - if _config == "5" - stereoCam=1 - end - if _config == "6" - lidar3d=1 - end - "\n"\ - " #{_name}\n"\ - " false\n"\ - " #{_x} #{_y} 0.063494 0 0 0\n"\ - " #{$worldName}\n"\ - " true\n"\ - " \n"\ - " \n"\ - " #{_name}\n"\ - " https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X2 Config #{_config}\n"\ - " \n"\ - " \n"\ - " front_left_wheel_joint\n"\ - " rear_left_wheel_joint\n"\ - " front_right_wheel_joint\n"\ - " rear_right_wheel_joint\n"\ - " #{0.33559 * 1.23}\n"\ - " 0.098\n"\ - " /model/#{_name}/cmd_vel_relay\n"\ - " \n"\ - " \n"\ - " \n"\ - " true\n"\ - " true\n"\ - " false\n"\ - " false\n"\ - " #{$enableGroundTruth}\n"\ - " true\n"\ - " true\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " \n"\ - " linear_battery\n"\ - " 12.694\n"\ - " 12.694\n"\ - " -3.1424\n"\ - " 78.4\n"\ - " 78.4\n"\ - " 0.061523\n"\ - " 1.9499\n"\ - " 6.6\n"\ - " true\n"\ - " \n"\ - " \n"\ - " \n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros x2_description.launch world_name:=#{$worldName} name:=#{_name}\n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} lidar_3d:=#{lidar3d}\n"\ - "\n" - end - - def spawnX3(_name, _config, _x, _y) - uav=1 - laserScan=0 - stereoCam=0 - rgbdCam=0 - if _config == "3" || _config == "4" - rgbdCam=1 - end - if _config == "5" - stereoCam=1 - end - "\n"\ - " #{_name}\n"\ - " false\n"\ - " #{_x} #{_y} 0.2 0 0 0\n"\ - " #{$worldName}\n"\ - " true\n"\ - " \n"\ - " \n"\ - " #{_name}\n"\ - " https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X3 UAV Config #{_config}\n"\ - " \n"\ - " \n"\ - " true\n"\ - " true\n"\ - " false\n"\ - " false\n"\ - " #{$enableGroundTruth}\n"\ - " true\n"\ - " true\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_0_joint\n"\ - " rotor_0\n"\ - " ccw\n"\ - " 0.0125\n"\ - " 0.025\n"\ - " 800.0\n"\ - " 8.54858e-06\n"\ - " 0.016\n"\ - " command/motor_speed\n"\ - " 0\n"\ - " 8.06428e-05\n"\ - " 1e-06\n"\ - " motor_speed/0\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_1_joint\n"\ - " rotor_1\n"\ - " ccw\n"\ - " 0.0125\n"\ - " 0.025\n"\ - " 800.0\n"\ - " 8.54858e-06\n"\ - " 0.016\n"\ - " command/motor_speed\n"\ - " 1\n"\ - " 8.06428e-05\n"\ - " 1e-06\n"\ - " motor_speed/1\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_2_joint\n"\ - " rotor_2\n"\ - " cw\n"\ - " 0.0125\n"\ - " 0.025\n"\ - " 800.0\n"\ - " 8.54858e-06\n"\ - " 0.016\n"\ - " command/motor_speed\n"\ - " 2\n"\ - " 8.06428e-05\n"\ - " 1e-06\n"\ - " motor_speed/2\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_3_joint\n"\ - " rotor_3\n"\ - " cw\n"\ - " 0.0125\n"\ - " 0.025\n"\ - " 800.0\n"\ - " 8.54858e-06\n"\ - " 0.016\n"\ - " command/motor_speed\n"\ - " 3\n"\ - " 8.06428e-05\n"\ - " 1e-06\n"\ - " motor_speed/3\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " cmd_vel\n"\ - " command/motor_speed\n"\ - " velocity_controller/enable\n"\ - " base_link\n"\ - " 2.7 2.7 2.7\n"\ - " 2 3 0.15\n"\ - " 0.4 0.52 0.18\n"\ - " 1 1 2\n"\ - " 5 5 5\n"\ - " 3 3 3\n"\ - " 0 0 0.05\n"\ - " \n"\ - " \n"\ - " \n"\ - " 0.1105 0.1261 0.00947\n"\ - " 0 0 0\n"\ - " \n"\ - " \n"\ - " 0.004 0.004 0.004\n"\ - " \n"\ - " \n"\ - " rotor_0_joint\n"\ - " 8.54858e-06\n"\ - " 0.016\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " rotor_1_joint\n"\ - " 8.54858e-06\n"\ - " 0.016\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " rotor_2_joint\n"\ - " 8.54858e-06\n"\ - " 0.016\n"\ - " -1\n"\ - " \n"\ - " \n"\ - " rotor_3_joint\n"\ - " 8.54858e-06\n"\ - " 0.016\n"\ - " -1\n"\ - " \n"\ - " \n"\ - " \n"\ - " \n"\ - " \n"\ - " linear_battery\n"\ - " 12.694\n"\ - " 12.694\n"\ - " -3.1424\n"\ - " 18.0\n"\ - " 18.0\n"\ - " 0.061523\n"\ - " 1.9499\n"\ - " 6.6\n"\ - " true\n"\ - " \n"\ - " \n"\ - " \n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros x3_description.launch world_name:=#{$worldName} name:=#{_name}\n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} rgbd_cam:=#{rgbdCam}\n"\ - "\n" - end - - def spawnX4(_name, _config, _x, _y) - 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 - "\n"\ - " #{_name}\n"\ - " false\n"\ - " #{_x} #{_y} 0.2 0 0 0\n"\ - " #{$worldName}\n"\ - " true\n"\ - " \n"\ - " \n"\ - " #{_name}\n"\ - " https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X4 UAV Config #{_config}\n"\ - " \n"\ - " \n"\ - " true\n"\ - " true\n"\ - " false\n"\ - " false\n"\ - " #{$enableGroundTruth}\n"\ - " true\n"\ - " true\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_0_joint\n"\ - " rotor_0\n"\ - " ccw\n"\ - " 0.0182\n"\ - " 0.0182\n"\ - " 1000.0\n"\ - " 1.269e-05\n"\ - " 0.016754\n"\ - " command/motor_speed\n"\ - " 0\n"\ - " 2.0673e-04\n"\ - " 0\n"\ - " motor_speed/0\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_1_joint\n"\ - " rotor_1\n"\ - " cw\n"\ - " 0.0182\n"\ - " 0.0182\n"\ - " 1000.0\n"\ - " 1.269e-05\n"\ - " 0.016754\n"\ - " command/motor_speed\n"\ - " 1\n"\ - " 2.0673e-04\n"\ - " 0\n"\ - " motor_speed/1\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_2_joint\n"\ - " rotor_2\n"\ - " ccw\n"\ - " 0.0182\n"\ - " 0.0182\n"\ - " 1000.0\n"\ - " 1.269e-05\n"\ - " 0.016754\n"\ - " command/motor_speed\n"\ - " 2\n"\ - " 2.0673e-04\n"\ - " 0\n"\ - " motor_speed/2\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_3_joint\n"\ - " rotor_3\n"\ - " cw\n"\ - " 0.0182\n"\ - " 0.0182\n"\ - " 1000.0\n"\ - " 1.269e-05\n"\ - " 0.016754\n"\ - " command/motor_speed\n"\ - " 3\n"\ - " 2.0673e-04\n"\ - " 0\n"\ - " motor_speed/3\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_4_joint\n"\ - " rotor_4\n"\ - " ccw\n"\ - " 0.0182\n"\ - " 0.0182\n"\ - " 1000.0\n"\ - " 1.269e-05\n"\ - " 0.016754\n"\ - " command/motor_speed\n"\ - " 4\n"\ - " 2.0673e-04\n"\ - " 0\n"\ - " motor_speed/4\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " rotor_5_joint\n"\ - " rotor_5\n"\ - " cw\n"\ - " 0.0182\n"\ - " 0.0182\n"\ - " 1000.0\n"\ - " 1.269e-05\n"\ - " 0.016754\n"\ - " command/motor_speed\n"\ - " 5\n"\ - " 2.0673e-04\n"\ - " 0\n"\ - " motor_speed/5\n"\ - " 10\n"\ - " velocity\n"\ - " \n"\ - " \n"\ - " \n"\ - " model/#{_name}\n"\ - " cmd_vel\n"\ - " command/motor_speed\n"\ - " velocity_controller/enable\n"\ - " base_link\n"\ - " 6 6 10\n"\ - " 4 4 2\n"\ - " 0.7 0.7 0.7\n"\ - " 1 1 2\n"\ - " 5 5 5\n"\ - " 3 3 3\n"\ - " 0 0 0\n"\ - " \n"\ - " \n"\ - " \n"\ - " 0.1105 0.1261 0.0947\n"\ - " 0 0 0\n"\ - " \n"\ - " \n"\ - " 0.004 0.004 0.004\n"\ - " \n"\ - " \n"\ - " rotor_0_joint\n"\ - " 1.269e-05\n"\ - " 1.6754e-2\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " rotor_1_joint\n"\ - " 1.269e-05\n"\ - " 1.6754e-2\n"\ - " -1\n"\ - " \n"\ - " \n"\ - " rotor_2_joint\n"\ - " 1.269e-05\n"\ - " 1.6754e-2\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " rotor_3_joint\n"\ - " 1.269e-05\n"\ - " 1.6754e-2\n"\ - " -1\n"\ - " \n"\ - " \n"\ - " rotor_4_joint\n"\ - " 1.269e-05\n"\ - " 1.6754e-2\n"\ - " 1\n"\ - " \n"\ - " \n"\ - " rotor_5_joint\n"\ - " 1.269e-05\n"\ - " 1.6754e-2\n"\ - " -1\n"\ - " \n"\ - " \n"\ - " \n"\ - " \n"\ - " \n"\ - " linear_battery\n"\ - " 12.694\n"\ - " 12.694\n"\ - " -3.1424\n"\ - " 18.0\n"\ - " 18.0\n"\ - " 0.061523\n"\ - " 1.9499\n"\ - " 6.6\n"\ - " true\n"\ - " \n"\ - " \n"\ - " \n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros x4_description.launch world_name:=#{$worldName} name:=#{_name}\n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} rgbd_cam:=#{rgbdCam}\n"\ - "\n" - end -%> - -<% - robotSpawned = 0 - robots.each do |name, config| - robotType = config[1] - robotConfigN = config[-1] - posX = -(robotSpawned / spawnColSize * spawnGridSize - spawnRowOffset) - posY = -(robotSpawned % spawnColSize * spawnGridSize - spawnColOffset) - if robotType == "1" and (robotConfigN == "1" or robotConfigN == "2" or robotConfigN == "3" or robotConfigN == "4" or robotConfigN == "5") - robotSpawned += 1 -%> -<%= spawnX1(name, robotConfigN, posX, posY) %> -<% - elsif robotType == "2" and (robotConfigN == "1" or robotConfigN == "2" or robotConfigN == "3" or robotConfigN == "4" or robotConfigN == "5" or robotConfigN == "6") - robotSpawned += 1 -%> -<%= spawnX2(name, robotConfigN, posX, posY) %> -<% - elsif robotType == "3" and (robotConfigN == "1" or robotConfigN == "2" or robotConfigN == "3" or robotConfigN == "4") - robotSpawned += 1 -%> -<%= spawnX3(name, robotConfigN, posX, posY) %> -<% - elsif robotType == "4" and (robotConfigN == "1" or robotConfigN == "2" or robotConfigN == "3" or robotConfigN == "4" or robotConfigN == "5") - robotSpawned += 1 -%> -<%= spawnX4(name, robotConfigN, posX, posY) %> -<% - 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 - robotType = config[1] - robotConfigN = config[-1] - if robotType == "3" or robotType == "4" - mUAVStr = "UAV " - else - mUAVStr = "" - end - robotName = "X#{robotType} #{mUAVStr}Config #{robotConfigN}" - else - robotName = config - end - modelURI = "#{fuelPrefix}/#{robotName}" - end - robotSpawned += 1 -%> -<%= spawner(name, modelURI, $worldName, posX, posY, 0.2, 0, 0, 0) %> -<%= rosExecutables(name, $worldName) %> -<% - end - end - end -%> - - diff --git a/subt_ign/src/Common.cc b/subt_ign/src/Common.cc index ceac28fb..0da38093 100644 --- a/subt_ign/src/Common.cc +++ b/subt_ign/src/Common.cc @@ -135,7 +135,8 @@ bool FullWorldPath(const std::string &_worldName, } } else if (_worldName.find("simple") == std::string::npos && - _worldName.find("_qual") == std::string::npos) + _worldName.find("_qual") == std::string::npos && + _worldName.find("_stix") == std::string::npos) { return false; } diff --git a/subt_ign/src/ConnectionHelper.cc b/subt_ign/src/ConnectionHelper.cc index 46826c85..98092401 100644 --- a/subt_ign/src/ConnectionHelper.cc +++ b/subt_ign/src/ConnectionHelper.cc @@ -172,7 +172,18 @@ std::map> {"Jenolan Section 08", {{-82.6196, 39.5982, 140.961}, {-76.3975, 30.5907, 134.982}, {-71.9846, 10.5912, 139.379}, {-16.465, 17.3448, 159.529}}}, {"Jenolan Section 09", {{-194.72, -9.770950, 132.04}, {-82.6196, 39.5982, 140.961}, {-76.3975, 30.5907, 134.982}, {-71.9846, 10.5912, 139.379}}}, {"Jenolan Section 10", {{-264.458, 25.7639, 43.2923}, {-241.489, -6.8719, 112.301}, {-194.72, -9.770950, 132.04}}}, - {"Jenolan Section 11", {{-264.458, 25.7639, 43.2923}, {-241.489, -6.8719, 112.301}}} + {"Jenolan Section 11", {{-264.458, 25.7639, 43.2923}, {-241.489, -6.8719, 112.301}}}, + {"Edgar Mine Virtual STIX Staging", {{36.59, -98.89, 0.22}}}, + {"Edgar Mine Virtual STIX 1", {{-5.357750, -3.895970, 1.542500}, {14.732500, -54.425900, 1.309480}, {22.352600, -47.677800, 1.840880}, {36.688300, -99.091400, 0.502152}}}, + {"Edgar Mine Virtual STIX 2", {{-28.023300, -8.152720, 1.523010}, {-45.176900, -15.067200, 2.253390}, {-19.575200, -76.385600, 1.611400}, {6.422350, -64.002200, 1.529890}, {14.710200, -54.129700, 1.529890}}}, + {"Edgar Mine Virtual STIX 3", {{-19.323300, -76.383000, 1.596170}, {-57.390400, -91.166200, 0.836131}, {-43.042400, -105.988000, 1.170930}}}, + {"Edgar Mine Virtual STIX 4", {{-76.194300, -42.517800, 1.360100},{-57.313300, -90.930900, 0.757498}}}, + {"Edgar Mine Virtual STIX 5", {{-5.551090, -3.690460, 1.977060},{-10.253500, 4.161460, 1.977060},{-28.316800, -7.717830, 1.677650},{-45.286200, -14.888900, 2.101940},{-76.032100, -42.339900, 1.440770}}}, + {"Edgar Mine Virtual STIX 6", {{-10.134400, 4.764300, 1.700290}, {-21.417500, 20.110100, 1.645820}, {13.817500, 26.329800, 2.133420}, {61.013200, 34.655400, 2.746590}}}, + {"Edgar Mine Virtual STIX 7", {{-21.210300, 20.283600, 1.809090}, {-57.365300, 62.626300, 1.809090} ,{-58.171400, 76.645800, 1.761010}, {-24.210600, 97.656100, 13.359000}}}, + {"Edgar Mine Virtual STIX 8", {{13.824900, 26.428100, 2.063650}, {-24.076000, 97.654300, 12.870300}}}, + {"Edgar Mine Virtual STIX 9", {{69.842000, -14.046400, 2.208530}, {61.017200, 34.481600, 2.769580} ,{106.348000, 8.531670, 2.551910}}}, + {"Edgar Mine Virtual STIX 10", {{22.650300, -47.697300, 1.670630} ,{69.681400, -14.199900, 2.066190}, {106.374000, 8.160290, 2.428980}}}, }; std::map diff --git a/subt_ign/src/apps/dot_generator.cc b/subt_ign/src/apps/dot_generator.cc index 10dfc3cb..b58c4a9d 100644 --- a/subt_ign/src/apps/dot_generator.cc +++ b/subt_ign/src/apps/dot_generator.cc @@ -57,48 +57,51 @@ void printGraph(std::vector &_vertexData) out << "\n /* ==== Edges ==== */\n\n"; - for (unsigned int i = 0u; i < _vertexData.size() -1; ++i) + if (!_vertexData.empty()) { - for (unsigned int j = i+1; j < _vertexData.size(); ++j) + for (unsigned int i = 0u; i < _vertexData.size() -1; ++i) { - math::Vector3d point; - if (subt::ConnectionHelper::ComputePoint( - &_vertexData[i], &_vertexData[j], point)) + for (unsigned int j = i+1; j < _vertexData.size(); ++j) { - int cost = 1; - auto tp1 = + math::Vector3d point; + if (subt::ConnectionHelper::ComputePoint( + &_vertexData[i], &_vertexData[j], point)) + { + int cost = 1; + auto tp1 = subt::ConnectionHelper::connectionTypes[_vertexData[i].tileType]; - auto tp2 = + auto tp2 = subt::ConnectionHelper::connectionTypes[_vertexData[j].tileType]; - // Is one of the tile a starting area? If so, the cost should be 1. - bool connectsToStaging = - _vertexData[i].tileType == "Cave Starting Area Type B" || - _vertexData[i].tileType == "Urban Starting Area" || - _vertexData[j].tileType == "Cave Starting Area Type B" || - _vertexData[j].tileType == "Urban Starting Area"; - - if ((tp1 == subt::ConnectionHelper::STRAIGHT && - tp2 == subt::ConnectionHelper::STRAIGHT) || connectsToStaging) - cost = 1; - else if (tp1 == subt::ConnectionHelper::TURN && - tp2 == subt::ConnectionHelper::STRAIGHT) - cost = 3; - else if (tp1 == subt::ConnectionHelper::STRAIGHT && - tp2 == subt::ConnectionHelper::TURN) - cost = 3; - else - cost = 6; - - if (connectsToStaging) - out << " /* Base station */\n"; - out << " " << _vertexData[i].id; - if (_vertexData[i].id < 10) - out << " "; - out << " -- " << _vertexData[j].id; - if (_vertexData[j].id < 10) - out << " "; - out << " " << "[label=" << cost << "];\n"; + // Is one of the tile a starting area? If so, the cost should be 1. + bool connectsToStaging = + _vertexData[i].tileType == "Cave Starting Area Type B" || + _vertexData[i].tileType == "Urban Starting Area" || + _vertexData[j].tileType == "Cave Starting Area Type B" || + _vertexData[j].tileType == "Urban Starting Area"; + + if ((tp1 == subt::ConnectionHelper::STRAIGHT && + tp2 == subt::ConnectionHelper::STRAIGHT) || connectsToStaging) + cost = 1; + else if (tp1 == subt::ConnectionHelper::TURN && + tp2 == subt::ConnectionHelper::STRAIGHT) + cost = 3; + else if (tp1 == subt::ConnectionHelper::STRAIGHT && + tp2 == subt::ConnectionHelper::TURN) + cost = 3; + else + cost = 6; + + if (connectsToStaging) + out << " /* Base station */\n"; + out << " " << _vertexData[i].id; + if (_vertexData[i].id < 10) + out << " "; + out << " -- " << _vertexData[j].id; + if (_vertexData[j].id < 10) + out << " "; + out << " " << "[label=" << cost << "];\n"; + } } } } diff --git a/subt_ign/worlds/virtual_stix.dat b/subt_ign/worlds/virtual_stix.dat new file mode 100644 index 00000000..820cddfb Binary files /dev/null and b/subt_ign/worlds/virtual_stix.dat differ diff --git a/subt_ign/worlds/virtual_stix.dot b/subt_ign/worlds/virtual_stix.dot new file mode 100644 index 00000000..61a4ad78 --- /dev/null +++ b/subt_ign/worlds/virtual_stix.dot @@ -0,0 +1,34 @@ +/* Visibility graph generated by dot_generator */ + +graph { + /* ==== Vertices ==== */ + + 0 [label="0::Edgar Mine Virtual STIX Staging::edgar_staging"]; + 1 [label="1::Edgar Mine Virtual STIX 1::edgar1"]; + 2 [label="2::Edgar Mine Virtual STIX 2::edgar2"]; + 3 [label="3::Edgar Mine Virtual STIX 3::edgar3"]; + 4 [label="4::Edgar Mine Virtual STIX 4::edgar4"]; + 5 [label="5::Edgar Mine Virtual STIX 5::edgar5"]; + 6 [label="6::Edgar Mine Virtual STIX 6::edgar6"]; + 7 [label="7::Edgar Mine Virtual STIX 7::edgar7"]; + 8 [label="8::Edgar Mine Virtual STIX 8::edgar8"]; + 9 [label="9::Edgar Mine Virtual STIX 9::edgar9"]; + 10 [label="10::Edgar Mine Virtual STIX 10::edgar10"]; + + /* ==== Edges ==== */ + + 0 -- 1 [label=1]; + 1 -- 2 [label=1]; + 1 -- 5 [label=1]; + 1 -- 10 [label=1]; + 2 -- 3 [label=1]; + 2 -- 5 [label=1]; + 3 -- 4 [label=1]; + 4 -- 5 [label=1]; + 5 -- 6 [label=1]; + 6 -- 7 [label=1]; + 6 -- 8 [label=1]; + 6 -- 9 [label=1]; + 7 -- 8 [label=1]; + 9 -- 10 [label=1]; +} diff --git a/subt_ign/worlds/virtual_stix.sdf b/subt_ign/worlds/virtual_stix.sdf index 02f743f2..3565978b 100644 --- a/subt_ign/worlds/virtual_stix.sdf +++ b/subt_ign/worlds/virtual_stix.sdf @@ -141,8 +141,68 @@ - edgar - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Edgar Mine Virtual STIX + edgar_staging + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Edgar Mine Virtual STIX Staging + 117 0 0.455351322889328 0 0 -1.923350 + + + + edgar1 + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Edgar Mine Virtual STIX 1 + 117 0 0.455351322889328 0 0 -1.923350 + + + + edgar2 + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Edgar Mine Virtual STIX 2 + 117 0 0.455351322889328 0 0 -1.923350 + + + + edgar3 + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Edgar Mine Virtual STIX 3 + 117 0 0.455351322889328 0 0 -1.923350 + + + + edgar4 + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Edgar Mine Virtual STIX 4 + 117 0 0.455351322889328 0 0 -1.923350 + + + + edgar5 + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Edgar Mine Virtual STIX 5 + 117 0 0.455351322889328 0 0 -1.923350 + + + + edgar6 + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Edgar Mine Virtual STIX 6 + 117 0 0.455351322889328 0 0 -1.923350 + + + + edgar7 + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Edgar Mine Virtual STIX 7 + 117 0 0.455351322889328 0 0 -1.923350 + + + + edgar8 + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Edgar Mine Virtual STIX 8 + 117 0 0.455351322889328 0 0 -1.923350 + + + + edgar9 + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Edgar Mine Virtual STIX 9 + 117 0 0.455351322889328 0 0 -1.923350 + + + + edgar10 + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Edgar Mine Virtual STIX 10 117 0 0.455351322889328 0 0 -1.923350 @@ -273,5 +333,44 @@ 51.116401672363281 -5.0199999809265137 0.60094296932220459 0 0 0 + + + 0.000000 0.000000 0.000000 + + + + + + + 10 + + 0.05 + 60 + + + 0 + 0.0002 + + + + 30 + + 5 + 20 + + + 0 + 0.03 + + + + + + 0 + 0.03 + + + +