Merge pull request #7 from NVIDIA-ISAAC-ROS/release-2.1

Isaac ROS 2.1.0

Author: hemalshahNV

isaac_ros_benchmark/package.xml

@@ -21,7 +21,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>isaac_ros_benchmark</name>
-  <version>2.0.0</version>
+  <version>2.1.0</version>
   <description>Isaac ROS benchmarking utilities</description>
 
   <maintainer email="[email protected]">Hemal Shah</maintainer>

isaac_ros_benchmark/src/nitros_monitor_node.cpp

@@ -167,11 +167,25 @@ void NitrosMonitorNode::CreateNitrosMonitorSubscriber()
 template<typename T>
 void NitrosMonitorNode::ROSTypeMonitorSubscriberCallback(const std::shared_ptr<T> msg)
 {
+  std::lock_guard<std::mutex> lock(is_monitoring_mutex_);
+  if (!is_monitoring_) {
+    return;
+  }
+
+  uint32_t timestamp_key;
   if (revise_timestamps_as_message_ids_) {
-    RecordEndTimestamp(msg->header.stamp.sec);
+    timestamp_key = msg->header.stamp.sec;
   } else {
-    RecordEndTimestampAutoKey();
+    // Use increamental numbers as timestamp keys
+    timestamp_key = end_timestamps_.size();
   }
+  if (record_start_timestamps_) {
+    std::chrono::time_point<std::chrono::system_clock> start_timestamp(
+      std::chrono::seconds(msg->header.stamp.sec) +
+      std::chrono::nanoseconds(msg->header.stamp.nanosec));
+    RecordStartTimestamp(timestamp_key, start_timestamp);
+  }
+  RecordEndTimestamp(timestamp_key);
 }
 
 void NitrosMonitorNode::NitrosTypeMonitorSubscriberCallback(
@@ -180,22 +194,42 @@ void NitrosMonitorNode::NitrosTypeMonitorSubscriberCallback(
 {
   (void)data_format_name;
 
+  std::lock_guard<std::mutex> lock(is_monitoring_mutex_);
+  if (!is_monitoring_) {
+    return;
+  }
+
+  uint32_t timestamp_key;
   if (revise_timestamps_as_message_ids_) {
-    gxf_result_t code;
     std_msgs::msg::Header ros_header;
-
-    code = nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getEntityTimestamp(
-      msg_base.handle, ros_header);
-    if (code != GXF_SUCCESS) {
+    if (nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getEntityTimestamp(
+        msg_base.handle, ros_header) != GXF_SUCCESS)
+    {
       RCLCPP_ERROR(
         get_logger(),
         "[NitrosMonitorNode] getEntityTimestamp Error");
     }
-
-    RecordEndTimestamp(ros_header.stamp.sec);
+    timestamp_key = ros_header.stamp.sec;
   } else {
-    RecordEndTimestampAutoKey();
+    timestamp_key = end_timestamps_.size();
+  }
+
+  if (record_start_timestamps_) {
+    std_msgs::msg::Header ros_header;
+    if (nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getEntityTimestamp(
+        msg_base.handle, ros_header) != GXF_SUCCESS)
+    {
+      RCLCPP_ERROR(
+        get_logger(),
+        "[NitrosMonitorNode] getEntityTimestamp Error");
+    } else {
+      std::chrono::time_point<std::chrono::system_clock> start_timestamp(
+        std::chrono::seconds(ros_header.stamp.sec) +
+        std::chrono::nanoseconds(ros_header.stamp.nanosec));
+      RecordStartTimestamp(timestamp_key, start_timestamp);
+    }
   }
+  RecordEndTimestamp(timestamp_key);
 }
 
 }  // namespace isaac_ros_benchmark

scripts/isaac_ros_argus_stereo_node.py

@@ -0,0 +1,126 @@
+# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
+# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+#
+# 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.
+#
+# SPDX-License-Identifier: Apache-2.0
+"""
+Live benchmarking Isaac ROS ArgusStereoNode.
+
+The graph consists of the following:
+- Graph under Test:
+    1. ArgusStereoNode: publishes images
+
+Required:
+- Packages:
+    - isaac_ros_argus_camera
+"""
+
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+from ros2_benchmark import ImageResolution
+from ros2_benchmark import BasicPerformanceCalculator, BenchmarkMode
+from ros2_benchmark import ROS2BenchmarkConfig, ROS2BenchmarkTest
+from ros2_benchmark import MonitorPerformanceCalculatorsInfo
+
+IMAGE_RESOLUTION = ImageResolution.HD
+
+def launch_setup(container_prefix, container_sigterm_timeout):
+    """Generate launch description for live benchmarking Isaac ROS ArgusStereoNode."""
+    argus_node = ComposableNode(
+        name='ArgusStereoNode',
+        namespace=TestIsaacROSArgusStereoNode.generate_namespace(),
+        package='isaac_ros_argus_camera',
+        plugin='nvidia::isaac_ros::argus::ArgusStereoNode',
+        parameters=[{
+            'module_id': 5,
+        }]
+    )
+
+    left_image_monitor_node = ComposableNode(
+        name='LeftImageMonitorNode',
+        namespace=TestIsaacROSArgusStereoNode.generate_namespace(),
+        package='isaac_ros_benchmark',
+        plugin='isaac_ros_benchmark::NitrosMonitorNode',
+        parameters=[{
+            'monitor_index': 0,
+            'monitor_data_format': 'nitros_image_rgb8',
+            'use_nitros_type_monitor_sub': True,
+        }],
+        remappings=[('output', 'left/image_raw')]
+    )
+
+    right_image_monitor_node = ComposableNode(
+        name='RightImageMonitorNode',
+        namespace=TestIsaacROSArgusStereoNode.generate_namespace(),
+        package='isaac_ros_benchmark',
+        plugin='isaac_ros_benchmark::NitrosMonitorNode',
+        parameters=[{
+            'monitor_index': 1,
+            'monitor_data_format': 'nitros_image_rgb8',
+            'use_nitros_type_monitor_sub': True,
+        }],
+        remappings=[('output', 'right/image_raw')]
+    )
+
+    composable_node_container = ComposableNodeContainer(
+        name='container',
+        namespace=TestIsaacROSArgusStereoNode.generate_namespace(),
+        package='rclcpp_components',
+        executable='component_container_mt',
+        prefix=container_prefix,
+        sigterm_timeout=container_sigterm_timeout,
+        composable_node_descriptions=[
+            argus_node,
+            left_image_monitor_node,
+            right_image_monitor_node,
+        ],
+        output='screen'
+    )
+
+    return [composable_node_container]
+
+def generate_test_description():
+    return TestIsaacROSArgusStereoNode.generate_test_description_with_nsys(launch_setup)
+
+
+class TestIsaacROSArgusStereoNode(ROS2BenchmarkTest):
+    """Live performance test for Isaac ROS ArgusStereoNode."""
+
+    # Custom configurations
+    config = ROS2BenchmarkConfig(
+        benchmark_name='Isaac ROS ArgusStereoNode Live Benchmark',
+        benchmark_mode=BenchmarkMode.LIVE,
+        benchmark_duration=5,
+        test_iterations=5,
+        collect_start_timestamps_from_monitors=True,
+        custom_report_info={'data_resolution': IMAGE_RESOLUTION},
+        monitor_info_list=[
+            MonitorPerformanceCalculatorsInfo(
+                'start_monitoring0',
+                [BasicPerformanceCalculator({
+                    'report_prefix': 'Argus Left Image',
+                    'message_key_match': True
+                })]),
+            MonitorPerformanceCalculatorsInfo(
+                'start_monitoring1',
+                [BasicPerformanceCalculator({
+                    'report_prefix': 'Argus Right Image',
+                    'message_key_match': True
+                })])
+        ]
+    )
+
+    def test_benchmark(self):
+        self.run_benchmark()