diff --git a/Changelog.md b/Changelog.md index 6055004f31..c804d2728a 100644 --- a/Changelog.md +++ b/Changelog.md @@ -348,6 +348,20 @@ ## Gazebo Sim 6.x +### Gazebo Sim 6.12.0 (2022-08-30) + +1. Add topic parameter to thrust plugin + * [Pull request #1681](https://github.com/gazebosim/gz-sim/pull/1681) + +1. Add information about `` system parameter + * [Pull request #1671](https://github.com/gazebosim/gz-sim/pull/1671) + +1. Adding tests for hydrodynamics + * [Pull request #1617](https://github.com/gazebosim/gz-sim/pull/1617) + +1. Fix Windows and Doxygen + * [Pull request #1643](https://github.com/gazebosim/gz-sim/pull/1643) + ### Gazebo Sim 6.11.0 (2022-08-17) 1. Add support for specifying log record period diff --git a/Migration.md b/Migration.md index ee70cae11c..679847aafc 100644 --- a/Migration.md +++ b/Migration.md @@ -53,6 +53,11 @@ message's header. gazebo` should be replaced with `import gz.sim7` and `from gz import sim7`. Note the change from `ignition` to `gz` and the addition of the major version number as a suffix to the package name. +## Ignition Gazebo 6.11.X to 6.12.X + + * **Modified**: + + In the Hydrodynamics plugin, inverted the added mass contribution to make it + act in the correct direction. ## Gazebo Sim 6.1 to 6.2 diff --git a/examples/scripts/blender/procedural_dataset_generator.py b/examples/scripts/blender/procedural_dataset_generator.py new file mode 100755 index 0000000000..ee0b7b50eb --- /dev/null +++ b/examples/scripts/blender/procedural_dataset_generator.py @@ -0,0 +1,1945 @@ +#!/usr/bin/env -S blender --python +""" +This script contains a Blender exporter of SDF models `sdf_model_exporter` for Gazebo +and a generator of procedural SDF datasets `procedural_dataset_generator`. + +The SDF exporter outputs models that are compatible with Fuel. It processes all selected +objects in the Blender scene (or all existing objects if none is selected). The exporter +is fully configurable via CLI arguments. It can export separate visual and collision +geometry at different resolution levels. It supports automatic estimation of inertial +properties for non-static models by `trimesh` Python module. Several attributes such as +model mass and surface friction can be randomized via uniform distribution by specifying +a range of minimum and maximum values. + +The dataset generator is based on Blender's `Geometry Nodes` modifiers with support +for variations via `random_seed` input attribute (seed for the pseudorandom generation). +This module-based class inherits all functionalities of the SDF model exporter. + +You can run this script in multiple ways: +a) Directly in Blender's [Text Editor] tab | with the default parameters (configurable) + 1. Copy this script into a [New Text] data block in your 'file.blend' + 2. Configure the default script parameters for your model via constants below + 3. Run the script using the [Run script] button at the top of the [Text Editor] tab +b) Running an internal script (saved within 'file.blend') | with CLI args (configurable) + $ blender [blender options] file.blend --python-text script.py -- [script options] +c) Running an external script | with CLI args (configurable) + $ blender [blender options] file.blend --python external_script.py -- [script options] + +Show the help message of this script ([script options]): + $ blender -b file.blend --python-text script.py -- -h + $ blender -b file.blend --python external_script.py -- -h + +Show the help message of Blender ([blender options]): + $ blender -h +""" +from __future__ import annotations + +# Raise an informative error in case the script is run outside Python env of Blender +try: + import bpy +except ImportError as err: + raise ImportError( + "Python module of Blender 'bpy' not found! Please, execute this script inside " + "a Blender environment (e.g. run inside the Scripting tab of Blender)." + ) from err + +import argparse +import enum +import os +import random +import shutil +import sys +from os import path +from types import ModuleType +from typing import Any, Dict, Iterable, List, Optional, TextIO, Tuple, Union +from xml.dom import minidom +from xml.etree import ElementTree + +# Last tested and working version of Blender for this script (MAJOR, MINOR) +LAST_WORKING_VERSION: Tuple(int, int) = (3, 2) + +### Default script parameters (adjustable via CLI arguments) +## Parameters for SDF model exporter `sdf_model_exporter` +OUTPUT_DIR: str = path.join(os.getcwd(), "sdf_models") +MODEL_VERSION: Optional[int] = None +## Parameters for procedural dataset generator `procedural_dataset_generator` +FIRST_SEED: int = 0 +NUMBER_OF_VARIANTS: int = 8 + +### Default model-specific parameters (adjustable via CLI arguments) +## Default level of detail for exported geometry, e.g. subdivision level +DETAIL_LEVEL_VISUAL: int = 1 +DETAIL_LEVEL_COLLISION: int = 0 +## Default objects to ignore while exporting visual/collision geometry, even if selected +IGNORE_OBJECTS_VISUAL: List[str] = [] +IGNORE_OBJECTS_COLLISION: List[str] = [] +## Default source of textures for the model +# Options: "collada" == "dae" | "wavefront" == "obj" | "stl" +FILETYPE_VISUAL: str = "wavefront" +FILETYPE_COLLISION: str = "stl" +## Default source of textures for the model +# If true, symbolic links will be created for all textures instead of copies +SYMLINK_EXTERNAL_TEXTURES: bool = True +# Options: "none" | "path" +TEXTURE_SOURCE_MODE: str = "none" +TEXTURE_SOURCE_VALUE: Optional[str] = None +MATERIAL_TEXTURE_DIFFUSE: Optional[Tuple[float, float, float]] = (1.0, 1.0, 1.0) +MATERIAL_TEXTURE_SPECULAR: Optional[Tuple[float, float, float]] = (0.2, 0.2, 0.2) +## Default inertial and dynamic properties of exported models +# If true, the model is immovable and it won't be updated by physics engine +STATIC: bool = False +# Options: "none" | "density" | "random_density" | "mass" | "random_mass" +INERTIAL_ESTIMATION_MODE: str = "none" +INERTIAL_ESTIMATION_VALUE: Optional[List[float]] = None +# (Random) coefficient of the surface friction (equal in both directions) +FRICTION_COEFFICIENT: List[float] = [1.0] + +### Default keyword arguments for additional parameters (overwritten by CLI arguments) +DEFAULT_KWARGS: Any = {} + + +class sdf_model_exporter(ModuleType): + """ + Blender exporter of objects as SDF models. + + Note: This class is designed as a "module class" to enable simple copy-pasting + among Blender projects and environments, i.e. methods do not require an instance + during their call while still supporting inheritance. + """ + + ## Directory structure compatible with Gazebo Fuel + MODEL_ROOT: str = "" + BASENAME_SDF: str = path.join(MODEL_ROOT, "model.sdf") + BASENAME_SDF_MODEL_CONFIG: str = path.join(MODEL_ROOT, "model.config") + DIRNAME_MESHES: str = path.join(MODEL_ROOT, "meshes") + DIRNAME_MESHES_VISUAL: str = path.join(DIRNAME_MESHES, "visual") + DIRNAME_MESHES_COLLISION: str = path.join(DIRNAME_MESHES, "collision") + DIRNAME_MATERIALS: str = path.join(MODEL_ROOT, "materials") + DIRNAME_TEXTURES: str = path.join(DIRNAME_MATERIALS, "textures") + DIRNAME_THUMBNAILS: str = path.join(MODEL_ROOT, "thumbnails") + + # Targeted version of SDF + SDF_VERSION: str = "1.9" + + @enum.unique + class ExportFormat(enum.Enum): + """ + Helper enum of supported mesh file formats coupled with their configured + exporters. + """ + + COLLADA = enum.auto() + STL = enum.auto() + WAVEFRONT = enum.auto() + + def export(self, filepath: str) -> str: + """ + Use Blender exporter of the corresponding mesh format. The desired + `filepath` might be adjusted to include the appropriate file extension. Upon + success, the absolute filepath of the exported mesh is returned. Note, that + only the selected object(s) will be exported. + """ + + # Make sure the file has the correct extension + if not filepath.endswith(self.extension): + # Truncate incorrect extension(s) if there are any + split_path = path.basename(filepath).split(".") + if len(split_path) > 1: + filepath = path.join(path.dirname(filepath), split_path[0]) + # Append the correct extension + filepath += self.extension + filepath = path.abspath(filepath) + os.makedirs(name=path.dirname(filepath), exist_ok=True) + + # Export with Blender based on the file format + if self.COLLADA == self: + bpy.ops.wm.collada_export( + filepath=filepath, + check_existing=False, + filter_blender=False, + filter_backup=False, + filter_image=False, + filter_movie=False, + filter_python=False, + filter_font=False, + filter_sound=False, + filter_text=False, + filter_archive=False, + filter_btx=False, + filter_collada=True, + filter_alembic=False, + filter_usd=False, + filter_obj=False, + filter_volume=False, + filter_folder=True, + filter_blenlib=False, + filemode=8, + display_type="DEFAULT", + sort_method="DEFAULT", + prop_bc_export_ui_section="main", + apply_modifiers=True, + export_mesh_type=0, + export_mesh_type_selection="view", + export_global_forward_selection="Y", + export_global_up_selection="Z", + apply_global_orientation=False, + selected=True, + include_children=False, + include_armatures=False, + include_shapekeys=False, + deform_bones_only=False, + include_animations=False, + include_all_actions=False, + export_animation_type_selection="sample", + sampling_rate=1, + keep_smooth_curves=False, + keep_keyframes=False, + keep_flat_curves=False, + active_uv_only=False, + use_texture_copies=True, + triangulate=True, + use_object_instantiation=True, + use_blender_profile=True, + sort_by_name=False, + export_object_transformation_type=0, + export_object_transformation_type_selection="matrix", + export_animation_transformation_type=0, + export_animation_transformation_type_selection="matrix", + open_sim=False, + limit_precision=False, + keep_bind_info=False, + ) + elif self.STL == self: + bpy.ops.export_mesh.stl( + filepath=filepath, + check_existing=False, + use_selection=True, + axis_forward="Y", + axis_up="Z", + global_scale=1, + use_scene_unit=False, + ascii=False, + use_mesh_modifiers=True, + ) + elif self.WAVEFRONT == self: + bpy.ops.export_scene.obj( + filepath=filepath, + check_existing=False, + axis_forward="Y", + axis_up="Z", + use_selection=True, + use_animation=False, + use_mesh_modifiers=True, + use_edges=True, + use_smooth_groups=False, + use_smooth_groups_bitflags=False, + use_normals=True, + use_uvs=True, + use_materials=True, + use_triangles=True, + use_nurbs=False, + use_vertex_groups=False, + use_blen_objects=True, + group_by_object=False, + group_by_material=False, + keep_vertex_order=False, + global_scale=1, + path_mode="AUTO", + ) + else: + raise ValueError(f"Filetype '{self}' is not supported for export.") + + return filepath + + @classmethod + def from_str(cls, filetype_str: str) -> sdf_model_exporter.ExportFormat: + """ + Return ExportFormat that corresponds with a matched `filetype_str` string. + """ + + filetype_str = filetype_str.strip().upper() + if "COLLADA" in filetype_str or "DAE" in filetype_str: + return cls.COLLADA + elif "STL" in filetype_str: + return cls.STL + elif "WAVEFRONT" in filetype_str or "OBJ" in filetype_str: + return cls.WAVEFRONT + else: + raise ValueError(f"Unknown '{filetype_str}' filetype.") + + @classmethod + def default_visual(cls) -> sdf_model_exporter.ExportFormat: + """ + Return the default filetype for visual mesh geometry. + """ + + return cls.from_str(FILETYPE_VISUAL) + + @classmethod + def default_collision(cls) -> sdf_model_exporter.ExportFormat: + """ + Return the default filetype for collision mesh geometry. + """ + + return cls.from_str(FILETYPE_COLLISION) + + @property + def extension(self) -> str: + """ + Return file extension of the corresponding filetype. + """ + + if self.COLLADA == self: + return ".dae" + elif self.STL == self: + return ".stl" + elif self.WAVEFRONT == self: + return ".obj" + else: + raise ValueError(f"Unknown extension for '{self}' filetype.") + + @enum.unique + class TextureSource(enum.Enum): + """ + Helper enum of possible sources for textures. Each source should provide a path + to searchable directory with the following structure (each texture type is + optional and image format must be supported by Gazebo): + ├── ./ + ├── texture_0/ + ├── *albedo*.png || *color*.png + ├── *normal*.png + ├── *roughness*.png + └── *specular*.png || *metalness*.png + ├── ... + └── texture_n/ + Alternatively, it can point to a directory that directly contains textures and + no other subdirectories. + """ + + NONE = enum.auto() + BLENDER_MODEL = enum.auto() + FILEPATH = enum.auto() + ONLINE = enum.auto() + + def get_path(self, value: Optional[str] = None) -> Optional[str]: + """ + Return a path to a directory with PBR textures. + """ + + if self.NONE == self: + return None + elif self.BLENDER_MODEL == self: + return path.abspath(self._generate_baked_textures()) + elif self.FILEPATH == self: + if not value or not path.isdir(value): + raise ValueError( + f"Value '{value}' is an invalid path to a directory with " + "textures." + ) + return path.abspath(value) + elif self.ONLINE == self: + return path.abspath(self._pull_online_textures(value)) + else: + raise ValueError(f"Texture source '{self}' is not supported.") + + def is_enabled(self) -> bool: + """ + Returns true if textures are enabled. + """ + + return not self.NONE == self + + @classmethod + def from_str(cls, source_str: str) -> sdf_model_exporter.TextureSource: + """ + Return TextureSource that corresponds with a matched `source_str` string. + """ + + source_str = source_str.strip().upper() + if not source_str or "NONE" in source_str: + return cls.NONE + elif "BLENDER" in source_str: + return cls.BLENDER_MODEL + elif "PATH" in source_str: + return cls.FILEPATH + elif "ONLINE" in source_str: + return cls.ONLINE + else: + raise ValueError(f"Unknown '{source_str}' texture source.") + + @classmethod + def default(cls) -> sdf_model_exporter.TextureSource: + """ + Return the default texture source. + """ + + return cls.from_str(TEXTURE_SOURCE_MODE) + + def _generate_baked_textures( + cls, + ) -> str: + """ + Bake PBR textures for the model and return a path to a directory that + contains them. + """ + + # TODO[feature]: Implement baking of textures from Blender materials + return NotImplementedError("Baking of textures is not yet implemented!") + + def _pull_online_textures( + cls, + value: Optional[Any] = None, + ) -> str: + """ + Get PBR textures from an online source. + """ + + # TODO[feature]: Add option for pulling PBR textures from an online source + return NotImplementedError( + "Getting textures from an online source is not yet implemented!" + ) + + @enum.unique + class InertialEstimator(enum.Enum): + """ + Helper enum for estimating inertial properties of a model. + """ + + ## Modes of estimation + NONE = enum.auto() + DENSITY = enum.auto() + RANDOM_DENSITY = enum.auto() + MASS = enum.auto() + RANDOM_MASS = enum.auto() + + def estimate_inertial_properties( + self, + analysed_mesh_filepath: str, + value: List[float], + ) -> Dict[ + str, + Union[ + float, + Tuple[float, float, float], + Tuple[ + Tuple[float, float, float], + Tuple[float, float, float], + Tuple[float, float, float], + ], + ], + ]: + """ + Estimate inertial properties of the mesh assuming uniform density. + """ + + # Try to import trimesh or try to install it within the Python environment + # Note: This step might throw an exception if it is not possible + # TODO[enhancement]: Add other methods for estimating inertial properties + self._try_install_trimesh() + import trimesh + + # Load the mesh + mesh = trimesh.load( + analysed_mesh_filepath, force="mesh", ignore_materials=True + ) + + # Extract or sample a floating point value from the input + value = self.sample_value(value) + + # Set the density (either via density or mass) + if self.DENSITY == self or self.RANDOM_DENSITY == self: + mesh.density = value + elif self.MASS == self or self.RANDOM_MASS == self: + mesh.density = value / mesh.volume + else: + raise ValueError( + f"Mode for estimation of inertial properties '{self}' is not " + "supported." + ) + + return { + "mass": mesh.mass, + "inertia": mesh.moment_inertia, + "centre_of_mass": mesh.center_mass, + } + + def sample_value(self, value: Optional[List[float]] = None) -> Optional[float]: + """ + Extract or sample a value that should be used when estimating inertial + properties. + """ + + if self.NONE == self: + return None + elif self.DENSITY == self or self.MASS == self: + if not value or (isinstance(value, Iterable) and len(value) != 1): + raise ValueError( + "Exactly a single value must be provided when estimating " + "inertial properties with either target density of mass." + ) + return value[0] if isinstance(value, Iterable) else value + elif self.RANDOM_DENSITY == self or self.RANDOM_MASS == self: + if not value or (isinstance(value, Iterable) and len(value) != 2): + raise ValueError( + "A range with two values (MIN and MAX) must be provided when " + "sampling a random density of mass during estimation of " + "inertial properties." + ) + return random.uniform(value[0], value[1]) + else: + raise ValueError( + f"Mode for estimation of inertial properties '{self}' is not " + "supported." + ) + + def is_enabled(self) -> bool: + """ + Returns true if estimation of inertial properties is enabled. + """ + + return not self.NONE == self + + @classmethod + def from_str(cls, source_str: str) -> sdf_model_exporter.InertialEstimator: + """ + Return InertialEstimator that corresponds with a matched `source_str` + string. + """ + + source_str = source_str.strip().upper() + if not source_str or "NONE" in source_str: + return cls.NONE + elif "RANDOM" in source_str: + if "DENSITY" in source_str: + return cls.RANDOM_DENSITY + elif "MASS" in source_str: + return cls.RANDOM_MASS + elif "DENSITY" in source_str: + return cls.DENSITY + elif "MASS" in source_str: + return cls.MASS + else: + raise ValueError( + f"Unknown '{source_str}' mode for estimation of inertial " + "properties." + ) + + @classmethod + def default(cls) -> sdf_model_exporter.InertialEstimator: + """ + Return the default mode for estimating inertial properties. + """ + + return cls.from_str(INERTIAL_ESTIMATION_MODE) + + @classmethod + def _try_install_trimesh(cls): + """ + Return the default mode for estimating inertial properties. + """ + + try: + import trimesh + except ImportError as err: + import site + import subprocess + + cls._print_bpy( + "Warn: Python module 'trimesh' could not found! This module is " + "necessary to estimate inertial properties of objects. Attempting " + "to install the module automatically via 'pip'...", + file=sys.stderr, + ) + try: + subprocess.check_call([sys.executable, "-m", "ensurepip", "--user"]) + subprocess.check_call( + [ + sys.executable, + "-m", + "pip", + "install", + "--upgrade", + "trimesh", + "pycollada", + "--target", + str(site.getsitepackages()[0]), + ] + ) + import trimesh + except subprocess.CalledProcessError as err: + err_msg = ( + "Python module 'trimesh' cannot be installed automatically! To " + "enable estimation of inertial properties, please install the " + "module manually (within Blender's Python environment) or use " + "the Python environment of your system (by passing Blender " + "option `--python-use-system-env`). Alternatively, you can " + "disable the estimation of inertial properties via script " + "argument `--inertial-estimation-mode none`." + ) + cls._print_bpy( + f"Err: {err_msg}", + file=sys.stderr, + ) + raise ImportError(err_msg) from err + + @classmethod + def export( + cls, + output_dir: Optional[str] = OUTPUT_DIR, + model_version: Optional[int] = MODEL_VERSION, + model_name_prefix: str = "", + model_name_suffix: str = "", + filetype_visual: Union[ExportFormat, str] = ExportFormat.default_visual(), + filetype_collision: Union[ExportFormat, str] = ExportFormat.default_collision(), + detail_level_visual: int = DETAIL_LEVEL_VISUAL, + detail_level_collision: int = DETAIL_LEVEL_COLLISION, + ignore_objects_visual: List[str] = IGNORE_OBJECTS_VISUAL, + ignore_objects_collision: List[str] = IGNORE_OBJECTS_COLLISION, + symlink_external_textures: bool = SYMLINK_EXTERNAL_TEXTURES, + texture_source_mode: Union[TextureSource, str] = TextureSource.default(), + texture_source_value: Optional[str] = TEXTURE_SOURCE_VALUE, + material_texture_diffuse: Optional[ + Tuple[float, float, float] + ] = MATERIAL_TEXTURE_DIFFUSE, + material_texture_specular: Optional[ + Tuple[float, float, float] + ] = MATERIAL_TEXTURE_SPECULAR, + static: bool = STATIC, + inertial_estimation_mode: Union[ + InertialEstimator, str + ] = InertialEstimator.default(), + inertial_estimation_value: Optional[List[float]] = INERTIAL_ESTIMATION_VALUE, + inertial_estimation_use_collision_mesh: bool = True, + friction_coefficient: Optional[ + Union[float, Tuple[float, float]] + ] = FRICTION_COEFFICIENT, + generate_thumbnails: bool = True, + **kwargs, + ): + """ + The primary function enables exporting of a single SDF model. + """ + + # Store the list of originally selected objects + selected_objects_original = bpy.context.selected_objects + # Create a list of objects to process + if selected_objects_original: + # Use currently selected objects if any is selected + objects_to_process = selected_objects_original + else: + # Otherwise, use all objects if nothing is selected + objects_to_process = bpy.context.selectable_objects + + # Separate different object types to process them differently + # Note: Not all types are currently supported by this script + # TODO[feature]: Support exporting of cameras and lights as SDF entities + meshes_to_process = cameras_to_process = lights_to_process = [] + for obj in objects_to_process: + if obj.type == "MESH": + meshes_to_process.append(obj) + cls._print_bpy( + f"Info: Processing mesh '{obj.name}'...", + ) + # elif obj.type == "CAMERA": + # cameras_to_process.append(obj) + # cls._print_bpy( + # f"Info: Processing camera '{obj.name}'...", + # ) + # elif obj.type == "LIGHT": + # lights_to_process.append(obj) + # cls._print_bpy( + # f"Info: Processing light '{obj.name}'...", + # ) + else: + cls._print_bpy( + f"Warn: Blender object '{obj.name}' with type '{obj.type}' will " + "not be processed.", + file=sys.stderr, + ) + + # Determine name of the exported model + if len(meshes_to_process) == 0: + raise EnvironmentError("Blender scene has no mesh models to export.") + elif len(meshes_to_process) == 1: + # Use object name as the model name if there is only one + model_name = meshes_to_process[0].name + else: + # Use the project name as a joint model name if there are multiple meshes + model_name = bpy.path.basename(bpy.data.filepath).split(".")[0] + # Add prefix and suffix if desired + model_name = f"{model_name_prefix}{model_name}{model_name_suffix}" + + # If output directory is not set (None), use the default Fuel model path + if output_dir is None: + output_dir = path.join( + os.environ.get( + "GZ_FUEL_CACHE_PATH", + default=path.join(path.expanduser("~"), ".gazebo", "fuel"), + ), + "fuel.gazebosim.org", + os.getlogin(), + "models", + ) + # Fuel requires model versioning, therefore, default to the next available + # version if the model already exists (begins with version 1) + if model_version is None: + model_version = -1 + + # Get the absolute path and create the directory if it does not already exist + output_dir = path.abspath(output_dir) + os.makedirs(name=output_dir, exist_ok=True) + + # Get a versioned model path (if desired) + output_path = cls._try_get_versioned_model_path( + output_dir=output_dir, + model_name=model_name, + model_version=model_version, + ) + + # Store all data necessary for SDF creation inside a dictionary + sdf_data = {} + + # Process and export meshes + exported_meshes = cls._process_meshes( + meshes_to_process=meshes_to_process, + output_path=output_path, + model_name=model_name, + filetype_visual=filetype_visual, + filetype_collision=filetype_collision, + detail_level_visual=detail_level_visual, + detail_level_collision=detail_level_collision, + ignore_objects_visual=ignore_objects_visual, + ignore_objects_collision=ignore_objects_collision, + ) + sdf_data.update(exported_meshes) + + # Sample textures + texture_source = ( + texture_source_mode + if isinstance(texture_source_mode, cls.TextureSource) + else cls.TextureSource.from_str(texture_source_mode) + ) + if texture_source.is_enabled(): + textures_dirpath = texture_source.get_path(texture_source_value) + textures = cls._sample_textures(textures_dirpath=textures_dirpath) + sdf_data.update(textures) + + # Estimate inertial properties (if enabled) + if not static: + inertial_estimator = ( + inertial_estimation_mode + if isinstance(inertial_estimation_mode, cls.InertialEstimator) + else cls.InertialEstimator.from_str(inertial_estimation_mode) + ) + if inertial_estimator.is_enabled(): + analysis_mesh_filepath = path.join( + output_path, + exported_meshes["filepath_mesh_collision"] + if inertial_estimation_use_collision_mesh + else exported_meshes["filepath_mesh_visual"], + ) + inertial_properties = inertial_estimator.estimate_inertial_properties( + analysed_mesh_filepath=analysis_mesh_filepath, + value=inertial_estimation_value, + ) + sdf_data.update(inertial_properties) + + # Write data into an SDF file + cls._generate_sdf_file( + output_path=output_path, + model_name=model_name, + static=static, + friction_coefficient=friction_coefficient, + symlink_external_textures=symlink_external_textures, + material_texture_diffuse=material_texture_diffuse, + material_texture_specular=material_texture_specular, + **sdf_data, + ) + + # Create a corresponding config file for the SDF model + cls._generate_model_config_file(output_path=output_path, model_name=model_name) + + # Render few images to generate thumbnails + if not bpy.app.background and generate_thumbnails: + cls._generate_thumbnails(output_path=output_path) + + cls._print_bpy( + f"Info: Model '{model_name}' exported to 'file://{output_path}'." + ) + + # Select the original objects again to keep the UI (almost) the same + bpy.ops.object.select_all(action="DESELECT") + # Select all desired meshes at the same time + for obj in selected_objects_original: + obj.select_set(True) + + def _print_bpy(msg: Any, file: Optional[TextIO] = sys.stdout, *args, **kwargs): + """ + Helper print function that also provides output inside the Blender console, + in addition to the system console. + """ + + print(msg, file=file, *args, **kwargs) + for window in bpy.context.window_manager.windows: + for area in window.screen.areas: + if area.type == "CONSOLE": + with bpy.context.temp_override( + window=window, screen=window.screen, area=area + ): + bpy.ops.console.scrollback_append( + text=str(msg), + type="ERROR" if file == sys.stderr else "OUTPUT", + ) + + @classmethod + def _process_meshes( + cls, + meshes_to_process: List, + output_path: str, + model_name: str, + filetype_visual: Union[ExportFormat, str], + filetype_collision: Union[ExportFormat, str], + detail_level_visual: int, + detail_level_collision: int, + ignore_objects_visual: List[str], + ignore_objects_collision: List[str], + ) -> Dict[str, str]: + """ + Process and export meshes of the model. + """ + + # Convert to Enum if strings were passed + if isinstance(filetype_visual, str): + filetype_visual = cls.ExportFormat.from_str(filetype_visual) + if isinstance(filetype_collision, str): + filetype_collision = cls.ExportFormat.from_str(filetype_collision) + + # Keep only object names that need processing in the ignore list + meshes_to_process_names = [mesh.name for mesh in meshes_to_process] + ignore_objects_visual = [ + name for name in ignore_objects_visual if name in meshes_to_process_names + ] + ignore_objects_collision = [ + name for name in ignore_objects_collision if name in meshes_to_process_names + ] + + # Deselect all objects + bpy.ops.object.select_all(action="DESELECT") + # Select all desired meshes at the same time + for obj in meshes_to_process: + obj.select_set(True) + + return cls._export_geometry( + output_path=output_path, + model_name=model_name, + filetype_visual=filetype_visual, + filetype_collision=filetype_collision, + detail_level_visual=detail_level_visual, + detail_level_collision=detail_level_collision, + ignore_objects_visual=ignore_objects_visual, + ignore_objects_collision=ignore_objects_collision, + ) + + @classmethod + def _export_geometry( + cls, + output_path: str, + model_name: str, + filetype_visual: ExportFormat, + filetype_collision: ExportFormat, + detail_level_visual: int, + detail_level_collision: int, + ignore_objects_visual: List[str], + ignore_objects_collision: List[str], + ) -> Dict[str, str]: + """ + Export both visual and collision mesh geometry. + """ + + filepath_collision = cls._export_geometry_collision( + output_path=output_path, + model_name=model_name, + filetype=filetype_collision, + detail_level=detail_level_collision, + ignore_object_names=ignore_objects_collision, + ) + filepath_visual = cls._export_geometry_visual( + output_path=output_path, + model_name=model_name, + filetype=filetype_visual, + detail_level=detail_level_visual, + ignore_object_names=ignore_objects_visual, + ) + + return { + "filepath_mesh_collision": path.relpath( + path=filepath_collision, start=output_path + ), + "filepath_mesh_visual": path.relpath( + path=filepath_visual, start=output_path + ), + } + + @classmethod + def _export_geometry_collision( + cls, + output_path: str, + model_name: str, + filetype: ExportFormat, + detail_level: int, + ignore_object_names: List[str], + ) -> str: + """ + Export collision geometry of the model with the specified `filetype`. + Method `_pre_export_geometry_collision()` is called before the export. + Method `_post_export_geometry_collision()` is called after the export. + """ + + # Hook call before export of collision geometry + cls._pre_export_geometry_collision( + detail_level=detail_level, ignore_object_names=ignore_object_names + ) + + resulting_output_path = filetype.export( + path.join(output_path, cls.DIRNAME_MESHES_COLLISION, model_name) + ) + + # Hook call after export of collision geometry + cls._post_export_geometry_collision(ignore_object_names=ignore_object_names) + + return resulting_output_path + + @classmethod + def _export_geometry_visual( + cls, + output_path: str, + model_name: str, + filetype: ExportFormat, + detail_level: int, + ignore_object_names: List[str], + ) -> str: + """ + Export visual geometry of the model with the specified `filetype`. + Method `_pre_export_geometry_visual()` is called before the export. + Method `_post_export_geometry_visual()` is called after the export. + """ + + # Hook call before export of visual geometry + cls._pre_export_geometry_visual( + detail_level=detail_level, + ignore_object_names=ignore_object_names, + ) + + resulting_output_path = filetype.export( + path.join(output_path, cls.DIRNAME_MESHES_VISUAL, model_name) + ) + + # Hook call after export of visual geometry + cls._post_export_geometry_collision(ignore_object_names=ignore_object_names) + + return resulting_output_path + + @classmethod + def _sample_textures( + cls, textures_dirpath: Optional[str] + ) -> Dict[str, Optional[str]]: + """ + Get PBR textures for the exported model from `textures_dirpath`. If the + directory contains multiple texture sets, it is selected at random. + """ + + # Do not process if the texture directory is None + if not textures_dirpath: + return {} + + # Get the content of the texture directory + textures = os.listdir(textures_dirpath) + + # Determine whether the directory contains multiple sets of textures + choose_sample_random: bool = False + for texture in textures: + if path.isdir(path.join(textures_dirpath, texture)): + choose_sample_random = True + break + + if choose_sample_random: + # Select a random set of PBR textures + texture_dirpath = path.join(textures_dirpath, random.choice(textures)) + else: + # The directory already points to a set of PBR textures + texture_dirpath = textures_dirpath + + # Get the appropriate texture files + texture_albedo: Optional[str] = None + texture_roughness: Optional[str] = None + texture_metalness: Optional[str] = None + texture_normal: Optional[str] = None + for texture in os.listdir(texture_dirpath): + texture_cmp = cls._unify_string(texture) + if not texture_albedo and ( + "color" in texture_cmp or "albedo" in texture_cmp + ): + texture_albedo = path.join(texture_dirpath, texture) + elif not texture_roughness and "roughness" in texture_cmp: + texture_roughness = path.join(texture_dirpath, texture) + elif not texture_metalness and ( + "specular" in texture_cmp or "metalness" in texture_cmp + ): + texture_metalness = path.join(texture_dirpath, texture) + elif not texture_normal and "normal" in texture_cmp: + texture_normal = path.join(texture_dirpath, texture) + else: + cls._print_bpy( + f"Warn: File 'file://{path.join(texture_dirpath, texture)}' is " + "not a recognized texture type or there are multiple textures " + "of the same type inside 'file://{texture_dirpath}'.", + file=sys.stderr, + ) + + return { + "texture_albedo": texture_albedo, + "texture_roughness": texture_roughness, + "texture_metalness": texture_metalness, + "texture_normal": texture_normal, + } + + @classmethod + def _generate_sdf_file( + cls, + output_path: str, + model_name: str, + filepath_mesh_visual: str, + filepath_mesh_collision: str, + texture_albedo: Optional[str] = None, + texture_roughness: Optional[str] = None, + texture_metalness: Optional[str] = None, + texture_normal: Optional[str] = None, + material_texture_diffuse: Optional[ + Tuple[float, float, float] + ] = MATERIAL_TEXTURE_DIFFUSE, + material_texture_specular: Optional[ + Tuple[float, float, float] + ] = MATERIAL_TEXTURE_SPECULAR, + symlink_external_textures: bool = SYMLINK_EXTERNAL_TEXTURES, + static: bool = STATIC, + mass: Optional[float] = None, + inertia: Optional[ + Tuple[ + Tuple[float, float, float], + Tuple[float, float, float], + Tuple[float, float, float], + ] + ] = None, + centre_of_mass: Optional[Tuple[float, float, float]] = None, + friction_coefficient: Optional[ + Union[float, Tuple[float, float]] + ] = FRICTION_COEFFICIENT, + ): + """ + Generate SDF file from passed arguments and export to `output_path`. + """ + + # Initialize SDF with a single model and a link + sdf = ElementTree.Element("sdf", attrib={"version": cls.SDF_VERSION}) + model = ElementTree.SubElement(sdf, "model", attrib={"name": model_name}) + statit_xml = ElementTree.SubElement(model, "static") + statit_xml.text = str(static) + link = ElementTree.SubElement( + model, "link", attrib={"name": f"{model_name}_link"} + ) + + # Visual geometry + visual = ElementTree.SubElement( + link, "visual", attrib={"name": f"{model_name}_visual"} + ) + visual_geometry = ElementTree.SubElement(visual, "geometry") + visual_mesh = ElementTree.SubElement(visual_geometry, "mesh") + visual_mesh_uri = ElementTree.SubElement(visual_mesh, "uri") + visual_mesh_uri.text = filepath_mesh_visual + # Material + textures = ( + texture_albedo, + texture_roughness, + texture_metalness, + texture_normal, + ) + if any(texture for texture in textures): + material = ElementTree.SubElement(visual, "material") + pbr = ElementTree.SubElement(material, "pbr") + metal = ElementTree.SubElement(pbr, "metal") + + texture_albedo, texture_roughness, texture_metalness, texture_normal = ( + cls._preprocess_texture_path( + texture, + output_path=output_path, + symlink_external_textures=symlink_external_textures, + ) + for texture in textures + ) + + if texture_albedo: + diffuse = ElementTree.SubElement(material, "diffuse") + if material_texture_diffuse: + diffuse.text = " ".join(map(str, material_texture_diffuse)) + specular = ElementTree.SubElement(material, "specular") + if material_texture_specular: + specular.text = " ".join(map(str, material_texture_specular)) + albedo_map = ElementTree.SubElement(metal, "albedo_map") + albedo_map.text = texture_albedo + if texture_roughness: + roughness_map = ElementTree.SubElement(metal, "roughness_map") + roughness_map.text = texture_roughness + if texture_metalness: + metalness_map = ElementTree.SubElement(metal, "metalness_map") + metalness_map.text = texture_metalness + if texture_normal: + normal_map = ElementTree.SubElement(metal, "normal_map") + normal_map.text = texture_normal + + # Collision geometry + collision = ElementTree.SubElement( + link, "collision", attrib={"name": f"{model_name}_collision"} + ) + collision_geometry = ElementTree.SubElement(collision, "geometry") + collision_mesh = ElementTree.SubElement(collision_geometry, "mesh") + collision_mesh_uri = ElementTree.SubElement(collision_mesh, "uri") + collision_mesh_uri.text = filepath_mesh_collision + # Surface friction + surface = ElementTree.SubElement(collision, "surface") + surface_friction = ElementTree.SubElement(surface, "friction") + ode_friction = ElementTree.SubElement(surface_friction, "ode") + ode_friction_mu = ElementTree.SubElement(ode_friction, "mu") + ode_friction_mu2 = ElementTree.SubElement(ode_friction, "mu2") + if isinstance(friction_coefficient, Iterable): + if len(friction_coefficient) == 2: + # Randomize friction coefficient if a range is passed + friction_coefficient = random.uniform( + friction_coefficient[0], friction_coefficient[1] + ) + else: + friction_coefficient = friction_coefficient[0] + + ode_friction_mu.text = ode_friction_mu2.text = str(friction_coefficient) + bullet_friction = ElementTree.SubElement(surface_friction, "bullet") + bullet_friction_coef = ElementTree.SubElement(bullet_friction, "friction") + bullet_friction_coef2 = ElementTree.SubElement(bullet_friction, "friction2") + bullet_friction_coef.text = bullet_friction_coef2.text = str( + friction_coefficient + ) + + # Inertial + if not static and ( + centre_of_mass is not None and mass is not None and inertia is not None + ): + inertial = ElementTree.SubElement(link, "inertial") + pose = ElementTree.SubElement(inertial, "pose") + pose.text = f"{' '.join(map(str, centre_of_mass))} 0.0 0.0 0.0" + mass_xml = ElementTree.SubElement(inertial, "mass") + mass_xml.text = str(mass) + inertia_xml = ElementTree.SubElement(inertial, "inertia") + inertia_xx = ElementTree.SubElement(inertia_xml, "ixx") + inertia_xx.text = str(inertia[0][0]) + inertia_xy = ElementTree.SubElement(inertia_xml, "ixy") + inertia_xy.text = str(inertia[0][1]) + inertia_xz = ElementTree.SubElement(inertia_xml, "ixz") + inertia_xz.text = str(inertia[0][2]) + inertia_yy = ElementTree.SubElement(inertia_xml, "iyy") + inertia_yy.text = str(inertia[1][1]) + inertia_yz = ElementTree.SubElement(inertia_xml, "iyz") + inertia_yz.text = str(inertia[1][2]) + inertia_zz = ElementTree.SubElement(inertia_xml, "izz") + inertia_zz.text = str(inertia[2][2]) + + # Convert SDF to xml string and write to file + sdf_xml_string = minidom.parseString( + ElementTree.tostring(sdf, encoding="unicode") + ).toprettyxml(indent=" ") + sdf_file = open(path.join(output_path, cls.BASENAME_SDF), "w") + sdf_file.write(sdf_xml_string) + sdf_file.close() + + @classmethod + def _generate_model_config_file( + cls, + output_path: str, + model_name: str, + ): + """ + Generate model config for the SDF model. + """ + + # Initialize model config with its corresponding name + model_config = ElementTree.Element("model") + name = ElementTree.SubElement(model_config, "name") + name.text = model_name + + # Version of the model (try to match version from the exported path) + maybe_version = path.basename(output_path) + if maybe_version.isnumeric(): + version = ElementTree.SubElement(model_config, "version") + version.text = maybe_version + + # Path to SDF + sdf_tag = ElementTree.SubElement( + model_config, "sdf", attrib={"version": cls.SDF_VERSION} + ) + sdf_tag.text = cls.BASENAME_SDF + + # Author name is based on ${USER} + author = ElementTree.SubElement(model_config, "author") + name = ElementTree.SubElement(author, "name") + name.text = os.getlogin() + # Keep track of Blender version that produced the model + producer = ElementTree.SubElement(author, "producer") + producer.text = f"Blender {bpy.app.version_string}" + + # Describe how the model was generated + description = ElementTree.SubElement(model_config, "description") + description.text = ( + f"Model generated from '{bpy.path.basename(bpy.data.filepath)}' by " + f"'{path.relpath(path=__file__, start=bpy.data.filepath)}' Python script" + ) + + # Convert config to xml string and write to file + model_config_xml_string = minidom.parseString( + ElementTree.tostring(model_config, encoding="unicode") + ).toprettyxml(indent=" ") + model_config_file = open( + path.join(output_path, cls.BASENAME_SDF_MODEL_CONFIG), "w" + ) + model_config_file.write(model_config_xml_string) + model_config_file.close() + + @classmethod + def _generate_thumbnails(cls, output_path: str): + """ + Render thumbnails for the SDF model. + Currently, only a single viewport render is created using OpenGL. + """ + + # Create thumbnails directory for the model + thumbnails_dir = path.join(output_path, cls.DIRNAME_THUMBNAILS) + os.makedirs(name=thumbnails_dir, exist_ok=True) + + # Specify path for the thumbnail + bpy.context.scene.render.filepath = path.join(thumbnails_dir, "0") + + # Set render parameters + bpy.context.scene.render.resolution_x = 256 + bpy.context.scene.render.resolution_y = 256 + bpy.context.scene.render.pixel_aspect_x = 1.0 + bpy.context.scene.render.pixel_aspect_x = 1.0 + + # Render image through viewport + bpy.ops.render.opengl(write_still=True) + + @classmethod + def _pre_export_geometry_collision( + cls, detail_level: int, ignore_object_names: List[str] = [] + ): + """ + A hook that is called before exporting collision geometry. Always chain up the + parent implementation. + By default, this hook handles reselecting objects from `ignore_object_names`. + """ + + cls._select_objects(names=ignore_object_names, type_filter="MESH", select=False) + + @classmethod + def _post_export_geometry_collision(cls, ignore_object_names: List[str] = []): + """ + A hook that is called after exporting collision geometry. Always chain up the + parent implementation. + By default, this hook handles deselecting objects from `ignore_object_names`. + """ + + cls._select_objects(names=ignore_object_names, type_filter="MESH", select=True) + + @classmethod + def _pre_export_geometry_visual( + cls, + detail_level: int, + ignore_object_names: List[str] = [], + ): + """ + A hook that is called before exporting visual geometry. Always chain up the + parent implementation. + By default, this hook handles reselecting objects from `ignore_object_names`. + """ + + cls._select_objects(names=ignore_object_names, type_filter="MESH", select=False) + + @classmethod + def _post_export_geometry_visual(cls, ignore_object_names: List[str] = []): + """ + A hook that is called after exporting visual geometry. Always chain up the + parent implementation. + By default, this hook handles deselecting objects from `ignore_object_names`. + """ + + cls._select_objects(names=ignore_object_names, type_filter="MESH", select=True) + + def _select_objects( + names=List[str], type_filter: Optional[str] = None, select: bool = True + ): + """ + (De)select list of objects based on their `names`. List can be filtered + according to the object type via `type_filter`. + """ + + for obj in bpy.context.selectable_objects: + if type_filter and obj.type != type_filter: + continue + if obj.name in names: + obj.select_set(select) + + def _unify_string( + string: str, + chars_to_remove: Union[str, Iterable[str]] = ( + " ", + "_", + "-", + "(", + ")", + "[", + "]", + "{", + "}", + ), + lowercase: bool = True, + ) -> str: + """ + Helper function unifies string for a more robust matching. The strings is + changed to lower-case and `chars_to_remove` are removed. + """ + + string = string.strip() + for char in chars_to_remove: + string = string.replace(char, "") + if lowercase: + return string.lower() + else: + return string.upper() + + @classmethod + def _preprocess_texture_path( + cls, + texture_original_filepath: Optional[str], + output_path: str, + symlink_external_textures: bool, + ): + """ + Preprocess filepath of a texture such that it is in the local model directory + path. If `symlink_external_textures` is enabled, symbolic links will be + created. No copy or symlink will be made if the `texture_original_filepath` is + already a subpath of `output_path`. + This can fail due to the lack of filesystem permissions. + """ + + # Skip processing of unset textures + if not texture_original_filepath: + return None + + # Make sure the texture is valid + if not path.exists(texture_original_filepath): + raise ValueError( + f"Texture 'file://{texture_original_filepath}' does not exist!" + ) + + # Copy over the texture inside the model directory, or create a symbolic link + # Only do this if the texture is not already under the export directory + if texture_original_filepath.startswith(output_path): + texture_target_filepath = texture_original_filepath + else: + texture_dir = path.join(output_path, cls.DIRNAME_TEXTURES) + texture_target_filepath = path.join( + texture_dir, path.basename(texture_original_filepath) + ) + os.makedirs(name=texture_dir, exist_ok=True) + if symlink_external_textures: + try: + os.symlink( + src=texture_original_filepath, dst=texture_target_filepath + ) + except OSError as err: + import errno + + if err.errno == errno.EEXIST: + os.remove(texture_target_filepath) + os.symlink( + src=texture_original_filepath, dst=texture_target_filepath + ) + else: + raise err + else: + shutil.copy(src=texture_original_filepath, dst=texture_target_filepath) + + return path.relpath(path=texture_target_filepath, start=output_path) + + @classmethod + def _try_get_versioned_model_path( + cls, output_dir: str, model_name: str, model_version: Optional[int] + ) -> str: + """ + Return versioned model directory path if `model_version` is specified. For + negative `model_version` and an existing model directory, the next version will + be used to avoid overwriting. + """ + + unversioned_model_path = path.join(output_dir, model_name) + + if model_version is None: + return unversioned_model_path + elif model_version < 0: + return path.join( + unversioned_model_path, + str(cls._get_next_model_version(model_path=unversioned_model_path)), + ) + else: + return path.join(unversioned_model_path, model_version) + + def _get_next_model_version(model_path: str) -> int: + """ + Return the next version if model already exists. Otherwise, return '1' as the + initial (first) version. + """ + + if path.exists(model_path): + last_version = max( + [ + int(path.basename(subdir)) + for subdir in os.scandir(model_path) + if subdir.is_dir() and path.basename(subdir).isnumeric() + ] + ) + return last_version + 1 + else: + return 1 + + +class procedural_dataset_generator(sdf_model_exporter): + """ + Generator of procedural datasets using Blender's Geometry Nodes. + """ + + # The following lookup phrases are used to find the corresponding input attributes + # of the node system (exact match, insensitive to case, insensitive to '_'/'-'/...) + LOOKUP_PHRASES_RANDOM_SEED: List[str] = [ + "rng", + "seed", + "randomseed", + "pseodorandomseed", + ] + LOOKUP_PHRASES_DETAIL_LEVEL: List[str] = [ + "detail", + "detaillevel", + "detailobject", + "levelofdetail", + "subdivisionlevel", + "subdivlevel", + ] + + @classmethod + def generate( + cls, + first_seed: int = FIRST_SEED, + number_of_variants: int = NUMBER_OF_VARIANTS, + redraw_viewport_during_processing: bool = True, + **kwargs, + ): + """ + Generate `number_of_variants` different models by changing the random seed in + the Geometry Nodes system of the individual meshes. + """ + + cls._print_bpy( + f"Info: Generating {number_of_variants} model variants in the seed range " + f"[{first_seed}:{first_seed + number_of_variants}]." + ) + + # Export models for the entire range of random seeds + global current_seed + for seed in range(first_seed, first_seed + number_of_variants): + current_seed = seed + random.seed(seed) + name_suffix = str(seed) + if "model_name_suffix" in kwargs: + name_suffix = f'{kwargs.pop("model_name_suffix")}{name_suffix}' + cls.export(model_name_suffix=name_suffix, **kwargs) + + # Update the viewport to keep track of progress (only if GUI is enabled) + # Performance might be lowered because the scene needs to be re-rendered + if not bpy.app.background and redraw_viewport_during_processing: + bpy.ops.wm.redraw_timer(type="DRAW_WIN_SWAP", iterations=1) + + @classmethod + def _pre_export_geometry_collision( + cls, detail_level: int, ignore_object_names: List[str] = [] + ): + """ + A hook that is called before exporting collision geometry. This implementation + adjusts input attributes of the Geometry Nodes system for each mesh. + """ + + # Call parent impl + sdf_model_exporter._pre_export_geometry_collision( + detail_level=detail_level, + ignore_object_names=ignore_object_names, + ) + + global current_seed + selected_meshes = bpy.context.selected_objects + for obj in selected_meshes: + for nodes_modifier in cls._get_all_nodes_modifiers(obj): + cls._try_set_nodes_input_attribute( + obj, + nodes_modifier, + cls.LOOKUP_PHRASES_RANDOM_SEED, + current_seed, + print_warning=True, + ) + cls._try_set_nodes_input_attribute( + obj, + nodes_modifier, + cls.LOOKUP_PHRASES_DETAIL_LEVEL, + detail_level, + ) + cls._trigger_modifier_update(obj) + + @classmethod + def _pre_export_geometry_visual( + cls, + detail_level: int, + ignore_object_names: List[str] = [], + ): + """ + A hook that is called before exporting visual geometry. This implementation + adjusts input attributes of the Geometry Nodes system for each mesh. + """ + + # Call parent impl + sdf_model_exporter._pre_export_geometry_visual( + detail_level=detail_level, + ignore_object_names=ignore_object_names, + ) + + global current_seed + selected_meshes = bpy.context.selected_objects + for obj in selected_meshes: + for nodes_modifier in cls._get_all_nodes_modifiers(obj): + cls._try_set_nodes_input_attribute( + obj, + nodes_modifier, + cls.LOOKUP_PHRASES_RANDOM_SEED, + current_seed, + print_warning=True, + ) + cls._try_set_nodes_input_attribute( + obj, + nodes_modifier, + cls.LOOKUP_PHRASES_DETAIL_LEVEL, + detail_level, + ) + + cls._trigger_modifier_update(obj) + + def _get_all_nodes_modifiers(obj: bpy.types.Object) -> List[bpy.types.Modifier]: + """ + Return all node-based modifiers of an object. + """ + + return [modifier for modifier in obj.modifiers if modifier.type == "NODES"] + + @classmethod + def _try_set_nodes_input_attribute( + cls, + obj: bpy.types.Object, + modifier: bpy.types.NodesModifier, + lookup_phrases: Iterable[str], + value: Any, + print_warning: bool = False, + ): + """ + Try to set an input attribute of a nodes system to a `value`. The attribute + looked is performed by using `lookup_phrases`. + """ + + # Try to find the corresponding ID of the input attribute + input_id: Optional[str] = None + for attribute in modifier.node_group.inputs: + if cls._unify_string(attribute.name) in lookup_phrases: + input_id = attribute.identifier + break + if input_id is None: + if print_warning: + cls._print_bpy( + "Warn: Unable to find a matching input attribute of the object's " + f"'{obj.name}' modifier '{modifier.name}' for any of the requested " + f"lookup phrases {lookup_phrases}. You can ignore this warning if " + "the modifier is not supposed to have the requested input.", + file=sys.stderr, + ) + return + + # Set the attribute + modifier[input_id] = value + + def _trigger_modifier_update(obj: bpy.types.Object): + """ + Trigger an update of object's modifiers after changing its attributes. + """ + + # Not sure how else to trigger update of the modifiers, but setting the index + # of any modifier does the trick (even if the index stays the same) + # TODO[enhancement]: Improve updating of modifiers after programmatic changes + bpy.context.view_layer.objects.active = obj + if len(obj.modifiers.values()): + bpy.ops.object.modifier_move_to_index( + modifier=obj.modifiers.values()[0].name, + index=0, + ) + + +def main(**kwargs): + + # Warn the user in case an untested version of Blender is used + if bpy.app.version[0] != LAST_WORKING_VERSION[0]: + sdf_model_exporter._print_bpy( + f"Err: Untested major version of Blender ({bpy.app.version_string})! " + "This script will likely fail. Please, use Blender version " + f"[~{LAST_WORKING_VERSION[0]}.{LAST_WORKING_VERSION[1]}].", + file=sys.stderr, + ) + elif bpy.app.version[1] < LAST_WORKING_VERSION[1]: + sdf_model_exporter._print_bpy( + f"Warn: Untested minor version of Blender ({bpy.app.version_string})! " + "This script might not work as intended. Please, consider using Blender " + f"version [~{LAST_WORKING_VERSION[0]}.{LAST_WORKING_VERSION[1]}].", + file=sys.stderr, + ) + + # Update default keyword arguments with parsed arguments + export_kwargs = DEFAULT_KWARGS + export_kwargs.update(kwargs) + + if export_kwargs.pop("export_dataset"): + # Generate a dataset of procedural models + procedural_dataset_generator.generate(**export_kwargs) + sdf_model_exporter._print_bpy( + f'Info: A dataset with {export_kwargs["number_of_variants"]} models was ' + "exported." + ) + else: + # Export a single SDF model + sdf_model_exporter.export(**export_kwargs) + sdf_model_exporter._print_bpy("Info: A single SDF models was exported.") + + # Inform user how to make the models discoverable by Gazebo + if export_kwargs["output_dir"] is not None: + output_dir = path.abspath(export_kwargs["output_dir"]) + sdf_model_exporter._print_bpy( + "Info: Please, set GZ_SIM_RESOURCE_PATH environment variable to make the " + "exported models discoverable." + f'\n\texport GZ_SIM_RESOURCE_PATH="{output_dir}' + '${GZ_SIM_RESOURCE_PATH:+:${GZ_SIM_RESOURCE_PATH}}"', + ) + + +if __name__ == "__main__": + + # Setup argument parser + parser = argparse.ArgumentParser( + description="Generate a procedural dataset of SDF models using Blender.", + usage=( + f"\n\t{sys.argv[0]} [blender options] file.blend --python-text " + "script.py -- [script options]" + f"\n\t{sys.argv[0]} [blender options] file.blend --python " + "external_script.py -- [script options]" + "\nlist script options:" + f"\n\t{sys.argv[0]} -b file.blend --python-text script.py -- -h" + f"\n\t{sys.argv[0]} -b file.blend --python external_script.py -- -h" + "\nlist blender options:" + f"\n\t{sys.argv[0]} -h" + ), + epilog=f"Default keyword arguments of the script (model): {DEFAULT_KWARGS}" + if DEFAULT_KWARGS + else "", + argument_default=argparse.SUPPRESS, + formatter_class=argparse.ArgumentDefaultsHelpFormatter, + ) + + # Append example to usage (with the specific internal/external script) + if "--python-text" in sys.argv and bpy.data.filepath: + blend_relpath = path.relpath(path=bpy.data.filepath, start=os.getcwd()) + py_relpath = path.relpath(path=__file__, start=bpy.data.filepath) + parser.usage += ( + f"\nexample:\n\t{sys.argv[0]} {blend_relpath} --python-text {py_relpath} " + f"-- -o {OUTPUT_DIR}" + ) + else: + py_relpath = path.relpath(path=__file__, start=os.getcwd()) + parser.usage += ( + f"\nexample:\n\t{sys.argv[0]} file.blend --python {py_relpath} " + f"-- -o {OUTPUT_DIR}" + ) + + # Exit if opened without a Blender project + if not bpy.data.filepath: + print( + "Err: Blender project was not opened before running the script.", + file=sys.stderr, + ) + parser.print_usage() + + sys.exit(22) + + ### Specify all CLI arguments + ## Helper argparse types + def str2bool(value: Union[str, bool]) -> bool: + """ + Convert logical string to boolean. Can be used as argparse type. + """ + + if isinstance(value, bool): + return value + if value.lower() in ("yes", "true", "t", "y", "1"): + return True + elif value.lower() in ("no", "false", "f", "n", "0"): + return False + else: + raise argparse.ArgumentTypeError("Boolean value expected.") + + def str_empty2none(value: Optional[str]) -> Optional[str]: + """ + If string is empty, convert to None. Can be used as argparse type. + """ + + return str(value) if value else None + + ## Command mode of the script + parser.add_argument( + "--export-dataset", + action="store_true", + default=True, + help="[default mode] Generate a procedural dataset.", + ) + parser.add_argument( + "--export-model", + dest="export_dataset", + action="store_false", + help="[alternative mode] Export a single model.", + ) + ## Parameters for SDF model exporter `sdf_model_exporter` + parser.add_argument( + "-o", + "--output-dir", + type=str_empty2none, + default=OUTPUT_DIR, + help="The output directory for all models (each model is located under its own " + "subdirectory). When exporting a single SDF model, its output path is " + "'OUTPUT_DIR/MODEL_NAME'. For datasets, the path of each SDF model name is " + "appended by the random seed used during its generation as " + "'OUTPUT_DIR/MODEL_NAME+VARIANT_SEED'. If set to empty, Fuel cache " + "'${GZ_FUEL_CACHE_PATH}/fuel.gazebosim.org/${USER}/models' is used.", + ) + parser.add_argument( + "-v", + "--model-version", + type=int, + default=MODEL_VERSION, + help="The version of exported model path that is joined with the model output " + "path 'OUTPUT_DIR/MODEL_NAME/MODEL_VERSION', or " + "'OUTPUT_DIR/MODEL_NAME+VARIANT_SEED/MODEL_VERSION'. The output path is " + "modified only if 'MODEL_VERSION' is set (not None). For negative values " + "and an existing model directory, the next unique version is used to avoid " + "overwriting. If 'OUTPUT_DIR' is empty (export to Fuel cache), a negative " + "value is used to guarantee a unique version.", + ) + parser.add_argument( + "--model-name-prefix", + type=str, + help="Prefix of exported model as 'OUTPUT_DIR/MODEL_NAME_PREFIX+MODEL_NAME' " + ", or 'OUTPUT_DIR/MODEL_NAME_PREFIX+MODEL_NAME+VARIANT_SEED' (default: '').", + ) + parser.add_argument( + "--model-name-suffix", + type=str, + help="Suffix of exported model as 'OUTPUT_DIR/MODEL_NAME+MODEL_NAME_SUFFIX' " + ", or 'OUTPUT_DIR/MODEL_NAME+MODEL_NAME_SUFFIX+VARIANT_SEED' (default: '').", + ) + ## Parameters for procedural dataset generator `procedural_dataset_generator` + parser.add_argument( + "-s", + "--first-seed", + type=int, + default=FIRST_SEED, + help="The random seed of the first model when generating a dataset.", + ) + parser.add_argument( + "-n", + "--number-of-variants", + type=int, + default=NUMBER_OF_VARIANTS, + help="Number of model variants to export when generating a dataset.", + ) + ## Filetypes of exported geometry + parser.add_argument( + "--filetype-visual", + type=str, + default=FILETYPE_VISUAL, + choices=["collada", "dae", "wavefront", "obj", "stl"], + help="The format of exported visual geometry ['collada' == 'dae', " + "'wavefront' == 'obj'].", + ) + parser.add_argument( + "--filetype-collision", + type=str, + default=FILETYPE_COLLISION, + choices=["collada", "dae", "wavefront", "obj", "stl"], + help="The format of exported collision geometry ['collada' == 'dae', " + "'wavefront' == 'obj'].", + ) + ## Level of detail for exported visual/collision geometry, e.g. subdivision level + parser.add_argument( + "--detail-level-visual", + type=int, + default=DETAIL_LEVEL_VISUAL, + help="Level of detail for exported visual geometry, e.g. subdivision level.", + ) + parser.add_argument( + "--detail-level-collision", + type=int, + default=DETAIL_LEVEL_COLLISION, + help="Level of detail for exported collision geometry, e.g. subdivision level.", + ) + ## Objects to ignore while exporting visual/collision geometry, even if selected + parser.add_argument( + "--ignore-objects-visual", + type=str, + nargs="+", + default=IGNORE_OBJECTS_VISUAL, + help="List of objects to ignore when exporting visual geometry.", + ) + parser.add_argument( + "--ignore-objects-collision", + type=str, + nargs="+", + default=IGNORE_OBJECTS_COLLISION, + help="List of objects to ignore when exporting collision geometry.", + ) + ## Source of textures for the model + parser.add_argument( + "--symlink-external-textures", + type=str2bool, + default=SYMLINK_EXTERNAL_TEXTURES, + help="If true, symbolic links will be created for all textures instead of " + "copies. No copy or symlink will be if the texture is already under the output " + "path.", + ) + parser.add_argument( + "--texture-source-mode", + type=str, + choices=["none", "path", "online", "blender"], + default=TEXTURE_SOURCE_MODE, + help="The source from which to select/extract (PBR) textures for the model. " + "Option 'none' disables texturing in SDF and relies on mesh exporters. " + "Option 'path' should either point to a single set of textures or a number of " + "texture sets, from which a set would be sampled at random. " + "Options 'online' (textures from an online source) and 'blender' (baking of " + "textures) are currently not implemented. " + "The value must be specified using the 'TEXTURE_SOURCE_VALUE' option.", + ) + parser.add_argument( + "--texture-source-value", + type=str_empty2none, + default=TEXTURE_SOURCE_VALUE, + help="Value for the texture source, with the context based on the selected " + "'TEXTURE_SOURCE_MODE'. For example, this value expresses path to a directory " + "with textures or a name of the environment.", + ) + parser.add_argument( + "--material-texture-diffuse", + type=float, + nargs="+", + default=MATERIAL_TEXTURE_DIFFUSE, + help="Diffuse intensity of the albedo texture map. " + "Please, enter values for each channel as `--material-texture-diffuse R G B`.", + ) + parser.add_argument( + "--material-texture-specular", + type=float, + nargs="+", + default=MATERIAL_TEXTURE_SPECULAR, + help="Specular intensity of the albedo texture map. " + "Please, enter values for each channel as `--material-texture-specular R G B`.", + ) + ## Inertial and dynamic properties of exported models + parser.add_argument( + "--static", + type=str2bool, + default=STATIC, + help="If true, the SDF model is exported as immovable and it won't be updated " + "by the physics engine.", + ) + parser.add_argument( + "--inertial-estimation-mode", + type=str, + choices=["none", "density", "random_density", "mass", "random_mass"], + default=INERTIAL_ESTIMATION_MODE, + help="The mode used during the estimation of inertial properties. " + "Option 'none' disables estimation of inertial properties. " + "Option '[random_]density' assumes a uniform density of the model. " + "Option '[random_]mass' determines a uniform density based on the target mass. " + "Random options uniformly sample the target value from a specified range. " + "The value must be specified using the 'INERTIAL_ESTIMATION_VALUE' option. " + "Estimation of inertial properties is always disabled for 'STATIC' models.", + ) + parser.add_argument( + "--inertial-estimation-value", + type=float, + nargs="+", + default=INERTIAL_ESTIMATION_VALUE, + help="Value for the inertial estimation, with the context based on the " + "selected 'INERTIAL_ESTIMATION_MODE'. " + "For non-random modes, please use a single value as " + "`--inertial-estimation-value TARGER_VALUE`. " + "For a random range, please enter min and max values as " + "`--inertial-estimation-value MIN MAX`.", + ) + parser.add_argument( + "--inertial-estimation-use-collision-mesh", + type=str2bool, + help="If true, collision geometry will be used for the estimation of inertial " + "properties instead of the visual geometry of the model. (default: True)", + ) + parser.add_argument( + "--friction-coefficient", + type=float, + nargs="+", + default=FRICTION_COEFFICIENT, + help="Coefficient of the surface friction, equal in both directions. " + "For a random range, please enter min and max values as " + "`--friction-coefficient MIN MAX`.", + ) + ## Miscellaneous + parser.add_argument( + "--generate-thumbnails", + type=str2bool, + help="If true, thumbnails will be generated for the exported models. Only " + "applicable if Blender is run in the foreground without `-b`. (default: True)", + ) + + # Parse arguments + if "--" in sys.argv: + args = parser.parse_args(sys.argv[sys.argv.index("--") + 1 :]) + else: + args, unknown_args = parser.parse_known_args() + sdf_model_exporter._print_bpy( + f"Warn: The following arguments are not recognized by '{py_relpath}'!" + f"\n\t{unknown_args}\nHint: Please, delimit your arguments for Python " + "script with '--' (see usage).", + file=sys.stderr, + ) + + # Pass as keyword arguments to the main function + main(**vars(args)) diff --git a/examples/worlds/resource_spawner.sdf b/examples/worlds/resource_spawner.sdf new file mode 100644 index 0000000000..ec9eb90392 --- /dev/null +++ b/examples/worlds/resource_spawner.sdf @@ -0,0 +1,167 @@ + + + + + + 0.001 + 1.0 + + + + + ogre2 + + + + + + + + + 3D View + false + docked + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + World control + false + false + 72 + 121 + 1 + floating + + + + + + true + true + true + true + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + docked + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 20 20 0.1 + + + + + + + 20 20 0.1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index c2072441b0..2c7135ac0e 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -454,7 +454,24 @@ start falling. linear: {x: 3} - + + + + linear: {x: 0} + + + + + data: true diff --git a/include/gz/sim/SystemLoader.hh b/include/gz/sim/SystemLoader.hh index 9276b88e1b..05b280f904 100644 --- a/include/gz/sim/SystemLoader.hh +++ b/include/gz/sim/SystemLoader.hh @@ -17,6 +17,7 @@ #ifndef GZ_SIM_SYSTEMLOADER_HH_ #define GZ_SIM_SYSTEMLOADER_HH_ +#include #include #include #include @@ -79,6 +80,10 @@ namespace gz /// \returns A pretty string public: std::string PrettyStr() const; + /// \brief Get the plugin search paths used for loading system plugins + /// \return Paths to search for plugins + public: std::list PluginPaths() const; + /// \brief Pointer to private data. private: std::unique_ptr dataPtr; }; diff --git a/include/gz/sim/detail/EntityComponentManager.hh b/include/gz/sim/detail/EntityComponentManager.hh index 7eea6491ba..601458bf6f 100644 --- a/include/gz/sim/detail/EntityComponentManager.hh +++ b/include/gz/sim/detail/EntityComponentManager.hh @@ -52,11 +52,14 @@ namespace traits template struct HasEqualityOperator { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wnonnull" enum { // False positive codecheck "Using C-style cast" value = !std::is_same::value // NOLINT }; +#pragma GCC diagnostic pop }; } diff --git a/include/gz/sim/rendering/RenderUtil.hh b/include/gz/sim/rendering/RenderUtil.hh index 38133cc5ad..6015494f7d 100644 --- a/include/gz/sim/rendering/RenderUtil.hh +++ b/include/gz/sim/rendering/RenderUtil.hh @@ -201,6 +201,10 @@ inline namespace GZ_SIM_VERSION_NAMESPACE { /// \brief Clears the set of selected entities and lowlights all of them. public: void DeselectAllEntities(); + /// \brief Init render engine plugins paths. This lets gz-rendering know + /// paths to find render engine plugins + public: void InitRenderEnginePluginPaths(); + /// \brief Helper function to get all child links of a model entity. /// \param[in] _entity Entity to find child links /// \return Vector of child links found for the parent entity diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 28859bd44f..ab2ef24c4b 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -93,7 +93,6 @@ set (gtest_sources TestFixture_TEST.cc Util_TEST.cc World_TEST.cc - gz_TEST.cc comms/Broker_TEST.cc comms/MsgManager_TEST.cc network/NetworkConfig_TEST.cc @@ -101,14 +100,20 @@ set (gtest_sources network/NetworkManager_TEST.cc ) +# gz_TEST and ModelCommandAPI_TEST are not supported with multi config +# CMake generators, see also cmd/CMakeLists.txt +get_property(GENERATOR_IS_MULTI_CONFIG GLOBAL PROPERTY GENERATOR_IS_MULTI_CONFIG) +if(NOT GENERATOR_IS_MULTI_CONFIG) + list(APPEND gtest_sources gz_TEST.cc) +endif() + + # Tests that require a valid display set(tests_needing_display - ModelCommandAPI_TEST.cc ) -# Disable tests that need CLI if gz-tools is not found -if (MSVC OR NOT GZ_TOOLS_PROGRAM) - list(REMOVE_ITEM gtest_sources gz_TEST.cc ModelCommandAPI_TEST.cc) +if(NOT GENERATOR_IS_MULTI_CONFIG) + list(APPEND tests_needing_display ModelCommandAPI_TEST.cc) endif() # Add systems that need a valid display here. @@ -228,8 +233,19 @@ foreach(CMD_TEST set_tests_properties(${CMD_TEST} PROPERTIES ENVIRONMENT "${_env_vars}") + # On Windows there is no RPATH, so an alternative way for tests for finding .dll libraries + # in build directory in necessary. For regular tests, the trick is to place all libraries + # and executables in a common CMAKE_RUNTIME_OUTPUT_DIRECTORY, so that the .dll are found + # as they are in the same directory where the executable is loaded. For tests that are + # launched via Ruby, this does not work, so we need to manually add CMAKE_RUNTIME_OUTPUT_DIRECTORY + # to the PATH. This is done via the ENVIRONMENT_MODIFICATION that is only available + # since CMake 3.22. However, if an older CMake is used another trick to install the libraries + # beforehand + if (WIN32 AND CMAKE_VERSION STRGREATER "3.22") + set_tests_properties(${CMD_TEST} PROPERTIES + ENVIRONMENT_MODIFICATION "PATH=path_list_prepend:${CMAKE_RUNTIME_OUTPUT_DIRECTORY}") + endif() + endforeach() -if(NOT WIN32) - add_subdirectory(cmd) -endif() +add_subdirectory(cmd) diff --git a/src/Conversions.cc b/src/Conversions.cc index b90b47e600..edf049e4d7 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -815,6 +815,13 @@ msgs::Scene gz::sim::convert(const sdf::Scene &_in) skyMsg->set_mean_cloud_size(_in.Sky()->CloudMeanSize()); msgs::Set(skyMsg->mutable_cloud_ambient(), _in.Sky()->CloudAmbient()); + + if (!_in.Sky()->CubemapUri().empty()) + { + auto header = skyMsg->mutable_header()->add_data(); + header->set_key("cubemap_uri"); + header->add_value(_in.Sky()->CubemapUri()); + } } return out; @@ -845,6 +852,16 @@ sdf::Scene gz::sim::convert(const msgs::Scene &_in) sky.SetCloudHumidity(_in.sky().humidity()); sky.SetCloudMeanSize(_in.sky().mean_cloud_size()); sky.SetCloudAmbient(msgs::Convert(_in.sky().cloud_ambient())); + + for (int i = 0; i < _in.sky().header().data_size(); ++i) + { + auto data = _in.sky().header().data(i); + if (data.key() == "cubemap_uri" && data.value_size() > 0) + { + sky.SetCubemapUri(data.value(0)); + } + } + out.SetSky(sky); } return out; diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index c0934944e9..19f14bde38 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -718,6 +718,7 @@ TEST(Conversions, Scene) sky.SetCloudHumidity(0.11); sky.SetCloudMeanSize(0.88); sky.SetCloudAmbient(math::Color::Red); + sky.SetCubemapUri("test.dds"); scene.SetSky(sky); auto sceneSkyMsg = convert(scene); @@ -731,6 +732,10 @@ TEST(Conversions, Scene) EXPECT_DOUBLE_EQ(0.88, sceneSkyMsg.sky().mean_cloud_size()); EXPECT_EQ(math::Color::Red, msgs::Convert(sceneSkyMsg.sky().cloud_ambient())); + ASSERT_GT(sceneSkyMsg.sky().header().data_size(), 0); + auto header = sceneSkyMsg.sky().header().data(0); + EXPECT_EQ("cubemap_uri", header.key()); + EXPECT_EQ("test.dds", header.value(0)); auto newSceneSky = convert(sceneSkyMsg); ASSERT_NE(nullptr, newSceneSky.Sky()); @@ -742,6 +747,7 @@ TEST(Conversions, Scene) EXPECT_DOUBLE_EQ(0.11, newSceneSky.Sky()->CloudHumidity()); EXPECT_DOUBLE_EQ(0.88, newSceneSky.Sky()->CloudMeanSize()); EXPECT_EQ(math::Color::Red, newSceneSky.Sky()->CloudAmbient()); + EXPECT_EQ("test.dds", newSceneSky.Sky()->CubemapUri()); } ///////////////////////////////////////////////// diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index d5418728ca..8b1d0ac4ec 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -494,9 +494,9 @@ bool ServerPrivate::ResourcePathsResolveService( std::string req = _req.data(); // Handle the case where the request is already a valid path - if (common::exists(req)) + if (common::exists(common::absPath(req))) { - _res.set_data(req); + _res.set_data(common::absPath(req)); return true; } diff --git a/src/SystemLoader.cc b/src/SystemLoader.cc index 191beaf0b7..90d86748e2 100644 --- a/src/SystemLoader.cc +++ b/src/SystemLoader.cc @@ -41,6 +41,29 @@ class gz::sim::SystemLoaderPrivate ////////////////////////////////////////////////// public: explicit SystemLoaderPrivate() = default; + ////////////////////////////////////////////////// + public: std::list PluginPaths() const + { + common::SystemPaths systemPaths; + systemPaths.SetPluginPathEnv(pluginPathEnv); + + for (const std::string &path : this->systemPluginPaths) + systemPaths.AddPluginPaths(path); + + std::string homePath; + common::env(GZ_HOMEDIR, homePath); + systemPaths.AddPluginPaths(common::joinPaths( + homePath, ".gz", "sim", "plugins")); + systemPaths.AddPluginPaths(GZ_SIM_PLUGIN_INSTALL_DIR); + + // TODO(CH3): Deprecated. Remove on tock. + systemPaths.AddPluginPaths(homePath + "/.ignition/gazebo/plugins"); + + systemPaths.AddPluginPaths(GZ_SIM_PLUGIN_INSTALL_DIR); + + return systemPaths.PluginPaths(); + } + ////////////////////////////////////////////////// public: bool InstantiateSystemPlugin(const sdf::Plugin &_sdfPlugin, gz::plugin::PluginPtr &_gzPlugin) @@ -56,20 +79,12 @@ class gz::sim::SystemLoaderPrivate << "]. Using [" << filename << "] instead." << std::endl; } - gz::common::SystemPaths systemPaths; - systemPaths.SetPluginPathEnv(pluginPathEnv); - - for (const auto &path : this->systemPluginPaths) - systemPaths.AddPluginPaths(path); - - std::string homePath; - gz::common::env(GZ_HOMEDIR, homePath); - systemPaths.AddPluginPaths(homePath + "/.gz/sim/plugins"); - - // TODO(CH3): Deprecated. Remove on tock. - systemPaths.AddPluginPaths(homePath + "/.ignition/gazebo/plugins"); - - systemPaths.AddPluginPaths(GZ_SIM_PLUGIN_INSTALL_DIR); + std::list paths = this->PluginPaths(); + common::SystemPaths systemPaths; + for (const auto &p : paths) + { + systemPaths.AddPluginPaths(p); + } auto pathToLib = systemPaths.FindSharedLibrary(filename); if (pathToLib.empty()) @@ -139,7 +154,11 @@ class gz::sim::SystemLoaderPrivate return false; } - _gzPlugin = this->loader.Instantiate(_sdfPlugin.Name()); + // use the first plugin name in the library if not specified + std::string pluginToInstantiate = _sdfPlugin.Name().empty() ? + pluginName : _sdfPlugin.Name(); + + _gzPlugin = this->loader.Instantiate(pluginToInstantiate); if (!_gzPlugin) { std::stringstream ss; @@ -208,8 +227,6 @@ class gz::sim::SystemLoaderPrivate /// \brief Paths to search for system plugins. public: std::unordered_set systemPluginPaths; - - /// \brief System plugins that have instances loaded via the manager. }; ////////////////////////////////////////////////// @@ -221,6 +238,12 @@ SystemLoader::SystemLoader() ////////////////////////////////////////////////// SystemLoader::~SystemLoader() = default; +////////////////////////////////////////////////// +std::list SystemLoader::PluginPaths() const +{ + return this->dataPtr->PluginPaths(); +} + ////////////////////////////////////////////////// void SystemLoader::AddSystemPluginPath(const std::string &_path) { @@ -233,11 +256,10 @@ std::optional SystemLoader::LoadPlugin( const std::string &_name, const sdf::ElementPtr &_sdf) { - if (_filename == "" || _name == "") + if (_filename == "") { gzerr << "Failed to instantiate system plugin: empty argument " - "[(filename): " << _filename << "] " << - "[(name): " << _name << "]." << std::endl; + "[(filename): " << _filename << "] " << std::endl; return {}; } @@ -265,11 +287,10 @@ std::optional SystemLoader::LoadPlugin( std::optional SystemLoader::LoadPlugin( const sdf::Plugin &_plugin) { - if (_plugin.Filename() == "" || _plugin.Name() == "") + if (_plugin.Filename() == "") { gzerr << "Failed to instantiate system plugin: empty argument " - "[(filename): " << _plugin.Filename() << "] " << - "[(name): " << _plugin.Name() << "]." << std::endl; + "[(filename): " << _plugin.Filename() << "] " << std::endl; return {}; } diff --git a/src/SystemLoader_TEST.cc b/src/SystemLoader_TEST.cc index d603f86299..b654924f34 100644 --- a/src/SystemLoader_TEST.cc +++ b/src/SystemLoader_TEST.cc @@ -27,6 +27,7 @@ #include "test_config.hh" // NOLINT(build/include) using namespace gz; +using namespace sim; ///////////////////////////////////////////////// TEST(SystemLoader, Constructor) @@ -72,6 +73,41 @@ TEST(SystemLoader, EmptyNames) EXPECT_NE(std::string::npos, output.find("empty argument")); } +///////////////////////////////////////////////// +TEST(SystemLoader, PluginPaths) +{ + SystemLoader sm; + + // verify that there should exist some default paths + std::list paths = sm.PluginPaths(); + unsigned int pathCount = paths.size(); + EXPECT_LT(0u, pathCount); + + // Add test path and verify that the loader now contains this path + auto testBuildPath = common::joinPaths( + std::string(PROJECT_BINARY_PATH), "lib/"); + sm.AddSystemPluginPath(testBuildPath); + paths = sm.PluginPaths(); + + // Number of paths should increase by 1 + EXPECT_EQ(pathCount + 1, paths.size()); + + // verify newly added paths exists + bool hasPath = false; + for (const auto &s : paths) + { + // the returned path string may not be exact match due to extra '/' + // appended at the end of the string. So use absPath to generate + // a path string that matches the format returned by joinPaths + if (common::absPath(s) == testBuildPath) + { + hasPath = true; + break; + } + } + EXPECT_TRUE(hasPath); +} + ///////////////////////////////////////////////// TEST(SystemLoader, BadLibraryPath) { diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 0e1a760382..72a66960ce 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -15,6 +15,11 @@ * */ +#include +#include + +#include + #include "gz/sim/components/SystemPluginInfo.hh" #include "gz/sim/Conversions.hh" #include "SystemManager.hh" @@ -34,11 +39,15 @@ SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader, transport::NodeOptions opts; opts.SetNameSpace(_namespace); this->node = std::make_unique(opts); - std::string entitySystemService{"entity/system/add"}; - this->node->Advertise(entitySystemService, + std::string entitySystemAddService{"entity/system/add"}; + this->node->Advertise(entitySystemAddService, &SystemManager::EntitySystemAddService, this); gzmsg << "Serving entity system service on [" - << "/" << entitySystemService << "]" << std::endl; + << "/" << entitySystemAddService << "]" << std::endl; + + std::string entitySystemInfoService{"system/info"}; + this->node->Advertise(entitySystemInfoService, + &SystemManager::EntitySystemInfoService, this); } ////////////////////////////////////////////////// @@ -307,6 +316,51 @@ bool SystemManager::EntitySystemAddService(const msgs::EntityPlugin_V &_req, return true; } +////////////////////////////////////////////////// +bool SystemManager::EntitySystemInfoService(const msgs::Empty &, + msgs::EntityPlugin_V &_res) +{ + // loop through all files in paths and populate the list of + // plugin libraries. + std::list paths = this->systemLoader->PluginPaths(); + std::set filenames; + for (const auto &p : paths) + { + if (common::exists(p)) + { + for (common::DirIter file(p); + file != common::DirIter(); ++file) + { + std::string current(*file); + std::string filename = common::basename(current); + if (common::isFile(current) && + (common::EndsWith(filename, ".so") || + common::EndsWith(filename, ".dll") || + common::EndsWith(filename, ".dylib"))) + { + // remove extension and lib prefix + size_t extensionIndex = filename.rfind("."); + std::string nameWithoutExtension = + filename.substr(0, extensionIndex); + if (common::StartsWith(nameWithoutExtension, "lib")) + { + nameWithoutExtension = nameWithoutExtension.substr(3); + } + filenames.insert(nameWithoutExtension); + } + } + } + } + + for (const auto &fn : filenames) + { + auto plugin = _res.add_plugins(); + plugin->set_filename(fn); + } + + return true; +} + ////////////////////////////////////////////////// void SystemManager::ProcessPendingEntitySystems() { diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 434e2d2404..8a44f2bd4e 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -160,6 +160,14 @@ namespace gz private: bool EntitySystemAddService(const msgs::EntityPlugin_V &_req, msgs::Boolean &_res); + /// \brief Callback for entity info system service. + /// \param[in] _req Empty request message + /// \param[out] _res Response containing a list of plugin names + /// and filenames + /// \return True if request received. + private: bool EntitySystemInfoService(const msgs::Empty &_req, + msgs::EntityPlugin_V &_res); + /// \brief All the systems. private: std::vector systems; diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index c7dae12ac6..facaaca434 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -2,12 +2,19 @@ # Generate the ruby script. # Note that the major version of the library is included in the name. # Ex: cmdsim0.rb -set(cmd_script_generated "${CMAKE_CURRENT_BINARY_DIR}/cmd${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}.rb") -set(cmd_script_configured "${cmd_script_generated}.configured") +set(cmd_script_name "cmd${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}.rb") +set(cmd_script_generated "${CMAKE_CURRENT_BINARY_DIR}/$_${cmd_script_name}") +set(cmd_script_configured "${CMAKE_CURRENT_BINARY_DIR}/${cmd_script_name}.configured") # Set the library_location variable to the relative path to the library file # within the install directory structure. -set(library_location "../../../${CMAKE_INSTALL_LIBDIR}/$") +if(WIN32) + set(plugin_location ${CMAKE_INSTALL_BINDIR}) +else() + set(plugin_location ${CMAKE_INSTALL_LIBDIR}) +endif() + +set(library_location "../../../${plugin_location}/$") configure_file( "cmd${GZ_DESIGNATION}.rb.in" @@ -19,7 +26,7 @@ file(GENERATE INPUT "${cmd_script_configured}") # Install the ruby command line library in an unversioned location. -install(FILES ${cmd_script_generated} DESTINATION lib/ruby/gz) +install(FILES ${cmd_script_generated} DESTINATION lib/ruby/gz RENAME ${cmd_script_name}) set(gz_library_path "${CMAKE_INSTALL_PREFIX}/lib/ruby/gz/cmd${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}") @@ -40,8 +47,9 @@ install( FILES # Used for the installed model command version. # Generate the ruby script that gets installed. # Note that the major version of the library is included in the name. -set(cmd_model_script_generated "${CMAKE_CURRENT_BINARY_DIR}/cmdmodel${PROJECT_VERSION_MAJOR}.rb") -set(cmd_model_script_configured "${cmd_model_script_generated}.configured") +set(cmd_model_script_name "cmdmodel${PROJECT_VERSION_MAJOR}.rb") +set(cmd_model_script_generated "${CMAKE_CURRENT_BINARY_DIR}/$_${cmd_model_script_name}") +set(cmd_model_script_configured "${CMAKE_CURRENT_BINARY_DIR}/${cmd_model_script_name}.configured") configure_file( "cmdmodel.rb.in" @@ -51,7 +59,7 @@ file(GENERATE OUTPUT "${cmd_model_script_generated}" INPUT "${cmd_model_script_configured}") -install(FILES ${cmd_model_script_generated} DESTINATION lib/ruby/gz) +install(FILES ${cmd_model_script_generated} DESTINATION lib/ruby/gz RENAME ${cmd_model_script_name}) # Used for the installed version. set(gz_model_ruby_path "${CMAKE_INSTALL_PREFIX}/lib/ruby/gz/cmdmodel${PROJECT_VERSION_MAJOR}") @@ -68,55 +76,64 @@ install(FILES ${model_configured} DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_IN # Generate the ruby script for internal testing. # Note that the major version of the library is included in the name. # Ex: cmdsim0.rb -set(cmd_script_generated_test "${CMAKE_BINARY_DIR}/test/lib/ruby/gz/cmd${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}.rb") -set(cmd_script_configured_test "${cmd_script_generated_test}.configured") - -# Set the library_location variable to the relative path to the library file -# within the install directory structure. -set(library_location "$") - -configure_file( - "cmd${GZ_DESIGNATION}.rb.in" - "${cmd_script_configured_test}" - @ONLY) - -file(GENERATE - OUTPUT "${cmd_script_generated_test}" - INPUT "${cmd_script_configured_test}") - -# Used only for internal testing. -set(gz_library_path - "${CMAKE_BINARY_DIR}/test/lib/ruby/gz/cmd${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}") - -# Generate a configuration file for internal testing. -# Note that the major version of the library is included in the name. -# Ex: sim0.yaml -configure_file( - "${GZ_DESIGNATION}.yaml.in" - "${CMAKE_BINARY_DIR}/test/conf/${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}.yaml" @ONLY) +# The logic is valid only for single-config CMake generators, so no script is +# generated if a multiple-config CMake geneator is used +get_property(GENERATOR_IS_MULTI_CONFIG GLOBAL PROPERTY GENERATOR_IS_MULTI_CONFIG) +if(NOT GENERATOR_IS_MULTI_CONFIG) + set(cmd_script_generated_test "${CMAKE_BINARY_DIR}/test/lib/ruby/gz/cmd${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}.rb") + set(cmd_script_configured_test "${cmd_script_generated_test}.configured") + + # Set the library_location variable to the relative path to the library file + # within the install directory structure. + set(library_location "$") + + configure_file( + "cmd${GZ_DESIGNATION}.rb.in" + "${cmd_script_configured_test}" + @ONLY) + + file(GENERATE + OUTPUT "${cmd_script_generated_test}" + INPUT "${cmd_script_configured_test}") + + # Used only for internal testing. + set(gz_library_path + "${CMAKE_BINARY_DIR}/test/lib/ruby/gz/cmd${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}") + + # Generate a configuration file for internal testing. + # Note that the major version of the library is included in the name. + # Ex: sim0.yaml + configure_file( + "${GZ_DESIGNATION}.yaml.in" + "${CMAKE_BINARY_DIR}/test/conf/${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}.yaml" @ONLY) +endif() #=============================================================================== # Generate the ruby script for internal testing. # Note that the major version of the library is included in the name. -set(cmd_model_ruby_test_dir "${CMAKE_BINARY_DIR}/test/lib/ruby/gz") -set(cmd_model_script_generated_test "${cmd_model_ruby_test_dir}/cmdmodel${PROJECT_VERSION_MAJOR}.rb") -set(cmd_model_script_configured_test "${cmd_model_script_generated_test}.configured") - -configure_file( - "cmdmodel.rb.in" - "${cmd_model_script_configured_test}" - @ONLY) - -file(GENERATE - OUTPUT "${cmd_model_script_generated_test}" - INPUT "${cmd_model_script_configured_test}") - -# Used for internal testing. -set(gz_model_ruby_path "${cmd_model_script_generated_test}") - -configure_file( - "model.yaml.in" - "${CMAKE_BINARY_DIR}/test/conf/model${PROJECT_VERSION_MAJOR}.yaml" @ONLY) +# The logic is valid only for single-config CMake generators, so no script is +# generated if a multiple-config CMake geneator is used +if(NOT GENERATOR_IS_MULTI_CONFIG) + set(cmd_model_ruby_test_dir "${CMAKE_BINARY_DIR}/test/lib/ruby/gz") + set(cmd_model_script_generated_test "${cmd_model_ruby_test_dir}/cmdmodel${PROJECT_VERSION_MAJOR}.rb") + set(cmd_model_script_configured_test "${cmd_model_script_generated_test}.configured") + + configure_file( + "cmdmodel.rb.in" + "${cmd_model_script_configured_test}" + @ONLY) + + file(GENERATE + OUTPUT "${cmd_model_script_generated_test}" + INPUT "${cmd_model_script_configured_test}") + + # Used for internal testing. + set(gz_model_ruby_path "${cmd_model_script_generated_test}") + + configure_file( + "model.yaml.in" + "${CMAKE_BINARY_DIR}/test/conf/model${PROJECT_VERSION_MAJOR}.yaml" @ONLY) +endif() #=============================================================================== # Bash completion diff --git a/src/cmd/ModelCommandAPI.hh b/src/cmd/ModelCommandAPI.hh index 2ca7248c60..fca2059a8d 100644 --- a/src/cmd/ModelCommandAPI.hh +++ b/src/cmd/ModelCommandAPI.hh @@ -15,8 +15,10 @@ * */ +#include "gz/sim/Export.hh" + /// \brief External hook to get a list of available models. -extern "C" void cmdModelList(); +extern "C" GZ_SIM_VISIBLE void cmdModelList(); /// \brief External hook to dump model information. /// \param[in] _modelName Model name. @@ -24,7 +26,7 @@ extern "C" void cmdModelList(); /// \param[in] _linkName Link name. /// \param[in] _jointName Joint name. /// \param[in] _sensorName Sensor name. -extern "C" void cmdModelInfo( +extern "C" GZ_SIM_VISIBLE void cmdModelInfo( const char *_modelName, int _pose, const char *_linkName, const char *_jointName, const char *_sensorName); diff --git a/src/cmd/cmdmodel.rb.in b/src/cmd/cmdmodel.rb.in index ffe928d6c3..0d445121e8 100644 --- a/src/cmd/cmdmodel.rb.in +++ b/src/cmd/cmdmodel.rb.in @@ -26,6 +26,7 @@ else end require 'optparse' +require 'pathname' # Constants. LIBRARY_NAME = '@library_location@' @@ -157,7 +158,8 @@ class Cmd options = parse(args) # Read the plugin that handles the command. - if LIBRARY_NAME[0] == '/' + library_name_path = Pathname.new(LIBRARY_NAME) + if library_name_path.absolute? # If the first character is a slash, we'll assume that we've been given an # absolute path to the library. This is only used during test mode. plugin = LIBRARY_NAME diff --git a/src/cmd/cmdsim.rb.in b/src/cmd/cmdsim.rb.in index b93523945f..1ee6829614 100755 --- a/src/cmd/cmdsim.rb.in +++ b/src/cmd/cmdsim.rb.in @@ -27,6 +27,7 @@ end require 'optparse' require 'erb' +require 'pathname' # Constants. LIBRARY_NAME = '@library_location@' @@ -343,7 +344,8 @@ class Cmd def execute(args) options = parse(args) - if LIBRARY_NAME[0] == '/' + library_name_path = Pathname.new(LIBRARY_NAME) + if library_name_path.absolute? # If the first character is a slash, we'll assume that we've been given an # absolute path to the library. This is only used during test mode. plugin = LIBRARY_NAME @@ -485,6 +487,12 @@ See https://github.com/gazebosim/gz-sim/issues/44 for more info." exit(-1) end + if plugin.end_with? ".dll" + puts "`ign gazebo` currently only works with the -s argument on Windows. +See https://github.com/gazebosim/gz-sim/issues/168 for more info." + exit(-1) + end + serverPid = Process.fork do ENV['RMT_PORT'] = '1500' Process.setpgid(0, 0) @@ -538,6 +546,18 @@ See https://github.com/gazebosim/gz-sim/issues/44 for more info." options['wait_gui'], options['headless-rendering'], options['record-period']) # Otherwise run the gui else options['gui'] + if plugin.end_with? ".dylib" + puts "`gz sim` currently only works with the -s argument on macOS. +See https://github.com/gazebosim/gz-sim/issues/44 for more info." + exit(-1) + end + + if plugin.end_with? ".dll" + puts "`gz sim` currently only works with the -s argument on Windows. +See https://github.com/gazebosim/gz-sim/issues/168 for more info." + exit(-1) + end + ENV['RMT_PORT'] = '1501' Importer.runGui(options['gui_config'], options['file'], options['wait_gui'], options['render_engine_gui']) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index d2dae34430..9965a71a13 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -72,6 +73,7 @@ #include "gz/sim/components/Volume.hh" #include "gz/sim/components/WindMode.hh" #include "gz/sim/components/World.hh" +#include "gz/sim/config.hh" #include "gz/sim/EntityComponentManager.hh" #include "gz/sim/gui/GuiEvents.hh" @@ -122,9 +124,35 @@ namespace gz::sim /// \brief Handles all system info components. public: std::unique_ptr systemInfo; + + /// \brief A list of system plugin human readable names. + public: QStringList systemNameList; + + /// \brief Maps plugin display names to their filenames. + public: std::unordered_map systemMap; }; } +// Helper to remove a prefix from a string if present +void removePrefix(const std::string &_prefix, std::string &_s) +{ + auto id = _s.find(_prefix); + if (id != std::string::npos) + { + _s = _s.substr(_prefix.length()); + } +} + +// Helper to remove a suffix from a string if present +void removeSuffix(const std::string &_suffix, std::string &_s) +{ + auto id = _s.find(_suffix); + if (id != std::string::npos && id + _suffix.length() == _s.length()) + { + _s.erase(id, _suffix.length()); + } +} + using namespace gz; using namespace sim; @@ -869,7 +897,6 @@ void ComponentInspector::Update(const UpdateInfo &, auto comp = _ecm.Component(this->dataPtr->entity); if (comp) { - this->SetType("material"); setData(item, comp->Data()); } } @@ -1175,6 +1202,106 @@ transport::Node &ComponentInspector::TransportNode() return this->dataPtr->node; } +///////////////////////////////////////////////// +void ComponentInspector::QuerySystems() +{ + msgs::Empty req; + msgs::EntityPlugin_V res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/" + this->dataPtr->worldName + + "/system/info"}; + if (!this->dataPtr->node.Request(service, req, timeout, res, result)) + { + ignerr << "Unable to query available systems." << std::endl; + return; + } + + this->dataPtr->systemNameList.clear(); + this->dataPtr->systemMap.clear(); + for (const auto &plugin : res.plugins()) + { + if (plugin.filename().empty()) + { + ignerr << "Received empty plugin name. This shouldn't happen." + << std::endl; + continue; + } + + // Remove common prefixes and suffixes + auto humanReadable = plugin.filename(); + removePrefix("ignition-gazebo-", humanReadable); + removePrefix("ignition-gazebo" + + std::string(GZ_SIM_MAJOR_VERSION_STR) + "-", humanReadable); + removeSuffix("-system", humanReadable); + removeSuffix("system", humanReadable); + removeSuffix("-plugin", humanReadable); + removeSuffix("plugin", humanReadable); + + // Replace - with space, capitalize + std::replace(humanReadable.begin(), humanReadable.end(), '-', ' '); + humanReadable[0] = std::toupper(humanReadable[0]); + + this->dataPtr->systemMap[humanReadable] = plugin.filename(); + this->dataPtr->systemNameList.push_back( + QString::fromStdString(humanReadable)); + } + this->dataPtr->systemNameList.sort(); + this->dataPtr->systemNameList.removeDuplicates(); + this->SystemNameListChanged(); +} + +///////////////////////////////////////////////// +QStringList ComponentInspector::SystemNameList() const +{ + return this->dataPtr->systemNameList; +} + +///////////////////////////////////////////////// +void ComponentInspector::SetSystemNameList(const QStringList &_list) +{ + this->dataPtr->systemNameList = _list; +} + +///////////////////////////////////////////////// +void ComponentInspector::OnAddSystem(const QString &_name, + const QString &_filename, const QString &_innerxml) +{ + auto filenameStr = _filename.toStdString(); + auto it = this->dataPtr->systemMap.find(filenameStr); + if (it == this->dataPtr->systemMap.end()) + { + ignerr << "Internal error: failed to find [" << filenameStr + << "] in system map." << std::endl; + return; + } + + msgs::EntityPlugin_V req; + auto ent = req.mutable_entity(); + ent->set_id(this->dataPtr->entity); + auto plugin = req.add_plugins(); + std::string name = _name.toStdString(); + std::string filename = this->dataPtr->systemMap[filenameStr]; + std::string innerxml = _innerxml.toStdString(); + plugin->set_name(name); + plugin->set_filename(filename); + plugin->set_innerxml(innerxml); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/" + this->dataPtr->worldName + + "/entity/system/add"}; + if (!this->dataPtr->node.Request(service, req, timeout, res, result)) + { + ignerr << "Error adding new system to entity: " + << this->dataPtr->entity << "\n" + << "Name: " << name << "\n" + << "Filename: " << filename << "\n" + << "Inner XML: " << innerxml << std::endl; + } +} + // Register this plugin GZ_ADD_PLUGIN(gz::sim::ComponentInspector, gz::gui::Plugin) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index af98930bf3..9a369f710b 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -213,6 +213,14 @@ namespace sim NOTIFY NestedModelChanged ) + /// \brief System display name list + Q_PROPERTY( + QStringList systemNameList + READ SystemNameList + WRITE SetSystemNameList + NOTIFY SystemNameListChanged + ) + /// \brief Constructor public: ComponentInspector(); @@ -373,6 +381,28 @@ namespace sim /// \return Transport node public: transport::Node &TransportNode(); + /// \brief Query system plugin info. + public: Q_INVOKABLE void QuerySystems(); + + /// \brief Get the system plugin display name list + /// \return A list of names that are potentially system plugins + public: Q_INVOKABLE QStringList SystemNameList() const; + + /// \brief Set the system plugin display name list + /// \param[in] _systempFilenameList A list of system plugin display names + public: Q_INVOKABLE void SetSystemNameList( + const QStringList &_systemNameList); + + /// \brief Notify that system plugin display name list has changed + signals: void SystemNameListChanged(); + + /// \brief Callback when a new system is to be added to an entity + /// \param[in] _name Name of system + /// \param[in] _filename Filename of system + /// \param[in] _innerxml Inner XML content of the system + public: Q_INVOKABLE void OnAddSystem(const QString &_name, + const QString &_filename, const QString &_innerxml); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 6dbccb673b..6b13775a17 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -15,9 +15,9 @@ * */ import QtQuick 2.9 -import QtQuick.Controls 1.4 import QtQuick.Controls 2.2 import QtQuick.Controls.Material 2.1 +import QtQuick.Dialogs 1.1 import QtQuick.Layouts 1.3 import QtQuick.Controls.Styles 1.4 import GzSim 1.0 as GzSim @@ -216,6 +216,27 @@ Rectangle { } } + ToolButton { + id: addSystemButton + checkable: false + text: "\u002B" + contentItem: Text { + text: addSystemButton.text + color: "#b5b5b5" + horizontalAlignment: Text.AlignHCenter + verticalAlignment: Text.AlignVCenter + } + visible: (entityType == "model" || entityType == "visual" || + entityType == "sensor" || entityType == "world") + ToolTip.text: "Add a system to this entity" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + onClicked: { + addSystemDialog.open() + } + } + + Label { id: entityLabel text: 'Entity ' + ComponentInspector.entity @@ -227,6 +248,111 @@ Rectangle { } } + Dialog { + id: addSystemDialog + modal: false + focus: true + title: "Add System" + closePolicy: Popup.CloseOnEscape + width: parent.width + + ColumnLayout { + width: parent.width + GridLayout { + columns: 2 + columnSpacing: 30 + Text { + text: "Name" + Layout.row: 0 + Layout.column: 0 + } + + TextField { + id: nameField + selectByMouse: true + Layout.row: 0 + Layout.column: 1 + Layout.fillWidth: true + Layout.minimumWidth: 250 + onTextEdited: { + addSystemDialog.updateButtonState(); + } + placeholderText: "Leave empty to load first plugin" + } + + Text { + text: "Filename" + Layout.row: 1 + Layout.column: 0 + } + + ComboBox { + id: filenameCB + Layout.row: 1 + Layout.column: 1 + Layout.fillWidth: true + Layout.minimumWidth: 250 + model: ComponentInspector.systemNameList + currentIndex: 0 + onCurrentIndexChanged: { + if (currentIndex < 0) + return; + addSystemDialog.updateButtonState(); + } + ToolTip.visible: hovered + ToolTip.delay: tooltipDelay + ToolTip.text: currentText + } + } + + Text { + id: innerxmlLabel + text: "Inner XML" + } + + Flickable { + id: innerxmlFlickable + Layout.minimumHeight: 300 + Layout.fillWidth: true + Layout.fillHeight: true + + flickableDirection: Flickable.VerticalFlick + TextArea.flickable: TextArea { + id: innerxmlField + wrapMode: Text.WordWrap + selectByMouse: true + textFormat: TextEdit.PlainText + font.pointSize: 10 + } + ScrollBar.vertical: ScrollBar { + policy: ScrollBar.AlwaysOn + } + } + } + + footer: DialogButtonBox { + id: buttons + standardButtons: Dialog.Ok | Dialog.Cancel + } + + onOpened: { + ComponentInspector.QuerySystems(); + addSystemDialog.updateButtonState(); + } + + onAccepted: { + ComponentInspector.OnAddSystem(nameField.text.trim(), + filenameCB.currentText.trim(), innerxmlField.text.trim()) + } + + function updateButtonState() { + buttons.standardButton(Dialog.Ok).enabled = + (filenameCB.currentText.trim() != '') + } + } + + + ListView { anchors.top: header.bottom anchors.bottom: parent.bottom diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index c2cecb005e..9bc1007876 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -59,6 +59,9 @@ inline namespace GZ_SIM_VERSION_NAMESPACE { /// \brief Rendering utility public: RenderUtil renderUtil; + /// \brief True if render engine plugins paths are initialized + public: bool renderEnginePluginPathsInit{false}; + /// \brief List of new entities from a gui event public: std::set newEntities; @@ -123,6 +126,12 @@ void GzSceneManager::Update(const UpdateInfo &_info, GZ_PROFILE("GzSceneManager::Update"); + if (!this->dataPtr->renderEnginePluginPathsInit) + { + this->dataPtr->renderUtil.InitRenderEnginePluginPaths(); + this->dataPtr->renderEnginePluginPathsInit = true; + } + this->dataPtr->renderUtil.UpdateECM(_info, _ecm); std::lock_guard lock(this->dataPtr->newRemovedEntityMutex); diff --git a/src/gz.hh b/src/gz.hh index d594588613..7f54fed938 100644 --- a/src/gz.hh +++ b/src/gz.hh @@ -21,18 +21,18 @@ /// \brief External hook to read the library version. /// \return C-string representing the version. Ex.: 0.1.2 -extern "C" char *gzSimVersion(); +extern "C" GZ_SIM_VISIBLE char *gzSimVersion(); /// \brief Get the Gazebo version header. /// \return C-string containing the Gazebo version information. -extern "C" char *simVersionHeader(); +extern "C" GZ_SIM_VISIBLE char *simVersionHeader(); /// \brief Set verbosity level /// \param[in] _verbosity 0 to 4 -extern "C" void cmdVerbosity( +extern "C" GZ_SIM_VISIBLE void cmdVerbosity( const char *_verbosity); -extern "C" const char *worldInstallDir(); +extern "C" GZ_SIM_VISIBLE const char *worldInstallDir(); /// \brief External hook to run simulation server. /// \param[in] _sdfString SDF file to run, as a string. @@ -59,7 +59,7 @@ extern "C" const char *worldInstallDir(); /// \param[in] _headless True if server rendering should run headless /// \param[in] _recordPeriod --record-period option /// \return 0 if successful, 1 if not. -extern "C" int runServer(const char *_sdfString, +extern "C" GZ_SIM_VISIBLE int runServer(const char *_sdfString, int _iterations, int _run, float _hz, int _levels, const char *_networkRole, int _networkSecondaries, int _record, const char *_recordPath, int _recordResources, int _logOverwrite, @@ -77,14 +77,14 @@ extern "C" int runServer(const char *_sdfString, /// it receives a world path from GUI. /// \param[in] _renderEngine --render-engine-gui option /// \return 0 if successful, 1 if not. -extern "C" int runGui(const char *_guiConfig, const char *_file, int _waitGui, - const char *_renderEngine); +extern "C" GZ_SIM_VISIBLE int runGui(const char *_guiConfig, + const char *_file, int _waitGui, const char *_renderEngine); /// \brief External hook to find or download a fuel world provided a URL. /// \param[in] _pathToResource Path to the fuel world resource, ie, /// https://staging-fuel.ignitionrobotics.org/1.0/gmas/worlds/ShapesClone /// \return C-string containing the path to the local world sdf file -extern "C" const char *findFuelResource( +extern "C" GZ_SIM_VISIBLE const char *findFuelResource( char *_pathToResource); #endif diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index 02f211d097..73dbd7de1d 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -63,8 +63,7 @@ std::string customExecStr(std::string _cmd) } ///////////////////////////////////////////////// -// See https://github.com/gazebosim/gz-sim/issues/1175 -TEST(CmdLine, GZ_UTILS_TEST_DISABLED_ON_WIN32(Server)) +TEST(CmdLine, Server) { std::string cmd = kGzCommand + " -r -v 4 --iterations 5 " + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/plugins.sdf"; @@ -79,6 +78,9 @@ TEST(CmdLine, GZ_UTILS_TEST_DISABLED_ON_WIN32(Server)) << output; } +// Disable on WIN32 as on Windows it is not support to prepend +// a command with the env variable to set +#ifndef _WIN32 // Use GZ_SIM_RESOURCE_PATH instead of specifying the complete path cmd = std::string("GZ_SIM_RESOURCE_PATH=") + PROJECT_SOURCE_PATH + "/test/worlds " + kGzCommand + @@ -93,10 +95,11 @@ TEST(CmdLine, GZ_UTILS_TEST_DISABLED_ON_WIN32(Server)) EXPECT_NE(output.find("iteration " + std::to_string(i)), std::string::npos) << output; } +#endif } ///////////////////////////////////////////////// -TEST(CmdLine, GZ_UTILS_TEST_DISABLED_ON_WIN32(CachedFuelWorld)) +TEST(CmdLine, CachedFuelWorld) { std::string projectPath = std::string(PROJECT_SOURCE_PATH) + "/test/worlds"; gz::common::setenv("GZ_FUEL_CACHE_PATH", projectPath.c_str()); @@ -110,7 +113,7 @@ TEST(CmdLine, GZ_UTILS_TEST_DISABLED_ON_WIN32(CachedFuelWorld)) } ///////////////////////////////////////////////// -TEST(CmdLine, GZ_UTILS_TEST_DISABLED_ON_WIN32(SimServer)) +TEST(CmdLine, GazeboServer) { std::string cmd = kGzCommand + " -r -v 4 --iterations 5 " + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/plugins.sdf"; @@ -127,7 +130,7 @@ TEST(CmdLine, GZ_UTILS_TEST_DISABLED_ON_WIN32(SimServer)) } ///////////////////////////////////////////////// -TEST(CmdLine, GZ_UTILS_TEST_DISABLED_ON_WIN32(Gazebo)) +TEST(CmdLine, Gazebo) { std::string cmd = kGzCommand + " -r -v 4 --iterations 5 " + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/plugins.sdf"; diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 9df962214f..db257ef9f9 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -2563,13 +2563,9 @@ bool RenderUtil::HeadlessRendering() const } ///////////////////////////////////////////////// -void RenderUtil::Init() +void RenderUtil::InitRenderEnginePluginPaths() { - // Already initialized - if (nullptr != this->dataPtr->scene) - return; - - gz::common::SystemPaths pluginPath; + common::SystemPaths pluginPath; // TODO(CH3): Deprecated. Remove on tock. std::string result; @@ -2592,6 +2588,16 @@ void RenderUtil::Init() } rendering::setPluginPaths(pluginPath.PluginPaths()); +} + +///////////////////////////////////////////////// +void RenderUtil::Init() +{ + // Already initialized + if (nullptr != this->dataPtr->scene) + return; + + this->InitRenderEnginePluginPaths(); std::map params; #ifdef __APPLE__ diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 6c0f5b402b..92ee225c60 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -84,10 +84,9 @@ function(gz_add_system system_name) set(unversioned ${CMAKE_SHARED_LIBRARY_PREFIX}${PROJECT_NAME_NO_VERSION_LOWER}-${system_name}${CMAKE_SHARED_LIBRARY_SUFFIX}) if(WIN32) # symlinks on Windows require admin priviledges, fallback to copy - ADD_CUSTOM_COMMAND(TARGET ${system_target} POST_BUILD - COMMAND "${CMAKE_COMMAND}" -E copy - "$" - "$/${unversioned}") + INSTALL(CODE "EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E copy + ${IGNITION_GAZEBO_PLUGIN_INSTALL_DIR}\/${versioned} + ${IGNITION_GAZEBO_PLUGIN_INSTALL_DIR}\/${unversioned})") else() file(MAKE_DIRECTORY "${PROJECT_BINARY_DIR}/lib") EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${versioned} ${unversioned} WORKING_DIRECTORY "${PROJECT_BINARY_DIR}/lib") diff --git a/src/systems/ackermann_steering/AckermannSteering.hh b/src/systems/ackermann_steering/AckermannSteering.hh index 8bbdc7c304..b0f9c75353 100644 --- a/src/systems/ackermann_steering/AckermannSteering.hh +++ b/src/systems/ackermann_steering/AckermannSteering.hh @@ -111,7 +111,7 @@ namespace systems /// right_steering_joint /// /// References: - /// https://github.com/gazebosim/gz-sim/tree/main/src/systems/diff_drive + /// https://github.com/gazebosim/gz-sim/tree/main/src/systems/ackermann_steering /// https://www.auto.tuwien.ac.at/bib/pdf_TR/TR0183.pdf /// https://github.com/froohoo/ackermansteer/blob/master/ackermansteer/ diff --git a/src/systems/ackermann_steering/SpeedLimiter.hh b/src/systems/ackermann_steering/SpeedLimiter.hh deleted file mode 100644 index 459b3196a6..0000000000 --- a/src/systems/ackermann_steering/SpeedLimiter.hh +++ /dev/null @@ -1,130 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, PAL Robotics, S.L. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the PAL Robotics nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* - * Author: Enrique Fernández - * Modified: Carlos Agüero - */ - -#ifndef GZ_SIM_SYSTEMS_SPEEDLIMITER_HH_ -#define GZ_SIM_SYSTEMS_SPEEDLIMITER_HH_ - -#include - -#include - -namespace gz -{ -namespace sim -{ -// Inline bracket to help doxygen filtering. -inline namespace GZ_SIM_VERSION_NAMESPACE { -namespace systems -{ - // Forward declaration. - class SpeedLimiterPrivate; - - /// \brief Class to limit velocity, acceleration and jerk. - /// \ref https://github.com/ros-controls/ros_controllers/tree/melodic-devel/diff_drive_controller - class SpeedLimiter - { - /// \brief Constructor. - /// \param [in] _hasVelocityLimits if true, applies velocity limits. - /// \param [in] _hasAccelerationLimits if true, applies acceleration limits. - /// \param [in] _hasJerkLimits if true, applies jerk limits. - /// \param [in] _minVelocity Minimum velocity [m/s], usually <= 0. - /// \param [in] _maxVelocity Maximum velocity [m/s], usually >= 0. - /// \param [in] _minAcceleration Minimum acceleration [m/s^2], usually <= 0. - /// \param [in] _maxAcceleration Maximum acceleration [m/s^2], usually >= 0. - /// \param [in] _minJerk Minimum jerk [m/s^3], usually <= 0. - /// \param [in] _maxJerk Maximum jerk [m/s^3], usually >= 0. - public: SpeedLimiter(bool _hasVelocityLimits = false, - bool _hasAccelerationLimits = false, - bool _hasJerkLimits = false, - double _minVelocity = 0.0, - double _maxVelocity = 0.0, - double _minAcceleration = 0.0, - double _maxAcceleration = 0.0, - double _minJerk = 0.0, - double _maxJerk = 0.0); - - /// \brief Destructor. - public: ~SpeedLimiter(); - - /// \brief Limit the velocity and acceleration. - /// \param [in, out] _v Velocity [m/s]. - /// \param [in] _v0 Previous velocity to v [m/s]. - /// \param [in] _v1 Previous velocity to v0 [m/s]. - /// \param [in] _dt Time step [s]. - /// \return Limiting factor (1.0 if none). - public: double Limit(double &_v, - double _v0, - double _v1, - double _dt) const; - - /// \brief Limit the velocity. - /// \param [in, out] _v Velocity [m/s]. - /// \return Limiting factor (1.0 if none). - public: double LimitVelocity(double &_v) const; - - /// \brief Limit the acceleration. - /// \param [in, out] _v Velocity [m/s]. - /// \param [in] _v0 Previous velocity [m/s]. - /// \param [in] _dt Time step [s]. - /// \return Limiting factor (1.0 if none). - public: double LimitAcceleration(double &_v, - double _v0, - double _dt) const; - - /// \brief Limit the jerk. - /// \param [in, out] _v Velocity [m/s]. - /// \param [in] _v0 Previous velocity to v [m/s]. - /// \param [in] _v1 Previous velocity to v0 [m/s]. - /// \param [in] _dt Time step [s]. - /// \return Limiting factor (1.0 if none). - /// \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control. - public: double LimitJerk(double &_v, - double _v0, - double _v1, - double _dt) const; - - /// \brief Private data pointer. - private: std::unique_ptr dataPtr; - }; - } -} -} -} - -#endif diff --git a/src/systems/hydrodynamics/Hydrodynamics.cc b/src/systems/hydrodynamics/Hydrodynamics.cc index fe0b3f8492..bd405a4c30 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.cc +++ b/src/systems/hydrodynamics/Hydrodynamics.cc @@ -104,6 +104,14 @@ class gz::sim::systems::HydrodynamicsPrivateData /// \brief The Gazebo Transport node public: transport::Node node; + /// \brief Plugin Parameter: Disable coriolis as part of equation. This is + /// occasionally useful for testing. + public: bool disableCoriolis = false; + + /// \brief Plugin Parameter: Disable added mass as part of equation. This is + /// occasionally useful for testing. + public: bool disableAddedMass = false; + /// \brief Ocean current experienced by this body public: math::Vector3d currentVector {0, 0, 0}; @@ -224,6 +232,9 @@ void Hydrodynamics::Configure( this->dataPtr->paramNr = SdfParamDouble(_sdf, "nR" , 20); this->dataPtr->paramNrr = SdfParamDouble(_sdf, "nRR" , 0); + _sdf->Get("disable_coriolis", this->dataPtr->disableCoriolis, false); + _sdf->Get("disable_added_mass", this->dataPtr->disableAddedMass, false); + // Create model object, to access convenient functions auto model = gz::sim::Model(_entity); @@ -381,7 +392,16 @@ void Hydrodynamics::PreUpdate( const Eigen::VectorXd kDvec = Dmat * state; - const Eigen::VectorXd kTotalWrench = kAmassVec + kDvec + kCmatVec; + Eigen::VectorXd kTotalWrench = kDvec; + + if (!this->dataPtr->disableAddedMass) + { + kTotalWrench += kAmassVec; + } + if (!this->dataPtr->disableCoriolis) + { + kTotalWrench += kCmatVec; + } gz::math::Vector3d totalForce(-kTotalWrench(0), -kTotalWrench(1), -kTotalWrench(2)); diff --git a/src/systems/hydrodynamics/Hydrodynamics.hh b/src/systems/hydrodynamics/Hydrodynamics.hh index ddb736bcaf..9a71b9c256 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.hh +++ b/src/systems/hydrodynamics/Hydrodynamics.hh @@ -75,6 +75,9 @@ namespace systems /// listens on `/model/{namespace name}/ocean_current`.[String, Optional] /// * - A generic current. /// [vector3d m/s, optional, default = [0,0,0]m/s] + /// * - Disable Coriolis force [Boolean, Default: false] + /// * - Disable Added Mass [Boolean, Default: false]. + /// To be deprecated in Garden. /// /// # Example /// An example configuration is provided in the examples folder. The example diff --git a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc index 5e88d25129..fec06f802d 100644 --- a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc +++ b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc @@ -6,6 +6,7 @@ * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland * Copyright 2016 Geoffrey Hunter * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 2022 Benjamin Perseghetti, Rudis Laboratories * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -51,6 +52,7 @@ #include "gz/sim/components/Wind.hh" #include "gz/sim/Link.hh" #include "gz/sim/Model.hh" +#include "gz/sim/Util.hh" // from rotors_gazebo_plugins/include/rotors_gazebo_plugins/common.h /// \brief This class can be used to apply a first order filter on a signal. @@ -252,7 +254,8 @@ void MulticopterMotorModel::Configure(const Entity &_entity, } else { - gzerr << "Please specify a robotNamespace.\n"; + gzwarn << "No robotNamespace set using entity name.\n"; + this->dataPtr->robotNamespace = this->dataPtr->model.Name(_ecm); } // Get params from SDF @@ -367,6 +370,10 @@ void MulticopterMotorModel::Configure(const Entity &_entity, << "]" << std::endl; return; } + else + { + gzdbg << "Listening to topic: " << topic << std::endl; + } this->dataPtr->node.Subscribe(topic, &MulticopterMotorModelPrivate::OnActuatorMsg, this->dataPtr.get()); } diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 99e7485185..c58740d1e1 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -614,8 +614,7 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) this->sensorSize, this->forceLength, this->cameraUpdateRate, - this->depthCameraOffset, - this->visualizationResolution); + this->depthCameraOffset); this->initialized = true; } diff --git a/src/systems/optical_tactile_plugin/Visualization.cc b/src/systems/optical_tactile_plugin/Visualization.cc index 5f1bad70fd..74f79cfa41 100644 --- a/src/systems/optical_tactile_plugin/Visualization.cc +++ b/src/systems/optical_tactile_plugin/Visualization.cc @@ -32,14 +32,12 @@ OpticalTactilePluginVisualization::OpticalTactilePluginVisualization( gz::math::Vector3d &_sensorSize, double &_forceLength, float &_cameraUpdateRate, - gz::math::Pose3f &_depthCameraOffset, - int &_visualizationResolution) : + gz::math::Pose3f &_depthCameraOffset) : modelName(_modelName), sensorSize(_sensorSize), forceLength(_forceLength), cameraUpdateRate(_cameraUpdateRate), - depthCameraOffset(_depthCameraOffset), - visualizationResolution(_visualizationResolution) + depthCameraOffset(_depthCameraOffset) { } diff --git a/src/systems/optical_tactile_plugin/Visualization.hh b/src/systems/optical_tactile_plugin/Visualization.hh index 0e9cc527e7..f3fc8fff3d 100644 --- a/src/systems/optical_tactile_plugin/Visualization.hh +++ b/src/systems/optical_tactile_plugin/Visualization.hh @@ -52,15 +52,12 @@ namespace optical_tactile_sensor /// \param[in] _forceLength Value of the forceLength attribute /// \param[in] _cameraUpdateRate Value of the cameraUpdateRate attribute /// \param[in] _depthCameraOffset Value of the depthCameraOffset attribute - /// \param[in] _visualizationResolution Value of the - /// visualizationResolution attribute public: OpticalTactilePluginVisualization( std::string &_modelName, gz::math::Vector3d &_sensorSize, double &_forceLength, float &_cameraUpdateRate, - gz::math::Pose3f &_depthCameraOffset, - int &_visualizationResolution); + gz::math::Pose3f &_depthCameraOffset); /// \brief Initialize the marker message representing the optical tactile /// sensor @@ -153,9 +150,6 @@ namespace optical_tactile_sensor /// \brief Offset between depth camera pose and model pose private: gz::math::Pose3f depthCameraOffset; - /// \brief Resolution of the sensor in pixels to skip. - private: int visualizationResolution; - /// \brief Whether the normal forces messages are initialized or not private: bool normalForcesMsgsAreInitialized{false}; }; diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index d048c27b25..5ab1187863 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -16,6 +16,7 @@ */ #include #include +#include #include #include @@ -114,15 +115,39 @@ class gz::sim::systems::ThrusterPrivateData /// thrust public: double thrustCoefficient = 1; + /// \brief True if the thrust coefficient was set by configuration. + public: bool thrustCoefficientSet = false; + + /// \brief Relative speed reduction between the water at the propeller vs + /// behind the vessel. + public: double wakeFraction = 0.2; + + /// \brief Constant given by the open water propeller diagram. Used in the + /// calculation of the thrust coefficient. + public: double alpha1 = 1; + + /// \brief Constant given by the open water propeller diagram. Used in the + /// calculation of the thrust coefficient. + public: double alpha2 = 0; + /// \brief Density of fluid in kgm^-3, default: 1000kgm^-3 public: double fluidDensity = 1000; /// \brief Diameter of propeller in m, default: 0.02 public: double propellerDiameter = 0.02; + /// \brief Linear velocity of the vehicle. + public: double linearVelocity = 0.0; + + /// \brief Topic name used to control thrust. Optional + public: std::string topic = ""; + /// \brief Callback for handling thrust update public: void OnCmdThrust(const msgs::Double &_msg); + /// \brief Recalculates and updates the thrust coefficient. + public: void UpdateThrustCoefficient(); + /// \brief callback for handling angular velocity update public: void OnCmdAngVel(const gz::msgs::Double &_msg); @@ -181,6 +206,7 @@ void Thruster::Configure( if (_sdf->HasElement("thrust_coefficient")) { this->dataPtr->thrustCoefficient = _sdf->Get("thrust_coefficient"); + this->dataPtr->thrustCoefficientSet = true; } // Get propeller diameter @@ -203,6 +229,47 @@ void Thruster::Configure( ThrusterPrivateData::OperationMode::ForceCmd; } + // Get wake fraction number, default 0.2 otherwise + if (_sdf->HasElement("wake_fraction")) + { + this->dataPtr->wakeFraction = _sdf->Get("wake_fraction"); + } + + // Get alpha_1, default to 1 othwewise + if (_sdf->HasElement("alpha_1")) + { + this->dataPtr->alpha1 = _sdf->Get("alpha_1"); + if (this->dataPtr->thrustCoefficientSet) + { + gzwarn << " The [alpha_2] value will be ignored as a " + << "[thrust_coefficient] was also defined through the SDF file." + << " If you want the system to use the alpha values to calculate" + << " and update the thrust coefficient please remove the " + << "[thrust_coefficient] value from the SDF file." << std::endl; + } + } + + // Get alpha_2, default to 1 othwewise + if (_sdf->HasElement("alpha_2")) + { + this->dataPtr->alpha2 = _sdf->Get("alpha_2"); + if (this->dataPtr->thrustCoefficientSet) + { + gzwarn << " The [alpha_2] value will be ignored as a " + << "[thrust_coefficient] was also defined through the SDF file." + << " If you want the system to use the alpha values to calculate" + << " and update the thrust coefficient please remove the " + << "[thrust_coefficient] value from the SDF file." << std::endl; + } + } + + // Get a custom topic. + if (_sdf->HasElement("topic")) + { + this->dataPtr->topic = transport::TopicUtils::AsValidTopic( + _sdf->Get("topic")); + } + this->dataPtr->jointEntity = model.JointByName(_ecm, jointName); if (kNullEntity == this->dataPtr->jointEntity) { @@ -224,21 +291,39 @@ void Thruster::Configure( this->dataPtr->jointEntity); this->dataPtr->linkEntity = model.LinkByName(_ecm, childLink->Data()); - if (this->dataPtr->opmode == ThrusterPrivateData::OperationMode::ForceCmd) + std::string thrusterTopic; + std::string feedbackTopic; + if (!this->dataPtr->topic.empty()) { - // Keeping cmd_pos for backwards compatibility - // TODO(chapulina) Deprecate cmd_pos, because the commands aren't positions - std::string thrusterTopicOld = - gz::transport::TopicUtils::AsValidTopic( - "/model/" + ns + "/joint/" + jointName + "/cmd_pos"); + // Subscribe to specified topic for force commands + thrusterTopic = gz::transport::TopicUtils::AsValidTopic( + ns + "/" + this->dataPtr->topic); + if (this->dataPtr->opmode == ThrusterPrivateData::OperationMode::ForceCmd) + { + this->dataPtr->node.Subscribe( + thrusterTopic, + &ThrusterPrivateData::OnCmdThrust, + this->dataPtr.get()); - this->dataPtr->node.Subscribe( - thrusterTopicOld, - &ThrusterPrivateData::OnCmdThrust, - this->dataPtr.get()); + feedbackTopic = gz::transport::TopicUtils::AsValidTopic( + ns + "/" + this->dataPtr->topic + "/ang_vel"); + } + else + { + this->dataPtr->node.Subscribe( + thrusterTopic, + &ThrusterPrivateData::OnCmdAngVel, + this->dataPtr.get()); + feedbackTopic = gz::transport::TopicUtils::AsValidTopic( + ns + "/" + this->dataPtr->topic + "/force"); + } + } + else if (this->dataPtr->opmode == + ThrusterPrivateData::OperationMode::ForceCmd) + { // Subscribe to force commands - std::string thrusterTopic = gz::transport::TopicUtils::AsValidTopic( + thrusterTopic = gz::transport::TopicUtils::AsValidTopic( "/model/" + ns + "/joint/" + jointName + "/cmd_thrust"); this->dataPtr->node.Subscribe( @@ -246,20 +331,14 @@ void Thruster::Configure( &ThrusterPrivateData::OnCmdThrust, this->dataPtr.get()); - gzmsg << "Thruster listening to commands in [" << thrusterTopic << "]" - << std::endl; - - std::string feedbackTopic = gz::transport::TopicUtils::AsValidTopic( + feedbackTopic = gz::transport::TopicUtils::AsValidTopic( "/model/" + ns + "/joint/" + jointName + "/ang_vel"); - this->dataPtr->pub = this->dataPtr->node.Advertise( - feedbackTopic - ); } else { gzdbg << "Using angular velocity mode" << std::endl; // Subscribe to angvel commands - std::string thrusterTopic = gz::transport::TopicUtils::AsValidTopic( + thrusterTopic = gz::transport::TopicUtils::AsValidTopic( "/model/" + ns + "/joint/" + jointName + "/cmd_vel"); this->dataPtr->node.Subscribe( @@ -267,20 +346,22 @@ void Thruster::Configure( &ThrusterPrivateData::OnCmdAngVel, this->dataPtr.get()); - gzmsg << "Thruster listening to commands in [" << thrusterTopic << "]" - << std::endl; - - std::string feedbackTopic = gz::transport::TopicUtils::AsValidTopic( - "/model/" + ns + "/joint/" + jointName + "/force"); - this->dataPtr->pub = this->dataPtr->node.Advertise( - feedbackTopic - ); + feedbackTopic = gz::transport::TopicUtils::AsValidTopic( + "/model/" + ns + "/joint/" + jointName + "/force"); } + gzmsg << "Thruster listening to commands on [" << thrusterTopic << "]" + << std::endl; + + this->dataPtr->pub = this->dataPtr->node.Advertise( + feedbackTopic); + // Create necessary components if not present. enableComponent(_ecm, this->dataPtr->linkEntity); enableComponent(_ecm, this->dataPtr->linkEntity); + enableComponent(_ecm, + this->dataPtr->linkEntity); double minThrustCmd = this->dataPtr->cmdMin; double maxThrustCmd = this->dataPtr->cmdMax; @@ -380,6 +461,14 @@ void ThrusterPrivateData::OnCmdAngVel(const gz::msgs::Double &_msg) ///////////////////////////////////////////////// double ThrusterPrivateData::ThrustToAngularVec(double _thrust) { + // Only update if the thrust coefficient was not set by configuration + // and angular velocity is not zero. Some velocity is needed to calculate + // the thrust coefficient otherwise it will never start moving. + if (!this->thrustCoefficientSet && + std::abs(this->propellerAngVel) > std::numeric_limits::epsilon()) + { + this->UpdateThrustCoefficient(); + } // Thrust is proportional to the Rotation Rate squared // See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 246 auto propAngularVelocity = sqrt(abs( @@ -392,6 +481,14 @@ double ThrusterPrivateData::ThrustToAngularVec(double _thrust) return propAngularVelocity; } +///////////////////////////////////////////////// +void ThrusterPrivateData::UpdateThrustCoefficient() +{ + this->thrustCoefficient = this->alpha1 + this->alpha2 * + (((1 - this->wakeFraction) * this->linearVelocity) + / (this->propellerAngVel * this->propellerDiameter)); +} + ///////////////////////////////////////////////// double ThrusterPrivateData::AngularVelToThrust(double _angVel) { @@ -450,6 +547,8 @@ void Thruster::PreUpdate( { std::lock_guard lock(this->dataPtr->mtx); desiredThrust = this->dataPtr->thrust; + this->dataPtr->propellerAngVel = + this->dataPtr->ThrustToAngularVec(this->dataPtr->thrust); desiredPropellerAngVel = this->dataPtr->propellerAngVel; } @@ -501,6 +600,11 @@ void Thruster::PreUpdate( _ecm, unitVector * desiredThrust, unitVector * torque); + + // Update the LinearVelocity of the vehicle + this->dataPtr->linearVelocity = + _ecm.Component( + this->dataPtr->linkEntity)->Data().Length(); } ///////////////////////////////////////////////// @@ -519,4 +623,4 @@ GZ_ADD_PLUGIN( GZ_ADD_PLUGIN_ALIAS(Thruster, "gz::sim::systems::Thruster") // TODO(CH3): Deprecated, remove on version 8 -GZ_ADD_PLUGIN_ALIAS(Thruster, "ignition::gazebo::systems::Thruster") +GZ_ADD_PLUGIN_ALIAS(Thruster, "gz::sim::systems::Thruster") diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index 3af5e9f1ad..81cbb12e94 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -49,8 +49,10 @@ namespace systems /// - - The namespace in which the robot exists. The plugin will /// listen on the topic `/model/{namespace}/joint/{joint_name}/cmd_thrust`or /// `/model/{namespace}/joint/{joint_name}/cmd_vel` depending on the mode of - /// operation. + /// operation. If {topic} is set then the plugin will listen on + /// {namespace}/{topic} /// [Optional] + /// - - The topic for receiving thrust commands. [Optional] /// - - This is the joint in the model which corresponds to the /// propeller. [Required] /// - - If set to true will make the thruster @@ -83,7 +85,19 @@ namespace systems /// [Optional, defaults to 1000N or 1000rad/s] /// - - Minimum input thrust or angular velocity command. /// [Optional, defaults to -1000N or -1000rad/s] - /// + /// - - Relative speed reduction between the water + /// at the propeller (Va) vs behind the vessel. + /// [Optional, defults to 0.2] + /// See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 95: + /// Va = (1 - wake_fraction) * advance_speed + /// - - Constant given by the open water propeller diagram. Used + /// in the calculation of the thrust coefficient (Kt). + /// [Optional, defults to 1] + /// - - Constant given by the open water propeller diagram. Used + /// in the calculation of the thrust coefficient (Kt). + /// [Optional, defults to 0] + /// See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 95: + /// Kt = alpha_1 * alpha_2 * (Va/(propeller_revolution * propeller_diameter)) /// ## Example /// An example configuration is installed with Gazebo. The example /// uses the LiftDrag plugin to apply steering controls. It also uses the diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 5672949f3a..7f38ec03df 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -595,9 +595,52 @@ void TriggeredPublisher::Configure(const Entity &, } } } - else + + if (sdfClone->HasElement("service")) + { + for (auto serviceElem = sdfClone->GetElement("service"); serviceElem; + serviceElem = serviceElem->GetNextElement("service")) + { + SrvOutputInfo serviceInfo; + serviceInfo.srvName = serviceElem->Get("name"); + if (serviceInfo.srvName.empty()) + { + ignerr << "Service name cannot be empty\n"; + return; + } + serviceInfo.reqType = serviceElem->Get("reqType"); + if (serviceInfo.reqType.empty()) + { + ignerr << "Service request type cannot be empty\n"; + return; + } + serviceInfo.repType = serviceElem->Get("repType"); + if (serviceInfo.repType.empty()) + { + ignerr << "Service reply type cannot be empty\n"; + return; + } + serviceInfo.reqMsg = serviceElem->Get("reqMsg"); + if (serviceInfo.reqMsg.empty()) + { + ignerr << "Service request message cannot be empty\n"; + return; + } + std::string timeoutInfo = serviceElem->Get("timeout"); + if (timeoutInfo.empty()) + { + ignerr << "Timeout value cannot be empty\n"; + return; + } + + serviceInfo.timeout = std::stoi(timeoutInfo); + this->srvOutputInfo.push_back(std::move(serviceInfo)); + } + } + if (!sdfClone->HasElement("service") && !sdfClone->HasElement("output")) { - gzerr << "No ouptut specified" << std::endl; + ignerr << "No output and service specified. Make sure to specify at least" + "one of them." << std::endl; return; } @@ -606,23 +649,27 @@ void TriggeredPublisher::Configure(const Entity &, { if (this->MatchInput(_msg)) { + if (this->delay > 0ms) + { + std::lock_guard lock(this->publishQueueMutex); + this->publishQueue.push_back(this->delay); + } + else { - if (this->delay > 0ms) - { - std::lock_guard lock(this->publishQueueMutex); - this->publishQueue.push_back(this->delay); - } - else { - { - std::lock_guard lock(this->publishCountMutex); - ++this->publishCount; - } - this->newMatchSignal.notify_one(); + std::lock_guard lock(this->publishCountMutex); + ++this->publishCount; } + this->newMatchSignal.notify_one(); + } + if (this->srvOutputInfo.size() > 0) + { + std::lock_guard lock(this->triggerSrvMutex); + ++this->serviceCount; } } }); + if (!this->node.Subscribe(this->inputTopic, msgCb)) { gzerr << "Input subscriber could not be created for topic [" @@ -645,11 +692,69 @@ void TriggeredPublisher::Configure(const Entity &, std::thread(std::bind(&TriggeredPublisher::DoWork, this)); } +////////////////////////////////////////////////// +void TriggeredPublisher::PublishMsg(std::size_t pending) +{ + for (auto &info : this->outputInfo) + { + for (std::size_t i = 0; i < pending; ++i) + { + info.pub.Publish(*info.msgData); + } + } +} + +////////////////////////////////////////////////// +void TriggeredPublisher::CallService(std::size_t pendingSrv) +{ + for (auto &serviceInfo : this->srvOutputInfo) + { + for (std::size_t i = 0; i < pendingSrv; ++i) + { + bool result; + auto req = msgs::Factory::New(serviceInfo.reqType, serviceInfo.reqMsg); + if (!req) + { + ignerr << "Unable to create request for type [" + << serviceInfo.reqType << "].\n"; + return; + } + + auto rep = msgs::Factory::New(serviceInfo.repType); + if (!rep) + { + ignerr << "Unable to create response for type [" + << serviceInfo.repType << "].\n"; + return; + } + + bool executed = this->node.Request(serviceInfo.srvName, *req, + serviceInfo.timeout, *rep, result); + if (executed) + { + if (!result) + { + ignerr << "Service call [" << serviceInfo.srvName << "] failed\n"; + } + else + { + ignmsg << "Service call [" << serviceInfo.srvName << "] succeeded\n"; + } + } + else + { + ignerr << "Service call [" << serviceInfo.srvName << "] timed out\n"; + } + } + } +} + ////////////////////////////////////////////////// void TriggeredPublisher::DoWork() { while (!this->done) { + // check whether to publish a msg by checking publishCount std::size_t pending{0}; { using namespace std::chrono_literals; @@ -666,13 +771,19 @@ void TriggeredPublisher::DoWork() std::swap(pending, this->publishCount); } - for (auto &info : this->outputInfo) + PublishMsg(pending); + + // check whether to call a service by checking serviceCount + std::size_t pendingSrv{0}; { - for (std::size_t i = 0; i < pending; ++i) - { - info.pub.Publish(*info.msgData); + std::lock_guard lock(this->triggerSrvMutex); + if (this->serviceCount == 0 || this->done){ + continue; } + std::swap(pendingSrv, this->serviceCount); } + + CallService(pendingSrv); } } diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index 3fa75a96d4..f503c48b11 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -20,7 +20,7 @@ #include #include #include - +#include #include #include "gz/sim/System.hh" @@ -37,8 +37,10 @@ namespace systems /// \brief The triggered publisher system publishes a user specified message /// on an output topic in response to an input message that matches user - /// specified criteria. An optional simulation time delay can be used - /// delay message publication. + /// specified criteria. It can also call a user specified service as an + /// output in response to an input message. It currently supports blocking + /// service call. An optional simulation time delay can be used delay message + /// publication. /// /// ## System Parameters /// @@ -75,9 +77,19 @@ namespace systems /// the human-readable representation of a protobuf message as used by /// `gz topic` for publishing messages /// - /// ``: Integer number of milliseconds, in simulation time, to + /// - ``: Integer number of milliseconds, in simulation time, to /// delay publication. /// + /// - ``: Contains configuration for service to call: Multiple + /// `` tags are possible. A service will be called for each input + /// that matches. + /// * Attributes: + /// * `name`: Service name (eg. `/world/triggered_publisher/set_pose`) + /// * `timeout`: Service timeout + /// * `reqType`: Service request message type (eg. ignition.msgs.Pose) + /// * `repType`: Service response message type (eg. ignition.msgs.Empty) + /// * `reqMsg`: String used to construct the service protobuf message. + /// /// Examples: /// 1. Any receipt of a Boolean messages on the input topic triggers an output /// \code{.xml} @@ -182,6 +194,12 @@ namespace systems /// \brief Thread that handles publishing output messages public: void DoWork(); + /// \brief Method that calls a service + private: void CallService(std::size_t pendingSrv); + + /// \brief Method that publishes a message + private: void PublishMsg(std::size_t pending); + /// \brief Helper function that calls Match on every InputMatcher available /// \param[in] _inputMsg Input message /// \return True if all of the matchers return true @@ -210,21 +228,49 @@ namespace systems transport::Node::Publisher pub; }; + /// \brief Class that holds necessary bits for each specified service output + private: struct SrvOutputInfo + { + /// \brief Service name + std::string srvName; + + /// \brief Service request type + std::string reqType; + + /// \brief Service reply type + std::string repType; + + /// \brief Service request message + std::string reqMsg; + + /// \brief Serivce timeout + int timeout; + }; + /// \brief List of InputMatchers private: std::vector> matchers; /// \brief List of outputs private: std::vector outputInfo; + /// \brief List of service outputs + private: std::vector srvOutputInfo; + /// \brief Gazebo communication node. private: transport::Node node; + /// \brief Counter that tells how manny times to call the service + private: std::size_t serviceCount{0}; + /// \brief Counter that tells the publisher how many times to publish private: std::size_t publishCount{0}; /// \brief Mutex to synchronize access to publishCount private: std::mutex publishCountMutex; + /// \brief Mutex to synchronize access to serviceCount + private: std::mutex triggerSrvMutex; + /// \brief Condition variable to signal that new matches have occured private: std::condition_variable newMatchSignal; diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 8ab09eb248..f3b611d73f 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -26,6 +26,7 @@ set(tests fuel_cached_server.cc halt_motion.cc hydrodynamics.cc + hydrodynamics_flags.cc imu_system.cc joint_controller_system.cc joint_position_controller_system.cc diff --git a/test/integration/entity_system.cc b/test/integration/entity_system.cc index ed6eb8de6c..3bf25c67da 100644 --- a/test/integration/entity_system.cc +++ b/test/integration/entity_system.cc @@ -158,6 +158,54 @@ TEST_P(EntitySystemTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PublishCmd)) "/model/vehicle/cmd_vel"); } +///////////////////////////////////////////////// +TEST_P(EntitySystemTest, SystemInfo) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/empty.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // get info on systems available + msgs::Empty req; + msgs::EntityPlugin_V res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/empty/system/info"}; + transport::Node node; + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + + // verify plugins are not empty + EXPECT_FALSE(res.plugins().empty()); + + // check for a few known plugins that we know should exist in gazebo + std::set knownPlugins; + knownPlugins.insert("user-commands-system"); + knownPlugins.insert("physics-system"); + knownPlugins.insert("scene-broadcaster-system"); + knownPlugins.insert("sensors-system"); + + for (const auto &plugin : res.plugins()) + { + for (const auto &kp : knownPlugins) + { + if (plugin.filename().find(kp) != std::string::npos) + { + knownPlugins.erase(kp); + break; + } + } + } + // verify all known plugins are found and removed from the set + EXPECT_TRUE(knownPlugins.empty()); +} + // Run multiple times INSTANTIATE_TEST_SUITE_P(ServerRepeat, EntitySystemTest, ::testing::Range(1, 2)); diff --git a/test/integration/hydrodynamics_flags.cc b/test/integration/hydrodynamics_flags.cc new file mode 100644 index 0000000000..fa0044dfca --- /dev/null +++ b/test/integration/hydrodynamics_flags.cc @@ -0,0 +1,151 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include + +#include +#include +#include +#include + +#include "gz/sim/Link.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "gz/sim/TestFixture.hh" +#include "gz/sim/Util.hh" +#include "gz/sim/World.hh" + +#include "test_config.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +class HydrodynamicsFlagsTest : public InternalFixture<::testing::Test> +{ + /// \brief Test a world file. + /// \param[in] _world Path to world file. + /// \param[in] _modelName Name of the model. + /// \param[in] _numsteps Number of steps to run the server. + /// \param[out] _linearVel Linear velocityies after each step. + /// \param[out] _angularVel Linear velocityies after each step. + public: void TestWorld(const std::string &_world, + const std::string &_modelName, const unsigned int &_numsteps, + std::vector &_linearVel, + std::vector &_angularVel); +}; + +////////////////////////////////////////////////// +void HydrodynamicsFlagsTest::TestWorld(const std::string &_world, + const std::string &_modelName, const unsigned int &_numsteps, + std::vector &_linearVel, + std::vector &_angularVel) +{ + // Maximum verbosity for debugging + common::Console::SetVerbosity(4); + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_world); + + TestFixture fixture(serverConfig); + + Model model; + Link body; + std::vector bodyVels; + fixture. + OnConfigure( + [&](const Entity &_worldEntity, + const std::shared_ptr &/*_sdf*/, + EntityComponentManager &_ecm, + EventManager &/*eventMgr*/) + { + World world(_worldEntity); + + auto modelEntity = world.ModelByName(_ecm, _modelName); + EXPECT_NE(modelEntity, kNullEntity); + model = Model(modelEntity); + + auto bodyEntity = model.LinkByName(_ecm, "base_link"); + EXPECT_NE(bodyEntity, kNullEntity); + + body = Link(bodyEntity); + body.EnableVelocityChecks(_ecm); + + // Forces in Y and Z are needed to make the coriolis + // force appear. + math::Vector3d force(0.0, 1000.0, 1000.0); + math::Vector3d torque(0.0, 0.0, 10.0); + body.AddWorldWrench(_ecm, force, torque); + + }). + OnPostUpdate([&](const UpdateInfo &/*_info*/, + const EntityComponentManager &_ecm) + { + auto bodyVel = body.WorldLinearVelocity(_ecm); + ASSERT_TRUE(bodyVel); + _linearVel.push_back(bodyVel.value()); + auto bodyAngularVel = body.WorldAngularVelocity(_ecm); + ASSERT_TRUE(bodyAngularVel); + _angularVel.push_back(bodyAngularVel.value()); + }). + Finalize(); + + fixture.Server()->Run(true, _numsteps, false); + EXPECT_EQ(_numsteps, _linearVel.size()); + EXPECT_EQ(_numsteps, _angularVel.size()); + + EXPECT_NE(model.Entity(), kNullEntity); + EXPECT_NE(body.Entity(), kNullEntity); + +} + +///////////////////////////////////////////////// +/// This test makes sure that the linear velocity is reuduced +/// disbling the coriolis force and also when disabling the added mass. +TEST_F(HydrodynamicsFlagsTest, AddedMassCoriolisFlags) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "hydrodynamics_flags.sdf"); + + unsigned int numsteps = 1000; + + std::vector angularVels, angularCoriolisVels, + angularAddedMassVels; + std::vector linearVels, linearCoriolisVels, + linearAddedMassVels; + + this->TestWorld(world, "tethys", numsteps, linearVels, angularVels); + this->TestWorld(world, "triton", numsteps, linearCoriolisVels, + angularCoriolisVels); + this->TestWorld(world, "daphne", numsteps, linearAddedMassVels, + angularAddedMassVels); + + // Wait a couple of iterations for the body to move + for (unsigned int i = 4; i < numsteps; ++i) + { + // Angular and linear velocity should be lower + // with the produced coriolis and added mass + EXPECT_LT(angularCoriolisVels[i].Z(), angularVels[i].Z()); + EXPECT_LT(linearCoriolisVels[i].Z(), linearVels[i].Z()); + EXPECT_LT(angularAddedMassVels[i].Z(), angularVels[i].Z()); + EXPECT_LT(linearAddedMassVels[i].Z(), linearVels[i].Z()); + } +} diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index 9428069b9d..a43ec0816c 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -32,6 +33,9 @@ #include "gz/sim/Util.hh" #include "gz/sim/World.hh" +#include "gz/sim/components/JointAxis.hh" +#include "gz/sim/components/Pose.hh" + #include "test_config.hh" #include "../helpers/EnvTestFixture.hh" @@ -43,7 +47,8 @@ class ThrusterTest : public InternalFixture<::testing::Test> /// \brief Test a world file /// \param[in] _world Path to world file /// \param[in] _namespace Namespace for topic - /// \param[in] _coefficient Thrust coefficient + /// \param[in] _topic Thrust topic + /// \param[in] _thrustCoefficient Thrust coefficient /// \param[in] _density Fluid density /// \param[in] _diameter Propeller diameter /// \param[in] _baseTol Base tolerance for most quantities @@ -51,15 +56,19 @@ class ThrusterTest : public InternalFixture<::testing::Test> /// force /// \param[in] _mass Mass of the body being propelled. public: void TestWorld(const std::string &_world, - const std::string &_namespace, double _coefficient, double _density, - double _diameter, double _baseTol, bool _useAngVelCmd = false, - double _mass = 100.1); + const std::string &_namespace, const std::string &_topic, + double _thrustCoefficient, double _density, double _diameter, + double _baseTol, double _wakeFraction = 0.2, double _alpha_1 = 1, + double _alpha_2 = 0, bool _calculateCoefficient = false, + bool _useAngVelCmd = false, double _mass = 100.1); }; ////////////////////////////////////////////////// void ThrusterTest::TestWorld(const std::string &_world, - const std::string &_namespace, double _coefficient, double _density, - double _diameter, double _baseTol, bool _useAngVelCmd, double _mass) + const std::string &_namespace, const std::string &_topic, + double _thrustCoefficient, double _density, double _diameter, + double _baseTol, double _wakeFraction, double _alpha1, double _alpha2, + bool _calculateCoefficient, bool _useAngVelCmd, double _mass) { // Start server ServerConfig serverConfig; @@ -71,6 +80,9 @@ void ThrusterTest::TestWorld(const std::string &_world, Link propeller; std::vector modelPoses; std::vector propellerAngVels; + std::vector propellerLinVels; + math::Pose3d jointPose, linkWorldPose; + math::Vector3d jointAxis; double dt{0.0}; fixture. OnConfigure( @@ -85,6 +97,13 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_NE(modelEntity, kNullEntity); model = Model(modelEntity); + auto jointEntity = model.JointByName(_ecm, "propeller_joint"); + jointAxis = + _ecm.Component( + jointEntity)->Data().Xyz(); + jointPose = _ecm.Component( + jointEntity)->Data(); + auto propellerEntity = model.LinkByName(_ecm, "propeller"); EXPECT_NE(propellerEntity, kNullEntity); @@ -102,6 +121,10 @@ void ThrusterTest::TestWorld(const std::string &_world, auto propellerAngVel = propeller.WorldAngularVelocity(_ecm); ASSERT_TRUE(propellerAngVel); propellerAngVels.push_back(propellerAngVel.value()); + + auto proellerLinVel = propeller.WorldLinearVelocity(_ecm); + ASSERT_TRUE(proellerLinVel); + propellerLinVels.push_back(proellerLinVel.value()); }). Finalize(); @@ -109,6 +132,7 @@ void ThrusterTest::TestWorld(const std::string &_world, fixture.Server()->Run(true, 100, false); EXPECT_EQ(100u, modelPoses.size()); EXPECT_EQ(100u, propellerAngVels.size()); + EXPECT_EQ(100u, propellerLinVels.size()); EXPECT_NE(model.Entity(), kNullEntity); EXPECT_NE(propeller.Entity(), kNullEntity); @@ -123,20 +147,11 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_EQ(math::Vector3d::Zero, vel); } propellerAngVels.clear(); + propellerLinVels.clear(); // Publish command and check that vehicle moved transport::Node node; - std::string cmdTopic; - if (!_useAngVelCmd) - { - cmdTopic = "/model/" + _namespace + "/joint/propeller_joint/cmd_thrust"; - } - else - { - cmdTopic = "/model/" + _namespace + "/joint/propeller_joint/cmd_vel"; - } - auto pub = node.Advertise( - cmdTopic); + auto pub = node.Advertise(_topic); int sleep{0}; int maxSleep{30}; @@ -154,7 +169,7 @@ void ThrusterTest::TestWorld(const std::string &_world, // else: // min_thrust = 0 // max_thrust = 300 - double invalidCmd = (_useAngVelCmd && _coefficient < 0) ? 1000 : -1000; + double invalidCmd = (_useAngVelCmd && _thrustCoefficient < 0) ? 1000 : -1000; msgs::Double msg; msg.set_data(invalidCmd); pub.Publish(msg); @@ -165,8 +180,10 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_DOUBLE_EQ(0.0, modelPoses.back().Pos().X()); EXPECT_EQ(100u, modelPoses.size()); EXPECT_EQ(100u, propellerAngVels.size()); + EXPECT_EQ(100u, propellerLinVels.size()); modelPoses.clear(); propellerAngVels.clear(); + propellerLinVels.clear(); // max allowed force double force{300.0}; @@ -174,9 +191,10 @@ void ThrusterTest::TestWorld(const std::string &_world, // See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 246 // omega = sqrt(thrust / // (fluid_density * thrust_coefficient * propeller_diameter ^ 4)) - auto omega = sqrt(abs(force / (_density * _coefficient * pow(_diameter, 4)))); + auto omega = sqrt(abs(force / (_density * _thrustCoefficient * + pow(_diameter, 4)))); // Account for negative thrust and/or negative thrust coefficient - omega *= (force * _coefficient > 0 ? 1 : -1); + omega *= (force * _thrustCoefficient > 0 ? 1 : -1); msg.Clear(); if(!_useAngVelCmd) @@ -203,6 +221,7 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_EQ(100u * sleep, modelPoses.size()); EXPECT_EQ(100u * sleep, propellerAngVels.size()); + EXPECT_EQ(100u * sleep, propellerLinVels.size()); } // F = m * a @@ -236,9 +255,28 @@ void ThrusterTest::TestWorld(const std::string &_world, EXPECT_NEAR(0.0, pose.Rot().Roll(), _baseTol); } + auto jointWorldPose = linkWorldPose * jointPose; + auto unitVector = jointWorldPose.Rot().RotateVector(jointAxis).Normalize(); + double omegaTol{1e-1}; for (unsigned int i = 0; i < propellerAngVels.size(); ++i) { + auto angularVelocity = propellerAngVels[i].Dot(unitVector); + // See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 246: + // thrust_coefficient = alpha_1 + alpha_2 * (((1-wake_fraction) * + // linear_velocity) / (angular_velocity * propeller_diameter)) + // omega = sqrt(thrust / + // (fluid_density * thrust_coefficient * propeller_diameter ^ 4)) + if (_calculateCoefficient && !gz::math::equal(angularVelocity, 0.0)) + { + _thrustCoefficient = _alpha1 + _alpha2 * (((1 - _wakeFraction) * + propellerLinVels[i].Length()) / (angularVelocity * _diameter)); + } + omega = sqrt(abs(force / (_density * _thrustCoefficient * + pow(_diameter, 4)))); + // Account for negative thrust and/or negative thrust coefficient + omega *= (force * _thrustCoefficient > 0 ? 1 : -1); + auto angVel = propellerAngVels[i]; // It takes a few iterations to reach the speed if (i > 25) @@ -260,64 +298,98 @@ void ThrusterTest::TestWorld(const std::string &_world, ///////////////////////////////////////////////// TEST_F(ThrusterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AngVelCmdControl)) { + const std::string ns{"custom"}; + const std::string topic = "/model/" + ns + + "/joint/propeller_joint/cmd_vel"; + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "thruster_ang_vel_cmd.sdf"); // Tolerance is high because the joint command disturbs the vehicle body - this->TestWorld(world, "custom", 0.005, 950, 0.2, 1e-2, true, 100.01); + this->TestWorld(world, ns, topic, 0.005, 950, 0.2, 1e-2, 0.2, 1, 0, false, + true, 100.01); } ///////////////////////////////////////////////// TEST_F(ThrusterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(CcwForceCmdControl)) { + const std::string ns{"custom"}; + const std::string topic = "/model/" + ns + + "/joint/propeller_joint/cmd_thrust"; + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "thruster_ccw_force_cmd.sdf"); // Viewed from stern to bow the propeller spins counter-clockwise // Tolerance is high because the joint command disturbs the vehicle body - this->TestWorld(world, "custom", -0.005, 950, 0.2, 1e-2); + this->TestWorld(world, ns, topic, -0.005, 950, 0.2, 1e-2); } ///////////////////////////////////////////////// TEST_F(ThrusterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(CcwAngVelCmdControl)) { + const std::string ns{"custom"}; + const std::string topic = "/model/" + ns + + "/joint/propeller_joint/cmd_vel"; + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "thruster_ccw_ang_vel_cmd.sdf"); // Viewed from stern to bow the propeller spins counter-clockwise // Tolerance is high because the joint command disturbs the vehicle body - this->TestWorld(world, "custom", -0.005, 950, 0.2, 1e-2, true); + this->TestWorld(world, ns, topic, -0.005, 950, 0.2, 1e-2, 0.2, 1, 0, false, + true); } ///////////////////////////////////////////////// // See https://github.com/gazebosim/gz-sim/issues/1175 TEST_F(ThrusterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PIDControl)) { + const std::string ns{"sub"}; + const std::string topic = "/model/" + ns + + "/joint/propeller_joint/cmd_thrust"; auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "thruster_pid.sdf"); // Tolerance could be lower (1e-6) if the joint pose had a precise 180 // rotation - this->TestWorld(world, "sub", 0.004, 1000, 0.2, 1e-4); + this->TestWorld(world, ns, topic, 0.004, 1000, 0.2, 1e-4); } ///////////////////////////////////////////////// TEST_F(ThrusterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(VelocityControl)) { + const std::string ns = "custom"; + const std::string topic = "/model/" + ns + + "/joint/propeller_joint/cmd_thrust"; auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "thruster_vel_cmd.sdf"); // Tolerance is high because the joint command disturbs the vehicle body - this->TestWorld(world, "custom", 0.005, 950, 0.25, 1e-2); + this->TestWorld(world, ns, topic, 0.005, 950, 0.25, 1e-2); } ///////////////////////////////////////////////// TEST_F(ThrusterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(BatteryIntegration)) { + const std::string ns = "lowbattery"; + const std::string topic = ns + "/thrust"; auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "thruster_battery.sdf"); // Tolerance is high because the joint command disturbs the vehicle body - this->TestWorld(world, "lowbattery", 0.005, 950, 0.25, 1e-2); + this->TestWorld(world, ns, topic, 0.005, 950, 0.25, 1e-2); } +///////////////////////////////////////////////// +TEST_F(ThrusterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(ThrustCoefficient)) +{ + const std::string ns = "custom"; + const std::string topic = "/model/" + ns + + "/joint/propeller_joint/cmd_thrust"; + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "thrust_coefficient.sdf"); + + // Tolerance is high because the joint command disturbs the vehicle body + this->TestWorld(world, ns, topic, 1, 950, 0.25, 1e-2, 0.2, 0.9, 0.01, true); +} diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index d4fc911c4e..e3fb8a14ab 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -637,3 +637,272 @@ TEST_F(TriggeredPublisherTest, EXPECT_EQ(0u, recvCount); } + +///////////////////////////////////////////////// +/// Test for invalid service name. A service, `/srv-dummy-test` is advertised +/// and the callback is also provided in this test. Everytime an input msg is +/// published to `/in_14` topic, triggered_publisher plugin will call the +/// service `srv-test`, specified in the test/worlds/triggered_publisher.sdf. +/// However, since the two service names do not match, the callback provided in +/// this test will not be invoked. Therefore, the pubCount, which is set to 10, +/// will not equal to recvCount. The recvCount will be 0, since the callback +/// isn't invoked. +TEST_F(TriggeredPublisherTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(InvalidServiceName)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_14"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + return true; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-dummy-test"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + GZ_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return recvCount == 0u;}); + EXPECT_EQ(recvCount, 0u); +} + +///////////////////////////////////////////////// +/// Test for triggering a service call in response to an input msg. A service, +/// `srv-test` is advertised and the callback is also provided in this test. +/// Everytime an input msg is published to `/in_14` topic, triggered_publisher +/// plugin will call the service `/srv-test`, specified in the test/worlds/ +/// triggered_publisher.sdf. This time, the name of the services match. By +/// publishing input msg 10 times, the service callback will also be invoked 10 +/// times. The `pubCount` is set to 10 and recvCount is increased everytime +/// request data matches the string "test" inside the service callback. For a +/// successful test, the pubCount will equal to recvCount. +TEST_F(TriggeredPublisherTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(InputMessagesTriggerServices)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_14"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + return true; + } + return false; + }); + + std::string service = "/srv-test"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + GZ_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} + +///////////////////////////////////////////////// +/// Test for triggering multiple services (in sequence) in response to an input +/// msg by publishing 10 times. Two services, `srv-test-0` and `srv-test-1` are +/// specified in the test/worlds/triggered_publisher.sdf. Everytime an input msg +/// is published, triggered_publisher will call the service and invoke the +/// callback. std::vector is passed as a reference and will be populated with +/// the request message, which will be a boolean value of `true`. If successful, +/// `recvMsg0` and `recvMsg1` vectors should both have a size of 10 with all +/// true boolean values. +TEST_F(TriggeredPublisherTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(MultipleServiceForOneInput)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_15"); + std::mutex recvMsgMutex; + std::vector recvMsgs0; + std::vector recvMsgs1; + auto cbCreator = [&recvMsgMutex](std::vector &_msgVector) + { + return std::function( + [&_msgVector, &recvMsgMutex](const auto &req, auto &) + { + std::lock_guard lock(recvMsgMutex); + if (req.data()) + { + _msgVector.push_back(req.data()); + return true; + } + return false; + }); + }; + + auto msgCb0 = cbCreator(recvMsgs0); + auto msgCb1 = cbCreator(recvMsgs1); + + // Advertise two dummy services + node.Advertise("/srv-test-0", msgCb0); + node.Advertise("/srv-test-1", msgCb1); + + const int pubCount{10}; + for (int i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + GZ_SLEEP_MS(100); + } + + waitUntil(5000, [&] + { + std::lock_guard lock(recvMsgMutex); + return static_cast(pubCount) == recvMsgs0.size() && + static_cast(pubCount) == recvMsgs1.size(); + }); + + EXPECT_EQ(static_cast(pubCount), recvMsgs0.size()); + EXPECT_EQ(static_cast(pubCount), recvMsgs1.size()); + + // The plugin has two outputs. We expect 10 messages in each output topic + EXPECT_EQ(pubCount, std::count(recvMsgs0.begin(), recvMsgs0.end(), true)); + EXPECT_EQ(pubCount, std::count(recvMsgs1.begin(), recvMsgs1.end(), true)); +} + +///////////////////////////////////////////////// +/// Test for triggering a service call with incorrect request type or reply +/// type specified in test/worlds/triggered_publisher.sdf. `InvalidReqType` and +/// `InvalidRepType` do not exist. Hence, the callback will never be invoked and +/// the recvCount will be 0. +TEST_F(TriggeredPublisherTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(WrongRequestOrResponseType)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_16"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + return true; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-test"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + GZ_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return recvCount == 0u;}); + EXPECT_EQ(0u, recvCount); +} + +///////////////////////////////////////////////// +/// Test for triggering a service call with different request (Boolean) and +/// reply type (StringMsg). Check `InputMessagesTriggerServices` test for more +/// details on how the test works. This test is very similar except that it has +/// different request and reply type. +TEST_F(TriggeredPublisherTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(BooleanReqStringMsgRep)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_18"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &) + { + EXPECT_EQ(req.data(), true); + if (req.data() == true) + { + ++recvCount; + return true; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-diff-type-0"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + GZ_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} + +///////////////////////////////////////////////// +/// Test for triggering a service call with different request (StringMsg) and +/// reply type (Boolean). Check `InputMessagesTriggerServices` test for more +/// details on how the test works. This test is very similar except that it has +/// different request and reply type. +TEST_F(TriggeredPublisherTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(StringMsgReqBooleanRep)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_19"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + return true; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-diff-type-1"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + GZ_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(recvCount, recvCount); +} diff --git a/test/worlds/diff_drive_no_plugin.sdf b/test/worlds/diff_drive_no_plugin.sdf index c8c4eb69ce..55790f45ef 100644 --- a/test/worlds/diff_drive_no_plugin.sdf +++ b/test/worlds/diff_drive_no_plugin.sdf @@ -10,6 +10,10 @@ filename="gz-sim-physics-system" name="gz::sim::systems::Physics"> + + true diff --git a/test/worlds/hydrodynamics_flags.sdf b/test/worlds/hydrodynamics_flags.sdf new file mode 100644 index 0000000000..4690545b3c --- /dev/null +++ b/test/worlds/hydrodynamics_flags.sdf @@ -0,0 +1,352 @@ + + + + + + + + + 0.0 1.0 1.0 + 0.0 0.7 0.8 + + + + 0.001 + 1.0 + + + + + + + + + + + + 1000 + + + + 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 + + + + 0 0 1 0 0 1.57 + https://fuel.gazebosim.org/1.0/accurrent/models/MBARI Tethys LRAUV + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + tethys + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + true + true + + + + + + 5 0 1 0 0 1.57 + https://fuel.gazebosim.org/1.0/accurrent/models/MBARI Tethys LRAUV + triton + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + triton + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + true + + + + + + -5 0 1 0 0 1.57 + https://fuel.gazebosim.org/1.0/accurrent/models/MBARI Tethys LRAUV + daphne + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + daphne + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + true + + + + + + 0 0 -1 0 0 3.1415926 + https://fuel.gazebosim.org/1.0/mabelzhang/models/ABCSign_5m + start_line + + + 0 -25 -1 0 0 3.1415926 + https://fuel.gazebosim.org/1.0/mabelzhang/models/ABCSign_5m + finish_line + + + + diff --git a/test/worlds/shapes_scene_broadcaster_only.sdf b/test/worlds/shapes_scene_broadcaster_only.sdf index 6d9ac49a76..ab93f83eba 100644 --- a/test/worlds/shapes_scene_broadcaster_only.sdf +++ b/test/worlds/shapes_scene_broadcaster_only.sdf @@ -3,7 +3,7 @@ 0.001 - 0 + 1 + + + + + + 0 + + + + 0 0 0 + + + + + + + + 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 + + + + + + 0 0 0 0 1.57 0 + + 100 + + 33.89 + 0 + 0 + 33.89 + 0 + 1.125 + + + + + + 2 + 0.15 + + + + + + + -1.05 0 0 0 0 0 + + 0.1 + + 0.000354167 + 0 + 0 + 0.000021667 + 0 + 0.000334167 + + + + + + 0.01 0.25 0.05 + + + + + + + body + propeller + + 1 0 0 + + -1e+12 + 1e+12 + 1e6 + 1e6 + + + + + + custom + propeller_joint + 950 + 0.25 + true + 300 + 0 + 0.2 + 0.9 + 0.01 + + + + + diff --git a/test/worlds/thruster_battery.sdf b/test/worlds/thruster_battery.sdf index c675791ade..a72d395b50 100644 --- a/test/worlds/thruster_battery.sdf +++ b/test/worlds/thruster_battery.sdf @@ -105,6 +105,7 @@ true 300 0 + thrust + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - diff --git a/tutorials.md.in b/tutorials.md.in index cabfb1f42c..9914dd6d0c 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -35,6 +35,7 @@ Gazebo @GZ_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage blender_sdf_exporter "Blender SDF Exporter": Use a Blender script to export a model to the SDF format. * \subpage model_and_optimize_meshes "Model and optimize meshes": Some recomendations when creating meshes in Blender for simulations. * \subpage blender_distort_meshes "Blender mesh distortion": Use a Blender Python script to programmatically deform and distort meshes to customized extents. +* \subpage blender_procedural_datasets "Generation of Procedural Datasets with Blender": Use Blender with a Python script to generate procedural datasets of SDF models. * \subpage model_command "Model Command": Use the CLI to get information about the models in a simulation. * \subpage test_fixture "Test Fixture": Writing automated CI tests * \subpage model_photo_shoot "Model Photo Shoot" Taking perspective, top, front, and side pictures of a model. diff --git a/tutorials/blender_procedural_datasets.md b/tutorials/blender_procedural_datasets.md new file mode 100644 index 0000000000..4f8104c186 --- /dev/null +++ b/tutorials/blender_procedural_datasets.md @@ -0,0 +1,257 @@ +\page blender_procedural_datasets Generation of Procedural Datasets with Blender + +Procedurally-generated datasets of 3D models allow us to create diverse +simulation environments with nearly unlimited variety. Such environments enable +developers to evaluate their algorithms under various conditions that their +robots might experience. Furthermore, it is a simple way of producing a large +quantity of synthetic training data for applications within machine learning. + +## Overview + +[Blender](https://blender.org) is a free and open-source 3D software suite with +several tools and features for creating models that can be utilized inside +robotics simulators like Gazebo. In addition to manually modelling, sculpting +and texturing models, Blender also enables procedural generation of datasets +that can facilitate diversity in simulation environments. Furthermore, its +Python API allows us to generate such datasets programmatically. + +### Procedural Geometry + +[Geometry Nodes](https://docs.blender.org/manual/en/latest/modeling/geometry_nodes/introduction.html) +is a system of Blender for creating and modifying geometry through node-based +operations. It enables the synthesis of many model variations by combining +procedural operations with randomization. Although it enables a relatively +simple and fast method for creating datasets, it can be tricky to get started. +Fortunately, there are several helpful resources that can help you with the +creation of your own custom 3D models, e.g. an [overview video](https://www.youtube.com/watch?v=kMDB7c0ZiKA), +[demo files](https://blender.org/download/demo-files/#geometry-nodes), +[training from Blender Studio](https://studio.blender.org/training/geometry-nodes-from-scratch), +[tutorial by Blender Guru](https://www.youtube.com/watch?v=aO0eUnu0hO0) and many +others. + + + +### Python Script for Generating Procedural Datasets of SDF models + +Python script [`procedural_dataset_generator.py`](https://github.com/gazebosim/gz-sim/tree/gz-sim7/examples/scripts/blender/procedural_dataset_generator.py) +contains a Blender exporter of SDF models and a generator of procedural SDF +dataset. The exporter outputs models that are compatible with the SDF format +and the directory structure of [Fuel](https://app.gazebosim.org), which can be +used with any existing `.blend` project. The dataset generator leverages +Blender's Geometry Nodes modifiers that are configured to generate countless +model variations via the Random Seed input attribute (seed for the pseudorandom +generation). + +This script processes all selected objects in the opened Blender scene (or all +existing objects if none is selected). The exporter is configurable via CLI +arguments (or by adjusting default model-specific parameters). Currently, the +following features are supported: + +- Export of mesh geometry as COLLADA (`.dae`), Wavefront (`.obj`) or STL + (`.stl`) file formats. +- Option to exclude specific Blender objects from being exported to visual or + collision geometry. +- Export separate visual and collision geometry at different levels of + resolution to improve collision-checking performance with low-poly geometry. +- Estimation of inertial properties while assuming a uniform density. +- Optional randomization of model properties via uniform distribution, e.g. mass + and surface friction. +- Linking of PBR textures sets via symbolic links or copies. +- Generation of thumbnails for the exported models. + +## Instructions + +### Dependencies + +Blender is required to open `.blend` projects and run the script that employs +the Python API of Blender (`bpy`). Versions `[>=3.0, <3.3]` were tested with the +script, but newer versions are also expected to work. + +- [Blender `[>=3.0]`](https://blender.org/download) + +### Run the Python Script + +#### Option A — Run Script Inside Blender + +The first option is to run the Python script directly in Blender's *Text Editor* +tab for scripting while using the default parameters that are configurable via +constants at the beginning of the script. For new projects, you can follow these +steps: + +1. Open the desired Blender `.blend` project. It is recommended to open Blender + using a console to get full access to the standard output and error. + +```bash +blender [blender options] file.blend +``` + +2. Copy the [`procedural_dataset_generator.py`](https://github.com/gazebosim/gz-sim/tree/gz-sim7/examples/scripts/blender/procedural_dataset_generator.py) + Python script into a new text data block in your `.blend` under the + *Text Editor* tab. +3. Configure the default parameters of the script for your specific models via + constants at the beginning of the file. +4. Run the script using the *Run script* button in the panel of the + *Text Editor* tab at the top of the screen. + +@image html https://github.com/gazebosim/gz-sim/tree/gz-sim7/tutorials/files/blender_procedural_datasets/blender_instructions.png "Instructions in Blender" width=100% + +Once you follow these steps and configure the script for your `.blend` project, +you can save it and use Option B in the future. + +#### Option B — Run an Internal Script + +The second option is to run a script that is already saved inside a `.blend` +project, i.e. saved and configured through the procedure above. Therefore, this +approach is applicable only for `.blend` projects previously configured with the +[`procedural_dataset_generator.py`](https://github.com/gazebosim/gz-sim/tree/gz-sim7/examples/scripts/blender/procedural_dataset_generator.py) +Python script. This option enables configuring the script via CLI arguments +instead of modifying its contents, which is unnecessary if the project is +already configured. + +```bash +# Note: `procedural_dataset_generator.py` is the name of the Python script inside the `file.blend` +blender [blender options] file.blend --python-text procedural_dataset_generator.py -- [script options] +``` + +#### Option C — Run an External Script + +It is also possible to run the Python script externally without configuring it +first inside a `.blend` project. This option also enables configuring the script +via CLI arguments, which is often required for new models. + +```bash +# Note: `/path/to/procedural_dataset_generator.py` can be an absolute or relative system path +blender [blender options] file.blend --python /path/to/procedural_dataset_generator.py -- [script options] +``` + +## Examples + +In order to demonstrate the generation of procedural datasets, the following two +`.blend` projects are provided: + +- [rock.blend](https://github.com/gazebosim/gz-sim/tree/gz-sim7/tutorials/files/blender_procedural_datasets/rock.blend) + — Models of randomized rocks for Gazebo that robots can interact with +- [woodland.blend](https://github.com/gazebosim/gz-sim/tree/gz-sim7/tutorials/files/blender_procedural_datasets/woodland.blend) + — Static environments of natural scenery with randomly scattered assets of + low-poly trees, rocks, grass and flowers (these assets were adapted from + [Blender Studio](https://studio.blender.org)) + +You need to download these `.blend` projects to follow the examples. You can do +that either manually on GitHub or via `wget`/`curl`. + +```bash +wget https://raw.githubusercontent.com/gazebosim/gz-sim/gz-sim7/tutorials/files/blender_procedural_datasets/rock.blend +wget https://raw.githubusercontent.com/gazebosim/gz-sim/gz-sim7/tutorials/files/blender_procedural_datasets/woodland.blend +``` + +### Try Demo Files + +Now you can open these projects with Blender and run the scripts. Alternatively, +you could run the pre-configured Python scripts for each `.blend` project +directly from the console and configure CLI arguments as desired. + +```bash +blender rock.blend --python-text procedural_dataset_generator.py -- -o sdf_models/rock +blender woodland.blend --python-text procedural_dataset_generator.py -- -o sdf_models/woodland +``` + +@image html https://github.com/gazebosim/gz-sim/tree/gz-sim7/tutorials/files/blender_procedural_datasets/demo_blender_rock.gif "Example of generating a dataset of rock SDF models" width=100% + +@image html https://github.com/gazebosim/gz-sim/tree/gz-sim7/tutorials/files/blender_procedural_datasets/demo_blender_woodland.gif "Example of generating a dataset of woodland SDF models" width=100% + +You can configure the script in several ways (see +`blender rock.blend --python-text procedural_dataset_generator.py -- -h`). For +example, you can set a combination of `--texture-source-mode` and +`--texture-source-value` arguments to link models to (random) PBR textures. + +```bash +## Configure `--texture-source-value` with a path to one of the following directories +# ├── ./ # <-- Here to randomly sample a texture set +# ├── texture0/ # <-- Here to use texture0 +# ├── *albedo*.png || *color*.png +# ├── *normal*.png +# ├── *roughness*.png +# └── *specular*.png || *metalness*.png +# ├── texture1/ +# └── ... +blender rock.blend --python-text procedural_dataset_generator.py -- --texture-source-mode path --texture-source-value /path/to/textures +``` + +### Expected Output + +Once the models are generated, you can see the generated directory structure for +each model. + +```bash +./sdf_models/ +├── rock/ # Parent directory of the generated dataset for rock models + ├── rock0/ # SDF Model directory of the 1st model + │ ├── meshes/ # Directory for mesh geometry + │ │ ├── collision/ # Collision geometry, defaults to `.stl` + │ │ └── visual/ # Visual geometry, defaults to `.obj`+`.mtl` + │ ├── thumbnails/ # Directory for the generated thumbnail + │ ├── model.config # Meta data about the model + │ └── model.sdf # SDF description of the model + ├── rock1/ # SDF Model directory of the 2nd model + └── ... +└── woodland/ # Identical structure for the dataset of woodland models ... +  ├── woodland0/ + ├── woodland1/ + └── ... +``` + +### `GZ_SIM_RESOURCE_PATH` Environment Variable + +In order to make the models discoverable by Gazebo, you need to set the +`GZ_SIM_RESOURCE_PATH` environment variable such that it includes the path to +the parent directory of the models. + +Note that the Python script will always print the adequate command with the +exact path after the export of models is finished. + +```bash +export GZ_SIM_RESOURCE_PATH="${PWD}/sdf_models/rock${GZ_SIM_RESOURCE_PATH:+:${GZ_SIM_RESOURCE_PATH}}" +export GZ_SIM_RESOURCE_PATH="${PWD}/sdf_models/woodland${GZ_SIM_RESOURCE_PATH:+:${GZ_SIM_RESOURCE_PATH}}" +``` + +### Spawn Models in Gazebo + +Hereafter, you can spawn the generated models inside Gazebo with your preferred +approach, e.g. via the Resource Spawner GUI plugin. Below are some examples of +Gazebo environments using the rock and woodland SDF models. + +@image html https://github.com/gazebosim/gz-sim/tree/gz-sim7/tutorials/files/blender_procedural_datasets/demo_gazebo_rock.png "Example of the generated rock SDF models in Gazebo" width=100% + +@image html https://github.com/gazebosim/gz-sim/tree/gz-sim7/tutorials/files/blender_procedural_datasets/demo_gazebo_woodland.png "Example of the generated woodland SDF models in Gazebo" width=100% + +Every object that uses Geometry Nodes in these projects has several input +attributes that can be configured. You can open the `.blend` projects again, +navigate to the `Modifier Properties` tab for each object, and try tweaking them +yourself. + +## Known Issues + +### Installation of Modules in Blender's Python Environment (`trimesh`) + +The [trimesh](https://trimsh.org) Python module is used to estimate inertial +properties. This module is not included by default with the Python environment +of Blender and thus needs to be installed. It is installed automatically during +the first run of the Python script. However, installing a new Python module +within Blender will not be possible if you use it from +[Snap Store](https://snapcraft.io/blender). If this is the case, you must use +the `--python-use-system-env` Blender option when running the Python script. +Furthermore, you might need to run the script as `sudo` for the first time if +the installation fails due to a lack of permission. + +## Future Work + +The Python script is not fully featured yet. Below is a list of features that +could be eventually added to improve its functionality: + +- Add support for exporting processed objects as separate `` and + `` entities of a single SDF model. +- Add support for baking textures for all processed models. Separate `` + entities would be required to prevent overlapping of UV maps. +- Add support for pulling PBR textures from an online source, e.g. repository. +- Add support for exporting lights as SDF entities. +- Add support for exporting cameras as SDF entities. diff --git a/tutorials/files/blender_procedural_datasets/blender_instructions.png b/tutorials/files/blender_procedural_datasets/blender_instructions.png new file mode 100644 index 0000000000..3fbf50619a Binary files /dev/null and b/tutorials/files/blender_procedural_datasets/blender_instructions.png differ diff --git a/tutorials/files/blender_procedural_datasets/demo_blender_rock.gif b/tutorials/files/blender_procedural_datasets/demo_blender_rock.gif new file mode 100644 index 0000000000..e6978faab4 Binary files /dev/null and b/tutorials/files/blender_procedural_datasets/demo_blender_rock.gif differ diff --git a/tutorials/files/blender_procedural_datasets/demo_blender_woodland.gif b/tutorials/files/blender_procedural_datasets/demo_blender_woodland.gif new file mode 100644 index 0000000000..ce48e6daa9 Binary files /dev/null and b/tutorials/files/blender_procedural_datasets/demo_blender_woodland.gif differ diff --git a/tutorials/files/blender_procedural_datasets/demo_gazebo_rock.png b/tutorials/files/blender_procedural_datasets/demo_gazebo_rock.png new file mode 100644 index 0000000000..128cea4173 Binary files /dev/null and b/tutorials/files/blender_procedural_datasets/demo_gazebo_rock.png differ diff --git a/tutorials/files/blender_procedural_datasets/demo_gazebo_woodland.png b/tutorials/files/blender_procedural_datasets/demo_gazebo_woodland.png new file mode 100644 index 0000000000..4570d88547 Binary files /dev/null and b/tutorials/files/blender_procedural_datasets/demo_gazebo_woodland.png differ diff --git a/tutorials/files/blender_procedural_datasets/rock.blend b/tutorials/files/blender_procedural_datasets/rock.blend new file mode 100644 index 0000000000..afcf231c23 Binary files /dev/null and b/tutorials/files/blender_procedural_datasets/rock.blend differ diff --git a/tutorials/files/blender_procedural_datasets/woodland.blend b/tutorials/files/blender_procedural_datasets/woodland.blend new file mode 100644 index 0000000000..7e2ba8f689 Binary files /dev/null and b/tutorials/files/blender_procedural_datasets/woodland.blend differ diff --git a/tutorials/install.md b/tutorials/install.md index d67a58742b..aa98cfcc5b 100644 --- a/tutorials/install.md +++ b/tutorials/install.md @@ -61,7 +61,7 @@ feature which hasn't been released yet. 1. Install tools ``` - sudo apt install -y build-essential cmake g++-8 git gnupg lsb-release wget + sudo apt install -y build-essential cmake git gnupg lsb-release wget ``` 2. Enable the Gazebo software repositories: diff --git a/tutorials/triggered_publisher.md b/tutorials/triggered_publisher.md index 73c8d2ea6d..e7af7f6b71 100644 --- a/tutorials/triggered_publisher.md +++ b/tutorials/triggered_publisher.md @@ -1,17 +1,20 @@ \page triggeredpublisher Triggered Publisher The `TriggeredPublisher` system publishes a user specified message on an output -topic in response to an input message that matches user specified criteria. The -system works by checking the input against a set of Matchers. Matchers -contain string representations of protobuf messages which are compared for -equality or containment with the input message. Matchers can match the whole -input message or only a specific field inside the message. +topic in response to an input message that matches user specified criteria. It +can also call a user specified service in response to an input +message. The system works by checking the input against a set of Matchers. +Matchers contain string representations of protobuf messages which are compared +for equality or containment with the input message. Matchers can match the +whole input message or only a specific field inside the message. + This tutorial describes how the Triggered Publisher system can be used to cause a box to fall from its initial position by detaching a detachable joint in response to the motion of a vehicle. The tutorial also covers how Triggered Publisher systems can be chained together by showing how the falling of the box -can trigger another box to fall. The finished world SDFormat file for this +can trigger another box to fall. Last, it covers how a service call can be +triggered to reset the robot pose. The finished world SDFormat file for this tutorial can be found in [examples/worlds/triggered_publisher.sdf](https://github.com/gazebosim/gz-sim/blob/ign-gazebo2/examples/worlds/triggered_publisher.sdf) @@ -263,3 +266,27 @@ and publish the start message ``` gz topic -t "/start" -m gz.msgs.Empty -p " " ``` + +Once both boxes have fallen, we can publish a message to invoke a service call +to reset the robot position as well as set the speed to 0. As shown below, the +`` sets the linear x speed to 0, and the `` tag contains +metadata to invoke a service call to `/world/triggered_publisher/set_pose`. The +`reqMsg` is expressed in the human-readable form of Google Protobuf meesages. +Multiple `` tags can be used as well as with the `` tag. + +```xml + + + + linear: {x: 0} + + + + +```