diff --git a/lrauv_ignition_plugins/CMakeLists.txt b/lrauv_ignition_plugins/CMakeLists.txt index 606164b2..02e75df3 100644 --- a/lrauv_ignition_plugins/CMakeLists.txt +++ b/lrauv_ignition_plugins/CMakeLists.txt @@ -254,11 +254,15 @@ configure_file( #============================================================================ # World generation set(WORLD_NAME "portuguese_ledge") +set(WORLD_NAME_EMPTY "empty_environment_with_tiles") add_custom_command( OUTPUT world_gen_cmd COMMAND python3 ${CMAKE_CURRENT_SOURCE_DIR}/worlds/empy_expander.py ${CMAKE_CURRENT_SOURCE_DIR}/worlds/${WORLD_NAME}.sdf.em ${CMAKE_CURRENT_BINARY_DIR}/worlds/${WORLD_NAME}.sdf + COMMAND python3 ${CMAKE_CURRENT_SOURCE_DIR}/worlds/empy_expander.py + ${CMAKE_CURRENT_SOURCE_DIR}/worlds/${WORLD_NAME_EMPTY}.sdf.em + ${CMAKE_CURRENT_BINARY_DIR}/worlds/${WORLD_NAME_EMPTY}.sdf ) add_custom_target(world_gen_target ALL @@ -267,6 +271,7 @@ add_custom_target(world_gen_target ALL install(FILES ${CMAKE_CURRENT_BINARY_DIR}/worlds/${WORLD_NAME}.sdf + ${CMAKE_CURRENT_BINARY_DIR}/worlds/${WORLD_NAME_EMPTY}.sdf DESTINATION share/${PROJECT_NAME}/worlds) #============================================================================ @@ -277,4 +282,4 @@ install( models worlds ${CMAKE_CURRENT_BINARY_DIR}/hooks - DESTINATION share/${PROJECT_NAME}) \ No newline at end of file + DESTINATION share/${PROJECT_NAME}) diff --git a/lrauv_ignition_plugins/worlds/empty_environment_with_tiles.sdf.em b/lrauv_ignition_plugins/worlds/empty_environment_with_tiles.sdf.em new file mode 100644 index 00000000..581db8e1 --- /dev/null +++ b/lrauv_ignition_plugins/worlds/empty_environment_with_tiles.sdf.em @@ -0,0 +1,493 @@ + + + + + +@{ + +import math +from dataclasses import dataclass +from ignition.math import SphericalCoordinates, Vector3d, Angle + +fuel_model_url = "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Portuguese Ledge" + +@dataclass +class Tile: + index: int + lat_deg: float + lon_deg: float + height: float + pos_enu: Vector3d = Vector3d() + +# Center of all 18 tiles in degrees +tiles = [ + Tile(1, 36.693509, -121.936568, 25.34), + Tile(2, 36.693583, -121.944962, 27.094), + Tile(3, 36.693658, -121.953356, 11.602), + Tile(4, 36.693731, -121.961751, 6.781), + Tile(5, 36.693804, -121.970145, 6.689), + Tile(6, 36.693876, -121.978539, 30.707), + Tile(7, 36.700269, -121.936475, 20.746), + Tile(8, 36.700343, -121.944870, 29.343), + Tile(9, 36.700418, -121.953265, 6.851), + Tile(10, 36.700491, -121.961660, 6.462), + Tile(11, 36.700564, -121.970055, 29.339), + Tile(12, 36.700636, -121.978450, 148.439), + Tile(13, 36.707029, -121.936382, 6.799), + Tile(14, 36.707103, -121.944777, 6.814), + Tile(15, 36.707178, -121.953173, 8.834), + Tile(16, 36.707251, -121.961569, 11.934), + Tile(17, 36.707324, -121.969965, 75.378), + Tile(18, 36.707396, -121.978360, 229.765)] + +# Convert to world ENU coordinates +sc = SphericalCoordinates( + SphericalCoordinates.EARTH_WGS84, + Angle(math.radians(tiles[0].lat_deg)), + Angle(math.radians(tiles[0].lon_deg)), + 0, Angle(0)) + +for tile in tiles: + vec = Vector3d(math.radians(tile.lat_deg), math.radians(tile.lon_deg), 0) + pos_enu = sc.position_transform(vec, + SphericalCoordinates.SPHERICAL, + SphericalCoordinates.LOCAL2) + tile.pos_enu = pos_enu + +}@ + + + + + 0.0 1.0 1.0 + 0.0 0.7 0.8 + + false + + + + EARTH_WGS84 + ENU + + + @(tiles[0].lat_deg) + @(tiles[0].lon_deg) + + 0 + 0 + + + + 0.02 + 0 + + + + + + + + + + + + + + + 1025 + + 0.5 + 1.125 + + + + + + + + + + + + + 2003080103_mb_l3_las.csv + + + + + /lrauv/init + + + + +@[for tile in tiles]@ + + @(tile.pos_enu.x()) @(tile.pos_enu.y()) @(tile.pos_enu.z()) 0 0 0 + + + 1000 1000 1000 + + + portuguese_ledge_tile_@(tile.index) + +@[end for]@ + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + + 0 6 6 0 0.5 -1.57 + + + + + + 0.1 + + 3000000 + + + + + + + floating + 5 + 5 + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + false + + + + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + Plot Tethys 3D path + docked_collapsed + + tethys + 0 0 1 + 10000 + 0.5 + + + + Inspector + docked_collapsed + + + + + Visualize science data + docked_collapsed + + + + + Camera controls + docked_collapsed + + + + + docked_collapsed + + + + 6 + 0 + + 50000 + 0 100000 0 0 0 0.32 + 0 1 0 1 + + + + 100 + 0 + + 1 + 0 0 0 0 0 0 + 0.5 0.5 0.5 1 + + + + + Tethys controls + docked_collapsed + + + + + Spawn LRAUVs + docked_collapsed + + + + + Reference axis + docked_collapsed + + tethys + + + + + + NavSat Map + docked_collapsed + + /tethys/navsat + true + + + + + + Environmental Configuration + docked_collapsed + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + +@[for tile in tiles]@ + + true + + + + + + + @(tile.pos_enu.x()) @(tile.pos_enu.y()) @(tile.pos_enu.z()) 0 0 0 + true + + @(fuel_model_url)/tip/files/materials/textures/dirt_diffusespecular.png + @(fuel_model_url)/tip/files/materials/textures/flat_normal.png + 10 + + @(fuel_model_url)/tip/files/meshes/PortugueseLedgeTile@(tile.index)_DecDeg.nc + 1000 1000 @(tile.height) + + + + + +@[end for]@ + + + + + + + + + + + + + + + + + + + + + + + +