diff --git a/ign_migration_scripts/launch/preview.ign b/ign_migration_scripts/launch/preview.ign index e0c13ca0..42109dc6 100644 --- a/ign_migration_scripts/launch/preview.ign +++ b/ign_migration_scripts/launch/preview.ign @@ -209,11 +209,21 @@ true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + @@ -258,11 +268,21 @@ true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + model/#{_name} diff --git a/ign_migration_scripts/scripts/spawn_preview_model.py b/ign_migration_scripts/scripts/spawn_preview_model.py index 5d64a928..e2fe426e 100755 --- a/ign_migration_scripts/scripts/spawn_preview_model.py +++ b/ign_migration_scripts/scripts/spawn_preview_model.py @@ -73,11 +73,23 @@ def spawn_preview_model(args): etree.SubElement(plugin, 'publish_sensor_pose').text = 'true' etree.SubElement(plugin, 'publish_collision_pose').text = 'false' etree.SubElement(plugin, 'publish_visual_pose').text = 'false' - etree.SubElement(plugin, 'publish_nested_model_pose').text = 'true' + etree.SubElement(plugin, 'publish_nested_model_pose').text = 'false' etree.SubElement(plugin, 'use_pose_vector_msg').text = 'true' etree.SubElement(plugin, 'static_publisher').text = 'true' etree.SubElement(plugin, 'static_update_frequency').text = '1' + plugin = etree.SubElement(model, 'plugin', + filename='libignition-gazebo-pose-publisher-system.so', + name='ignition::gazebo::systems::PosePublisher') + + etree.SubElement(plugin, 'publish_link_pose').text = 'false' + etree.SubElement(plugin, 'publish_sensor_pose').text = 'false' + etree.SubElement(plugin, 'publish_collision_pose').text = 'false' + etree.SubElement(plugin, 'publish_visual_pose').text = 'false' + etree.SubElement(plugin, 'publish_nested_model_pose').text = 'true' + etree.SubElement(plugin, 'use_pose_vector_msg').text = 'true' + etree.SubElement(plugin, 'static_publisher').text = 'false' + sdf_esc = etree.tostring(sdf, encoding="unicode", with_tail=False).replace('"', r'\"').replace('\n', '') diff --git a/submitted_models/cerberus_anymal_b_sensor_config_1/launch/spawner.rb b/submitted_models/cerberus_anymal_b_sensor_config_1/launch/spawner.rb index cdeca0c7..fe5d17d9 100644 --- a/submitted_models/cerberus_anymal_b_sensor_config_1/launch/spawner.rb +++ b/submitted_models/cerberus_anymal_b_sensor_config_1/launch/spawner.rb @@ -45,11 +45,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/cerberus_anymal_b_sensor_config_2/launch/spawner.rb b/submitted_models/cerberus_anymal_b_sensor_config_2/launch/spawner.rb index 34264fed..8ea5e6bf 100644 --- a/submitted_models/cerberus_anymal_b_sensor_config_2/launch/spawner.rb +++ b/submitted_models/cerberus_anymal_b_sensor_config_2/launch/spawner.rb @@ -45,11 +45,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/cerberus_anymal_c_sensor_config_1/launch/spawner.rb b/submitted_models/cerberus_anymal_c_sensor_config_1/launch/spawner.rb index 4c943b20..804890fa 100644 --- a/submitted_models/cerberus_anymal_c_sensor_config_1/launch/spawner.rb +++ b/submitted_models/cerberus_anymal_c_sensor_config_1/launch/spawner.rb @@ -45,11 +45,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/cerberus_anymal_c_sensor_config_2/launch/spawner.rb b/submitted_models/cerberus_anymal_c_sensor_config_2/launch/spawner.rb index 68fbb676..b0621857 100644 --- a/submitted_models/cerberus_anymal_c_sensor_config_2/launch/spawner.rb +++ b/submitted_models/cerberus_anymal_c_sensor_config_2/launch/spawner.rb @@ -45,11 +45,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/cerberus_gagarin_sensor_config_1/launch/spawner.rb b/submitted_models/cerberus_gagarin_sensor_config_1/launch/spawner.rb index 75c20842..11e70614 100755 --- a/submitted_models/cerberus_gagarin_sensor_config_1/launch/spawner.rb +++ b/submitted_models/cerberus_gagarin_sensor_config_1/launch/spawner.rb @@ -17,11 +17,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + model/#{_name} diff --git a/submitted_models/cerberus_m100_sensor_config_1/launch/spawner.rb b/submitted_models/cerberus_m100_sensor_config_1/launch/spawner.rb index 44958be7..c8642053 100755 --- a/submitted_models/cerberus_m100_sensor_config_1/launch/spawner.rb +++ b/submitted_models/cerberus_m100_sensor_config_1/launch/spawner.rb @@ -42,11 +42,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + model/#{_name} diff --git a/submitted_models/cerberus_rmf_sensor_config_1/launch/spawner.rb b/submitted_models/cerberus_rmf_sensor_config_1/launch/spawner.rb index f36589ca..0030a836 100755 --- a/submitted_models/cerberus_rmf_sensor_config_1/launch/spawner.rb +++ b/submitted_models/cerberus_rmf_sensor_config_1/launch/spawner.rb @@ -17,11 +17,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + model/#{_name} diff --git a/submitted_models/costar_husky_sensor_config_1/launch/spawner.rb b/submitted_models/costar_husky_sensor_config_1/launch/spawner.rb index 50a215e1..0bef6e80 100644 --- a/submitted_models/costar_husky_sensor_config_1/launch/spawner.rb +++ b/submitted_models/costar_husky_sensor_config_1/launch/spawner.rb @@ -32,11 +32,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/costar_husky_sensor_config_2/launch/spawner.rb b/submitted_models/costar_husky_sensor_config_2/launch/spawner.rb index 9b759d77..b9fc5703 100644 --- a/submitted_models/costar_husky_sensor_config_2/launch/spawner.rb +++ b/submitted_models/costar_husky_sensor_config_2/launch/spawner.rb @@ -32,11 +32,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/costar_shafter_sensor_config_1/launch/spawner.rb b/submitted_models/costar_shafter_sensor_config_1/launch/spawner.rb index 2dbda268..cd036845 100644 --- a/submitted_models/costar_shafter_sensor_config_1/launch/spawner.rb +++ b/submitted_models/costar_shafter_sensor_config_1/launch/spawner.rb @@ -17,11 +17,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + model/#{_name} diff --git a/submitted_models/csiro_data61_ozbot_atr_sensor_config_1/launch/spawner.rb b/submitted_models/csiro_data61_ozbot_atr_sensor_config_1/launch/spawner.rb index 10b8d567..15036ea3 100644 --- a/submitted_models/csiro_data61_ozbot_atr_sensor_config_1/launch/spawner.rb +++ b/submitted_models/csiro_data61_ozbot_atr_sensor_config_1/launch/spawner.rb @@ -64,11 +64,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_2/launch/spawner.rb b/submitted_models/ctu_cras_norlab_absolem_sensor_config_2/launch/spawner.rb index f8d5f4df..5913aafe 100644 --- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_2/launch/spawner.rb +++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_2/launch/spawner.rb @@ -104,11 +104,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/explorer_ds1_sensor_config_1/launch/spawner.rb b/submitted_models/explorer_ds1_sensor_config_1/launch/spawner.rb index 01e42a5d..f6c940ac 100755 --- a/submitted_models/explorer_ds1_sensor_config_1/launch/spawner.rb +++ b/submitted_models/explorer_ds1_sensor_config_1/launch/spawner.rb @@ -17,11 +17,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + model/#{_name} diff --git a/submitted_models/explorer_r2_sensor_config_1/launch/spawner.rb b/submitted_models/explorer_r2_sensor_config_1/launch/spawner.rb index ff0bbbd2..6d7b77fc 100644 --- a/submitted_models/explorer_r2_sensor_config_1/launch/spawner.rb +++ b/submitted_models/explorer_r2_sensor_config_1/launch/spawner.rb @@ -32,11 +32,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/explorer_r2_sensor_config_2/launch/spawner.rb b/submitted_models/explorer_r2_sensor_config_2/launch/spawner.rb index 1be8cf06..e2e0bb79 100644 --- a/submitted_models/explorer_r2_sensor_config_2/launch/spawner.rb +++ b/submitted_models/explorer_r2_sensor_config_2/launch/spawner.rb @@ -32,11 +32,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/explorer_x1_sensor_config_1/launch/spawner.rb b/submitted_models/explorer_x1_sensor_config_1/launch/spawner.rb index ae4114e7..a4121290 100644 --- a/submitted_models/explorer_x1_sensor_config_1/launch/spawner.rb +++ b/submitted_models/explorer_x1_sensor_config_1/launch/spawner.rb @@ -32,11 +32,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/explorer_x1_sensor_config_2/launch/spawner.rb b/submitted_models/explorer_x1_sensor_config_2/launch/spawner.rb index c70d6bf2..1873a62e 100644 --- a/submitted_models/explorer_x1_sensor_config_2/launch/spawner.rb +++ b/submitted_models/explorer_x1_sensor_config_2/launch/spawner.rb @@ -1,4 +1,9 @@ +# Library of functions for robots +require `rospack find subt_ign`.chomp + "/launch/robot_common_defs.rb" + def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) + config = File.dirname(File.dirname(__FILE__)).upcase + robot = Robot.new(_name, config) <<-HEREDOC #{_name} @@ -25,18 +30,10 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) -3 3 - - - true - true - false - false - #{$enableGroundTruth} - true - true - 1 - + + + #{robot.commonPlugins($enableGroundTruth)} + @@ -51,30 +48,10 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) 6.6 true - " - - /model/#{_name}/gas_detected - 10 - gas - - - /model/#{_name}/breadcrumb/deploy - 12" - 3.0 - true - " - - - -0.45 0 0 0 0 0 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Breadcrumb Node - - - - - + + + #{robot.breadcrumbsPlugins(12, '-0.45 0 0 0 0 0')} + diff --git a/submitted_models/marble_hd2_sensor_config_1/launch/spawner.rb b/submitted_models/marble_hd2_sensor_config_1/launch/spawner.rb index a0b8bf25..ea79f71a 100644 --- a/submitted_models/marble_hd2_sensor_config_1/launch/spawner.rb +++ b/submitted_models/marble_hd2_sensor_config_1/launch/spawner.rb @@ -36,11 +36,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/marble_hd2_sensor_config_2/launch/spawner.rb b/submitted_models/marble_hd2_sensor_config_2/launch/spawner.rb index 7fdb5c2f..e7715b51 100644 --- a/submitted_models/marble_hd2_sensor_config_2/launch/spawner.rb +++ b/submitted_models/marble_hd2_sensor_config_2/launch/spawner.rb @@ -36,11 +36,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/marble_husky_sensor_config_1/launch/spawner.rb b/submitted_models/marble_husky_sensor_config_1/launch/spawner.rb index f4559e6d..8fe792f3 100644 --- a/submitted_models/marble_husky_sensor_config_1/launch/spawner.rb +++ b/submitted_models/marble_husky_sensor_config_1/launch/spawner.rb @@ -32,11 +32,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/marble_husky_sensor_config_2/launch/spawner.rb b/submitted_models/marble_husky_sensor_config_2/launch/spawner.rb index 85db28f4..27f711e7 100644 --- a/submitted_models/marble_husky_sensor_config_2/launch/spawner.rb +++ b/submitted_models/marble_husky_sensor_config_2/launch/spawner.rb @@ -32,11 +32,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/marble_qav500_sensor_config_1/launch/spawner.rb b/submitted_models/marble_qav500_sensor_config_1/launch/spawner.rb index a0ab02aa..8760f8bb 100755 --- a/submitted_models/marble_qav500_sensor_config_1/launch/spawner.rb +++ b/submitted_models/marble_qav500_sensor_config_1/launch/spawner.rb @@ -17,11 +17,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + model/#{_name} diff --git a/submitted_models/robotika_freyja_sensor_config_1/launch/spawner.rb b/submitted_models/robotika_freyja_sensor_config_1/launch/spawner.rb index 44296220..b52c1544 100644 --- a/submitted_models/robotika_freyja_sensor_config_1/launch/spawner.rb +++ b/submitted_models/robotika_freyja_sensor_config_1/launch/spawner.rb @@ -32,11 +32,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/robotika_freyja_sensor_config_2/launch/spawner.rb b/submitted_models/robotika_freyja_sensor_config_2/launch/spawner.rb index 1f091c8c..e3556e70 100644 --- a/submitted_models/robotika_freyja_sensor_config_2/launch/spawner.rb +++ b/submitted_models/robotika_freyja_sensor_config_2/launch/spawner.rb @@ -32,11 +32,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/robotika_kloubak_sensor_config_1/launch/spawner.rb b/submitted_models/robotika_kloubak_sensor_config_1/launch/spawner.rb index 9c0f42d3..a058c31e 100644 --- a/submitted_models/robotika_kloubak_sensor_config_1/launch/spawner.rb +++ b/submitted_models/robotika_kloubak_sensor_config_1/launch/spawner.rb @@ -45,11 +45,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/robotika_kloubak_sensor_config_2/launch/spawner.rb b/submitted_models/robotika_kloubak_sensor_config_2/launch/spawner.rb index c3ccb2aa..45c16a1d 100644 --- a/submitted_models/robotika_kloubak_sensor_config_2/launch/spawner.rb +++ b/submitted_models/robotika_kloubak_sensor_config_2/launch/spawner.rb @@ -45,11 +45,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/robotika_x2_sensor_config_1/launch/spawner.rb b/submitted_models/robotika_x2_sensor_config_1/launch/spawner.rb index 9b5c3460..99f6b76c 100644 --- a/submitted_models/robotika_x2_sensor_config_1/launch/spawner.rb +++ b/submitted_models/robotika_x2_sensor_config_1/launch/spawner.rb @@ -32,11 +32,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/sophisticated_engineering_x2_sensor_config_1/launch/spawner.rb b/submitted_models/sophisticated_engineering_x2_sensor_config_1/launch/spawner.rb index 5032277c..181f2a08 100644 --- a/submitted_models/sophisticated_engineering_x2_sensor_config_1/launch/spawner.rb +++ b/submitted_models/sophisticated_engineering_x2_sensor_config_1/launch/spawner.rb @@ -34,11 +34,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/spawner.rb b/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/spawner.rb index f6731782..5b784715 100644 --- a/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/spawner.rb +++ b/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/spawner.rb @@ -19,11 +19,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + model/#{_name} diff --git a/submitted_models/ssci_x2_sensor_config_1/launch/spawner.rb b/submitted_models/ssci_x2_sensor_config_1/launch/spawner.rb index 7f8ca08b..02cd3571 100755 --- a/submitted_models/ssci_x2_sensor_config_1/launch/spawner.rb +++ b/submitted_models/ssci_x2_sensor_config_1/launch/spawner.rb @@ -46,11 +46,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/ssci_x4_sensor_config_1/launch/spawner.rb b/submitted_models/ssci_x4_sensor_config_1/launch/spawner.rb index e6f6bacb..4593e079 100755 --- a/submitted_models/ssci_x4_sensor_config_1/launch/spawner.rb +++ b/submitted_models/ssci_x4_sensor_config_1/launch/spawner.rb @@ -41,11 +41,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + model/#{_name} diff --git a/submitted_models/ssci_x4_sensor_config_2/launch/spawner.rb b/submitted_models/ssci_x4_sensor_config_2/launch/spawner.rb index 574df7d4..bca50c46 100755 --- a/submitted_models/ssci_x4_sensor_config_2/launch/spawner.rb +++ b/submitted_models/ssci_x4_sensor_config_2/launch/spawner.rb @@ -36,11 +36,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + model/#{_name} diff --git a/submitted_models/x1_sensor_config_6/launch/spawner.rb b/submitted_models/x1_sensor_config_6/launch/spawner.rb index 6441e890..eb938747 100644 --- a/submitted_models/x1_sensor_config_6/launch/spawner.rb +++ b/submitted_models/x1_sensor_config_6/launch/spawner.rb @@ -32,11 +32,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/x2_sensor_config_7/launch/spawner.rb b/submitted_models/x2_sensor_config_7/launch/spawner.rb index d79e59d8..d713cb2d 100644 --- a/submitted_models/x2_sensor_config_7/launch/spawner.rb +++ b/submitted_models/x2_sensor_config_7/launch/spawner.rb @@ -32,11 +32,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + diff --git a/submitted_models/x3_sensor_config_5/launch/spawner.rb b/submitted_models/x3_sensor_config_5/launch/spawner.rb index 13091559..6e346e41 100644 --- a/submitted_models/x3_sensor_config_5/launch/spawner.rb +++ b/submitted_models/x3_sensor_config_5/launch/spawner.rb @@ -17,11 +17,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + model/#{_name} diff --git a/submitted_models/x4_sensor_config_6/launch/spawner.rb b/submitted_models/x4_sensor_config_6/launch/spawner.rb index 02d0e3d4..4ce9a881 100644 --- a/submitted_models/x4_sensor_config_6/launch/spawner.rb +++ b/submitted_models/x4_sensor_config_6/launch/spawner.rb @@ -17,11 +17,21 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw) true false false - #{$enableGroundTruth} + false true true 1 + + false + false + false + false + #{$enableGroundTruth} + true + false + model/#{_name} diff --git a/subt_ign/launch/cloudsim_bridge.ign b/subt_ign/launch/cloudsim_bridge.ign index b5db900d..bf523cc9 100644 --- a/subt_ign/launch/cloudsim_bridge.ign +++ b/subt_ign/launch/cloudsim_bridge.ign @@ -30,6 +30,14 @@ If [marsupial] set set to true, this robot will be a marsupial child robot --> +<% + # Library of functions for worlds + require File.dirname(__FILE__) + "/world_common_defs.rb" + + # Library for spawning the builtin robots + require File.dirname(__FILE__) + "/robot_spawners.rb" +%> + <% # When 'enableGroundTruth = true' absolute poses of vehicles will be published. # This is useful for debugging purposes, but will not be available during @@ -43,27 +51,6 @@ 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? @@ -72,168 +59,43 @@ $worldName = worldName end - worldNumber = $worldName.split('_').last + world = World.new($worldName) %> +<% + # there should only be one robot in this script, but unless requiring marsupials, it supports more + robots = parseRobotArgs(binding) +%> - - roslaunch subt_ros competition_init.launch world_name:=<%=$worldName%> enable_ground_truth:=<%=($enableGroundTruth)?"1":"0"%> robot_names:=<%= robotNames.join(",") %> + roslaunch subt_ros competition_init.launch world_name:=<%=$worldName%> enable_ground_truth:=<%=($enableGroundTruth)?"1":"0"%> robot_names:=<%= robots.values.map{|r| r.name}.join(",") %> -<% - 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 - "\n"\ - " roslaunch --wait subt_ros x1_description.launch 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} breadcrumbs:=#{breadcrumbs}\n"\ - "\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 +<% + teamBaseSpawned = false - "\n"\ - " roslaunch --wait subt_ros x2_description.launch 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} breadcrumbs:=#{breadcrumbs}\n"\ - "\n" - end + robots.each_value do |robot| - 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 - "\n"\ - " roslaunch --wait subt_ros x3_description.launch 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} breadcrumbs:=0\n"\ - "\n" - end + _, executables = spawnRobot(robot, world) - 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 - "\n"\ - " roslaunch --wait subt_ros x4_description.launch 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} breadcrumbs:=0\n"\ - "\n" - end - - def spawnTeamBase(_name) - "\n"\ - " roslaunch --wait subt_ros teambase_description.launch world_name:=#{$worldName} name:=#{_name}\n"\ - "\n"\ - "\n"\ - " roslaunch --wait subt_ros teambase_topics.launch world_name:=#{$worldName} name:=#{_name}\n"\ - "\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 robot.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) %> -<% + next end end - +%> + <%= executables %> +<% if defined?(marsupial) and marsupial.downcase.strip == 'true' %> - - roslaunch --wait subt_ros marsupial_topics.launch name:=<%=name%> - + + roslaunch --wait subt_ros marsupial_topics.launch name:=<%=robot.name%> + <% end end diff --git a/subt_ign/launch/cloudsim_sim.ign b/subt_ign/launch/cloudsim_sim.ign index 30c042a5..8ed89cac 100644 --- a/subt_ign/launch/cloudsim_sim.ign +++ b/subt_ign/launch/cloudsim_sim.ign @@ -28,6 +28,14 @@ require "rexml/document" %> +<% + # Library of functions for worlds + require File.dirname(__FILE__) + "/world_common_defs.rb" + + # Library for spawning the builtin robots + require File.dirname(__FILE__) + "/robot_spawners.rb" +%> + <% # When 'enableGroundTruth = true' absolute poses of vehicles will be published. # This is useful for debugging purposes, but will not be available during @@ -52,173 +60,7 @@ else $logPath = logPath 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 - - $commsFadingExponent = 2.5 - if $circuit == "cave" - $commsFadingExponent = 1.5 - end - - # spawn world offset - if $circuit == "urban" - spawnWorldXPos = 6 - spawnWorldYPos = 27 - spawnWorldZPos = 7.5 - spawnWorldYaw = 0 - guiCameraPose = "-2.0 27 15 0 0.868 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", - "EXPLORER_X1" => "base_link", - "EXPLORER_R2" => "Rear_Rocker_Link", - "CSIRO_DATA61_OZBOT_ATR" => "base_link" - } - - MARSUPIAL_VALID_ROBOT_PAIRS = { - "X1" => ["X3", "MARBLE_QAV500", "CERBERUS_GAGARIN", "CERBERUS_RMF", "COSTAR_SHAFTER"], - "EXPLORER_X1" =>["X3", "MARBLE_QAV500", "CERBERUS_GAGARIN", "CERBERUS_RMF", "COSTAR_SHAFTER"], - "EXPLORER_R2" =>["X3", "X4", "SSCI_X4", "SOPHISTICATED_ENGINEERING_X4", "EXPLORER_DS1", "MARBLE_QAV500", "CERBERUS_M100", "CERBERUS_GAGARIN", "CERBERUS_RMF", "COSTAR_SHAFTER"], - "CSIRO_DATA61_OZBOT_ATR" =>["X3", "X4", "SSCI_X4", "SOPHISTICATED_ENGINEERING_X4","EXPLORER_DS1", "MARBLE_QAV500", "CERBERUS_M100", "CERBERUS_GAGARIN", "CERBERUS_RMF", "COSTAR_SHAFTER"] - } - - MARSUPIAL_PARENT_ROBOT_POSITION_OFFSETS = { - "X1" => Vector3d.new(0, 0, 0.717), - "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) - } - - MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS = { - "X1" => 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) - } - - 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 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 = 'simple_cave_01' @@ -241,15 +83,13 @@ 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. + # durationSec to zero, which equals 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. @@ -261,16 +101,17 @@ %> <% - # disable levels for simple cave worlds - $levels = "true" - if $worldName.include?('simple_cave_') || $worldName.include?('cave_qual') || $worldName.include?('niosh_') - $levels = "false" - end + world = World.new($worldName) +%> +<% + $levels = world.levels if defined?(levels) && levels != nil && !levels.empty? $levels = levels.downcase == "true" end +%> +<% $websocketAuthKey = "" if defined?(websocketAuthKey) && websocketAuthKey != nil && !websocketAuthKey.empty? $websocketAuthKey = websocketAuthKey @@ -298,22 +139,19 @@ %> +<% + robots = parseRobotArgs(binding) + marsupialParents = parseMarsupials(binding, robots) +%> + <%if $ros %> - roslaunch subt_ros cloudsim_init.launch world_name:=<%=$worldName%> enable_ground_truth:=<%=($enableGroundTruth)?"1":"0"%> robot_names:=<%= robotNames.join(",") %> + roslaunch subt_ros cloudsim_init.launch world_name:=<%=$worldName%> enable_ground_truth:=<%=($enableGroundTruth)?"1":"0"%> robot_names:=<%= robots.values.map{|r| r.name}.join(",") %> <%end%> - - IGN_GAZEBO_SYSTEM_PLUGIN_PATH - $LD_LIBRARY_PATH - - - - IGN_TRANSPORT_TOPIC_STATISTICS - 1 - + <%= world.commonLaunchEnvironment() %> - <% if $worldName.include?('tunnel_circuit_') && - !$worldName.include?('practice') %> - tunnel_circuit/<%= worldNumber %>/<%= $worldName %>.sdf - <% elsif $worldName.include?('urban_circuit_') && - !$worldName.include?('practice') %> - urban_circuit/<%= worldNumber %>/<%= $worldName %>.sdf - <% elsif $worldName.include?('cave_circuit_') && - !$worldName.include?('practice') %> - cave_circuit/<%= worldNumber %>/<%= $worldName %>.sdf - <% else %> - <%= $worldName %>.sdf - <% end %> + <%= world.path %> <%if defined?(updateRate) && updateRate != nil && !updateRate.empty?%> <%= updateRate %> <%end%> @@ -358,1156 +185,74 @@ <%= seed %> <%end%> - - - - ogre2 - - - - - - - - - - - - - - - - - - - <%= $worldName %> - <%= $ros %> - - <%= $durationSec %> - - - - <%= $logPath %> - subt_<%= $circuit %> - 5 - - + <%= world.commonServerPlugins() %> + <%= world.subtServerPlugins($durationSec, $logPath, $ros) %> + <%= world.subtLaunchPlugins() %> + <%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 - <%= guiCameraPose %> - - - - 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 - - - - - - - + <%= world.guiPlugin() %> <%end%> - - - true - <%= $worldName %> - - visibility_range - - - 500.0 - <%= $commsFadingExponent %> - 40 - 10.0 - - - - 0.2 - 10 - - - - 1000000 - 20 - -90 - QPSK - - - - - - - - - -<% - 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 - - #{_name} - false - #{_x} #{_y} #{z} 0 0 #{_yaw} - #{$worldName} - true - - - #{_name} - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X1 Config #{_config} - - - front_left_wheel_joint - rear_left_wheel_joint - front_right_wheel_joint - rear_right_wheel_joint - #{0.45649 * 1.5} - 0.1651 - /model/#{_name}/cmd_vel_relay - -1 - 1 - -3 - 3 - - - - true - true - false - false - #{$enableGroundTruth} - true - true - 1 - - - - linear_battery - 12.694 - 12.694 - -3.1424 - 78.4 - 78.4 - 0.061523 - 1.9499 - 6.6 - true - - - - /model/#{_name}/gas_detected - 10 - gas - - - /model/#{_name}/breadcrumb/deploy - #{max_breadcrumbs} - 3.0 - true - - - - -0.45 0 0 0 0 0 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Breadcrumb Node - - - - - - - - - 0.172 - 0 - 138.767 - 0.1651 - - - 0.172 - 0 - 138.767 - 0.1651 - - - 0.172 - 0 - 138.767 - 0.1651 - - - 0.172 - 0 - 138.767 - 0.1651 - - - - - - HEREDOC - - exec = <<-HEREDOC - - roslaunch --wait subt_ros x1_description.launch world_name:=#{$worldName} name:=#{_name} - - - 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} - - 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 - - #{_name} - false - #{_x} #{_y} #{z} 0 0 #{_yaw} - #{$worldName} - true - - - #{_name} - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X2 Config #{_config} - - - front_left_wheel_joint - rear_left_wheel_joint - front_right_wheel_joint - rear_right_wheel_joint - #{0.33559 * 1.23} - 0.098 - /model/#{_name}/cmd_vel_relay - -2 - 2 - -6 - 6 - - - - true - true - false - false - #{$enableGroundTruth} - true - true - 1 - - - - linear_battery - 12.694 - 12.694 - -3.1424 - 78.4 - 78.4 - 0.061523 - 1.9499 - 6.6 - true - - - - /model/#{_name}/gas_detected - 10 - gas - - - /model/#{_name}/breadcrumb/deploy - #{max_breadcrumbs} - 3.0 - true - - - - -0.24 0 0 0 0 0 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Breadcrumb Node - - - - - - - - - 0.116 - 0 - 45.20448 - 0.098 - - - 0.116 - 0 - 45.20448 - 0.098 - - - 0.116 - 0 - 45.20448 - 0.098 - - - 0.116 - 0 - 45.20448 - 0.098 - - - - - - HEREDOC - - exec = <<-HEREDOC - - roslaunch --wait subt_ros x2_description.launch world_name:=#{$worldName} name:=#{_name} - - - 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} - - 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 - - #{_name} - false - #{_x} #{_y} #{z} 0 0 #{_yaw} - #{$worldName} - true - - - #{_name} - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X3 UAV Config #{_config} - - - true - true - false - false - #{$enableGroundTruth} - true - true - 1 - - - model/#{_name} - rotor_0_joint - rotor_0 - ccw - 0.0125 - 0.025 - 800.0 - 8.54858e-06 - 0.016 - command/motor_speed - 0 - 8.06428e-05 - 1e-06 - motor_speed/0 - 10 - velocity - - - model/#{_name} - rotor_1_joint - rotor_1 - ccw - 0.0125 - 0.025 - 800.0 - 8.54858e-06 - 0.016 - command/motor_speed - 1 - 8.06428e-05 - 1e-06 - motor_speed/1 - 10 - velocity - - - model/#{_name} - rotor_2_joint - rotor_2 - cw - 0.0125 - 0.025 - 800.0 - 8.54858e-06 - 0.016 - command/motor_speed - 2 - 8.06428e-05 - 1e-06 - motor_speed/2 - 10 - velocity - - - model/#{_name} - rotor_3_joint - rotor_3 - cw - 0.0125 - 0.025 - 800.0 - 8.54858e-06 - 0.016 - command/motor_speed - 3 - 8.06428e-05 - 1e-06 - motor_speed/3 - 10 - velocity - - - - model/#{_name} - cmd_vel - command/motor_speed - velocity_controller/enable - base_link - 2.7 2.7 2.7 - 2 3 0.15 - 0.4 0.52 0.18 - 1 1 2 - 5 5 5 - 3 3 3 - 0 0 0.05 - - - - 0.1105 0.1261 0.00947 - 0 0 0 - - - 0.004 0.004 0.004 - - - rotor_0_joint - 8.54858e-06 - 0.016 - 1 - - - rotor_1_joint - 8.54858e-06 - 0.016 - 1 - - - rotor_2_joint - 8.54858e-06 - 0.016 - -1 - - - rotor_3_joint - 8.54858e-06 - 0.016 - -1 - - - - - - linear_battery - 12.694 - 12.694 - -3.1424 - 18.0 - 18.0 - 0.061523 - 1.9499 - 6.6 - true - - - - /model/#{_name}/gas_detected - 10 - gas - - - - - HEREDOC - - exec = <<-HEREDOC - - roslaunch --wait subt_ros x3_description.launch world_name:=#{$worldName} name:=#{_name} - - - 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 - - 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 - - #{_name} - false - #{_x} #{_y} #{z} 0 0 #{_yaw} - #{$worldName} - true - - - #{_name} - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X4 UAV Config #{_config} - - - true - true - false - false - #{$enableGroundTruth} - true - true - 1 - - - model/#{_name} - rotor_0_joint - rotor_0 - ccw - 0.0182 - 0.0182 - 1000.0 - 1.269e-05 - 0.016754 - command/motor_speed - 0 - 2.0673e-04 - 0 - motor_speed/0 - 10 - velocity - - - model/#{_name} - rotor_1_joint - rotor_1 - cw - 0.0182 - 0.0182 - 1000.0 - 1.269e-05 - 0.016754 - command/motor_speed - 1 - 2.0673e-04 - 0 - motor_speed/1 - 10 - velocity - - - model/#{_name} - rotor_2_joint - rotor_2 - ccw - 0.0182 - 0.0182 - 1000.0 - 1.269e-05 - 0.016754 - command/motor_speed - 2 - 2.0673e-04 - 0 - motor_speed/2 - 10 - velocity - - - model/#{_name} - rotor_3_joint - rotor_3 - cw - 0.0182 - 0.0182 - 1000.0 - 1.269e-05 - 0.016754 - command/motor_speed - 3 - 2.0673e-04 - 0 - motor_speed/3 - 10 - velocity - - - model/#{_name} - rotor_4_joint - rotor_4 - ccw - 0.0182 - 0.0182 - 1000.0 - 1.269e-05 - 0.016754 - command/motor_speed - 4 - 2.0673e-04 - 0 - motor_speed/4 - 10 - velocity - - - model/#{_name} - rotor_5_joint - rotor_5 - cw - 0.0182 - 0.0182 - 1000.0 - 1.269e-05 - 0.016754 - command/motor_speed - 5 - 2.0673e-04 - 0 - motor_speed/5 - 10 - velocity - - - - model/#{_name} - cmd_vel - command/motor_speed - velocity_controller/enable - base_link - 6 6 10 - 4 4 2 - 0.7 0.7 0.7 - 1 1 2 - 5 5 5 - 3 3 3 - 0 0 0 - - - - 0.1105 0.1261 0.0947 - 0 0 0 - - - 0.004 0.004 0.004 - - - rotor_0_joint - 1.269e-05 - 1.6754e-2 - 1 - - - rotor_1_joint - 1.269e-05 - 1.6754e-2 - -1 - - - rotor_2_joint - 1.269e-05 - 1.6754e-2 - 1 - - - rotor_3_joint - 1.269e-05 - 1.6754e-2 - -1 - - - rotor_4_joint - 1.269e-05 - 1.6754e-2 - 1 - - - rotor_5_joint - 1.269e-05 - 1.6754e-2 - -1 - - - - - - linear_battery - 12.694 - 12.694 - -3.1424 - 18.0 - 18.0 - 0.061523 - 1.9499 - 6.6 - true - - - - /model/#{_name}/gas_detected - 10 - gas - - - - - HEREDOC - - exec = <<-HEREDOC - - roslaunch --wait subt_ros x4_description.launch world_name:=#{$worldName} name:=#{_name} - - - 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 - - HEREDOC - - return spawn, exec - end - - def spawnTeamBase(_name, _x, _y, _z, _yaw) - spawn = <<-HEREDOC - - #{_name} - false - #{$worldName} - false - - - #{_x} #{_y} #{_z} 0 0 #{_yaw} - true - - 0 0 0.05 0 0 0 - - - - .1 .1 .1 - - - - - - - - HEREDOC - - exec = <<-HEREDOC - - roslaunch --wait subt_ros teambase_description.launch world_name:=#{$worldName} name:=#{_name} - - - roslaunch --wait subt_ros teambase_topics.launch world_name:=#{$worldName} name:=#{_name} - - HEREDOC - - return spawn, exec - end -%> - <% - # Reverse hash of mersupials, i.e, key: child, value: parent - marsupialChildren = marsupialParents.invert + world.assignSpawnPositions(robots, marsupialParents) + # Reverse hash of marsupials, 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 + spawnList = + robots.select { |name| not marsupialChildren.has_key?(name) }.values + + robots.select { |name| marsupialChildren.has_key?(name) }.values + + teamBaseSpawned = false - 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, _ = spawnRobot(robot, world, $localModel) - spawnStringTmp, executables = spawnX4(name, robotConfigN, robot.pos.x, robot.pos.y, robot.pos.z, robot.rot.yaw) - - elsif config.upcase == "TEAMBASE" - robotSpawned += 1 + if config.upcase == "TEAMBASE" 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) + puts "Error: Only 1 teambase is allowed. Skpping[%s]." % [name] + next end end - if executables != nil - allExecutables += executables - end - if marsupialParents.has_key?(name) childModelName = marsupialParents[name] parentModelName = marsupialChildren[childModelName] - detachableJointPlugin = REXML::Document.new <<-HEREDOC - - #{MARSUPIAL_PARENT_LINK_NAMES[robots[parentModelName].type]} - #{childModelName} - base_link - /model/#{childModelName}/detach - true - - 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 - - base_link - #{name}_platform - base_link - true - - HEREDOC - - # This will be used to spawn the drone platform. - platform = <<-HEREDOC - - #{name}_platform - false - #{robot.pos.x + 0.078} #{robot.pos.y} #{robot.pos.z-0.11} 0 0 0 - #{$worldName} - false - - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/DronePlatformX1 - - - - HEREDOC - end - rosExecutable = <<-HEREDOC - - roslaunch --wait subt_ros marsupial_topics.launch name:=#{childModelName} - - 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("#{spawnStringTmp}") - 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 + joints, platform, rosExecutable = getMarsupialSnippets( + robot, robots[childModelName], $worldName) + + # 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("#{spawnStringTmp}") + search = "//spawn[@name='#{name}']" + pluginElem = spawnXml.root.elements[search] + includeElem = pluginElem.elements['sdf/include'] + joints.each { |joint| includeElem.add_element(joint.root) } + + spawnString = "" + formatter = REXML::Formatters::Default.new + spawnXml.root.each { |el| formatter.write(el, spawnString) } + spawnString += platform + else + spawnString = spawnStringTmp end %> -<%= spawnString%> + <%= spawnString %> <% end %> diff --git a/subt_ign/launch/competition.ign b/subt_ign/launch/competition.ign index ad20b159..02484280 100644 --- a/subt_ign/launch/competition.ign +++ b/subt_ign/launch/competition.ign @@ -19,6 +19,14 @@ require "rexml/document" %> +<% + # Library of functions for worlds + require File.dirname(__FILE__) + "/world_common_defs.rb" + + # Library for spawning the builtin robots + require File.dirname(__FILE__) + "/robot_spawners.rb" +%> + <% # When 'enableGroundTruth = true' absolute poses of vehicles will be published. # This is useful for debugging purposes, but will not be available during @@ -33,165 +41,18 @@ # Check if circuit is not defined or is empty/nil # Also make sure the given circuit name is valid - validCircuits = ["cave", "tunnel", "urban"] if !defined?(circuit) || circuit == nil || circuit.empty? abort("circuit was not defined, exiting now") - elsif validCircuits.include?(circuit) + 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 + abort("circuit given is not valid. Please choose from #{$validCircuits}") end - # CommsBrokerPlugin parameters - # (default parameters can be found in subt_rf_model.h) - $commsFadingExponent = 2.5 - $commsScalingFactor = 1.0 - $commsRangePerHop = 2.0 - if $circuit == "cave" - $commsFadingExponent = 1.5 - $commsScalingFactor = 0.55 - $commsRangePerHop = 2.0 - end - - # spawn world offset - if $circuit == "urban" - spawnWorldXPos = 6 - spawnWorldYPos = 27 - spawnWorldZPos = 7.5 - spawnWorldYaw = 0 - guiCameraPose = "-2.0 27 15 0 0.868 0" + if !defined?(logPath) || logPath == nil || logPath.empty? + $logPath = '/tmp/ign/logs' 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", - "EXPLORER_X1" => "base_link", - "EXPLORER_R2" => "Rear_Rocker_Link", - "CSIRO_DATA61_OZBOT_ATR" => "base_link" - } - - MARSUPIAL_VALID_ROBOT_PAIRS = { - "X1" => ["X3", "MARBLE_QAV500", "CERBERUS_GAGARIN", "CERBERUS_RMF", "COSTAR_SHAFTER"], - "EXPLORER_X1" =>["X3", "MARBLE_QAV500", "CERBERUS_GAGARIN", "CERBERUS_RMF", "COSTAR_SHAFTER"], - "EXPLORER_R2" =>["X3", "X4", "SSCI_X4", "SOPHISTICATED_ENGINEERING_X4", "EXPLORER_DS1", "MARBLE_QAV500", "CERBERUS_M100", "CERBERUS_GAGARIN", "CERBERUS_RMF", "COSTAR_SHAFTER"], - "CSIRO_DATA61_OZBOT_ATR" =>["X3", "X4", "SSCI_X4", "SOPHISTICATED_ENGINEERING_X4","EXPLORER_DS1", "MARBLE_QAV500", "CERBERUS_M100", "CERBERUS_GAGARIN", "CERBERUS_RMF", "COSTAR_SHAFTER"] - } - - MARSUPIAL_PARENT_ROBOT_POSITION_OFFSETS = { - "X1" => Vector3d.new(0, 0, 0.717), - "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) - } - - MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS = { - "X1" => 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) - } - - 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 + $logPath = logPath end %> @@ -210,15 +71,13 @@ 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. + # durationSec to zero, which equals 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. @@ -230,27 +89,23 @@ %> <% - # disable levels for simple cave worlds - $levels = "true" - if $worldName.include?('simple_cave_') || $worldName.include?('cave_qual') - $levels = "false" - end + world = World.new($worldName) +%> +<% + $levels = world.levels if defined?(levels) && levels != nil && !levels.empty? $levels = levels.downcase == "true" end %> - - - IGN_GAZEBO_SYSTEM_PLUGIN_PATH - $LD_LIBRARY_PATH - +<% + robots = parseRobotArgs(binding) + marsupialParents = parseMarsupials(binding, robots) +%> - - IGN_TRANSPORT_TOPIC_STATISTICS - 1 - + + <%= world.commonLaunchEnvironment() %> @@ -260,18 +115,7 @@ - <% if $worldName.include?('tunnel_circuit_') && - !$worldName.include?('practice') %> - tunnel_circuit/<%= worldNumber %>/<%= $worldName %>.sdf - <% elsif $worldName.include?('urban_circuit_') && - !$worldName.include?('practice') %> - urban_circuit/<%= worldNumber %>/<%= $worldName %>.sdf - <% elsif $worldName.include?('cave_circuit_') && - !$worldName.include?('practice') %> - cave_circuit/<%= worldNumber %>/<%= $worldName %>.sdf - <% else %> - <%= $worldName %>.sdf - <% end %> + <%= world.path %> <%if defined?(updateRate) && updateRate != nil && !updateRate.empty?%> <%= updateRate %> <%end%> @@ -279,1086 +123,53 @@ <%= $levels %> true - - /tmp/ign/logs + <%= $logPath %> true <%if defined?(seed) && seed != nil && !seed.empty?%> <%= seed %> <%end%> - - - - ogre2 - - - - - - - - - - - - - - - - - - - <%= $worldName %> - false - - <%= $durationSec %> - - - - /tmp/ign/logs - subt_<%= $circuit %> - 5 - - + <%= world.commonServerPlugins() %> + <%= world.subtServerPlugins($durationSec, $logPath, false) %> + <%= world.subtLaunchPlugins() %> + <%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 - <%= guiCameraPose %> - - - - 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 - - - - - - - + <%= world.guiPlugin() %> <%end%> - - - true - <%= $worldName %> - - visibility_range - - - 500.0 - <%= $commsFadingExponent %> - 40 - 10.0 - <%= $commsScalingFactor %> - <%= $commsRangePerHop %> - - - - 0.2 - 10 - - - - 1000000 - 20 - -90 - QPSK - - - - - - - - - -<% - 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 - - #{_name} - false - #{_x} #{_y} #{z} 0 0 #{_yaw} - #{$worldName} - true - - - #{_name} - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X1 Config #{_config} - - - front_left_wheel_joint - rear_left_wheel_joint - front_right_wheel_joint - rear_right_wheel_joint - #{0.45649 * 1.5} - 0.1651 - /model/#{_name}/cmd_vel_relay - -1 - 1 - -3 - 3 - - - - true - true - false - false - #{$enableGroundTruth} - true - true - 1 - - - - linear_battery - 12.694 - 12.694 - -3.1424 - 78.4 - 78.4 - 0.061523 - 1.9499 - 6.6 - true - - - - /model/#{_name}/gas_detected - 10 - gas - - - /model/#{_name}/breadcrumb/deploy - #{max_breadcrumbs} - 3.0 - true - - - - -0.45 0 0 0 0 0 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Breadcrumb Node - - - - - - - - - 0.172 - 0 - 138.767 - 0.1651 - - - 0.172 - 0 - 138.767 - 0.1651 - - - 0.172 - 0 - 138.767 - 0.1651 - - - 0.172 - 0 - 138.767 - 0.1651 - - - - - - HEREDOC - - exec = <<-HEREDOC - - roslaunch --wait subt_ros x1_description.launch world_name:=#{$worldName} name:=#{_name} - - - 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} - - 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 - - #{_name} - false - #{_x} #{_y} #{z} 0 0 #{_yaw} - #{$worldName} - true - - - #{_name} - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X2 Config #{_config} - - - front_left_wheel_joint - rear_left_wheel_joint - front_right_wheel_joint - rear_right_wheel_joint - #{0.33559 * 1.23} - 0.098 - /model/#{_name}/cmd_vel_relay - -2 - 2 - -6 - 6 - - - - true - true - false - false - #{$enableGroundTruth} - true - true - 1 - - - - linear_battery - 12.694 - 12.694 - -3.1424 - 78.4 - 78.4 - 0.061523 - 1.9499 - 6.6 - true - - - - /model/#{_name}/gas_detected - 10 - gas - - - /model/#{_name}/breadcrumb/deploy - #{max_breadcrumbs} - 3.0 - true - - - - -0.24 0 0 0 0 0 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Breadcrumb Node - - - - - - - - - 0.116 - 0 - 45.20448 - 0.098 - - - 0.116 - 0 - 45.20448 - 0.098 - - - 0.116 - 0 - 45.20448 - 0.098 - - - 0.116 - 0 - 45.20448 - 0.098 - - - - - - HEREDOC - - exec = <<-HEREDOC - - roslaunch --wait subt_ros x2_description.launch world_name:=#{$worldName} name:=#{_name} - - - 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} - - 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 - - #{_name} - false - #{_x} #{_y} #{z} 0 0 #{_yaw} - #{$worldName} - true - - - #{_name} - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X3 UAV Config #{_config} - - - true - true - false - false - #{$enableGroundTruth} - true - true - 1 - - - model/#{_name} - rotor_0_joint - rotor_0 - ccw - 0.0125 - 0.025 - 800.0 - 8.54858e-06 - 0.016 - command/motor_speed - 0 - 8.06428e-05 - 1e-06 - motor_speed/0 - 10 - velocity - - - model/#{_name} - rotor_1_joint - rotor_1 - ccw - 0.0125 - 0.025 - 800.0 - 8.54858e-06 - 0.016 - command/motor_speed - 1 - 8.06428e-05 - 1e-06 - motor_speed/1 - 10 - velocity - - - model/#{_name} - rotor_2_joint - rotor_2 - cw - 0.0125 - 0.025 - 800.0 - 8.54858e-06 - 0.016 - command/motor_speed - 2 - 8.06428e-05 - 1e-06 - motor_speed/2 - 10 - velocity - - - model/#{_name} - rotor_3_joint - rotor_3 - cw - 0.0125 - 0.025 - 800.0 - 8.54858e-06 - 0.016 - command/motor_speed - 3 - 8.06428e-05 - 1e-06 - motor_speed/3 - 10 - velocity - - - - model/#{_name} - cmd_vel - command/motor_speed - velocity_controller/enable - base_link - 2.7 2.7 2.7 - 2 3 0.15 - 0.4 0.52 0.18 - 1 1 2 - 5 5 5 - 3 3 3 - 0 0 0.05 - - - - 0.1105 0.1261 0.00947 - 0 0 0 - - - 0.004 0.004 0.004 - - - rotor_0_joint - 8.54858e-06 - 0.016 - 1 - - - rotor_1_joint - 8.54858e-06 - 0.016 - 1 - - - rotor_2_joint - 8.54858e-06 - 0.016 - -1 - - - rotor_3_joint - 8.54858e-06 - 0.016 - -1 - - - - - - linear_battery - 12.694 - 12.694 - -3.1424 - 18.0 - 18.0 - 0.061523 - 1.9499 - 6.6 - true - - - - /model/#{_name}/gas_detected - 10 - gas - - - - - HEREDOC - - exec = <<-HEREDOC - - roslaunch --wait subt_ros x3_description.launch world_name:=#{$worldName} name:=#{_name} - - - 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 - - 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 - - #{_name} - false - #{_x} #{_y} #{z} 0 0 #{_yaw} - #{$worldName} - true - - - #{_name} - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X4 UAV Config #{_config} - - - true - true - false - false - #{$enableGroundTruth} - true - true - 1 - - - model/#{_name} - rotor_0_joint - rotor_0 - ccw - 0.0182 - 0.0182 - 1000.0 - 1.269e-05 - 0.016754 - command/motor_speed - 0 - 2.0673e-04 - 0 - motor_speed/0 - 10 - velocity - - - model/#{_name} - rotor_1_joint - rotor_1 - cw - 0.0182 - 0.0182 - 1000.0 - 1.269e-05 - 0.016754 - command/motor_speed - 1 - 2.0673e-04 - 0 - motor_speed/1 - 10 - velocity - - - model/#{_name} - rotor_2_joint - rotor_2 - ccw - 0.0182 - 0.0182 - 1000.0 - 1.269e-05 - 0.016754 - command/motor_speed - 2 - 2.0673e-04 - 0 - motor_speed/2 - 10 - velocity - - - model/#{_name} - rotor_3_joint - rotor_3 - cw - 0.0182 - 0.0182 - 1000.0 - 1.269e-05 - 0.016754 - command/motor_speed - 3 - 2.0673e-04 - 0 - motor_speed/3 - 10 - velocity - - - model/#{_name} - rotor_4_joint - rotor_4 - ccw - 0.0182 - 0.0182 - 1000.0 - 1.269e-05 - 0.016754 - command/motor_speed - 4 - 2.0673e-04 - 0 - motor_speed/4 - 10 - velocity - - - model/#{_name} - rotor_5_joint - rotor_5 - cw - 0.0182 - 0.0182 - 1000.0 - 1.269e-05 - 0.016754 - command/motor_speed - 5 - 2.0673e-04 - 0 - motor_speed/5 - 10 - velocity - - - - model/#{_name} - cmd_vel - command/motor_speed - velocity_controller/enable - base_link - 6 6 10 - 4 4 2 - 0.7 0.7 0.7 - 1 1 2 - 5 5 5 - 3 3 3 - 0 0 0 - - - - 0.1105 0.1261 0.0947 - 0 0 0 - - - 0.004 0.004 0.004 - - - rotor_0_joint - 1.269e-05 - 1.6754e-2 - 1 - - - rotor_1_joint - 1.269e-05 - 1.6754e-2 - -1 - - - rotor_2_joint - 1.269e-05 - 1.6754e-2 - 1 - - - rotor_3_joint - 1.269e-05 - 1.6754e-2 - -1 - - - rotor_4_joint - 1.269e-05 - 1.6754e-2 - 1 - - - rotor_5_joint - 1.269e-05 - 1.6754e-2 - -1 - - - - - - linear_battery - 12.694 - 12.694 - -3.1424 - 18.0 - 18.0 - 0.061523 - 1.9499 - 6.6 - true - - - - /model/#{_name}/gas_detected - 10 - gas - - - - - HEREDOC - - exec = <<-HEREDOC - - roslaunch --wait subt_ros x4_description.launch world_name:=#{$worldName} name:=#{_name} - - - 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 - - HEREDOC - - return spawn, exec - end - - def spawnTeamBase(_name, _x, _y, _z, _yaw) - spawn = <<-HEREDOC - - #{_name} - false - #{$worldName} - false - - - #{_x} #{_y} #{_z} 0 0 #{_yaw} - true - - 0 0 0.05 0 0 0 - - - - .1 .1 .1 - - - - - - - - HEREDOC - - exec = <<-HEREDOC - - roslaunch --wait subt_ros teambase_description.launch world_name:=#{$worldName} name:=#{_name} - - - roslaunch --wait subt_ros teambase_topics.launch world_name:=#{$worldName} name:=#{_name} - - HEREDOC - - return spawn, exec - end -%> - <% - # Reverse hash of mersupials, i.e, key: child, value: parent - marsupialChildren = marsupialParents.invert + world.assignSpawnPositions(robots, marsupialParents) + # Reverse hash of marsupials, 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 + teamBaseSpawned = false + allExecutables = "" + 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 = spawnRobot(robot, world, $localModel) - spawnStringTmp, executables = spawnX4(name, robotConfigN, robot.pos.x, robot.pos.y, robot.pos.z, robot.rot.yaw) - - elsif config.upcase == "TEAMBASE" - robotSpawned += 1 + if config.upcase == "TEAMBASE" 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) + puts "Error: Only 1 teambase is allowed. Skpping[%s]." % [name] + next end end @@ -1369,80 +180,35 @@ if marsupialParents.has_key?(name) childModelName = marsupialParents[name] parentModelName = marsupialChildren[childModelName] - detachableJointPlugin = REXML::Document.new <<-HEREDOC - - #{MARSUPIAL_PARENT_LINK_NAMES[robots[parentModelName].type]} - #{childModelName} - base_link - /model/#{childModelName}/detach - true - - 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 - - base_link - #{name}_platform - base_link - true - - HEREDOC + joints, platform, rosExecutable = getMarsupialSnippets( + robot, robots[childModelName], $worldName) - # This will be used to spawn the drone platform. - platform = <<-HEREDOC - - #{name}_platform - false - #{robot.pos.x + 0.078} #{robot.pos.y} #{robot.pos.z-0.11} 0 0 0 - #{$worldName} - false - - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/DronePlatformX1 - - - - HEREDOC - end + allExecutables += rosExecutable - rosExecutable = <<-HEREDOC - - roslaunch --wait subt_ros marsupial_topics.launch name:=#{childModelName} - - 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("#{spawnStringTmp}") + search = "//spawn[@name='#{name}']" + pluginElem = spawnXml.root.elements[search] + includeElem = pluginElem.elements['sdf/include'] + joints.each { |joint| includeElem.add_element(joint.root) } - # 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("#{spawnStringTmp}") - 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 + spawnString = "" + formatter = REXML::Formatters::Default.new + spawnXml.root.each { |el| formatter.write(el, spawnString) } + spawnString += platform + else + spawnString = spawnStringTmp end %> -<%= spawnString%> + <%= spawnString%> <% end %> + <%= allExecutables %> diff --git a/subt_ign/launch/competition_no_ros.ign b/subt_ign/launch/competition_no_ros.ign index 8760e206..91544f6a 100644 --- a/subt_ign/launch/competition_no_ros.ign +++ b/subt_ign/launch/competition_no_ros.ign @@ -430,11 +430,21 @@ " true\n"\ " false\n"\ " false\n"\ - " #{$enableGroundTruth}\n"\ + " false\n"\ " true\n"\ " true\n"\ " 1\n"\ " \n"\ + " \n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " #{$enableGroundTruth}\n"\ + " true\n"\ + " false\n"\ + " \n"\ " \n"\ " \n"\ @@ -497,11 +507,21 @@ " true\n"\ " false\n"\ " false\n"\ - " #{$enableGroundTruth}\n"\ + " false\n"\ " true\n"\ " true\n"\ " 1\n"\ " \n"\ + " \n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " #{$enableGroundTruth}\n"\ + " true\n"\ + " false\n"\ + " \n"\ " \n"\ " \n"\ @@ -550,11 +570,21 @@ " true\n"\ " false\n"\ " false\n"\ - " #{$enableGroundTruth}\n"\ + " false\n"\ " true\n"\ " true\n"\ " 1\n"\ " \n"\ + " \n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " #{$enableGroundTruth}\n"\ + " true\n"\ + " false\n"\ + " \n"\ " \n"\ " model/#{_name}\n"\ @@ -733,11 +763,21 @@ " true\n"\ " false\n"\ " false\n"\ - " #{$enableGroundTruth}\n"\ + " false\n"\ " true\n"\ " true\n"\ " 1\n"\ " \n"\ + " \n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " #{$enableGroundTruth}\n"\ + " true\n"\ + " false\n"\ + " \n"\ " \n"\ " model/#{_name}\n"\ diff --git a/subt_ign/launch/robot_common_defs.rb b/subt_ign/launch/robot_common_defs.rb new file mode 100644 index 00000000..694a436d --- /dev/null +++ b/subt_ign/launch/robot_common_defs.rb @@ -0,0 +1,250 @@ +# This is a common file intended to be included in all robot spawner scripts. + +$fuelPrefix = "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models" + +Vector3d = Struct.new(:x, :y, :z) +AngularVector3d = Struct.new(:roll, :pitch, :yaw) + +class Robot + attr_accessor :name, :config, :type, :configNumber, :pos, :rot, :isBuiltin, :package + 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] + @package = config.downcase + + @pos = Vector3d.new(0, 0, 0) + @rot = AngularVector3d.new(0, 0, 0) + + @isBuiltin = false + if @type == "X1" and ["1", "2", "3", "4", "5", "7", "8"].include?(@configNumber) + @isBuiltin = true + elsif @type == "X2" and ["1", "2", "3", "4", "5", "6", "8", "9"].include?(@configNumber) + @isBuiltin = true + elsif @type == "X3" and ["1", "2", "3", "4"].include?(@configNumber) + @isBuiltin = true + elsif @type == "X4" and ["1", "2", "3", "4", "5"].include?(@configNumber) + @isBuiltin = true + elsif @type.upcase == "TEAMBASE" + @isBuiltin = true + end + end + + def fuelName() + # 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 @type == "X3" or @type == "X4" + mUAVStr = "UAV " + else + mUAVStr = "" + end + robotName = "#{@type} #{mUAVStr}Config #{@configNumber}" + else + robotName = @config + end + return robotName + end + + def installDir() + dir = `rospack find #{@package}`.chomp + if dir.empty? + raise "Unknown robot configuration #{@config}. ROS package #{@package} could not be found." + end + return dir + end + + def modelURI(_allowLocalModel = false) + # 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 _allowLocalModel + return installDir() + else + return "#{$fuelPrefix}/#{fuelName()}" + end + end + + def spawnScriptPath() + return "#{installDir()}/launch/spawner.rb" + end + + # This is a snippet common to all robots in the competition. It adds a gas + # sensor and robot state publishers. + # @param [Boolean] _enableGroundTruth Whether to allow publishing of ground truth data. + # @return [String] The snippet to be included in spawner SDF. + def commonPlugins(_enableGroundTruth) + return <<-HEREDOC + + + true + true + false + false + false + true + true + 1 + + + + + false + false + false + false + #{_enableGroundTruth} + true + false + + + + + /model/#{@name}/gas_detected + 10 + gas + + HEREDOC + end + + # This is a snippet common to all robots carrying breadcrumbs. + # @param [int] _maxBreadcrumbs The initial number of breadcrumbs the robot will have (can be 0). + # @param [String] _breadcrumbDropOffset The offset at which a breadcrumb will drop relative to the + # current position of the robot's base link; the value is a string with 6 numbers + # similar to other pose specifications in SDF + # @return [String] The snippet to be included in spawner SDF. + def breadcrumbsPlugins(_maxBreadcrumbs, _breadcrumbDropOffset) + return <<-HEREDOC + + + /model/#{@name}/breadcrumb/deploy + #{_maxBreadcrumbs} + 3.0 + true + + + + #{_breadcrumbDropOffset} + + #{$fuelPrefix}/Breadcrumb Node + + + + + + HEREDOC + end +end + +MARSUPIAL_PARENT_LINK_NAMES = { + "X1" => "base_link", + "EXPLORER_X1" => "base_link", + "EXPLORER_R2" => "Rear_Rocker_Link", + "CSIRO_DATA61_OZBOT_ATR" => "base_link" + } + +MARSUPIAL_VALID_ROBOT_PAIRS = { + "X1" => ["X3", "MARBLE_QAV500", "CERBERUS_GAGARIN", "CERBERUS_RMF", "COSTAR_SHAFTER"], + "EXPLORER_X1" =>["X3", "MARBLE_QAV500", "CERBERUS_GAGARIN", "CERBERUS_RMF", "COSTAR_SHAFTER"], + "EXPLORER_R2" =>["X3", "X4", "SSCI_X4", "SOPHISTICATED_ENGINEERING_X4", "EXPLORER_DS1", "MARBLE_QAV500", "CERBERUS_M100", "CERBERUS_GAGARIN", "CERBERUS_RMF", "COSTAR_SHAFTER"], + "CSIRO_DATA61_OZBOT_ATR" =>["X3", "X4", "SSCI_X4", "SOPHISTICATED_ENGINEERING_X4","EXPLORER_DS1", "MARBLE_QAV500", "CERBERUS_M100", "CERBERUS_GAGARIN", "CERBERUS_RMF", "COSTAR_SHAFTER"] +} + +MARSUPIAL_PARENT_ROBOT_POSITION_OFFSETS = { + "X1" => Vector3d.new(0, 0, 0.717), + "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) +} + +MARSUPIAL_PARENT_ROBOT_ROTATION_OFFSETS = { + "X1" => 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) +} + +def parseRobotArgs(_binding) + # Check if robotNameX and robotConfigX exists + robots = Hash.new + for i in 1..$maxRobotCount do + if (_binding.local_variables.include?(:"robotName#{i}") && + _binding.local_variables.include?(:"robotConfig#{i}")) + name=_binding.eval("robotName#{i}") + config=_binding.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 + fail ArgumentError, "missing robotNameX or robotConfigX arguments" if robots.empty? + + return robots +end + +def parseMarsupials(_binding, _robots) + # 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 _binding.local_variables.include?(:"marsupial#{i}") + parent, child = _binding.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 + return marsupialParents +end diff --git a/subt_ign/launch/robot_spawners.rb b/subt_ign/launch/robot_spawners.rb new file mode 100644 index 00000000..d0da3c1e --- /dev/null +++ b/subt_ign/launch/robot_spawners.rb @@ -0,0 +1,811 @@ +# This is a common file intended to be included in all competition world +# launchers that want to spawn robots. + +# Library of functions for robots +require File.dirname(__FILE__) + "/robot_common_defs.rb" + +# Needed for adding marsupial robots +require "rexml/document" + +def spawnX1(_name, _config, _x = 0.0, _y = 0.0, _z = 0.0, _yaw = 0.0) + 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 + + breadcrumb_drop_offset = '-0.45 0 0 0 0 0' + + z = _z + 0.2 + + robot = Robot.new(_name, "X1_SENSOR_CONFIG_" + _config) + + spawn = <<-HEREDOC + + #{_name} + false + #{_x} #{_y} #{z} 0 0 #{_yaw} + #{$worldName} + true + + + #{_name} + #{robot.modelURI()} + + + front_left_wheel_joint + rear_left_wheel_joint + front_right_wheel_joint + rear_right_wheel_joint + #{0.45649 * 1.5} + 0.1651 + /model/#{_name}/cmd_vel_relay + -1 + 1 + -3 + 3 + + + + #{robot.commonPlugins($enableGroundTruth)} + + + + linear_battery + 12.694 + 12.694 + -3.1424 + 78.4 + 78.4 + 0.061523 + 1.9499 + 6.6 + true + + + + #{robot.breadcrumbsPlugins(max_breadcrumbs, breadcrumb_drop_offset)} + + + + + 0.172 + 0 + 138.767 + 0.1651 + + + 0.172 + 0 + 138.767 + 0.1651 + + + 0.172 + 0 + 138.767 + 0.1651 + + + 0.172 + 0 + 138.767 + 0.1651 + + + + + + HEREDOC + + exec = <<-HEREDOC + + roslaunch --wait subt_ros x1_description.launch world_name:=#{$worldName} name:=#{_name} + + + 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} + + HEREDOC + + return spawn, exec +end + +def spawnX2(_name, _config, _x = 0.0, _y = 0.0, _z = 0.0, _yaw = 0.0) + 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 + + breadcrumb_drop_offset = '-0.24 0 0 0 0 0' + + z = _z + 0.063494 + + robot = Robot.new(_name, "X2_SENSOR_CONFIG_" + _config) + + spawn = <<-HEREDOC + + #{_name} + false + #{_x} #{_y} #{z} 0 0 #{_yaw} + #{$worldName} + true + + + #{_name} + #{robot.modelURI()} + + + front_left_wheel_joint + rear_left_wheel_joint + front_right_wheel_joint + rear_right_wheel_joint + #{0.33559 * 1.23} + 0.098 + /model/#{_name}/cmd_vel_relay + -2 + 2 + -6 + 6 + + + + #{robot.commonPlugins($enableGroundTruth)} + + + + linear_battery + 12.694 + 12.694 + -3.1424 + 78.4 + 78.4 + 0.061523 + 1.9499 + 6.6 + true + + + + #{robot.breadcrumbsPlugins(max_breadcrumbs, breadcrumb_drop_offset)} + + + + + 0.116 + 0 + 45.20448 + 0.098 + + + 0.116 + 0 + 45.20448 + 0.098 + + + 0.116 + 0 + 45.20448 + 0.098 + + + 0.116 + 0 + 45.20448 + 0.098 + + + + + + HEREDOC + + exec = <<-HEREDOC + + roslaunch --wait subt_ros x2_description.launch world_name:=#{$worldName} name:=#{_name} + + + 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} + + HEREDOC + + return spawn, exec +end + +def spawnX3(_name, _config, _x = 0.0, _y = 0.0, _z = 0.0, _yaw = 0.0) + 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 + + robot = Robot.new(_name, "X3_SENSOR_CONFIG_" + _config) + + spawn = <<-HEREDOC + + #{_name} + false + #{_x} #{_y} #{z} 0 0 #{_yaw} + #{$worldName} + true + + + #{_name} + #{robot.modelURI()} + + #{robot.commonPlugins($enableGroundTruth)} + + + model/#{_name} + rotor_0_joint + rotor_0 + ccw + 0.0125 + 0.025 + 800.0 + 8.54858e-06 + 0.016 + command/motor_speed + 0 + 8.06428e-05 + 1e-06 + motor_speed/0 + 10 + velocity + + + model/#{_name} + rotor_1_joint + rotor_1 + ccw + 0.0125 + 0.025 + 800.0 + 8.54858e-06 + 0.016 + command/motor_speed + 1 + 8.06428e-05 + 1e-06 + motor_speed/1 + 10 + velocity + + + model/#{_name} + rotor_2_joint + rotor_2 + cw + 0.0125 + 0.025 + 800.0 + 8.54858e-06 + 0.016 + command/motor_speed + 2 + 8.06428e-05 + 1e-06 + motor_speed/2 + 10 + velocity + + + model/#{_name} + rotor_3_joint + rotor_3 + cw + 0.0125 + 0.025 + 800.0 + 8.54858e-06 + 0.016 + command/motor_speed + 3 + 8.06428e-05 + 1e-06 + motor_speed/3 + 10 + velocity + + + + model/#{_name} + cmd_vel + command/motor_speed + velocity_controller/enable + base_link + 2.7 2.7 2.7 + 2 3 0.15 + 0.4 0.52 0.18 + 1 1 2 + 5 5 5 + 3 3 3 + 0 0 0.05 + + + + 0.1105 0.1261 0.00947 + 0 0 0 + + + 0.004 0.004 0.004 + + + rotor_0_joint + 8.54858e-06 + 0.016 + 1 + + + rotor_1_joint + 8.54858e-06 + 0.016 + 1 + + + rotor_2_joint + 8.54858e-06 + 0.016 + -1 + + + rotor_3_joint + 8.54858e-06 + 0.016 + -1 + + + + + + linear_battery + 12.694 + 12.694 + -3.1424 + 18.0 + 18.0 + 0.061523 + 1.9499 + 6.6 + true + + + + + HEREDOC + + exec = <<-HEREDOC + + roslaunch --wait subt_ros x3_description.launch world_name:=#{$worldName} name:=#{_name} + + + 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 + + HEREDOC + + return spawn, exec +end + +def spawnX4(_name, _config, _x = 0.0, _y = 0.0, _z = 0.0, _yaw = 0.0) + 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 + + robot = Robot.new(_name, "X4_SENSOR_CONFIG_" + _config) + + spawn = <<-HEREDOC + + #{_name} + false + #{_x} #{_y} #{z} 0 0 #{_yaw} + #{$worldName} + true + + + #{_name} + #{robot.modelURI()} + + + #{robot.commonPlugins($enableGroundTruth)} + + + model/#{_name} + rotor_0_joint + rotor_0 + ccw + 0.0182 + 0.0182 + 1000.0 + 1.269e-05 + 0.016754 + command/motor_speed + 0 + 2.0673e-04 + 0 + motor_speed/0 + 10 + velocity + + + model/#{_name} + rotor_1_joint + rotor_1 + cw + 0.0182 + 0.0182 + 1000.0 + 1.269e-05 + 0.016754 + command/motor_speed + 1 + 2.0673e-04 + 0 + motor_speed/1 + 10 + velocity + + + model/#{_name} + rotor_2_joint + rotor_2 + ccw + 0.0182 + 0.0182 + 1000.0 + 1.269e-05 + 0.016754 + command/motor_speed + 2 + 2.0673e-04 + 0 + motor_speed/2 + 10 + velocity + + + model/#{_name} + rotor_3_joint + rotor_3 + cw + 0.0182 + 0.0182 + 1000.0 + 1.269e-05 + 0.016754 + command/motor_speed + 3 + 2.0673e-04 + 0 + motor_speed/3 + 10 + velocity + + + model/#{_name} + rotor_4_joint + rotor_4 + ccw + 0.0182 + 0.0182 + 1000.0 + 1.269e-05 + 0.016754 + command/motor_speed + 4 + 2.0673e-04 + 0 + motor_speed/4 + 10 + velocity + + + model/#{_name} + rotor_5_joint + rotor_5 + cw + 0.0182 + 0.0182 + 1000.0 + 1.269e-05 + 0.016754 + command/motor_speed + 5 + 2.0673e-04 + 0 + motor_speed/5 + 10 + velocity + + + + model/#{_name} + cmd_vel + command/motor_speed + velocity_controller/enable + base_link + 6 6 10 + 4 4 2 + 0.7 0.7 0.7 + 1 1 2 + 5 5 5 + 3 3 3 + 0 0 0 + + + + 0.1105 0.1261 0.0947 + 0 0 0 + + + 0.004 0.004 0.004 + + + rotor_0_joint + 1.269e-05 + 1.6754e-2 + 1 + + + rotor_1_joint + 1.269e-05 + 1.6754e-2 + -1 + + + rotor_2_joint + 1.269e-05 + 1.6754e-2 + 1 + + + rotor_3_joint + 1.269e-05 + 1.6754e-2 + -1 + + + rotor_4_joint + 1.269e-05 + 1.6754e-2 + 1 + + + rotor_5_joint + 1.269e-05 + 1.6754e-2 + -1 + + + + + + linear_battery + 12.694 + 12.694 + -3.1424 + 18.0 + 18.0 + 0.061523 + 1.9499 + 6.6 + true + + + + + HEREDOC + + exec = <<-HEREDOC + + roslaunch --wait subt_ros x4_description.launch world_name:=#{$worldName} name:=#{_name} + + + 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 + + HEREDOC + + return spawn, exec +end + +def spawnTeamBase(_name, _x, _y, _z, _yaw) + spawn = <<-HEREDOC + + #{_name} + false + #{$worldName} + false + + + #{_x} #{_y} #{_z} 0 0 #{_yaw} + true + + 0 0 0.05 0 0 0 + + + + .1 .1 .1 + + + + + + + + HEREDOC + + exec = <<-HEREDOC + + roslaunch --wait subt_ros teambase_description.launch world_name:=#{$worldName} name:=#{_name} + + + roslaunch --wait subt_ros teambase_topics.launch world_name:=#{$worldName} name:=#{_name} + + HEREDOC + + return spawn, exec +end + +def spawnRobot(_robot, _world, _allowLocalModel=false) + name = _robot.name + type = _robot.type + x = _robot.pos.x + y = _robot.pos.y + z = _robot.pos.z + yaw = _robot.rot.yaw + robotConfigN = _robot.configNumber + + if _robot.isBuiltin + # we already know it is a builtin robot, so no need to check which particular sensor config + if type == "X1" + return spawnX1(name, robotConfigN, x, y, z, yaw) + elsif type == "X2" + return spawnX2(name, robotConfigN, x, y, z, yaw) + elsif type == "X3" + return spawnX3(name, robotConfigN, x, y, z, yaw) + elsif type == "X4" + return spawnX4(name, robotConfigN, x, y, z, yaw) + elsif type.upcase == "TEAMBASE" + return spawnTeamBase(name, x, y, z, yaw) + else + raise "Unknown builtin robot type: #{_robot.config}" + end + else + spawnerScript = _robot.spawnScriptPath() + begin + load spawnerScript + rescue LoadError + raise "Unknown robot configuration #{_robot.config}. #{spawnerScript} could not be found." + else + modelURI = _robot.modelURI(_allowLocalModel) + spawnString = spawner(name, modelURI, _world.name, + _robot.pos.x, _robot.pos.y, _robot.pos.z, 0, 0, _robot.rot.yaw) + executables = rosExecutables(name, _world.name) + return spawnString, executables + end + end +end + +def getMarsupialSnippets(_parentRobot, _childRobot, _worldName) + childModelName = _childRobot.name + parentModelName = _parentRobot.name + + detachableJointPlugin = REXML::Document.new <<-HEREDOC + + #{MARSUPIAL_PARENT_LINK_NAMES[_parentRobot.type]} + #{childModelName} + base_link + /model/#{childModelName}/detach + true + + HEREDOC + + joints = [detachableJointPlugin] + + platform = "" + + # This will be used to attach the drone platform to the base. + if _parentRobot.type.include?("X1") + platformJointPlugin = REXML::Document.new <<-HEREDOC + + base_link + #{_parentRobot.name}_platform + base_link + true + + HEREDOC + + joints.push(platformJointPlugin) + + # This will be used to spawn the drone platform. + platform = <<-HEREDOC + + #{_parentRobot.name}_platform + false + #{_parentRobot.pos.x + 0.078} #{_parentRobot.pos.y} #{_parentRobot.pos.z-0.11} 0 0 0 + #{_worldName} + false + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/DronePlatformX1 + + + + HEREDOC + end + + rosExecutable = <<-HEREDOC + + roslaunch --wait subt_ros marsupial_topics.launch name:=#{childModelName} + + HEREDOC + + return joints, platform, rosExecutable +end \ No newline at end of file diff --git a/subt_ign/launch/virtual_stix.ign b/subt_ign/launch/virtual_stix.ign index 6ba2137d..6a101648 100644 --- a/subt_ign/launch/virtual_stix.ign +++ b/subt_ign/launch/virtual_stix.ign @@ -395,11 +395,21 @@ " true\n"\ " false\n"\ " false\n"\ - " #{$enableGroundTruth}\n"\ + " false\n"\ " true\n"\ " true\n"\ " 1\n"\ " \n"\ + " \n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " #{$enableGroundTruth}\n"\ + " true\n"\ + " false\n"\ + " \n"\ " \n"\ " \n"\ @@ -468,11 +478,21 @@ " true\n"\ " false\n"\ " false\n"\ - " #{$enableGroundTruth}\n"\ + " false\n"\ " true\n"\ " true\n"\ " 1\n"\ " \n"\ + " \n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " #{$enableGroundTruth}\n"\ + " true\n"\ + " false\n"\ + " \n"\ " \n"\ " \n"\ @@ -527,11 +547,21 @@ " true\n"\ " false\n"\ " false\n"\ - " #{$enableGroundTruth}\n"\ + " false\n"\ " true\n"\ " true\n"\ " 1\n"\ " \n"\ + " \n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " #{$enableGroundTruth}\n"\ + " true\n"\ + " false\n"\ + " \n"\ " \n"\ " model/#{_name}\n"\ @@ -716,11 +746,21 @@ " true\n"\ " false\n"\ " false\n"\ - " #{$enableGroundTruth}\n"\ + " false\n"\ " true\n"\ " true\n"\ " 1\n"\ " \n"\ + " \n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " #{$enableGroundTruth}\n"\ + " true\n"\ + " false\n"\ + " \n"\ " \n"\ " model/#{_name}\n"\ diff --git a/subt_ign/launch/virtual_stix_headless.ign b/subt_ign/launch/virtual_stix_headless.ign index 2250fb1d..69ebb922 100644 --- a/subt_ign/launch/virtual_stix_headless.ign +++ b/subt_ign/launch/virtual_stix_headless.ign @@ -305,11 +305,21 @@ " true\n"\ " false\n"\ " false\n"\ - " #{$enableGroundTruth}\n"\ + " false\n"\ " true\n"\ " true\n"\ " 1\n"\ " \n"\ + " \n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " #{$enableGroundTruth}\n"\ + " true\n"\ + " false\n"\ + " \n"\ " \n"\ " \n"\ @@ -378,11 +388,21 @@ " true\n"\ " false\n"\ " false\n"\ - " #{$enableGroundTruth}\n"\ + " false\n"\ " true\n"\ " true\n"\ " 1\n"\ " \n"\ + " \n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " #{$enableGroundTruth}\n"\ + " true\n"\ + " false\n"\ + " \n"\ " \n"\ " \n"\ @@ -437,11 +457,21 @@ " true\n"\ " false\n"\ " false\n"\ - " #{$enableGroundTruth}\n"\ + " false\n"\ " true\n"\ " true\n"\ " 1\n"\ " \n"\ + " \n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " #{$enableGroundTruth}\n"\ + " true\n"\ + " false\n"\ + " \n"\ " \n"\ " model/#{_name}\n"\ @@ -626,11 +656,21 @@ " true\n"\ " false\n"\ " false\n"\ - " #{$enableGroundTruth}\n"\ + " false\n"\ " true\n"\ " true\n"\ " 1\n"\ " \n"\ + " \n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " #{$enableGroundTruth}\n"\ + " true\n"\ + " false\n"\ + " \n"\ " \n"\ " model/#{_name}\n"\ diff --git a/subt_ign/launch/world_common_defs.rb b/subt_ign/launch/world_common_defs.rb new file mode 100644 index 00000000..44dcdeb9 --- /dev/null +++ b/subt_ign/launch/world_common_defs.rb @@ -0,0 +1,320 @@ +# This is a common file intended to be included in all competition world launchers. + +# Library of functions for robots +require File.dirname(__FILE__) + "/robot_common_defs.rb" + +$validCircuits = ["cave", "tunnel", "urban"] + +$spawnRowSize = 4 +$spawnColSize = 5 +$spawnGridSize = 2.0 +$maxRobotCount = $spawnRowSize * $spawnColSize + +class World + attr_accessor :name, :path, :number, :levels, :circuit, :guiCameraPose, :spawnBasePos, :spawnBaseRot, :commsFadingExponent, :commsScalingFactor, :commsRangePerHop + def initialize(name, circuit = "") + @name = name + @number = name.split('_').last + + @path = "#{name}.sdf" + if name.include?('tunnel_circuit_') && !name.include?('practice') + @path = "tunnel_circuit/#{@number}/#{name}.sdf" + elsif name.include?('urban_circuit_') && !name.include?('practice') + @path = "urban_circuit/#{@number}/#{name}.sdf" + elsif name.include?('cave_circuit_') && !name.include?('practice') + @path = "cave_circuit/#{@number}/#{name}.sdf" + end + + # disable levels for simple cave worlds + @levels = true + if name.include?('simple_cave_') || name.include?('cave_qual') || name.include?('niosh_') + @levels = false + end + + if circuit.empty? + if name.include?('tunnel') || name.include?('niosh') + circuit = "tunnel" + elsif name.include?('urban') + circuit = "urban" + else + circuit = "cave" + end + end + @circuit = circuit + + if @circuit == "urban" + @guiCameraPose = "-2.0 27 15 0 0.868 0" + else + @guiCameraPose = "-6.3 -4.2 3.6 0 0.268 0.304" + end + + @spawnBasePos = Vector3d.new(0, 0, 0) + @spawnBaseRot = AngularVector3d.new(0, 0, 0) + if @circuit == "urban" + @spawnBasePos.x = 6 + @spawnBasePos.y = 27 + @spawnBasePos.z = 7.5 + end + + @commsFadingExponent = 2.5 + @commsScalingFactor = 1.0 + @commsRangePerHop = 2.0 + if @circuit == "cave" + @commsFadingExponent = 1.5 + @commsScalingFactor = 0.55 + @commsRangePerHop = 2.0 + end + end + + def assignSpawnPositions(_robots, _marsupialParents) + spawnColOffset = $spawnColSize * $spawnGridSize / 2 + spawnRowOffset = $spawnRowSize * $spawnGridSize / 2 + + # Reverse hash of marsupials, 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(-@spawnBaseRot.yaw) * offsetX + Math.sin(-@spawnBaseRot.yaw) * offsetY + robot.pos.y = parentRobot.pos.y - Math.sin(-@spawnBaseRot.yaw) * offsetX + Math.cos(-@spawnBaseRot.yaw) * 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(-@spawnBaseRot.yaw) * posX + Math.sin(-@spawnBaseRot.yaw) * posY + y = -(Math.sin(-@spawnBaseRot.yaw)) * posX + Math.cos(-@spawnBaseRot.yaw) * posY + robot.pos.x = x + @spawnBasePos.x + robot.pos.y = y + @spawnBasePos.y + robot.pos.z = @spawnBasePos.z + robot.rot.yaw = @spawnBaseRot.yaw + end + robotSpawned += 1 + end + end + + def commonLaunchEnvironment() + return <<-HEREDOC + + IGN_GAZEBO_SYSTEM_PLUGIN_PATH + $LD_LIBRARY_PATH + + + + IGN_TRANSPORT_TOPIC_STATISTICS + 1 + + HEREDOC + end + + def commonServerPlugins() + return <<-HEREDOC + + + + ogre2 + + + + + + + + + + + + + + + HEREDOC + end + + def guiPlugin() + return <<-HEREDOC + + #{@name} + 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 + #{@guiCameraPose} + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + /world/#{@name}/control + /world/#{@name}/stats + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/#{@name}/stats + + + + + + + + HEREDOC + end + + def subtServerPlugins(_durationSec, _logPath, _ros) + return <<-HEREDOC + + + + #{@name} + #{_ros} + + #{_durationSec} + + + + #{_logPath} + subt_#{@circuit} + 5 + + + HEREDOC + end + + def subtLaunchPlugins() + return <<-HEREDOC + + + true + #{@name} + + visibility_range + + + 500.0 + #{@commsFadingExponent} + 40 + 10.0 + #{@commsScalingFactor} + #{@commsRangePerHop} + + + + 0.2 + 10 + + + + 1000000 + 20 + -90 + QPSK + + + + + + + + HEREDOC + end + +end diff --git a/subt_ign/test/test.ign b/subt_ign/test/test.ign index 33dd5dd0..a7dde393 100644 --- a/subt_ign/test/test.ign +++ b/subt_ign/test/test.ign @@ -324,11 +324,21 @@ " true\n"\ " false\n"\ " false\n"\ - " #{$enableGroundTruth}\n"\ + " false\n"\ " true\n"\ " true\n"\ " 1\n"\ " \n"\ + " \n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " #{$enableGroundTruth}\n"\ + " true\n"\ + " false\n"\ + " \n"\ " \n"\ " \n"\ @@ -390,11 +400,21 @@ " true\n"\ " false\n"\ " false\n"\ - " #{$enableGroundTruth}\n"\ + " false\n"\ " true\n"\ " true\n"\ " 1\n"\ " \n"\ + " \n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " #{$enableGroundTruth}\n"\ + " true\n"\ + " false\n"\ + " \n"\ " \n"\ " \n"\ @@ -442,11 +462,21 @@ " true\n"\ " false\n"\ " false\n"\ - " #{$enableGroundTruth}\n"\ + " false\n"\ " true\n"\ " true\n"\ " 1\n"\ " \n"\ + " \n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " #{$enableGroundTruth}\n"\ + " true\n"\ + " false\n"\ + " \n"\ " \n"\ " model/#{_name}\n"\ @@ -573,11 +603,21 @@ " true\n"\ " false\n"\ " false\n"\ - " #{$enableGroundTruth}\n"\ + " false\n"\ " true\n"\ " true\n"\ " 1\n"\ " \n"\ + " \n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " false\n"\ + " #{$enableGroundTruth}\n"\ + " true\n"\ + " false\n"\ + " \n"\ " \n"\ " model/#{_name}\n"\