diff --git a/exts/sl.sensor.camera/bin/linux-x86_64/release/libsl_zed_sim_streamer.so b/exts/sl.sensor.camera/bin/linux-x86_64/release/libsl_zed_sim_streamer.so index 30e1bfb..54d0d73 100644 Binary files a/exts/sl.sensor.camera/bin/linux-x86_64/release/libsl_zed_sim_streamer.so and b/exts/sl.sensor.camera/bin/linux-x86_64/release/libsl_zed_sim_streamer.so differ diff --git a/exts/sl.sensor.camera/bin/windows-x86_64/release/sl_zed_sim_streamer.dll b/exts/sl.sensor.camera/bin/windows-x86_64/release/sl_zed_sim_streamer.dll index 91fcb8b..7f8614e 100644 Binary files a/exts/sl.sensor.camera/bin/windows-x86_64/release/sl_zed_sim_streamer.dll and b/exts/sl.sensor.camera/bin/windows-x86_64/release/sl_zed_sim_streamer.dll differ diff --git a/exts/sl.sensor.camera/bin/windows-x86_64/release/sl_zed_sim_streamer.lib b/exts/sl.sensor.camera/bin/windows-x86_64/release/sl_zed_sim_streamer.lib index 0641586..96ef43a 100644 Binary files a/exts/sl.sensor.camera/bin/windows-x86_64/release/sl_zed_sim_streamer.lib and b/exts/sl.sensor.camera/bin/windows-x86_64/release/sl_zed_sim_streamer.lib differ diff --git a/exts/sl.sensor.camera/config/extension.toml b/exts/sl.sensor.camera/config/extension.toml index 296f68e..be13cb7 100644 --- a/exts/sl.sensor.camera/config/extension.toml +++ b/exts/sl.sensor.camera/config/extension.toml @@ -1,7 +1,7 @@ [package] # Semantic Versioning is used: https://semver.org/ -version = "1.0.3" +version = "1.1.0" # Lists people or organizations that are considered the "authors" of the package. authors = ["Stereolabs"] diff --git a/exts/sl.sensor.camera/docs/CHANGELOG.md b/exts/sl.sensor.camera/docs/CHANGELOG.md index 0ae9082..9d9b372 100644 --- a/exts/sl.sensor.camera/docs/CHANGELOG.md +++ b/exts/sl.sensor.camera/docs/CHANGELOG.md @@ -2,7 +2,10 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). -## [1.0.3] - 2024-02-28 +## [1.1.0] - 2024-04-04 +- Add support for ZED SDK 4.1 + +## [1.0.3] - 2024-02-16 - Add FPS and resolution parameters in Isaac Sim GUI - Add throttling of data fetch to improve performance on low FPS diff --git a/exts/sl.sensor.camera/sl/sensor/camera/nodes/SlCameraStreamer.ogn b/exts/sl.sensor.camera/sl/sensor/camera/nodes/SlCameraStreamer.ogn index 373caf5..20f7b38 100644 --- a/exts/sl.sensor.camera/sl/sensor/camera/nodes/SlCameraStreamer.ogn +++ b/exts/sl.sensor.camera/sl/sensor/camera/nodes/SlCameraStreamer.ogn @@ -49,11 +49,11 @@ }, "serial_number": { "type": "uint", - "description": "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", + "description": "Serial number (identification) of the camera to stream, can be left to default. It must be of one of the compatible values: 40976320, 41116066, 49123828, 45626933, 47890353, 45263213, 47800035, 47706147", "metadata": { "uiName": "Serial number" }, - "default" : 20976320 + "default" : 40976320 }, "use_system_time": { "type": "bool", diff --git a/exts/sl.sensor.camera/sl/sensor/camera/nodes/SlCameraStreamer.py b/exts/sl.sensor.camera/sl/sensor/camera/nodes/SlCameraStreamer.py index ce2cbbb..6242721 100644 --- a/exts/sl.sensor.camera/sl/sensor/camera/nodes/SlCameraStreamer.py +++ b/exts/sl.sensor.camera/sl/sensor/camera/nodes/SlCameraStreamer.py @@ -15,7 +15,6 @@ import copy from sl.sensor.camera.ogn.SlCameraStreamerDatabase import SlCameraStreamerDatabase -STREAM_PORT = 30000 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" @@ -226,7 +225,11 @@ def compute(db) -> bool: streamer_params.serial_number = serial_number streamer_params.port = port streamer_params.codec_type = 1 - db.internal_state.pyzed_streamer.init(streamer_params) + + init_error = db.internal_state.pyzed_streamer.init(streamer_params) + if not init_error: + carb.log_error(f"Failed to initialize the ZED SDK streamer with serial number {serial_number}.") + return False # 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}.") @@ -239,6 +242,7 @@ def compute(db) -> bool: ts : int = 0 if db.internal_state.initialized is True: left, right = None, None + # Get simulation time in seconds current_time = db.internal_state.core_nodes_interface.get_sim_time() # Reset last_timestamp between different play sessions @@ -276,13 +280,14 @@ def compute(db) -> bool: if db.internal_state.start_time == -1: carb.log_info(f"{db.internal_state.camera_prim_name} - Starting stream to the ZED SDK") if db.internal_state.start_time == -1 or current_time < db.internal_state.last_timestamp: - db.internal_state.start_time = int(time.time() * 1000) + db.internal_state.start_time = int(time.time_ns()) carb.log_info(f"{db.internal_state.camera_prim_name} - Setting initial streaming time stamp to: {db.internal_state.start_time}") - ts = int(db.internal_state.start_time + current_time * 1000) + + ts = int(db.internal_state.start_time + current_time * 1000000000) # override with system time if db.internal_state.override_simulation_time: - ts = int(time.time() * 1000) + ts = int(time.time_ns()) # fetch IMU data if the imu prim is there - this check allows user to basically delete # their IMUand still have access to the image functionality without issues in Isaac Sim diff --git a/exts/sl.sensor.camera/sl/sensor/camera/ogn/SlCameraStreamerDatabase.py b/exts/sl.sensor.camera/sl/sensor/camera/ogn/SlCameraStreamerDatabase.py index c8f4ea8..3e5fe23 100644 --- a/exts/sl.sensor.camera/sl/sensor/camera/ogn/SlCameraStreamerDatabase.py +++ b/exts/sl.sensor.camera/sl/sensor/camera/ogn/SlCameraStreamerDatabase.py @@ -36,7 +36,7 @@ class SlCameraStreamerDatabase(og.Database): ('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: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: 40976320, 41116066, 49123828, 45626933, 47890353, 45263213, 47800035, 47706147', {ogn.MetadataKeys.DEFAULT: '40976320'}, True, 40976320, 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, ''), ]) @@ -56,7 +56,7 @@ def __init__(self, node: og.Node, attributes, dynamic_attributes: og.DynamicAttr 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.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] + self._batchedReadValues = [0, "HD720", "30", 40976320, 30000, False] @property def camera_prim(self) -> og.BundleContents: diff --git a/exts/sl.sensor.camera/sl/sensor/camera/ogn/docs/SlCameraStreamer.rst b/exts/sl.sensor.camera/sl/sensor/camera/ogn/docs/SlCameraStreamer.rst index e30e0ca..a132d27 100644 --- a/exts/sl.sensor.camera/sl/sensor/camera/ogn/docs/SlCameraStreamer.rst +++ b/exts/sl.sensor.camera/sl/sensor/camera/ogn/docs/SlCameraStreamer.rst @@ -42,7 +42,7 @@ Streams ZED camera data to the ZED SDK sl.sensor.camera.ZED_Camera Inputs ---------------------------------- +------------------------+-----------+-----------------+-----------+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ -| Name | Type | Default | Required? | Descripton | +| Name | Type | Default | Required? | Description | +========================+===========+=================+===========+============================================================================================================================================================================================================+ | inputs:camera_prim | bundle | None | **Y** | ZED Camera prim used to stream data | +------------------------+-----------+-----------------+-----------+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ @@ -54,11 +54,11 @@ sl.sensor.camera.ZED_Camera Inputs +------------------------+-----------+-----------------+-----------+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | | __default | 0 | | | +------------------------+-----------+-----------------+-----------+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ -| inputs:serial_number | uint | 20976320 | **Y** | 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 | +| inputs:serial_number | uint | 40976320 | **Y** | Serial number (identification) of the camera to stream, can be left to default. It must be of one of the compatible values: 40976320, 41116066, 49123828, 45626933, 47890353, 45263213, 47800035, 47706147 | +------------------------+-----------+-----------------+-----------+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | | uiName | Serial number | | | +------------------------+-----------+-----------------+-----------+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ -| | __default | 20976320 | | | +| | __default | 40976320 | | | +------------------------+-----------+-----------------+-----------+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | inputs:streaming_port | uint | 30000 | **Y** | Streaming port - unique per camera | +------------------------+-----------+-----------------+-----------+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ diff --git a/exts/sl.sensor.camera/sl/sensor/camera/ogn/tests/TestSlCameraStreamer.py b/exts/sl.sensor.camera/sl/sensor/camera/ogn/tests/TestSlCameraStreamer.py index d776a82..4c76149 100644 --- a/exts/sl.sensor.camera/sl/sensor/camera/ogn/tests/TestSlCameraStreamer.py +++ b/exts/sl.sensor.camera/sl/sensor/camera/ogn/tests/TestSlCameraStreamer.py @@ -40,7 +40,7 @@ def _attr_error(attribute: og.Attribute, usd_test: bool) -> str: self.assertTrue(test_node.get_attribute_exists("inputs:serial_number")) attribute = test_node.get_attribute("inputs:serial_number") db_value = database.inputs.serial_number - expected_value = 20976320 + expected_value = 40976320 actual_value = og.Controller.get(attribute) ogts.verify_values(expected_value, actual_value, _attr_error(attribute, True)) ogts.verify_values(expected_value, db_value, _attr_error(attribute, False)) diff --git a/exts/sl.sensor.camera/sl/sensor/camera/ogn/tests/usd/SlCameraStreamerTemplate.usda b/exts/sl.sensor.camera/sl/sensor/camera/ogn/tests/usd/SlCameraStreamerTemplate.usda index 9d6a2f1..d946023 100644 --- a/exts/sl.sensor.camera/sl/sensor/camera/ogn/tests/usd/SlCameraStreamerTemplate.usda +++ b/exts/sl.sensor.camera/sl/sensor/camera/ogn/tests/usd/SlCameraStreamerTemplate.usda @@ -25,8 +25,8 @@ def OmniGraph "TestGraph" custom uint inputs:exec_in = 0 ( docs="""Triggers execution""" ) - custom uint inputs:serial_number = 20976320 ( - docs="""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""" + custom uint inputs:serial_number = 40976320 ( + docs="""Serial number (identification) of the camera to stream, can be left to default. It must be of one of the compatible values: 40976320, 41116066, 49123828, 45626933, 47890353, 45263213, 47800035, 47706147""" ) custom uint inputs:streaming_port = 30000 ( docs="""Streaming port - unique per camera""" diff --git a/exts/sl.sensor.camera/sl/sensor/camera/pyzed_sim_streamer.cp310-win_amd64.pyd b/exts/sl.sensor.camera/sl/sensor/camera/pyzed_sim_streamer.cp310-win_amd64.pyd index 70d08fb..c4bc099 100644 Binary files a/exts/sl.sensor.camera/sl/sensor/camera/pyzed_sim_streamer.cp310-win_amd64.pyd and b/exts/sl.sensor.camera/sl/sensor/camera/pyzed_sim_streamer.cp310-win_amd64.pyd differ diff --git a/exts/sl.sensor.camera/sl/sensor/camera/pyzed_sim_streamer.cpython-310-x86_64-linux-gnu.so b/exts/sl.sensor.camera/sl/sensor/camera/pyzed_sim_streamer.cpython-310-x86_64-linux-gnu.so index 37757db..6c6dae1 100755 Binary files a/exts/sl.sensor.camera/sl/sensor/camera/pyzed_sim_streamer.cpython-310-x86_64-linux-gnu.so and b/exts/sl.sensor.camera/sl/sensor/camera/pyzed_sim_streamer.cpython-310-x86_64-linux-gnu.so differ