diff --git a/apps/isaaclab.python.headless.rendering.kit b/apps/isaaclab.python.headless.rendering.kit index dfc113ff3a7..902a7021c0a 100644 --- a/apps/isaaclab.python.headless.rendering.kit +++ b/apps/isaaclab.python.headless.rendering.kit @@ -18,6 +18,7 @@ keywords = ["experience", "app", "isaaclab", "python", "camera", "minimal"] # Isaac Lab minimal app "isaaclab.python.headless" = {} "omni.replicator.core" = {} +"isaacsim.sensors.rtx" = {} # Rendering "omni.kit.material.library" = {} @@ -86,6 +87,11 @@ app.vulkan = true # disable replicator orchestrator for better runtime perf exts."omni.replicator.core".Orchestrator.enabled = false +# move the default lidar config file paths +app.sensors.nv.lidar.profileBaseFolder.'++' = [ + "${app}/../source/isaaclab/isaaclab/sensors/rtx_lidar/", +] + [settings.exts."omni.kit.registry.nucleus"] registries = [ { name = "kit/default", url = "https://ovextensionsprod.blob.core.windows.net/exts/kit/prod/106/shared" }, diff --git a/apps/isaaclab.python.rendering.kit b/apps/isaaclab.python.rendering.kit index 577fe9a5b7f..5e9d6593c35 100644 --- a/apps/isaaclab.python.rendering.kit +++ b/apps/isaaclab.python.rendering.kit @@ -17,6 +17,7 @@ keywords = ["experience", "app", "isaaclab", "python", "camera", "minimal"] [dependencies] # Isaac Lab minimal app "isaaclab.python" = {} +"isaacsim.sensors.rtx" = {} # PhysX "omni.kit.property.physx" = {} @@ -84,6 +85,11 @@ app.audio.enabled = false # disable replicator orchestrator for better runtime perf exts."omni.replicator.core".Orchestrator.enabled = false +# move the default lidar config file paths +app.sensors.nv.lidar.profileBaseFolder.'++' = [ + "${app}/../source/isaaclab/isaaclab/sensors/rtx_lidar/", +] + [settings.physics] updateToUsd = false updateParticlesToUsd = false diff --git a/source/isaaclab/isaaclab/sensors/rtx_lidar/__init__.py b/source/isaaclab/isaaclab/sensors/rtx_lidar/__init__.py new file mode 100644 index 00000000000..50697595980 --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/rtx_lidar/__init__.py @@ -0,0 +1,8 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from .rtx_lidar import RtxLidar +from .rtx_lidar_cfg import RtxLidarCfg +from .rtx_lidar_data import RTX_LIDAR_INFO_FIELDS, RtxLidarData diff --git a/source/isaaclab/isaaclab/sensors/rtx_lidar/rtx_lidar.py b/source/isaaclab/isaaclab/sensors/rtx_lidar/rtx_lidar.py new file mode 100644 index 00000000000..76d3c628a3a --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/rtx_lidar/rtx_lidar.py @@ -0,0 +1,342 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import os +import re +import torch +from collections.abc import Sequence +from typing import TYPE_CHECKING, Any + +import carb +import omni.kit.commands +import omni.usd +from isaacsim.core.prims import XFormPrim +from pxr import UsdGeom + +import isaaclab.sim as sim_utils +from isaaclab.utils.array import convert_to_torch + +from ..sensor_base import SensorBase +from .rtx_lidar_data import RTX_LIDAR_INFO_FIELDS, RtxLidarData + +if TYPE_CHECKING: + from .rtx_lidar_cfg import RtxLidarCfg + + +class RtxLidar(SensorBase): + """The RTX Lidar sensor to acquire render based lidar data. + + This class wraps over the `UsdGeom Camera`_ for providing a consistent API for acquiring LiDAR data. + + This implementation utilizes the "RtxSensorCpuIsaacCreateRTXLidarScanBuffer" annotator. + + .. RTX lidar: + .. RTX lidar Annotators: + """ + + cfg: RtxLidarCfg + + def __init__(self, cfg: RtxLidarCfg): + """Initializes the RTX lidar object. + + Args: + cfg: The configuration parameters. + + Raises: + RuntimeError: If provided path is a regex. + RuntimeError: If no camera prim is found at the given path. + """ + # check if sensor path is valid + # note: currently we do not handle environment indices if there is a regex pattern in the leaf + # For example, if the prim path is "/World/Sensor_[1,2]". + sensor_path = cfg.prim_path.split("/")[-1] + sensor_path_is_regex = re.match(r"^[a-zA-Z0-9/_]+$", sensor_path) is None + if sensor_path_is_regex: + raise RuntimeError( + f"Invalid prim path for the rtx lidar sensor: {self.cfg.prim_path}." + "\n\tHint: Please ensure that the prim path does not contain any regex patterns in the leaf." + ) + + # Initialize base class + super().__init__(cfg) + + # toggle rendering of rtx sensors as True + # this flag is read by SimulationContext to determine if rtx sensors should be rendered + carb_settings_iface = carb.settings.get_settings() + carb_settings_iface.set_bool("/isaaclab/render/rtx_sensors", True) + # spawn the asset + if self.cfg.spawn is not None: + self.cfg.spawn.func( + self.cfg.prim_path, self.cfg.spawn, translation=self.cfg.offset.pos, orientation=self.cfg.offset.rot + ) + # check that spawn was successful + matching_prims = sim_utils.find_matching_prims(self.cfg.prim_path) + if len(matching_prims) == 0: + raise RuntimeError(f"Could not find prim with path {self.cfg.prim_path}.") + + self._sensor_prims: list[UsdGeom.Camera] = list() + # Create empty variables for storing output data + self._data = RtxLidarData() + + def __del__(self): + """Unsubscribes from callbacks and detach from the replicator registry and clean up any custom lidar configs.""" + # unsubscribe callbacks + super().__del__() + # delete from replicator registry + for annotator, render_product_path in zip(self._rep_registry, self._render_product_paths): + annotator.detach([render_product_path]) + annotator = None + # delete custom lidar config temp files + if self.cfg.spawn.lidar_type == "Custom": + file_dir = self.cfg.spawn.sensor_profile_temp_dir + if os.path.isdir(file_dir): + for file in os.listdir(file_dir): + if self.cfg.spawn.sensor_profile_temp_prefix in file and os.path.isfile( + os.path.join(file_dir, file) + ): + os.remove(os.path.join(file_dir, file)) + + """ + Properties + """ + + @property + def num_instances(self) -> int: + return self._view.count + + @property + def data(self) -> RtxLidarData: + # update sensors if needed + self._update_outdated_buffers() + # return the data + return self._data + + @property + def frame(self) -> torch.tensor: + """Frame number when the measurement took place.""" + return self._frame + + @property + def render_product_paths(self) -> list[str]: + """The path of the render products for the cameras. + + This can be used via replicator interfaces to attach to writes or external annotator registry. + """ + return self._render_product_paths + + """ + Operations + """ + + def reset(self, env_ids: Sequence[int] | None = None): + if not self._is_initialized: + raise RuntimeError( + "Camera could not be initialized. Please ensure --enable_cameras is used to enable rendering." + ) + # reset the timestamps + super().reset(env_ids) + # resolve None + # note: cannot do smart indexing here since we do a for loop over data. + if env_ids is None: + env_ids = self._ALL_INDICES + # Reset the frame count + self._frame[env_ids] = 0 + + """ + Implementation. + """ + + def _initialize_impl(self): + """Initializes the sensor handles and internal buffers. + + This function creates handles and registers the optional output data fields with the replicator registry to + be able to access the data from the sensor. It also initializes the internal buffers to store the data. + + Raises: + RuntimeError: If the enable_camera flag is not set. + RuntimeError: If the number of camera prims in the view does not match the number of environments. + RuntimeError: If the provided USD prim is not a Camera. + """ + carb_settings_iface = carb.settings.get_settings() + if not carb_settings_iface.get("/isaaclab/cameras_enabled"): + raise RuntimeError( + "A camera was spawned without the --enable_cameras flag. Please use --enable_cameras to enable" + " rendering." + ) + + import omni.replicator.core as rep + + super()._initialize_impl() + # Create a view for the sensor + self._view = XFormPrim(self.cfg.prim_path, reset_xform_properties=False) + self._view.initialize() + # Check that sizes are correct + if self._view.count != self._num_envs: + raise RuntimeError( + f"Number of camera prims in the view ({self._view.count}) does not match" + f" the number of environments ({self._num_envs})." + ) + + # Create all env_ids buffer + self._ALL_INDICES = torch.arange(self._view.count, device=self._device, dtype=torch.long) + # Create frame count buffer + self._frame = torch.zeros(self._view.count, device=self._device, dtype=torch.long) + + # lidar_prim_paths = sim_utils.find_matching_prims(self.cfg.prim_path) + + self._render_product_paths: list[str] = list() + self._rep_registry: list[rep.annotators.Annotator] = [] + + # Obtain current stage + stage = omni.usd.get_context().get_stage() + + for lidar_prim_path in self._view.prim_paths: + # Get lidar prim + lidar_prim = stage.GetPrimAtPath(lidar_prim_path) + # Check if prim is a camera + if not lidar_prim.IsA(UsdGeom.Camera): + raise RuntimeError(f"Prim at path '{lidar_prim_path}' is not a Camera.") + # Add to list + sensor_prim = UsdGeom.Camera(lidar_prim) + self._sensor_prims.append(sensor_prim) + + init_params = { + "outputAzimuth": False, + "outputElevation": False, + "outputNormal": False, + "outputVelocity": False, + "outputBeamId": False, + "outputEmitterId": False, + "outputMaterialId": False, + "outputObjectId": False, + "outputTimestamp": True, # always turn on timestamp field + } + + # create annotator node + annotator_type = "RtxSensorCpuIsaacCreateRTXLidarScanBuffer" + rep_annotator = rep.AnnotatorRegistry.get_annotator(annotator_type) + # turn on any optional data type returns + for name in self.cfg.optional_data_types: + if name == "azimuth": + init_params["outputAzimuth"] = True + elif name == "elevation": + init_params["outputElevation"] = True + elif name == "normal": + init_params["outputNormal"] = True + elif name == "velocity": + init_params["outputVelocity"] = True + elif name == "beamId": + init_params["outputBeamId"] = True + elif name == "emitterId": + init_params["outputEmitterId"] = True + elif name == "materialId": + init_params["outputMaterialId"] = True + elif name == "objectId": + init_params["outputObjectId"] = True + + # transform the data output to be relative to sensor frame + if self.cfg.data_frame == "sensor": + init_params["transformPoints"] = False + + rep_annotator.initialize(**init_params) + + # Get render product + # From Isaac Sim 2023.1 onwards, render product is a HydraTexture so we need to extract the path + render_prod_path = rep.create.render_product(lidar_prim_path, [1, 1]) + if not isinstance(render_prod_path, str): + render_prod_path = render_prod_path.path + self._render_product_paths.append(render_prod_path) + + rep_annotator.attach(render_prod_path) + self._rep_registry.append(rep_annotator) + + # Debug draw + if self.cfg.debug_vis: + self.writer = rep.writers.get("RtxLidarDebugDrawPointCloudBuffer") + self.writer.attach([render_prod_path]) + + # Create internal buffers + self._create_buffers() + + def _create_buffers(self): + """Create buffers for storing data.""" + # create the data object + + # -- output data + # lazy allocation of data dictionary + # since the size of the output data is not known in advance, we leave it as None + # the memory will be allocated when the buffer() function is called for the first time. + self._data.output = {} + self._data.info = [{name: None for name in RTX_LIDAR_INFO_FIELDS.keys()} for _ in range(self._view.count)] + + def _update_buffers_impl(self, env_ids: Sequence[int]): + """Compute and fill sensor data buffers.""" + # Increment frame count + self._frame[env_ids] += 1 + data_all_lidar = list() + info_data_all_lidar: dict[str, list] = {} + + # iterate over all the annotators + for index in env_ids: + # get the output + output = self._rep_registry[index].get_data() + # process the output + data, info = self._process_annotator_output("", output) + + # add data to output + data_all_lidar.append(data) + + # store the info + for info_key, info_value in info.items(): + if info_key in RTX_LIDAR_INFO_FIELDS.keys(): + if info_key == "transform": + self._data.info[index][info_key] = torch.tensor(info_value, device=self.device) + else: + self._data.info[index][info_key] = info_value + else: + if info_key not in info_data_all_lidar: + info_data_all_lidar[info_key] = [torch.tensor(info_value, device=self._device)] + else: + info_data_all_lidar[info_key].append(torch.tensor(info_value, device=self._device)) + + # concatenate the data along the batch dimension + self._data.output["data"] = torch.stack(data_all_lidar, dim=0) + + for key in info_data_all_lidar: + self._data.output[key] = torch.stack(info_data_all_lidar[key], dim=0) + + def _process_annotator_output(self, name: str, output: Any) -> tuple[torch.tensor, dict | None]: + """Process the annotator output. + + This function is called after the data has been collected from all the cameras. + """ + # extract info and data from the output + if isinstance(output, dict): + data = output["data"] + info = output["info"] + else: + data = output + info = None + # convert data into torch tensor + data = convert_to_torch(data, device=self.device) + + # process data for different segmentation types + # Note: Replicator returns raw buffers of dtype int32 for segmentation types + # so we need to convert them to uint8 4 channel images for colorized types + + return data, info + + """ + Internal simulation callbacks. + """ + + def _invalidate_initialize_callback(self, event): + """Invalidates the scene elements.""" + # call parent + super()._invalidate_initialize_callback(event) + # set all existing views to None to invalidate them + self._view = None diff --git a/source/isaaclab/isaaclab/sensors/rtx_lidar/rtx_lidar_cfg.py b/source/isaaclab/isaaclab/sensors/rtx_lidar/rtx_lidar_cfg.py new file mode 100644 index 00000000000..236b3c00047 --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/rtx_lidar/rtx_lidar_cfg.py @@ -0,0 +1,57 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from dataclasses import MISSING +from typing import Literal + +from isaaclab.sensors import SensorBaseCfg +from isaaclab.sim import LidarCfg +from isaaclab.utils import configclass + +from .rtx_lidar import RtxLidar + + +@configclass +class RtxLidarCfg(SensorBaseCfg): + """Configuration for the RtxLidar sensor.""" + + @configclass + class OffsetCfg: + """The offset pose of the sensor's frame from the sensor's parent frame.""" + + pos: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Translation w.r.t. the parent frame. Defaults to (0.0, 0.0, 0.0).""" + rot: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) + """Quaternion rotation (w, x, y, z) w.r.t. the parent frame. Defaults to (1.0, 0.0, 0.0, 0.0).""" + + class_type: type = RtxLidar + + offset: OffsetCfg = OffsetCfg() + """The offset pose of the sensor's frame from the sensor's parent frame. Defaults to identity. + + Note: + The parent frame is the frame the sensor attaches to. For example, the parent frame of a + camera at path ``/World/envs/env_0/Robot/Camera`` is ``/World/envs/env_0/Robot``. + """ + optional_data_types: list[ + Literal["azimuth", "beamId", "elevation", "emitterId", "index", "materialId", "normal", "objectId", "velocity"] + ] = [] + """The optional output data types to include in RtxLidarData. + + Please refer to the :class:'RtxLidar' and :class:'RtxLidarData' for a list and description of available data types. + """ + data_frame: Literal["world", "sensor"] = "world" + """The frame to represent the output.data. + + If 'world' the output.data will be in the world frame. If 'sensor' the output.data will be in the sensor frame. + """ + spawn: LidarCfg = MISSING + """Spawn configuration for the asset. + + If None, then the prim is not spawned by the asset. Instead, it is assumed that the + asset is already present in the scene. + """ diff --git a/source/isaaclab/isaaclab/sensors/rtx_lidar/rtx_lidar_data.py b/source/isaaclab/isaaclab/sensors/rtx_lidar/rtx_lidar_data.py new file mode 100644 index 00000000000..1102d5188b3 --- /dev/null +++ b/source/isaaclab/isaaclab/sensors/rtx_lidar/rtx_lidar_data.py @@ -0,0 +1,64 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from dataclasses import dataclass +from typing import Any + +RTX_LIDAR_INFO_FIELDS = { + "numChannels": int, + "numEchos": int, + "numReturnsPerScan": int, + "renderProductPath": str, + "ticksPerScan": int, + "transform": torch.Tensor, +} + + +@dataclass +class RtxLidarData: + """Data container for the RtxLidar sensor.""" + + info: list[dict[str, Any]] = None + """The static descriptive information from the lidar sensor. + + Each dictionary corresponds to an instance of a lidar and contains the following fields from the + "RtxSensorCpuIsaacCreateRTXLidarScanBuffer" annotator: + numChannels: The maximum number of channels. + numEchos: The maximum number echos. + numReturnsPerScan: The maximum number of returns possible in the output. + renderProductPath: The render product from the camera prim sensor. + ticksPerScan: The maximum number of ticks. + transform: Transform of the latest data added to the scan buffer for transforming the data to world space. + + The product of ticksPerScan, numChannels, and numEchos will be the same as the number of returns if you initialize + the annotator with annotator.initialize(keepOnlyPositiveDistance=False) before attaching the render product. + """ + output: dict[str, torch.Tensor] = None + """The data that changes every sample. Some fields of the out will always be returned and some are optionally + returned when configured in RtxLidarCfg.optional_data_types. + + The following keys will ALWAYS be returned: + data: The position [x,y,z] of each return in meter expressed in the RtxLidarCfg.data_frame. If 'world' the + data will be returned relative to simulation world. If 'sensor' the data will be returned relative to + sensor frame. + distance: The distance of the return hit from sensor origin in meters. + intensity: The intensity value in the range [0.0, 1.0] of each return. + timestamp: The time since sensor creation time in nanoseconds for each return. + + The following keys will OPTIONALLY be returned: + azimuth: The horizontal polar angle (radians) of the return. + elevation: the vertical polar angle (radians) of the return. + normal: The normal at the hit location in world coordinates. + velocity: The normalized velocity at the hit location in world coordinates. + emmitterId: Same as the channel unless the RTX Lidar Config Parameters is set up so emitters fire through + different channels. + materialId: The sensor material Id at the hit location. Same as index from rtSensorNameToIdMap setting in the RTX Sensor Visual Materials. + objectId: The object Id at the hit location. The objectId can be used to get the prim path of the object with the following code: + from omni.syntheticdata._syntheticdata import acquire_syntheticdata_interface + primpath = acquire_syntheticdata_interface().get_uri_from_instance_segmentation_id(object_id) + """ diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/__init__.py b/source/isaaclab/isaaclab/sim/spawners/sensors/__init__.py index e2dd445788e..be24f75d8f4 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/__init__.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/__init__.py @@ -11,5 +11,5 @@ """ -from .sensors import spawn_camera -from .sensors_cfg import FisheyeCameraCfg, PinholeCameraCfg +from .sensors import spawn_camera, spawn_lidar +from .sensors_cfg import FisheyeCameraCfg, LidarCfg, PinholeCameraCfg diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py index b56134bfdc0..c9edd1fc122 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py @@ -5,12 +5,14 @@ from __future__ import annotations +import json +import os from typing import TYPE_CHECKING import isaacsim.core.utils.prims as prim_utils import omni.kit.commands import omni.log -from pxr import Sdf, Usd +from pxr import Gf, Sdf, Usd from isaaclab.sim.utils import clone from isaaclab.utils import to_camel_case @@ -140,3 +142,77 @@ def spawn_camera( prim.GetAttribute(prim_prop_name).Set(param_value) # return the prim return prim_utils.get_prim_at_path(prim_path) + + +@clone +def spawn_lidar( + prim_path: str, + cfg: sensors_cfg.LidarCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0), +) -> Usd.Prim: + """Create a USD Camera prim used for RTX Lidar models. + This function creates an RTX lidar model attached to a USD Camera prim. The RTX lidar model is configured from json + files located within ``omni.isaac.core``. + Custom configurations for the RTX lidar are passed in via the :class:`LidarCfg`. By setting the `lidar_type` to + custom and providing a `sensor_profile` dictionary a json configuration file will be created and passed to the + kit commands responsible for the RTX lidar creation. + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which case + this is set to the origin. + orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None, + in which case this is set to identity. + Returns: + The created prim. + Raises: + ValueError: If the LidarCfg.sensor_profile is None when LidarCfg.lidar_type == "Custom" + RuntimeError: If the creation of the RTXLidar fails + ValueError: If a prim already exists at the given path. + """ + + if cfg.lidar_type == "Custom": + if cfg.sensor_profile is None: + raise ValueError("LidarCfg sensor_profile cannot be none for lidar_type: Custom") + + # make directories + if not os.path.isdir(cfg.sensor_profile_temp_dir): + os.makedirs(cfg.sensor_profile_temp_dir) + + # create file path + file_name = cfg.sensor_profile_temp_prefix + ".json" + file_path = os.path.join(cfg.sensor_profile_temp_dir, file_name) + + # Check for tempfiles and remove + while os.path.isfile(file_path): + os.remove(file_path) + + # Write to file + with open(file_path, "w") as outfile: + json.dump(cfg.sensor_profile, outfile) + + print("Custom: ") + config = file_path.split("/")[-1].split(".")[0] + print(file_path) + else: + config = cfg.lidar_type + + if not prim_utils.is_prim_path_valid(prim_path): + # prim_utils.create_prim(prim_path, "Camera", translation=translation, orientation=orientation) + _, sensor = omni.kit.commands.execute( + "IsaacSensorCreateRtxLidar", + path=prim_path, + parent=None, + config=config, + translation=translation, + orientation=Gf.Quatd(orientation[0], orientation[1], orientation[2], orientation[3]), + ) + if not sensor.IsValid(): + raise RuntimeError("IsaacSensorCreateRtxLidar failed to create a USD.Prim") + + else: + raise ValueError(f"A prim already exists at path: '{prim_path}'.") + + return sensor diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py index cff5f9be17f..5f247076dab 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py @@ -5,9 +5,11 @@ from __future__ import annotations +import os from collections.abc import Callable -from typing import Literal +from typing import Any, Literal +from isaaclab import ISAACLAB_EXT_DIR from isaaclab.sim.spawners.spawner_cfg import SpawnerCfg from isaaclab.utils import configclass @@ -233,3 +235,81 @@ class FisheyeCameraCfg(PinholeCameraCfg): fisheye_polynomial_f: float = 0.0 """Sixth component of fisheye polynomial. Defaults to 0.0.""" + + +@configclass +class LidarCfg(SpawnerCfg): + """ + Lidar Configuration Table + +-------------+-----------+-----------------------------------+---------------------------+ + | Manufacturer| Model | UI Name | Config Name | + +=============+===========+===================================+===========================+ + | HESAI |PandarXT-32| PandarXT-32 10hz | Hesai_XT32_SD10 | + +-------------+-----------+-----------------------------------+---------------------------+ + | Ouster | OS0 | OS0 128 10hz @ 1024 resolution | OS0_128ch10hz1024res | + | | | OS0 128 10hz @ 2048 resolution | OS0_128ch10hz2048res | + | | | OS0 128 10hz @ 512 resolution | OS0_128ch10hz512res | + | | | OS0 128 20hz @ 1024 resolution | OS0_128ch20hz1024res | + | | | OS0 128 20hz @ 512 resolution | OS0_128ch20hz512res | + +-------------+-----------+-----------------------------------+---------------------------+ + | Ouster | OS1 | OS1 32 10hz @ 1024 resolution | OS1_32ch10hz1024res | + | | | OS1 32 10hz @ 2048 resolution | OS1_32ch10hz2048res | + | | | OS1 32 10hz @ 512 resolution | OS1_32ch10hz512res | + | | | OS1 32 20hz @ 1024 resolution | OS1_32ch20hz1024res | + | | | OS1 32 20hz @ 512 resolution | OS1_32ch20hz512res | + +-------------+-----------+-----------------------------------+---------------------------+ + | SICK | TiM781 | SICK TiM781 | Sick_TiM781 | + +-------------+-----------+-----------------------------------+---------------------------+ + | SLAMTEC |RPLidar S2E| RPLidar S2E | RPLIDAR_S2E | + +-------------+-----------+-----------------------------------+---------------------------+ + | Velodyne | VLS-128 | Velodyne VLS-128 | Velodyne_VLS128 | + +-------------+-----------+-----------------------------------+---------------------------+ + | ZVISION | ML-30s+ | ML-30s+ | ZVISION_ML30S | + | | ML-Xs | ML-Xs | ZVISION_MLXS | + +-------------+-----------+-----------------------------------+---------------------------+ + | NVIDIA | Generic | Rotating | Example_Rotary | + | | Generic | Solid State | Example_Solid_State | + | | Debug | Simple Solid State | Simple_Example_Solid_State| + +-------------+-----------+-----------------------------------+---------------------------+ + """ + + func = sensors.spawn_lidar + """The RTX lidar spawn function.""" + + lidar_type: str = "Example_Rotary" + """The name of the lidar sensor profile. Defaults to Example_Rotatry. + There are many built in configuration files specified by LidarType below. + If a user want to create a custom configuration file set lidar_type="Custom" and create a sensor_profile dictionary.""" + + class LidarType: + """Class variables for autocompletion""" + + HESAI_PandarXT_32 = "Hesai_XT32_SD10" + OUSTER_OS0_128_10HZ_1024RES = "OS0_128ch10hz1024res" + OUSTER_OS0_128_10HZ_2048RES = "OS0_128ch10hz2048res" + OUSTER_OS0_128_10HZ_512RES = "OS0_128ch10hz512res" + OUSTER_OS0_128_20HZ_1024RES = "OS0_128ch20hz1024res" + OUSTER_OS0_128_20HZ_512RES = "OS0_128ch20hz512res" + OUSTER_OS1_32_10HZ_1024RES = "OS1_32ch10hz1024res" + OUSTER_OS1_32_10HZ_2048RES = "OS1_32ch10hz2048res" + OUSTER_OS1_32_10HZ_512RES = "OS1_32ch10hz512res" + OUSTER_OS1_32_20HZ_1024RES = "OS1_32ch20hz1024res" + OUSTER_OS1_32_20HZ_512RES = "OS1_32ch20hz512res" + SICK_TIM781 = "Sick_TiM781" + SLAMTEC_RPLIDAR_S2E = "RPLIDAR_S2E" + VELODYNE_VLS128 = "Velodyne_VLS128" + ZVISION_ML30S = "ZVISION_ML30S" + ZVISION_MLXS = "ZVISION_MLXS" + EXAMPLE_ROTARY = "Example_Rotary" + EXAMPLE_SOLID_STATE = "Example_Solid_State" + SIMPLE_EXAMPLE_SOLID_STATE = "Simple_Example_Solid_State" + + sensor_profile: dict[str, Any] | None = None + """Custom lidar parameters to use if lidar_type="Custom" + see https://docs.omniverse.nvidia.com/kit/docs/omni.sensors.nv.lidar/latest/lidar_extension.html""" + + sensor_profile_temp_dir: str = os.path.abspath(os.path.join(ISAACLAB_EXT_DIR, "isaaclab/sensors/rtx_lidar")) + """The location of the generated custom sensor profile json file.""" + + sensor_profile_temp_prefix: str = "Temp_Config_" + """The custom sensor profile json file prefix. This is used for cleanup of the custom sensor profile.""" diff --git a/source/isaaclab/test/sensors/test_rtx_lidar.py b/source/isaaclab/test/sensors/test_rtx_lidar.py new file mode 100644 index 00000000000..51e84d4b020 --- /dev/null +++ b/source/isaaclab/test/sensors/test_rtx_lidar.py @@ -0,0 +1,258 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# ignore private usage of variables warning +# pyright: reportPrivateUsage=none + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +app_launcher = AppLauncher(headless=True, enable_cameras=True) +simulation_app = app_launcher.app + +import copy +import json +import numpy as np +import os +import scipy.spatial.transform as tf +import torch + +import isaacsim.core.utils.stage as stage_utils +import pytest +from isaacsim.core.utils.extensions import get_extension_path_from_name +from pxr import Usd, UsdGeom + +import isaaclab.sim as sim_utils +from isaaclab.sensors.rtx_lidar import RTX_LIDAR_INFO_FIELDS, RtxLidar, RtxLidarCfg +from isaaclab.sim import build_simulation_context +from isaaclab.terrains.trimesh.utils import make_border, make_plane +from isaaclab.terrains.utils import create_prim_from_mesh +from isaaclab.utils.math import convert_quat + +POSITION = (0.0, 0.0, 0.3) +QUATERNION = (0.0, 0.3461835, 0.0, 0.9381668) +# QUATERNION = (0.0, 0.0,0.0,1.0) + +# load example json +EXAMPLE_ROTARY_PATH = os.path.abspath( + os.path.join( + get_extension_path_from_name("isaacsim.sensors.rtx"), + "data/lidar_configs/NVIDIA/Simple_Example_Solid_State.json", + ) +) + + +@pytest.fixture +def sim(request): + """Create simulation context with the specified device.""" + device = request.getfixturevalue("device") + with build_simulation_context(device=device, dt=0.01) as sim: + sim._app_control_on_stop_handle = None + # Ground-plane + mesh = make_plane(size=(10, 10), height=0.0, center_zero=True) + border = make_border(size=(10, 10), inner_size=(5, 5), height=2.0, position=(0.0, 0.0, 0.0)) + + create_prim_from_mesh("/World/defaultGroundPlane", mesh) + for i, box in enumerate(border): + create_prim_from_mesh(f"/World/defaultBoarder{i}", box) + # load stage + stage_utils.update_stage() + yield sim + + +@pytest.fixture +def lidar_cfg(request): + # configure lidar + return RtxLidarCfg( + prim_path="/World/Lidar", + debug_vis=not app_launcher._headless, + optional_data_types=[ + "azimuth", + "elevation", + "emitterId", + "index", + "materialId", + "normal", + "objectId", + "velocity", + ], + spawn=sim_utils.LidarCfg(lidar_type=sim_utils.LidarCfg.LidarType.EXAMPLE_ROTARY), + ) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_lidar_init(sim, device, lidar_cfg): + """Test lidar initialization and data population.""" + # Create lidar + lidar = RtxLidar(cfg=lidar_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if lidar is initialized + assert lidar.is_initialized + # Check if lidar prim is set correctly and that it is a camera prim + assert lidar._sensor_prims[0].GetPath().pathString == lidar_cfg.prim_path + assert isinstance(lidar._sensor_prims[0], UsdGeom.Camera) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + lidar.update(sim.get_physics_dt(), force_recompute=True) + # check info data + for info_key, info_value in lidar.data.info[0].items(): + assert info_key in RTX_LIDAR_INFO_FIELDS.keys() + assert isinstance(info_value, RTX_LIDAR_INFO_FIELDS[info_key]) + + # check lidar data + for data_key, data_value in lidar.data.output.items(): + if data_key in lidar_cfg.optional_data_types: + assert data_value.shape[1] > 0 + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_offset_lidar_init(sim, device, lidar_cfg): + """Test lidar offset configuration.""" + lidar_cfg_offset = copy.deepcopy(lidar_cfg) + lidar_cfg_offset.offset = RtxLidarCfg.OffsetCfg(pos=POSITION, rot=QUATERNION) + lidar_cfg_offset.prim_path = "/World/LidarOffset" + lidar = RtxLidar(lidar_cfg_offset) + + # Play sim + sim.reset() + + # Retrieve lidar pose using USD API + prim_tf = lidar._sensor_prims[0].ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + prim_tf = np.transpose(prim_tf) + + # check that transform is set correctly + np.testing.assert_allclose(prim_tf[0:3, 3], lidar_cfg_offset.offset.pos) + np.testing.assert_allclose( + convert_quat(tf.Rotation.from_matrix(prim_tf[:3, :3]).as_quat(), "wxyz"), + lidar_cfg_offset.offset.rot, + rtol=1e-5, + atol=1e-5, + ) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + lidar.update(sim.get_physics_dt()) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_multiple_lidar_init(sim, device, lidar_cfg): + """Test multiple lidar initialization and check info and data outputs.""" + + sim._app_control_on_stop_handle = None + lidar_cfg_1 = copy.deepcopy(lidar_cfg) + lidar_cfg_1.prim_path = "/World/Lidar1" + lidar_1 = RtxLidar(lidar_cfg_1) + + lidar_cfg_2 = copy.deepcopy(lidar_cfg) + lidar_cfg_2.prim_path = "/World/Lidar2" + lidar_2 = RtxLidar(lidar_cfg_2) + + # play sim + sim.reset() + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + # Simulate physics + for i in range(10): + # perform rendering + sim.step() + # update lidar + lidar_1.update(sim.get_physics_dt()) + lidar_2.update(sim.get_physics_dt()) + # check lidar info + for lidar_info_key in lidar_1.data.info[0].keys(): + info1 = lidar_1.data.info[0][lidar_info_key] + info2 = lidar_2.data.info[0][lidar_info_key] + if isinstance(info1, torch.Tensor): + torch.testing.assert_close(info1, info2) + else: + if lidar_info_key == "renderProductPath": + assert info1 == info2.split("_")[0] + else: + assert info1 == info2 + # check lidar data shape both instances should produce the same amount of data + for lidar_data_key in lidar_1.data.output.keys(): + data1 = lidar_1.data.output[lidar_data_key] + data2 = lidar_2.data.output[lidar_data_key] + assert data1.shape == data2.shape + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_custom_lidar_init(sim, device, lidar_cfg): + """Test custom lidar initialization, data population, and cleanup.""" + # Create custom lidar profile dictionary + with open(EXAMPLE_ROTARY_PATH) as json_file: + sensor_profile = json.load(json_file) + + custom_lidar_cfg = copy.deepcopy(lidar_cfg) + custom_lidar_cfg.spawn = sim_utils.LidarCfg(lidar_type="Custom", sensor_profile=sensor_profile) + # Create custom lidar + lidar = RtxLidar(cfg=custom_lidar_cfg) + # Check simulation parameter is set correctly + assert sim.has_rtx_sensors() + # Play sim + sim.reset() + # Check if lidar is initialized + assert lidar.is_initialized + # Check if lidar prim is set correctly and that it is a camera prim + assert lidar._sensor_prims[0].GetPath().pathString == lidar_cfg.prim_path + assert isinstance(lidar._sensor_prims[0], UsdGeom.Camera) + + # Simulate for a few steps + # note: This is a workaround to ensure that the textures are loaded. + # Check "Known Issues" section in the documentation for more details. + for _ in range(5): + sim.step() + + # Simulate physics + for _ in range(10): + # perform rendering + sim.step() + # update camera + lidar.update(sim.get_physics_dt(), force_recompute=True) + # check info data + for info_key, info_value in lidar.data.info[0].items(): + assert info_key in RTX_LIDAR_INFO_FIELDS.keys() + assert isinstance(info_value, RTX_LIDAR_INFO_FIELDS[info_key]) + + # check lidar data + for data_key, data_value in lidar.data.output.items(): + if data_key in lidar_cfg.optional_data_types: + assert data_value.shape[1] > 0 + + del lidar + + # check proper file cleanup + custom_profile_name = lidar_cfg.spawn.sensor_profile_temp_prefix + custom_profile_dir = lidar_cfg.spawn.sensor_profile_temp_dir + files = os.listdir(custom_profile_dir) + for file in files: + assert custom_profile_name not in file + + +if __name__ == "__main__": + pytest.main([__file__, "-v", "--maxfail=1"])