diff --git a/src/nav2_turtlebot_simulator/launch/simulator.launch.py b/src/nav2_turtlebot_simulator/launch/simulator.launch.py index 2ac775e..32e6caa 100644 --- a/src/nav2_turtlebot_simulator/launch/simulator.launch.py +++ b/src/nav2_turtlebot_simulator/launch/simulator.launch.py @@ -12,8 +12,39 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +HOUSE_FURNITURE_BOXES = { + # mailbox.dae uses inch units and an internal transform. These values preserve + # its rendered bounds while keeping the visual box upright on the floor. + 'mailbox/': ('0.133 0.355 1.067', '0.002 0.017 0.533 0 0 0'), + 'cafe_table/': ('0.94 0.94 0.78', None), + 'first_2015_trash_can/': ('0.44 0.44 0.76', None), + 'table_marble/': ('1.40 0.86 0.76', None), +} + + +def replace_house_visual_meshes_with_boxes(house_model): + """Keep physical mesh collisions, but render furniture as inexpensive boxes.""" + for visual in house_model.findall('.//visual'): + geometry = visual.find('geometry') + mesh = geometry.find('mesh') if geometry is not None else None + uri = mesh.findtext('uri', default='') if mesh is not None else '' + box_config = next((value for key, value in HOUSE_FURNITURE_BOXES.items() if key in uri), None) + if box_config is None: + continue + + size, pose = box_config + geometry.remove(mesh) + box = ET.SubElement(geometry, 'box') + ET.SubElement(box, 'size').text = size + if pose is not None: + visual_pose = visual.find('pose') + if visual_pose is None: + visual_pose = ET.SubElement(visual, 'pose') + visual_pose.text = pose + + def create_flat_house_world(gazebo_dir): - """Create a texture-free copy of the house model while retaining its mesh hierarchy.""" + """Create a texture-free house copy with box-rendered furniture.""" world_path = os.path.join(gazebo_dir, 'worlds', 'turtlebot3_house.world') model_path = os.path.join(gazebo_dir, 'models', 'turtlebot3_house', 'model.sdf') flat_models_dir = os.path.join(tempfile.gettempdir(), 'nav2_flat_models') @@ -23,6 +54,7 @@ def create_flat_house_world(gazebo_dir): house_model_tree = ET.parse(model_path) house_model = house_model_tree.getroot() house_model.attrib['name'] = 'turtlebot3_house_flat' + replace_house_visual_meshes_with_boxes(house_model) for visual in house_model.findall('.//visual'): material = visual.find('material') @@ -47,6 +79,11 @@ def create_flat_house_world(gazebo_dir): shadows = ET.SubElement(visual, 'cast_shadows') shadows.text = 'false' + lighting = visual.find('lighting') + if lighting is None: + lighting = ET.SubElement(visual, 'lighting') + lighting.text = 'false' + # ElementTree serializes Gazebo's frame='' as frame="", which is invalid SDF. for element in house_model.iter(): if element.attrib.get('frame') == '':