Merge pull request #4 from stereolabs/add-resolution-fps-params

Add resolution fps params

Author: mattrouss

exts/sl.sensor.camera/config/extension.toml

@@ -1,7 +1,7 @@
 
 [package]
 # Semantic Versioning is used: https://semver.org/
-version = "1.0.2"
+version = "1.0.3"
 
 # Lists people or organizations that are considered the "authors" of the package.
 authors = ["Stereolabs"]
@@ -13,6 +13,9 @@ description="Streams virtual ZED camera data to the ZED SDK. "
 # Path (relative to the root) or content of readme markdown file for UI.
 readme  = "docs/README.md"
 
+# Path (relative to the root) or content of changelog markdown file for UI.
+changelog  = "docs/CHANGELOG.md"
+
 # URL of the extension source repository.
 repository="https://github.com/stereolabs/zed-isaac-sim"
 

exts/sl.sensor.camera/docs/CHANGELOG.md

@@ -2,6 +2,13 @@
 
 The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
+## [1.0.3] - 2024-02-28
+- Add FPS and resolution parameters in Isaac Sim GUI
+- Add throttling of data fetch to improve performance on low FPS
+
+## [1.0.2] - 2024-02-16
+- Improve ZED Camera extension streaming performance
+
 ## [1.0.1] - 2023-12-19
 - Update Stereolabs logo
 

exts/sl.sensor.camera/docs/README.md

@@ -29,3 +29,6 @@ Once you press play in the simulation, you can connect the virtual camera to the
 Please refer to the [StereoLabs' website for the full documentation](https://www.stereolabs.com/docs/isaac-sim/setting_up_zed_isaac_sim).
 
 
+### TODO
+
+- ZED Camera extension parameters are not reset after pressing Stop in Isaac Sim UI, but requires the node to be released. TODO: Clean parameters between each play/stop session

exts/sl.sensor.camera/sl/sensor/camera/nodes/SlCameraStreamer.ogn

@@ -23,6 +23,22 @@
                     "uiName": "ZED Camera prim"
                 }
             },
