diff --git a/src/nav2_turtlebot_simulator/launch/simulator.launch.py b/src/nav2_turtlebot_simulator/launch/simulator.launch.py index 1916b28..2ac775e 100644 --- a/src/nav2_turtlebot_simulator/launch/simulator.launch.py +++ b/src/nav2_turtlebot_simulator/launch/simulator.launch.py @@ -1,3 +1,4 @@ +import json import os import tempfile import xml.etree.ElementTree as ET @@ -76,6 +77,59 @@ def create_flat_house_world(gazebo_dir): return flat_world_path +def create_custom_layout_world(): + """Build a Gazebo world from the editor's persisted 2D layout.""" + layout_path = os.environ.get('WORLD_LAYOUT_PATH', '/modules/world-layout.json') + try: + with open(layout_path, 'r', encoding='utf-8') as source: + objects = json.load(source).get('objects', []) + except (OSError, ValueError, AttributeError): + objects = [] + + models = [] + for index, item in enumerate(objects): + if not isinstance(item, dict): + continue + position = item.get('position') or [0, 0, 0] + rotation = item.get('rotation') or [0, 0, 0] + size = item.get('size') or [1, 1, 1] + try: + x, y, z = (float(position[0]), float(position[1]), float(position[2])) + yaw = float(rotation[2]) + sx, sy, sz = (max(0.05, float(size[0])), max(0.05, float(size[1])), max(0.05, float(size[2]))) + except (IndexError, TypeError, ValueError): + continue + + name = f"custom_{item.get('type', 'box')}_{index}" + if item.get('type') == 'pillar': + geometry = f'{sx / 2.0}{sz}' + else: + geometry = f'{sx} {sy} {sz}' + models.append( + f''' + true + {x} {y} {z} 0 0 {yaw} + + {geometry} + {geometry}0.34 0.58 0.82 10.34 0.58 0.82 1 + +''' + ) + + world_path = os.path.join(tempfile.gettempdir(), 'nav2_custom_layout.world') + with open(world_path, 'w', encoding='utf-8') as destination: + destination.write(''' + + + model://sun + model://ground_plane + 1000 +''') + destination.write('\n'.join(models)) + destination.write('\n \n\n') + return world_path + + def launch_gazebo(context): gazebo_dir = get_package_share_directory('turtlebot3_gazebo') simulator_dir = get_package_share_directory('nav2_turtlebot_simulator') @@ -86,7 +140,9 @@ def launch_gazebo(context): if render_mode not in {'flat', 'full'}: raise RuntimeError('render_mode must be either flat or full') - if world == 'turtlebot3_house.world' and render_mode == 'flat': + if world == 'empty_world.world' and os.path.isfile(os.environ.get('WORLD_LAYOUT_PATH', '/modules/world-layout.json')): + world_path = create_custom_layout_world() + elif world == 'turtlebot3_house.world' and render_mode == 'flat': world_path = create_flat_house_world(gazebo_dir) else: world_path = os.path.join(