From e5ce7824823b8da9f82d6f0f0e5a9862b00489a4 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 18 Dec 2024 11:35:17 +0800 Subject: [PATCH 1/2] Add the ability to customize the base world file (#519) * Add the ability to customize the base world file This PR adds a small feature which allows end users to add a custom base simulation file. This allows power users to customize their simulation environments with custom gazebo plugins. Signed-off-by: Arjo Chakravarty --- rmf_building_map_tools/building_map/building.py | 14 ++++++++------ rmf_building_map_tools/building_map/generator.py | 5 +++-- .../building_map_generator/_init_argparse.py | 4 +++- .../building_map_generator.py | 3 ++- 4 files changed, 16 insertions(+), 10 deletions(-) diff --git a/rmf_building_map_tools/building_map/building.py b/rmf_building_map_tools/building_map/building.py index 5c93ec36..f7339381 100644 --- a/rmf_building_map_tools/building_map/building.py +++ b/rmf_building_map_tools/building_map/building.py @@ -394,13 +394,15 @@ def generate_nav_graphs(self): nav_graphs[f'{i}'] = g return nav_graphs - def generate_sdf_world(self): + def generate_sdf_world(self, template_file): """ Return an etree of this Building in SDF starting from a template""" - template_name = 'gz_world.sdf' - - template_path = os.path.join( - get_package_share_directory('rmf_building_map_tools'), - f'templates/{template_name}') + if template_file == "": + template_name = 'gz_world.sdf' + template_path = os.path.join( + get_package_share_directory('rmf_building_map_tools'), + f'templates/{template_name}') + else: + template_path = template_file tree = parse(template_path) sdf = tree.getroot() diff --git a/rmf_building_map_tools/building_map/generator.py b/rmf_building_map_tools/building_map/generator.py index 05ea9250..d7b2c142 100644 --- a/rmf_building_map_tools/building_map/generator.py +++ b/rmf_building_map_tools/building_map/generator.py @@ -21,7 +21,8 @@ def generate_sdf( self, input_filename, output_filename, - output_models_dir + output_models_dir, + template_file ): print('generating {} from {}'.format(output_filename, input_filename)) @@ -40,7 +41,7 @@ def generate_sdf( building.generate_sdf_models(output_models_dir) # generate a top-level SDF for convenience - sdf = building.generate_sdf_world() + sdf = building.generate_sdf_world(template_file) indent_etree(sdf) sdf_str = str(ElementToString(sdf), 'utf-8') diff --git a/rmf_building_map_tools/building_map_generator/_init_argparse.py b/rmf_building_map_tools/building_map_generator/_init_argparse.py index bfe01a47..e09f29fc 100644 --- a/rmf_building_map_tools/building_map_generator/_init_argparse.py +++ b/rmf_building_map_tools/building_map_generator/_init_argparse.py @@ -16,7 +16,9 @@ help="Name of the .world file to output") shared_parser.add_argument("OUTPUT_MODEL_DIR", type=str, help="Path to output the map model files") - +shared_parser.add_argument("--TEMPLATE_WORLD_FILE", type=str, default="", + help="Specify the template for" + + " the base simulation.") # Create subparsers for Gazebo and Nav generation gazebo_parser = subparsers.add_parser( 'gazebo', diff --git a/rmf_building_map_tools/building_map_generator/building_map_generator.py b/rmf_building_map_tools/building_map_generator/building_map_generator.py index 00510836..23930661 100755 --- a/rmf_building_map_tools/building_map_generator/building_map_generator.py +++ b/rmf_building_map_tools/building_map_generator/building_map_generator.py @@ -16,7 +16,8 @@ def main(): g.generate_sdf( args.INPUT, args.OUTPUT_WORLD, - args.OUTPUT_MODEL_DIR + args.OUTPUT_MODEL_DIR, + args.TEMPLATE_WORLD_FILE ) if args.command == "nav": From 5501a0735c9f73b148a49741c6f4e8b3b219de3d Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 6 Jan 2025 17:01:43 +0800 Subject: [PATCH 2/2] Add flag to skip calculating and setting camera pose (#523) Signed-off-by: Aaron Chong --- .../building_map/building.py | 28 ++++++++++--------- .../building_map/generator.py | 5 ++-- .../building_map_generator/_init_argparse.py | 5 ++++ .../building_map_generator.py | 3 +- 4 files changed, 25 insertions(+), 16 deletions(-) diff --git a/rmf_building_map_tools/building_map/building.py b/rmf_building_map_tools/building_map/building.py index f7339381..8b2c36d5 100644 --- a/rmf_building_map_tools/building_map/building.py +++ b/rmf_building_map_tools/building_map/building.py @@ -394,7 +394,7 @@ def generate_nav_graphs(self): nav_graphs[f'{i}'] = g return nav_graphs - def generate_sdf_world(self, template_file): + def generate_sdf_world(self, template_file, skip_camera_pose): """ Return an etree of this Building in SDF starting from a template""" if template_file == "": template_name = 'gz_world.sdf' @@ -474,18 +474,20 @@ def generate_sdf_world(self, template_file): crs_ele.text = self.global_transform.crs_name gui_ele = world.find('gui') - c = self.center() - # Transforming camera to account for offsets if - # not in reference_image mode and when a floor polygon is defined. - if self.global_transform and c != (0, 0): - camera_pose = f'{c[0] - self.global_transform.x} \ - {c[1]-20 - self.global_transform.y} 10 0 0.6 1.57' - else: - camera_pose = f'{c[0]} {c[1]-20} 10 0 0.6 1.57' - # add floor-toggle GUI plugin parameters - plugin_ele = gui_ele.find('.//plugin[@filename="MinimalScene"]') - camera_pose_ele = plugin_ele.find('camera_pose') - camera_pose_ele.text = camera_pose + + if not skip_camera_pose: + c = self.center() + # Transforming camera to account for offsets if + # not in reference_image mode and when a floor polygon is defined. + if self.global_transform and c != (0, 0): + camera_pose = f'{c[0] - self.global_transform.x} \ + {c[1]-20 - self.global_transform.y} 10 0 0.6 1.57' + else: + camera_pose = f'{c[0]} {c[1]-20} 10 0 0.6 1.57' + # add floor-toggle GUI plugin parameters + plugin_ele = gui_ele.find('.//plugin[@filename="MinimalScene"]') + camera_pose_ele = plugin_ele.find('camera_pose') + camera_pose_ele.text = camera_pose toggle_floors_ele = SubElement( gui_ele, diff --git a/rmf_building_map_tools/building_map/generator.py b/rmf_building_map_tools/building_map/generator.py index d7b2c142..f8157793 100644 --- a/rmf_building_map_tools/building_map/generator.py +++ b/rmf_building_map_tools/building_map/generator.py @@ -22,7 +22,8 @@ def generate_sdf( input_filename, output_filename, output_models_dir, - template_file + template_file, + skip_camera_pose ): print('generating {} from {}'.format(output_filename, input_filename)) @@ -41,7 +42,7 @@ def generate_sdf( building.generate_sdf_models(output_models_dir) # generate a top-level SDF for convenience - sdf = building.generate_sdf_world(template_file) + sdf = building.generate_sdf_world(template_file, skip_camera_pose) indent_etree(sdf) sdf_str = str(ElementToString(sdf), 'utf-8') diff --git a/rmf_building_map_tools/building_map_generator/_init_argparse.py b/rmf_building_map_tools/building_map_generator/_init_argparse.py index e09f29fc..0fc37501 100644 --- a/rmf_building_map_tools/building_map_generator/_init_argparse.py +++ b/rmf_building_map_tools/building_map_generator/_init_argparse.py @@ -19,6 +19,11 @@ shared_parser.add_argument("--TEMPLATE_WORLD_FILE", type=str, default="", help="Specify the template for" + " the base simulation.") +shared_parser.add_argument("--SKIP_CAMERA_POSE", action="store_true", + help="Skips calculating and setting the initial " + + "camera view pose. This flag should only be used " + + "if the template SDF file already has the camera " + + "pose defined.") # Create subparsers for Gazebo and Nav generation gazebo_parser = subparsers.add_parser( 'gazebo', diff --git a/rmf_building_map_tools/building_map_generator/building_map_generator.py b/rmf_building_map_tools/building_map_generator/building_map_generator.py index 23930661..2feba39c 100755 --- a/rmf_building_map_tools/building_map_generator/building_map_generator.py +++ b/rmf_building_map_tools/building_map_generator/building_map_generator.py @@ -17,7 +17,8 @@ def main(): args.INPUT, args.OUTPUT_WORLD, args.OUTPUT_MODEL_DIR, - args.TEMPLATE_WORLD_FILE + args.TEMPLATE_WORLD_FILE, + args.SKIP_CAMERA_POSE ) if args.command == "nav":