+            "fps": {
+                "type" : "uint",
+                "description": "Camera stream frame rate. Can be either 60, 30 or 15.",
+                "metadata": {
+                    "uiName": "FPS"
+                },
+                "default": 30
+            },
+            "resolution": {
+                "type" : "token",
+                "description": "Camera stream resolution. Can be either HD1080, HD720 or VGA",
+                "metadata": {
+                    "allowedTokens": ["HD1080", "HD720", "VGA"]
+                },
+                "default" : "HD720"
+            },
             "streaming_port": {
                 "type": "uint",
                 "description": "Streaming port - unique per camera",

exts/sl.sensor.camera/sl/sensor/camera/nodes/SlCameraStreamer.py

@@ -3,15 +3,13 @@
 """
 import carb
 from dataclasses import dataclass
-# imports needed if you want to create viewports of the camera
-# from omni.kit.viewport.utility import get_viewport_from_window_name
-# from omni.kit.viewport.utility import create_viewport_window
 import omni.replicator.core as rep
 from omni.isaac.core_nodes.bindings import _omni_isaac_core_nodes
 from omni.isaac.core.prims import XFormPrim
 from pxr import Vt
 from omni.isaac.core.utils.prims import is_prim_path_valid, get_prim_at_path
 from omni.isaac.sensor import _sensor
+
 from sl.sensor.camera.pyzed_sim_streamer import ZEDSimStreamer, ZEDSimStreamerParams 
 import time
 import copy
@@ -21,10 +19,9 @@
 LEFT_CAMERA_PATH = "/base_link/ZED_X/CameraLeft"
 RIGHT_CAMERA_PATH = "/base_link/ZED_X/CameraRight"
 IMU_PRIM_PATH = "/base_link/ZED_X/Imu_Sensor"
-IMAGE_WIDTH = 1920
-IMAGE_HEIGHT = 1080
 CHANNELS = 3
-    
+
+
 class SlCameraStreamer:
     """
          Streams camera data to the ZED SDK
@@ -42,6 +39,7 @@ class State:
         pyzed_streamer = None
         start_time = -1.0
         last_timestamp = 0.0
+        target_fps = 30
         camera_prim_name = None
         override_simulation_time = False
         imu_sensor_interface = None
@@ -55,12 +53,12 @@ def internal_state() -> State:
         return SlCameraStreamer.State()
 
     @staticmethod
-    def get_render_product_path(camera_path :str, render_product_size = [IMAGE_WIDTH, IMAGE_HEIGHT], force_new=True):
+    def get_render_product_path(camera_path :str, render_product_size = [1280, 720], force_new=True):
         """Helper function to get render product path
 
         Args:
             camera_path (str): the path of the camera prim
-            render_product_size (list, optional): the resolution of the image. Defaults to [IMAGE_WIDTH, IMAGE_HEIGHT].
+            render_product_size (list, optional): the resolution of the image. Defaults to HD720.
             force_new (bool, optional): forces the creation of a new render product. Defaults to True.
 
         Returns:
@@ -71,7 +69,7 @@ def get_render_product_path(camera_path :str, render_product_size = [IMAGE_WIDTH
         return render_product_path
 
     @staticmethod
-    def get_image_data(annotator):
+    def get_image_data(annotator, data_shape: tuple):
         """Helper function to fetch image data.
 
         Args:
@@ -88,14 +86,22 @@ def get_image_data(annotator):
                 result = False
                 return data, result
             data = data[:, :, :3]
-            if not data.shape == (IMAGE_HEIGHT, IMAGE_WIDTH, CHANNELS):
+            if not data.shape == data_shape:
                 result = False
         except:
             return None, False
         return data, result
 
     @staticmethod
     def check_camera(camera_prim_path : str):
+        """Check if the camera properties are valid. If not, set them to default values.
+
+        Args:
+            camera_prim_path (str): the path of the camera prim
+
+        Returns:
+            bool: True if the camera properties are valid, False otherwise
+        """
         result = False
         default_fl, default_apt_h, default_apt_v = 2.208, 5.76, 3.24
         default_projection_type = "pinhole"
@@ -119,6 +125,33 @@ def check_camera(camera_prim_path : str):
                 result = True
         return result
 
+    @staticmethod
+    def get_resolution(camera_resolution: str):
+        """Get the resolution of the camera
+        
+        Args:
+            camera_resolution (str): the resolution name of the camera
+            
+        Returns:
+            list: the resolution of the camera
+        """
+        if camera_resolution == "HD1080":
+            result = [1920, 1080]
+        elif camera_resolution == "HD720":
+            result = [1280, 720]
+        elif camera_resolution == "VGA":
+            result = [672, 376]
+        else:
+            result = None
+        return result
+
+    @staticmethod
+    def check_frame_rate(camera_frame_rate: int):
+        if camera_frame_rate not in [15, 30, 60]:
+            carb.log_warn(f"Invalid frame rate passed: {camera_frame_rate}. Defaulting to 30.")
+            return 30
+        return camera_frame_rate
+
 
     @staticmethod
     def compute(db) -> bool:
@@ -129,7 +162,7 @@ def compute(db) -> bool:
             if db.inputs.camera_prim is None:
                 carb.log_error("Invalid Camera prim")
                 return
-            
+
             # Check port
             port = db.inputs.streaming_port
             if  port <= 0 or port %2 == 1:
@@ -152,21 +185,28 @@ def compute(db) -> bool:
             if not res:
                 carb.log_warn(f"[{db.inputs.camera_prim[0].GetPrimPath()}] Invalid or non existing zed camera, try to re-import your camera prim.")
 
+            # Check resolution and retrieve width and height
+            resolution = SlCameraStreamer.get_resolution(db.inputs.resolution)
+            if resolution is None:
+                resolution = [1280, 720]
+                carb.log_warn(f"Invalid resolution passed. Defaulting to HD720.")
+
+            # Check frame rate
+            db.internal_state.target_fps = SlCameraStreamer.check_frame_rate(db.inputs.fps)
+
             if (db.internal_state.annotator_left is None):
-                render_product_path_left = SlCameraStreamer.get_render_product_path(left_cam_path)
+                render_product_path_left = SlCameraStreamer.get_render_product_path(left_cam_path, render_product_size=resolution)
                 db.internal_state.annotator_left = rep.AnnotatorRegistry.get_annotator("rgb", device="cuda", do_array_copy=False)
                 db.internal_state.annotator_left.attach([render_product_path_left])
 
             if (db.internal_state.annotator_right is None):
-                render_product_path_right = SlCameraStreamer.get_render_product_path(right_cam_path)
+                render_product_path_right = SlCameraStreamer.get_render_product_path(right_cam_path, render_product_size=resolution)
                 db.internal_state.annotator_right = rep.AnnotatorRegistry.get_annotator("rgb", device="cuda", do_array_copy=False)
                 db.internal_state.annotator_right.attach([render_product_path_right])
 
             db.internal_state.imu_prim_path = db.inputs.camera_prim[0].pathString + IMU_PRIM_PATH
             db.internal_state.imu_prim = XFormPrim(prim_path=db.internal_state.imu_prim_path)
 
-            carb.settings.get_settings().set("/omni/replicator/captureOnPlay", False)
-
             # Setup streamer parameters
             db.internal_state.pyzed_streamer = ZEDSimStreamer()
 
@@ -178,8 +218,9 @@ def compute(db) -> bool:
                 carb.log_warn(f"Invalid serial number passed. Defaulting to: {serial_number}.")
 
             streamer_params = ZEDSimStreamerParams()
-            streamer_params.image_width = IMAGE_WIDTH
-            streamer_params.image_height = IMAGE_HEIGHT
+            streamer_params.image_width = resolution[0]
+            streamer_params.image_height = resolution[1]
+            streamer_params.fps = db.internal_state.target_fps
             streamer_params.alpha_channel_included = False
             streamer_params.rgb_encoded = True
             streamer_params.serial_number = serial_number
@@ -190,24 +231,38 @@ def compute(db) -> bool:
             # set state to initialized
             carb.log_info(f"Streaming camera {db.internal_state.camera_prim_name} at port {port} and using serial number {serial_number}.")
             db.internal_state.invalid_images_count = 0
+            db.internal_state.last_timestamp = 0.0
+            db.internal_state.data_shape = (resolution[1], resolution[0], CHANNELS)
             db.internal_state.initialized = True
 
         try:
             ts : int = 0
             if db.internal_state.initialized is True:
                 left, right = None, None
                 current_time = db.internal_state.core_nodes_interface.get_sim_time()
+
+                # Reset last_timestamp between different play sessions
+                if db.internal_state.last_timestamp > current_time:
+                    db.internal_state.last_timestamp = current_time
+
+                delta_time = current_time - db.internal_state.last_timestamp
+
+                # Skip data fetch if the time between frames is too short
+                if delta_time < 1.0 / db.internal_state.target_fps:
+                    return False
 
                 # we fetch image data from annotators - we do it sequentially to avoid fetching both if one fails
-                left, res = SlCameraStreamer.get_image_data(db.internal_state.annotator_left)
+                left, res = SlCameraStreamer.get_image_data(db.internal_state.annotator_left,
+                                                            db.internal_state.data_shape)
                 if res == False:
                     db.internal_state.invalid_images_count +=1
                     if db.internal_state.invalid_images_count >= 10:
                         carb.log_warn(f"{db.internal_state.camera_prim_name} - Left camera retrieved unexpected "
                                       "data shape, skipping frame.")
                     return False # no need to continue compute
 
-                right, res = SlCameraStreamer.get_image_data(db.internal_state.annotator_right)
+                right, res = SlCameraStreamer.get_image_data(db.internal_state.annotator_right,
+                                                             db.internal_state.data_shape)
                 if res == False:
                     db.internal_state.invalid_images_count +=1
                     if db.internal_state.invalid_images_count >= 10:

exts/sl.sensor.camera/sl/sensor/camera/ogn/SlCameraStreamerDatabase.py

@@ -7,7 +7,6 @@
 import omni.graph.core._omni_graph_core as _og
 import omni.graph.tools.ogn as ogn
 import traceback
-import carb
 import sys
 class SlCameraStreamerDatabase(og.Database):
     """Helper class providing simplified access to data on nodes of type sl.sensor.camera.ZED_Camera
@@ -19,6 +18,8 @@ class SlCameraStreamerDatabase(og.Database):
         Inputs:
             inputs.camera_prim
             inputs.exec_in
+            inputs.resolution
+            inputs.fps
             inputs.serial_number
             inputs.streaming_port
             inputs.use_system_time
@@ -33,6 +34,8 @@ class SlCameraStreamerDatabase(og.Database):
     INTERFACE = og.Database._get_interface([
         ('inputs:camera_prim', 'bundle', 0, 'ZED Camera prim', 'ZED Camera prim used to stream data', {}, True, None, False, ''),
         ('inputs:exec_in', 'execution', 0, 'ExecIn', 'Triggers execution', {ogn.MetadataKeys.DEFAULT: '0'}, True, 0, False, ''),
+        ('inputs:resolution', 'token', 0, 'Resolution', 'Camera stream resolution. Can be either HD1080, HD720 or VGA', {ogn.MetadataKeys.DEFAULT: 'HD720'}, True, "HD720", False, ''),
+        ('inputs:fps', 'uint', 0, 'FPS', 'Camera stream frame rate. Can be either 60, 30 or 15', {ogn.MetadataKeys.DEFAULT: '30'}, True, '30', False, ''),
         ('inputs:serial_number', 'uint', 0, 'Serial number', 'Serial number (identification) of the camera to stream, can be left to default. It must be of one of the compatible values: 20976320, 29123828, 25626933, 27890353, 25263213, 21116066, 27800035, 27706147', {ogn.MetadataKeys.DEFAULT: '20976320'}, True, 20976320, False, ''),
         ('inputs:streaming_port', 'uint', 0, 'Streaming port', 'Streaming port - unique per camera', {ogn.MetadataKeys.DEFAULT: '30000'}, True, 30000, False, ''),
         ('inputs:use_system_time', 'bool', 0, 'Use system time', 'Override simulation time with system time for image timestamps', {ogn.MetadataKeys.DEFAULT: 'false'}, True, False, False, ''),
@@ -45,15 +48,15 @@ def _populate_role_data(cls):
         role_data.inputs.exec_in = og.Database.ROLE_EXECUTION
         return role_data
     class ValuesForInputs(og.DynamicAttributeAccess):
-        LOCAL_PROPERTY_NAMES = {"exec_in", "serial_number", "streaming_port", "use_system_time", "_setting_locked", "_batchedReadAttributes", "_batchedReadValues"}
+        LOCAL_PROPERTY_NAMES = {"exec_in", "resolution", "fps", "serial_number", "streaming_port", "use_system_time", "_setting_locked", "_batchedReadAttributes", "_batchedReadValues"}
         """Helper class that creates natural hierarchical access to input attributes"""
         def __init__(self, node: og.Node, attributes, dynamic_attributes: og.DynamicAttributeInterface):
             """Initialize simplified access for the attribute data"""
             context = node.get_graph().get_default_graph_context()
             super().__init__(context, node, attributes, dynamic_attributes)
             self.__bundles = og.BundleContainer(context, node, attributes, [], read_only=True, gpu_ptr_kinds={})
-            self._batchedReadAttributes = [self._attributes.exec_in, self._attributes.serial_number, self._attributes.streaming_port, self._attributes.use_system_time]
-            self._batchedReadValues = [0, 20976320, 30000, False]
+            self._batchedReadAttributes = [self._attributes.exec_in, self._attributes.resolution, self._attributes.fps, self._attributes.serial_number, self._attributes.streaming_port, self._attributes.use_system_time]
+            self._batchedReadValues = [0, "HD720", "30", 20976320, 30000, False]
 
         @property
         def camera_prim(self) -> og.BundleContents:
@@ -69,28 +72,44 @@ def exec_in(self, value):
             self._batchedReadValues[0] = value
 
         @property
-        def serial_number(self):
+        def resolution(self):
             return self._batchedReadValues[1]
 
+        @resolution.setter
+        def resolution(self, value):
+            self._batchedReadValues[1] = value
+
+        @property
+        def fps(self):
+            return self._batchedReadValues[2]
+
+        @fps.setter
+        def fps(self, value):
+            self._batchedReadValues[2] = value
+
+        @property
+        def serial_number(self):
+            return self._batchedReadValues[3]
+
         @serial_number.setter
         def serial_number(self, value):
-            self._batchedReadValues[1] = value
+            self._batchedReadValues[3] = value
 
         @property
         def streaming_port(self):
-            return self._batchedReadValues[2]
+            return self._batchedReadValues[4]
 
         @streaming_port.setter
         def streaming_port(self, value):
-            self._batchedReadValues[2] = value
+            self._batchedReadValues[4] = value
 
         @property
         def use_system_time(self):
-            return self._batchedReadValues[3]
+            return self._batchedReadValues[5]
 
         @use_system_time.setter
         def use_system_time(self, value):
-            self._batchedReadValues[3] = value
+            self._batchedReadValues[5] = value
 
         def __getattr__(self, item: str):
             if item in self.LOCAL_PROPERTY_NAMES: