chore: add gemini_intra_process_demo

README.MD

@@ -11,6 +11,10 @@ supports ROS 2 Foxy, Humble, and Jazzy distributions.
   * [Table of Contents](#table-of-contents)
   * [Installation Instructions](#installation-instructions)
   * [Getting start](#getting-start)
+  * [Efficient intra-process communication:](#efficient-intra-process-communication)
+    * [Example](#example)
+      * [Manually loading multiple components into the same process](#manually-loading-multiple-components-into-the-same-process)
+    * [Limitations](#limitations)
   * [Use V4L2 backend](#use-v4l2-backend)
   * [Launch parameters](#launch-parameters)
   * [Predefined presets](#predefined-presets)
@@ -184,6 +188,39 @@ ros2 service call /camera/toggle_ir std_srvs/srv/SetBool "{data : true}"
 ```bash
 ros2 service call /camera/save_point_cloud std_srvs/srv/Empty "{}"
 ```
+
+## Efficient intra-process communication:
+  
+Our ROS2 Wrapper node supports zero-copy communications if loaded in the same process as a subscriber node. This can reduce copy times on image/pointcloud topics, especially with big frame resolutions and high FPS.
+
+You will need to launch a component container and launch our node as a component together with other component nodes. Further details on "Composing multiple nodes in a single process" can be found [here](https://docs.ros.org/en/rolling/Tutorials/Composition.html).
+
+Further details on efficient intra-process communication can be found [here](https://docs.ros.org/en/humble/Tutorials/Intra-Process-Communication.html#efficient-intra-process-communication).
+
+### Example
+#### Manually loading multiple components into the same process
+* Start the component:
+  ```bash
+  ros2 run rclcpp_components component_container
+  ```
+
+* Add the wrapper:
+  ```bash
+  ros2 component load /ComponentManager orbbec_camera orbbec_camera::OBCameraNodeDriver -e use_intra_process_comms:=true
+  ```
+  Load other component nodes (consumers of the wrapper topics) in the same way.
+
+#### Using a launch file
+
+```bash
+ros2 launch orbbec_camera gemini_intra_process_demo_launch.py
+```
+
+### Limitations
+
+* Node components are currently not supported on RCLPY
+* Compressed images using `image_transport` will be disabled as this isn't supported with intra-process communication
+
 ## Use V4L2 backend
 To enable the V4L2 backend for the Gemini2 series cameras, follow these steps:
 

orbbec_camera/CMakeLists.txt

@@ -109,6 +109,7 @@ set(COMMON_INCLUDE_DIRS
   $<INSTALL_INTERFACE:include>
   ${ORBBEC_INCLUDE_DIR}
   ${OpenCV_INCLUDED_DIRS}
+  ${CMAKE_CURRENT_SOURCE_DIR}/tools
 )
 
 set(COMMON_LIBRARIES
@@ -137,6 +138,7 @@ endif ()
 set(SOURCE_FILES
   src/d2c_viewer.cpp
   src/dynamic_params.cpp
+  src/image_publisher.cpp
   src/ob_camera_node_driver.cpp
   src/ob_camera_node.cpp
   src/ros_param_backend.cpp
@@ -213,8 +215,18 @@ add_orbbec_executable(list_camera_profile_mode_node tools/list_camera_profile.cp
 
 add_orbbec_executable(topic_statistics_node tools/topic_statistics.cpp)
 
+
+add_library(frame_latency SHARED tools/frame_latency.cpp)
+target_include_directories(frame_latency PUBLIC ${COMMON_INCLUDE_DIRS})
+target_link_libraries(frame_latency ${COMMON_LIBRARIES})
+ament_target_dependencies(frame_latency ${dependencies})
+
+rclcpp_components_register_node(frame_latency
+        PLUGIN "orbbec_camera::FrameLatencyNode"
+        EXECUTABLE frame_latency_node
+)
 # Install rules
-install(TARGETS ${PROJECT_NAME}
+install(TARGETS ${PROJECT_NAME} frame_latency
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin

orbbec_camera/include/orbbec_camera/image_publisher.h

@@ -0,0 +1,52 @@
+// Copyright 2023 Intel Corporation. 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.
+
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+
+#include <image_transport/image_transport.hpp>
+namespace orbbec_camera {
+class image_publisher {
+ public:
+  virtual void publish(sensor_msgs::msg::Image::UniquePtr image_ptr) = 0;
+  virtual size_t get_subscription_count() const = 0;
+  virtual ~image_publisher() = default;
+};  // namespace image_publisher
+
+// Native RCL implementation of an image publisher (needed for intra-process communication)
+class image_rcl_publisher : public image_publisher {
+ public:
+  image_rcl_publisher(rclcpp::Node& node, const std::string& topic_name,
+                      const rmw_qos_profile_t& qos);
+  void publish(sensor_msgs::msg::Image::UniquePtr image_ptr) override;
+  size_t get_subscription_count() const override;
+
+ private:
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher_impl;
+};
+
+// image_transport implementation of an image publisher (adds a compressed image topic)
+class image_transport_publisher : public image_publisher {
+ public:
+  image_transport_publisher(rclcpp::Node& node, const std::string& topic_name,
+                            const rmw_qos_profile_t& qos);
+  void publish(sensor_msgs::msg::Image::UniquePtr image_ptr) override;
+  size_t get_subscription_count() const override;
+
+ private:
+  std::shared_ptr<image_transport::Publisher> image_publisher_impl;
+};
+}  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -60,6 +60,7 @@
 #include "orbbec_camera/dynamic_params.h"
 #include "orbbec_camera/d2c_viewer.h"
 #include "magic_enum/magic_enum.hpp"
+#include "orbbec_camera/image_publisher.h"
 #include "jpeg_decoder.h"
 #include <std_msgs/msg/string.hpp>
 
@@ -114,7 +115,7 @@ const stream_index_pair INFRA2{OB_STREAM_IR_RIGHT, 0};
 const stream_index_pair GYRO{OB_STREAM_GYRO, 0};
 const stream_index_pair ACCEL{OB_STREAM_ACCEL, 0};
 
-const std::vector<stream_index_pair> IMAGE_STREAMS = {DEPTH, INFRA0, COLOR, INFRA1, INFRA2};
+const std::vector<stream_index_pair> IMAGE_STREAMS = {COLOR,DEPTH, INFRA0, INFRA1, INFRA2};
 
 const std::vector<stream_index_pair> HID_STREAMS = {GYRO, ACCEL};
 
@@ -131,7 +132,7 @@ const std::map<OBStreamType, OBFrameType> STREAM_TYPE_TO_FRAME_TYPE = {
 class OBCameraNode {
  public:
   OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> device,
-               std::shared_ptr<Parameters> parameters);
+               std::shared_ptr<Parameters> parameters, bool use_intra_process = false);
 
   template <class T>
   void setAndGetNodeParameter(
@@ -315,7 +316,7 @@ class OBCameraNode {
   void onNewColorFrameCallback();
 
   void saveImageToFile(const stream_index_pair& stream_index, const cv::Mat& image,
-                       const sensor_msgs::msg::Image::SharedPtr& image_msg);
+                       const sensor_msgs::msg::Image& image_msg);
 
   void onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Frame>& accelframe,
                                        const std::shared_ptr<ob::Frame>& gryoframe);
@@ -391,7 +392,7 @@ class OBCameraNode {
   std::map<stream_index_pair, bool> enable_stream_;
   std::map<stream_index_pair, bool> flip_stream_;
   std::map<stream_index_pair, std::string> stream_name_;
-  std::map<stream_index_pair, image_transport::Publisher> image_publishers_;
+  std::map<stream_index_pair, std::shared_ptr<image_publisher>> image_publishers_;
   std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>
       camera_info_publishers_;
 
@@ -564,7 +565,8 @@ class OBCameraNode {
   std::chrono::milliseconds software_trigger_period_{33};
   bool enable_heartbeat_ = false;
   bool enable_color_undistortion_ = false;
-  image_transport::Publisher color_undistortion_publisher_;
+  std::shared_ptr<image_publisher> color_undistortion_publisher_;
   bool has_first_color_frame_ = false;
+  bool use_intra_process_ = false;
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h

@@ -69,6 +69,7 @@ class OBCameraNodeDriver : public rclcpp::Node {
                             std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
  private:
+ const rclcpp::NodeOptions node_options_;
   std::string config_path_;
   std::unique_ptr<ob::Context> ctx_ = nullptr;
   rclcpp::Logger logger_;

orbbec_camera/launch/gemini_intra_process_demo_launch.py

@@ -0,0 +1,220 @@
+import os
+import yaml
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, GroupAction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import PushRosNamespace, ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def load_yaml(file_path):
+    with open(file_path, 'r') as f:
+        return yaml.safe_load(f)
+
+
+def merge_params(default_params, yaml_params):
+    for key, value in yaml_params.items():
+        if key in default_params:
+            default_params[key] = value
+    return default_params
+
+
+def convert_value(value):
+    if isinstance(value, str):
+        try:
+            return int(value)
+        except ValueError:
+            pass
+        try:
+            return float(value)
+        except ValueError:
+            pass
+        if value.lower() == 'true':
+            return True
+        elif value.lower() == 'false':
+            return False
+    return value
+
+
+def load_parameters(context, args):
+    default_params = {arg.name: LaunchConfiguration(arg.name).perform(context) for arg in args}
+    config_file_path = LaunchConfiguration('config_file_path').perform(context)
+    if config_file_path:
+        yaml_params = load_yaml(config_file_path)
+        default_params = merge_params(default_params, yaml_params)
+    skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
+    return {
+        key: (value if key in skip_convert else convert_value(value))
+        for key, value in default_params.items()
+    }
+
+
+def generate_launch_description():
+    args = [
+        DeclareLaunchArgument('camera_name', default_value='camera'),
+        DeclareLaunchArgument('depth_registration', default_value='false'),
+        DeclareLaunchArgument('serial_number', default_value=''),
+        DeclareLaunchArgument('usb_port', default_value=''),
+        DeclareLaunchArgument('device_num', default_value='1'),
+        DeclareLaunchArgument('point_cloud_qos', default_value='default'),
+        DeclareLaunchArgument('enable_point_cloud', default_value='true'),
+        DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
+        DeclareLaunchArgument('connection_delay', default_value='10'),
+        DeclareLaunchArgument('color_width', default_value='0'),
+        DeclareLaunchArgument('color_height', default_value='0'),
+        DeclareLaunchArgument('color_fps', default_value='0'),
+        DeclareLaunchArgument('color_format', default_value='ANY'),
+        DeclareLaunchArgument('enable_color', default_value='true'),
+        DeclareLaunchArgument('color_qos', default_value='default'),
+        DeclareLaunchArgument('color_camera_info_qos', default_value='default'),
+        DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'),
+        DeclareLaunchArgument('color_exposure', default_value='-1'),
+        DeclareLaunchArgument('color_gain', default_value='-1'),
+        DeclareLaunchArgument('enable_color_auto_white_balance', default_value='true'),
+        DeclareLaunchArgument('color_white_balance', default_value='-1'),
+        DeclareLaunchArgument('depth_width', default_value='0'),
+        DeclareLaunchArgument('depth_height', default_value='0'),
+        DeclareLaunchArgument('depth_fps', default_value='0'),
+        DeclareLaunchArgument('depth_format', default_value='ANY'),
+        DeclareLaunchArgument('enable_depth', default_value='true'),
+        DeclareLaunchArgument('depth_qos', default_value='default'),
+        DeclareLaunchArgument('depth_camera_info_qos', default_value='default'),
+        DeclareLaunchArgument('left_ir_width', default_value='0'),
+        DeclareLaunchArgument('left_ir_height', default_value='0'),
+        DeclareLaunchArgument('left_ir_fps', default_value='0'),
+        DeclareLaunchArgument('left_ir_format', default_value='ANY'),
+        DeclareLaunchArgument('enable_left_ir', default_value='false'),
+        DeclareLaunchArgument('left_ir_qos', default_value='default'),
+        DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'),
+        DeclareLaunchArgument('right_ir_width', default_value='0'),
+        DeclareLaunchArgument('right_ir_height', default_value='0'),
+        DeclareLaunchArgument('right_ir_fps', default_value='0'),
+        DeclareLaunchArgument('right_ir_format', default_value='ANY'),
+        DeclareLaunchArgument('enable_right_ir', default_value='false'),
+        DeclareLaunchArgument('right_ir_qos', default_value='default'),
+        DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'),
+        DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
+        DeclareLaunchArgument('ir_exposure', default_value='-1'),
+        DeclareLaunchArgument('ir_gain', default_value='-1'),
+        DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='false'),
+        DeclareLaunchArgument('enable_accel', default_value='false'),
+        DeclareLaunchArgument('accel_rate', default_value='200hz'),
+        DeclareLaunchArgument('accel_range', default_value='4g'),
+        DeclareLaunchArgument('enable_gyro', default_value='false'),
+        DeclareLaunchArgument('gyro_rate', default_value='200hz'),
+        DeclareLaunchArgument('gyro_range', default_value='1000dps'),
+        DeclareLaunchArgument('liner_accel_cov', default_value='0.01'),
+        DeclareLaunchArgument('angular_vel_cov', default_value='0.01'),
+        DeclareLaunchArgument('publish_tf', default_value='true'),
+        DeclareLaunchArgument('tf_publish_rate', default_value='0.0'),
+        DeclareLaunchArgument('ir_info_url', default_value=''),
+        DeclareLaunchArgument('color_info_url', default_value=''),
+        DeclareLaunchArgument('log_level', default_value='none'),
+        DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
+        DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
+        DeclareLaunchArgument('enable_ldp', default_value='true'),
+        DeclareLaunchArgument('enable_soft_filter', default_value='true'),
+        DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
+        DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('sync_mode', default_value='standalone'),
+        DeclareLaunchArgument('depth_delay_us', default_value='0'),
+        DeclareLaunchArgument('color_delay_us', default_value='0'),
+        DeclareLaunchArgument('trigger2image_delay_us', default_value='0'),
+        DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
+        DeclareLaunchArgument('trigger_out_enabled', default_value='true'),
+        DeclareLaunchArgument('frames_per_trigger', default_value='2'),
+        DeclareLaunchArgument('software_trigger_period', default_value='33'),  # ms
+        DeclareLaunchArgument('enable_frame_sync', default_value='true'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
+        DeclareLaunchArgument('use_hardware_time', default_value='true'),
+        DeclareLaunchArgument('enable_depth_scale', default_value='true'),
+        DeclareLaunchArgument('enable_decimation_filter', default_value='false'),
+        DeclareLaunchArgument('enable_hdr_merge', default_value='false'),
+        DeclareLaunchArgument('enable_sequence_id_filter', default_value='false'),
+        DeclareLaunchArgument('enable_threshold_filter', default_value='false'),
+        DeclareLaunchArgument('enable_noise_removal_filter', default_value='true'),
+        DeclareLaunchArgument('enable_spatial_filter', default_value='false'),
+        DeclareLaunchArgument('enable_temporal_filter', default_value='false'),
+        DeclareLaunchArgument('enable_hole_filling_filter', default_value='false'),
+        DeclareLaunchArgument('decimation_filter_scale_', default_value='-1'),
+        DeclareLaunchArgument('sequence_id_filter_id', default_value='-1'),
+        DeclareLaunchArgument('threshold_filter_max', default_value='-1'),
+        DeclareLaunchArgument('threshold_filter_min', default_value='-1'),
+        DeclareLaunchArgument('noise_removal_filter_min_diff', default_value='256'),
+        DeclareLaunchArgument('noise_removal_filter_max_size', default_value='80'),
+        DeclareLaunchArgument('spatial_filter_alpha', default_value='-1.0'),
+        DeclareLaunchArgument('spatial_filter_diff_threshold', default_value='-1'),
+        DeclareLaunchArgument('spatial_filter_magnitude', default_value='-1'),
+        DeclareLaunchArgument('spatial_filter_radius', default_value='-1'),
+        DeclareLaunchArgument('temporal_filter_diff_threshold', default_value='-1.0'),
+        DeclareLaunchArgument('temporal_filter_weight', default_value='-1.0'),
+        DeclareLaunchArgument('hole_filling_filter_mode', default_value=''),
+        DeclareLaunchArgument('hdr_merge_exposure_1', default_value='-1'),
+        DeclareLaunchArgument('hdr_merge_gain_1', default_value='-1'),
+        DeclareLaunchArgument('hdr_merge_exposure_2', default_value='-1'),
+        DeclareLaunchArgument('hdr_merge_gain_2', default_value='-1'),
+        DeclareLaunchArgument('align_mode', default_value='SW'),
+        DeclareLaunchArgument('diagnostic_period', default_value='1.0'),
+        DeclareLaunchArgument('enable_laser', default_value='true'),
+        DeclareLaunchArgument('depth_precision', default_value=''),
+        DeclareLaunchArgument('device_preset', default_value='Default'),
+        DeclareLaunchArgument('laser_on_off_mode', default_value='0'),
+        DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
+        DeclareLaunchArgument('laser_energy_level', default_value='-1'),
+        DeclareLaunchArgument('enable_3d_reconstruction_mode', default_value='false'),
+        DeclareLaunchArgument('enable_sync_host_time', default_value='true'),
+        DeclareLaunchArgument('time_domain', default_value='device'),
+        DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
+        DeclareLaunchArgument('config_file_path', default_value=''),
+        DeclareLaunchArgument('enable_heartbeat', default_value='false'),
+        DeclareLaunchArgument('topic_type', default_value='points'),
+        DeclareLaunchArgument('topic_name', default_value='/camera/depth/points'),
+        DeclareLaunchArgument('use_intra_process_comms', default_value='true'),
+    ]
+
+    def get_params(context, args):
+        return [load_parameters(context, args)]
+
+    def create_node_action(context, args):
+        params = get_params(context, args)
+        ros_distro = os.environ.get("ROS_DISTRO", "humble")
+        if ros_distro == "humble":
+            return [
+                GroupAction([
+                    PushRosNamespace(LaunchConfiguration("camera_name")),
+                    ComposableNodeContainer(
+                        name="camera_container",
+                        namespace="",
+                        package="rclcpp_components",
+                        executable="component_container",
+                        composable_node_descriptions=[
+                            ComposableNode(
+                                package="orbbec_camera",
+                                plugin="orbbec_camera::OBCameraNodeDriver",
+                                name=LaunchConfiguration("camera_name"),
+                                parameters=params,
+                                extra_arguments=[{'use_intra_process_comms': True}],
+                            ),
+                            ComposableNode(
+                                package="orbbec_camera",
+                                plugin="orbbec_camera::FrameLatencyNode",
+                                name="frame_latency",
+                                parameters=[
+                                    {"topic_name": LaunchConfiguration("topic_name")},
+                                    {"topic_type": LaunchConfiguration("topic_type")},
+                                ],
+
+                                extra_arguments=[{'use_intra_process_comms': True}],
+                            ),
+                        ],
+                        output="screen",
+                        #prefix=['xterm -e gdb -ex run --args'],  
+                    )
+                ])
+            ]
+
+    return LaunchDescription(
+        args + [
+            OpaqueFunction(function=lambda context: create_node_action(context, args))
+        ]
+    )

orbbec_camera/src/image_publisher.cpp

@@ -0,0 +1,48 @@
+// Copyright 2023 Intel Corporation. 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.
+
+#include "orbbec_camera/image_publisher.h"
+
+namespace orbbec_camera {
+
+// --- image_rcl_publisher implementation ---
+image_rcl_publisher::image_rcl_publisher(rclcpp::Node& node, const std::string& topic_name,
+                                         const rmw_qos_profile_t& qos) {
+  image_publisher_impl = node.create_publisher<sensor_msgs::msg::Image>(
+      topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos));
+}
+
+void image_rcl_publisher::publish(sensor_msgs::msg::Image::UniquePtr image_ptr) {
+  image_publisher_impl->publish(std::move(image_ptr));
+}
+
+size_t image_rcl_publisher::get_subscription_count() const {
+  return image_publisher_impl->get_subscription_count();
+}
+
+// --- image_transport_publisher implementation ---
+image_transport_publisher::image_transport_publisher(rclcpp::Node& node,
+                                                     const std::string& topic_name,
+                                                     const rmw_qos_profile_t& qos) {
+  image_publisher_impl = std::make_shared<image_transport::Publisher>(
+      image_transport::create_publisher(&node, topic_name, qos));
+}
+void image_transport_publisher::publish(sensor_msgs::msg::Image::UniquePtr image_ptr) {
+  image_publisher_impl->publish(*image_ptr);
+}
+
+size_t image_transport_publisher::get_subscription_count() const {
+  return image_publisher_impl->getNumSubscribers();
+}
+}  // namespace orbbec_camera

orbbec_camera/src/ob_camera_node.cpp

@@ -35,11 +35,14 @@ namespace orbbec_camera {
 using namespace std::chrono_literals;
 
 OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> device,
-                           std::shared_ptr<Parameters> parameters)
+                           std::shared_ptr<Parameters> parameters, bool use_intra_process)
     : node_(node),
       device_(std::move(device)),
       parameters_(std::move(parameters)),
-      logger_(node->get_logger()) {
+      logger_(node->get_logger()),
+      use_intra_process_(use_intra_process) {
+  RCLCPP_INFO_STREAM(logger_,
+                     "OBCameraNode: use_intra_process: " << (use_intra_process ? "ON" : "OFF"));
   is_running_.store(true);
   stream_name_[COLOR] = "color";
   stream_name_[DEPTH] = "depth";
@@ -511,14 +514,14 @@ void OBCameraNode::setupDepthPostProcessFilter() {
     } else if (filter_name == "NoiseRemovalFilter" && enable_noise_removal_filter_) {
       auto noise_removal_filter = filter->as<ob::NoiseRemovalFilter>();
       OBNoiseRemovalFilterParams params = noise_removal_filter->getFilterParams();
-      RCLCPP_INFO_STREAM(
-          logger_, "Default noise removal filter params: " << "disp_diff: " << params.disp_diff
-                                                           << ", max_size: " << params.max_size);
+      RCLCPP_INFO_STREAM(logger_, "Default noise removal filter params: "
+                                      << "disp_diff: " << params.disp_diff
+                                      << ", max_size: " << params.max_size);
       params.disp_diff = noise_removal_filter_min_diff_;
       params.max_size = noise_removal_filter_max_size_;
-      RCLCPP_INFO_STREAM(logger_,
-                         "Set noise removal filter params: " << "disp_diff: " << params.disp_diff
-                                                             << ", max_size: " << params.max_size);
+      RCLCPP_INFO_STREAM(logger_, "Set noise removal filter params: "
+                                      << "disp_diff: " << params.disp_diff
+                                      << ", max_size: " << params.max_size);
       if (noise_removal_filter_min_diff_ != -1 && noise_removal_filter_max_size_ != -1) {
         noise_removal_filter->setFilterParams(params);
       }
@@ -527,11 +530,11 @@ void OBCameraNode::setupDepthPostProcessFilter() {
           hdr_merge_gain_2_ != -1) {
         auto hdr_merge_filter = filter->as<ob::HdrMerge>();
         hdr_merge_filter->enable(true);
-        RCLCPP_INFO_STREAM(
-            logger_, "Set HDR merge filter params: " << "exposure_1: " << hdr_merge_exposure_1_
-                                                     << ", gain_1: " << hdr_merge_gain_1_
-                                                     << ", exposure_2: " << hdr_merge_exposure_2_
-                                                     << ", gain_2: " << hdr_merge_gain_2_);
+        RCLCPP_INFO_STREAM(logger_, "Set HDR merge filter params: "
+                                        << "exposure_1: " << hdr_merge_exposure_1_
+                                        << ", gain_1: " << hdr_merge_gain_1_
+                                        << ", exposure_2: " << hdr_merge_exposure_2_
+                                        << ", gain_2: " << hdr_merge_gain_2_);
         auto config = OBHdrConfig();
         config.enable = true;
         config.exposure_1 = hdr_merge_exposure_1_;
@@ -607,10 +610,10 @@ void OBCameraNode::setupProfiles() {
       for (size_t i = 0; i < profiles->count(); i++) {
         auto profile = profiles->getProfile(i)->as<ob::VideoStreamProfile>();
         RCLCPP_DEBUG_STREAM(
-            logger_,
-            "Sensor profile: " << "stream_type: " << magic_enum::enum_name(profile->type())
-                               << "Format: " << profile->format() << ", Width: " << profile->width()
-                               << ", Height: " << profile->height() << ", FPS: " << profile->fps());
+            logger_, "Sensor profile: "
+                         << "stream_type: " << magic_enum::enum_name(profile->type())
+                         << "Format: " << profile->format() << ", Width: " << profile->width()
+                         << ", Height: " << profile->height() << ", FPS: " << profile->fps());
         supported_profiles_[elem].emplace_back(profile);
       }
       std::shared_ptr<ob::VideoStreamProfile> selected_profile;
@@ -912,7 +915,11 @@ void OBCameraNode::getParameters() {
     param_name = stream_name_[stream_index] + "_fps";
     setAndGetNodeParameter(fps_[stream_index], param_name, 0);
     param_name = "enable_" + stream_name_[stream_index];
-    setAndGetNodeParameter(enable_stream_[stream_index], param_name, false);
+    if (stream_index == DEPTH) {
+      setAndGetNodeParameter(enable_stream_[stream_index], param_name, true);
+    } else {
+      setAndGetNodeParameter(enable_stream_[stream_index], param_name, false);
+    }
     param_name = "flip_" + stream_name_[stream_index];
     setAndGetNodeParameter(flip_stream_[stream_index], param_name, false);
     param_name = camera_name_ + "_" + stream_name_[stream_index] + "_frame_id";
@@ -1162,6 +1169,9 @@ void OBCameraNode::setupPublishers() {
   using PointCloud2 = sensor_msgs::msg::PointCloud2;
   using CameraInfo = sensor_msgs::msg::CameraInfo;
   auto point_cloud_qos_profile = getRMWQosProfileFromString(point_cloud_qos_);
+  if (use_intra_process_) {
+    point_cloud_qos_profile = rmw_qos_profile_default;
+  }
   if (enable_colored_point_cloud_) {
     depth_registration_cloud_pub_ = node_->create_publisher<PointCloud2>(
         "depth_registered/points",
@@ -1184,11 +1194,23 @@ void OBCameraNode::setupPublishers() {
     std::string topic = name + "/image_raw";
     auto image_qos = image_qos_[stream_index];
     auto image_qos_profile = getRMWQosProfileFromString(image_qos);
-    image_publishers_[stream_index] =
-        image_transport::create_publisher(node_, topic, image_qos_profile);
+    if (use_intra_process_) {
+      image_qos_profile = rmw_qos_profile_default;
+    }
+    if (use_intra_process_) {
+      image_publishers_[stream_index] =
+          std::make_shared<image_rcl_publisher>(*node_, topic, image_qos_profile);
+    } else {
+      image_publishers_[stream_index] =
+          std::make_shared<image_transport_publisher>(*node_, topic, image_qos_profile);
+    }
+
     topic = name + "/camera_info";
     auto camera_info_qos = camera_info_qos_[stream_index];
     auto camera_info_qos_profile = getRMWQosProfileFromString(camera_info_qos);
+    if (use_intra_process_) {
+      camera_info_qos_profile = rmw_qos_profile_default;
+    }
     camera_info_publishers_[stream_index] = node_->create_publisher<CameraInfo>(
         topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
                            camera_info_qos_profile));
@@ -1200,14 +1222,22 @@ void OBCameraNode::setupPublishers() {
                           camera_info_qos_profile));
     }
     if (stream_index == COLOR && enable_color_undistortion_) {
-      color_undistortion_publisher_ =
-          image_transport::create_publisher(node_, "color/image_undistorted", image_qos_profile);
+      if (use_intra_process_) {
+        color_undistortion_publisher_ = std::make_shared<image_rcl_publisher>(
+            *node_, "color/image_undistorted", image_qos_profile);
+      } else {
+        color_undistortion_publisher_ = std::make_shared<image_transport_publisher>(
+            *node_, "color/image_undistorted", image_qos_profile);
+      }
     }
   }
 
   if (enable_sync_output_accel_gyro_) {
     std::string topic_name = stream_name_[GYRO] + "_" + stream_name_[ACCEL] + "/sample";
     auto data_qos = getRMWQosProfileFromString(imu_qos_[GYRO]);
+    if (use_intra_process_) {
+      data_qos = rmw_qos_profile_default;
+    }
     imu_gyro_accel_publisher_ = node_->create_publisher<sensor_msgs::msg::Imu>(
         topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
     topic_name = stream_name_[GYRO] + "/imu_info";
@@ -1223,6 +1253,9 @@ void OBCameraNode::setupPublishers() {
       }
       std::string data_topic_name = stream_name_[stream_index] + "/sample";
       auto data_qos = getRMWQosProfileFromString(imu_qos_[stream_index]);
+      if (use_intra_process_) {
+        data_qos = rmw_qos_profile_default;
+      }
       imu_publishers_[stream_index] = node_->create_publisher<sensor_msgs::msg::Imu>(
           data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
       data_topic_name = stream_name_[stream_index] + "/imu_info";
@@ -1232,38 +1265,43 @@ void OBCameraNode::setupPublishers() {
               rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
     }
   }
+
+  auto extrinsics_qos = rclcpp::QoS(1).transient_local();
+  if (use_intra_process_) {
+    extrinsics_qos = rclcpp::QoS(1);
+  }
   if (enable_stream_[DEPTH] && enable_stream_[INFRA0]) {
     depth_to_other_extrinsics_publishers_[INFRA0] =
         node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
-            "/" + camera_name_ + "/depth_to_ir", rclcpp::QoS(1).transient_local());
+            "/" + camera_name_ + "/depth_to_ir", extrinsics_qos);
   }
   if (enable_stream_[DEPTH] && enable_stream_[COLOR]) {
     depth_to_other_extrinsics_publishers_[COLOR] =
         node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
-            "/" + camera_name_ + "/depth_to_color", rclcpp::QoS(1).transient_local());
+            "/" + camera_name_ + "/depth_to_color", extrinsics_qos);
   }
   if (enable_stream_[DEPTH] && enable_stream_[INFRA1]) {
     depth_to_other_extrinsics_publishers_[INFRA1] =
         node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
-            "/" + camera_name_ + "/depth_to_left_ir", rclcpp::QoS(1).transient_local());
+            "/" + camera_name_ + "/depth_to_left_ir", extrinsics_qos);
   }
   if (enable_stream_[DEPTH] && enable_stream_[INFRA2]) {
     depth_to_other_extrinsics_publishers_[INFRA2] =
         node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
-            "/" + camera_name_ + "/depth_to_right_ir", rclcpp::QoS(1).transient_local());
+            "/" + camera_name_ + "/depth_to_right_ir", extrinsics_qos);
   }
   if (enable_stream_[DEPTH] && enable_stream_[ACCEL]) {
     depth_to_other_extrinsics_publishers_[ACCEL] =
         node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
-            "/" + camera_name_ + "/depth_to_accel", rclcpp::QoS(1).transient_local());
+            "/" + camera_name_ + "/depth_to_accel", extrinsics_qos);
   }
   if (enable_stream_[DEPTH] && enable_stream_[GYRO]) {
     depth_to_other_extrinsics_publishers_[GYRO] =
         node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
-            "/" + camera_name_ + "/depth_to_gyro", rclcpp::QoS(1).transient_local());
+            "/" + camera_name_ + "/depth_to_gyro", extrinsics_qos);
   }
-  filter_status_pub_ = node_->create_publisher<std_msgs::msg::String>(
-      "depth_filter_status", rclcpp::QoS(1).transient_local());
+  filter_status_pub_ =
+      node_->create_publisher<std_msgs::msg::String>("depth_filter_status", extrinsics_qos);
   std_msgs::msg::String msg;
   msg.data = filter_status_.dump(2);
   filter_status_pub_->publish(msg);
@@ -1564,7 +1602,6 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
     if (depth_frame_ && depth_frame_->hasMetadata(OB_FRAME_METADATA_TYPE_LASER_STATUS)) {
       depth_laser_status = depth_frame_->getMetadataValue(OB_FRAME_METADATA_TYPE_LASER_STATUS) == 1;
     }
-
     auto device_info = device_->getDeviceInfo();
     CHECK_NOTNULL(device_info.get());
     auto pid = device_info->pid();
@@ -1586,8 +1623,7 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
                      "Depth registration is disabled or align filter is null or depth frame is "
                      "null or color frame is null");
       }
-      if(depth_registration_ && align_filter_ && depth_frame_ && !has_first_color_frame_) {
-        RCLCPP_WARN(logger_, "Waiting for the first color frame to align depth frame");
+      if (depth_registration_ && align_filter_ && depth_frame_ && !has_first_color_frame_) {
         return;
       }
     }
@@ -1697,7 +1733,8 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
   if (!rgb_buffer_) {
     return false;
   }
-  bool has_subscriber = image_publishers_[COLOR].getNumSubscribers() > 0;
+  CHECK_NOTNULL(image_publishers_[COLOR]);
+  bool has_subscriber = image_publishers_[COLOR]->get_subscription_count() > 0;
   if (enable_colored_point_cloud_ && depth_registration_cloud_pub_->get_subscription_count() > 0) {
     has_subscriber = true;
   }
@@ -1786,7 +1823,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   if (frame == nullptr) {
     return;
   }
-  bool has_subscriber = image_publishers_[stream_index].getNumSubscribers() > 0;
+  CHECK_NOTNULL(image_publishers_[stream_index]);
+  bool has_subscriber = image_publishers_[stream_index]->get_subscription_count() > 0;
   has_subscriber =
       has_subscriber || camera_info_publishers_[stream_index]->get_subscription_count() > 0;
   has_subscriber =
@@ -1864,7 +1902,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   if (isGemini335PID(pid)) {
     publishMetadata(frame, stream_index, camera_info.header);
   }
-  if (image_publishers_[stream_index].getNumSubscribers() == 0) {
+  CHECK_NOTNULL(image_publishers_[stream_index]);
+  if (image_publishers_[stream_index]->get_subscription_count() == 0) {
     return;
   }
   auto &image = images_[stream_index];
@@ -1884,28 +1923,30 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
     auto depth_scale = video_frame->as<ob::DepthFrame>()->getValueScale();
     image = image * depth_scale;
   }
-  auto image_msg =
-      cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], image).toImageMsg();
+  sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image());
+
+  cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], image)
+      .toImageMsg(*image_msg);
   CHECK_NOTNULL(image_msg.get());
   image_msg->header.stamp = timestamp;
   image_msg->is_bigendian = false;
   image_msg->step = width * unit_step_size_[stream_index];
   image_msg->header.frame_id = frame_id;
   CHECK(image_publishers_.count(stream_index) > 0);
-  saveImageToFile(stream_index, image, image_msg);
-  image_publishers_[stream_index].publish(std::move(image_msg));
+  saveImageToFile(stream_index, image, *image_msg);
+  image_publishers_[stream_index]->publish(std::move(image_msg));
   if (stream_index == COLOR && enable_color_undistortion_ &&
-      color_undistortion_publisher_.getNumSubscribers() > 0) {
+      color_undistortion_publisher_->get_subscription_count() > 0) {
     auto undistorted_image = undistortImage(image, intrinsic, distortion);
-    auto undistorted_image_msg =
-        cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], undistorted_image)
-            .toImageMsg();
+    sensor_msgs::msg::Image::UniquePtr undistorted_image_msg(new sensor_msgs::msg::Image());
+    cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], undistorted_image)
+        .toImageMsg(*undistorted_image_msg);
     CHECK_NOTNULL(undistorted_image_msg.get());
     undistorted_image_msg->header.stamp = timestamp;
     undistorted_image_msg->is_bigendian = false;
     undistorted_image_msg->step = width * unit_step_size_[stream_index];
     undistorted_image_msg->header.frame_id = frame_id;
-    color_undistortion_publisher_.publish(std::move(undistorted_image_msg));
+    color_undistortion_publisher_->publish(std::move(undistorted_image_msg));
   }
 }
 
@@ -1937,7 +1978,7 @@ void OBCameraNode::publishMetadata(const std::shared_ptr<ob::Frame> &frame,
 }
 
 void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const cv::Mat &image,
-                                   const sensor_msgs::msg::Image::SharedPtr &image_msg) {
+                                   const sensor_msgs::msg::Image &image_msg) {
   if (save_images_[stream_index]) {
     auto now = time(nullptr);
     std::stringstream ss;
@@ -1947,8 +1988,8 @@ void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const
     int index = save_images_count_[stream_index];
     std::string file_suffix = stream_index == COLOR ? ".png" : ".raw";
     std::string filename = current_path + "/image/" + stream_name_[stream_index] + "_" +
-                           std::to_string(image_msg->width) + "x" +
-                           std::to_string(image_msg->height) + "_" + std::to_string(fps) + "hz_" +
+                           std::to_string(image_msg.width) + "x" +
+                           std::to_string(image_msg.height) + "_" + std::to_string(fps) + "hz_" +
                            ss.str() + "_" + std::to_string(index) + file_suffix;
     if (!std::filesystem::exists(current_path + "/image")) {
       std::filesystem::create_directory(current_path + "/image");
@@ -2242,7 +2283,6 @@ void OBCameraNode::calcAndPublishStaticTransform() {
     publishStaticTF(node_->now(), zero_trans, zero_rot, camera_link_frame_id_,
                     frame_id_[base_stream_]);
   }
-
   if (enable_stream_[DEPTH] && enable_stream_[COLOR]) {
     static const char *frame_id = "depth_to_color_extrinsics";
     OBExtrinsic ex;
@@ -2255,6 +2295,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
     }
     depth_to_other_extrinsics_[COLOR] = ex;
     auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
+    CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[COLOR]);
     depth_to_other_extrinsics_publishers_[COLOR]->publish(ex_msg);
   }
   if (enable_stream_[DEPTH] && enable_stream_[INFRA0]) {
@@ -2269,6 +2310,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
     }
     depth_to_other_extrinsics_[INFRA0] = ex;
     auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
+    CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA0]);
     depth_to_other_extrinsics_publishers_[INFRA0]->publish(ex_msg);
   }
   if (enable_stream_[DEPTH] && enable_stream_[INFRA1]) {
@@ -2283,6 +2325,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
     }
     depth_to_other_extrinsics_[INFRA1] = ex;
     auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
+    CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA1]);
     depth_to_other_extrinsics_publishers_[INFRA1]->publish(ex_msg);
   }
   if (enable_stream_[DEPTH] && enable_stream_[INFRA2]) {
@@ -2298,6 +2341,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
     ex.trans[0] = -std::abs(ex.trans[0]);
     depth_to_other_extrinsics_[INFRA2] = ex;
     auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
+    CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[INFRA2]);
     depth_to_other_extrinsics_publishers_[INFRA2]->publish(ex_msg);
   }
   if (enable_stream_[DEPTH] && enable_stream_[ACCEL]) {
@@ -2312,6 +2356,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
     }
     depth_to_other_extrinsics_[ACCEL] = ex;
     auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
+    CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[ACCEL]);
     depth_to_other_extrinsics_publishers_[ACCEL]->publish(ex_msg);
   }
   if (enable_stream_[DEPTH] && enable_stream_[GYRO]) {
@@ -2326,6 +2371,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
     }
     depth_to_other_extrinsics_[GYRO] = ex;
     auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
+    CHECK_NOTNULL(depth_to_other_extrinsics_publishers_[GYRO]);
     depth_to_other_extrinsics_publishers_[GYRO]->publish(ex_msg);
   }
 }

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -27,6 +27,7 @@
 namespace orbbec_camera {
 OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
     : Node("orbbec_camera_node", "/", node_options),
+      node_options_(node_options),
       config_path_(ament_index_cpp::get_package_share_directory("orbbec_camera") +
                    "/config/OrbbecSDKConfig_v1.0.xml"),
       ctx_(std::make_unique<ob::Context>(config_path_.c_str())),
@@ -37,6 +38,7 @@ OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
 OBCameraNodeDriver::OBCameraNodeDriver(const std::string &node_name, const std::string &ns,
                                        const rclcpp::NodeOptions &node_options)
     : Node(node_name, ns, node_options),
+      node_options_(node_options),
       ctx_(std::make_unique<ob::Context>()),
       logger_(this->get_logger()) {
   init();
@@ -311,7 +313,8 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
   if (ob_camera_node_) {
     ob_camera_node_.reset();
   }
-  ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_);
+  ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_, parameters_,
+                                                   node_options_.use_intra_process_comms());
   ob_camera_node_->startIMU();
   ob_camera_node_->startStreams();
   device_connected_ = true;

orbbec_camera/tools/frame_latency.cpp

@@ -0,0 +1,115 @@
+// Copyright 2023 Intel Corporation. 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.
+
+// DESCRIPTION: #
+// ------------ #
+// This tool created a node which can be used to calulate the specified topic's latency.
+// Input parameters:
+//    - topic_name : <String>
+//          - topic to which latency need to be calculated
+//    - topic_type : <String>
+//          - Message type of the topic.
+//          - Valid inputs: {'image','points','imu','metadata','camera_info','rgbd','imu_info','tf'}
+// Note:
+//    - This tool doesn't support calulating latency for extrinsic topics.
+//      Because, those topics doesn't have timestamp in it and this tool uses
+//      that timestamp as an input to calculate the latency.
+//
+
+#include <sstream>
+#include <string>
+#include <chrono>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+
+using namespace std::chrono_literals;
+#include "frame_latency.hpp"
+
+namespace orbbec_camera {
+
+FrameLatencyNode::FrameLatencyNode(const std::string& node_name, const std::string& ns,
+                                   const rclcpp::NodeOptions& node_options)
+    : Node(node_name, ns, node_options), logger_(this->get_logger()) {}
+
+std::string topic_name = "/camera/color/image_raw";
+std::string topic_type = "image";
+
+template <typename MsgType>
+void FrameLatencyNode::createListener(const std::string& topic_name,
+                                      const rmw_qos_profile_t qos_profile) {
+  RCLCPP_INFO_STREAM(logger_, "createListener");
+  sub_ = this->create_subscription<MsgType>(
+      topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile), qos_profile),
+      [&, this](const std::shared_ptr<MsgType> msg) {
+        rclcpp::Time curr_time = this->get_clock()->now();
+        auto latency = (curr_time - msg->header.stamp).seconds();
+        RCLCPP_INFO_STREAM_THROTTLE(logger_, *this->get_clock(), 1000.0,
+                                    "Got msg with "
+                                        << msg->header.frame_id << " frame id at address 0x"
+                                        << std::hex << reinterpret_cast<std::uintptr_t>(msg.get())
+                                        << std::dec << " with latency of " << latency << " [sec]");
+      });
+}
+
+void FrameLatencyNode::createTFListener(const std::string& topic_name,
+                                        const rmw_qos_profile_t qos_profile) {
+  sub_ = this->create_subscription<tf2_msgs::msg::TFMessage>(
+      topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile), qos_profile),
+      [&, this](const std::shared_ptr<tf2_msgs::msg::TFMessage> msg) {
+        rclcpp::Time curr_time = this->get_clock()->now();
+        auto latency = (curr_time - msg->transforms.back().header.stamp).seconds();
+        RCLCPP_INFO_STREAM_THROTTLE(
+            logger_, *this->get_clock(), 1000.0,
+            "Got msg with " << msg->transforms.back().header.frame_id << " frame id at address 0x"
+                            << std::hex << reinterpret_cast<std::uintptr_t>(msg.get()) << std::dec
+                            << " with latency of " << latency << " [sec]");
+      });
+}
+
+FrameLatencyNode::FrameLatencyNode(const rclcpp::NodeOptions& node_options)
+    : Node("frame_latency", "/", node_options), logger_(this->get_logger()) {
+  RCLCPP_INFO_STREAM(logger_, "frame_latency node is UP!");
+  RCLCPP_INFO_STREAM(
+      logger_,
+      "Intra-Process is " << (this->get_node_options().use_intra_process_comms() ? "ON" : "OFF"));
+
+  topic_name = this->declare_parameter("topic_name", topic_name);
+  topic_type = this->declare_parameter("topic_type", topic_type);
+
+  RCLCPP_INFO_STREAM(logger_, "Subscribing to Topic: " << topic_name);
+
+  if (topic_type == "image") {
+    createListener<sensor_msgs::msg::Image>(topic_name, rmw_qos_profile_default);
+  } else if (topic_type == "points") {
+    createListener<sensor_msgs::msg::PointCloud2>(topic_name, rmw_qos_profile_default);
+  } else if (topic_type == "imu") {
+    createListener<sensor_msgs::msg::Imu>(topic_name, rmw_qos_profile_sensor_data);
+  } else if (topic_type == "metadata") {
+    createListener<orbbec_camera_msgs::msg::Metadata>(topic_name, rmw_qos_profile_default);
+  } else if (topic_type == "camera_info") {
+    createListener<sensor_msgs::msg::CameraInfo>(topic_name, rmw_qos_profile_default);
+  } else if (topic_type == "rgbd") {
+    createListener<orbbec_camera_msgs::msg::RGBD>(topic_name, rmw_qos_profile_default);
+  } else if (topic_type == "imu_info") {
+    createListener<orbbec_camera_msgs::msg::IMUInfo>(topic_name, rmw_qos_profile_default);
+  } else if (topic_type == "tf") {
+    createTFListener(topic_name, rmw_qos_profile_default);
+  } else {
+    RCLCPP_ERROR_STREAM(logger_, "Specified message type '" << topic_type << "' is not supported");
+  }
+}
+}  // namespace orbbec_camera
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::FrameLatencyNode)

orbbec_camera/tools/frame_latency.hpp

@@ -0,0 +1,53 @@
+// Copyright 2023 Intel Corporation. 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.
+
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include "sensor_msgs/msg/image.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <diagnostic_updater/publisher.hpp>
+#include "orbbec_camera_msgs/msg/imu_info.hpp"
+#include "orbbec_camera_msgs/msg/extrinsics.hpp"
+#include "orbbec_camera_msgs/msg/metadata.hpp"
+#include "orbbec_camera_msgs/msg/rgbd.hpp"
+#include <sensor_msgs/image_encodings.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+
+namespace orbbec_camera {
+class FrameLatencyNode : public rclcpp::Node {
+ public:
+  explicit FrameLatencyNode(const rclcpp::NodeOptions& node_options =
+                                rclcpp::NodeOptions().use_intra_process_comms(true));
+
+  FrameLatencyNode(const std::string& node_name, const std::string& ns,
+                   const rclcpp::NodeOptions& node_options =
+                       rclcpp::NodeOptions().use_intra_process_comms(true));
+
+  template <typename MsgType>
+  void createListener(const std::string& topic_name, rmw_qos_profile_t qos_profile);
+
+  void createTFListener(const std::string& topic_name, rmw_qos_profile_t qos_profile);
+
+ private:
+  std::shared_ptr<void> sub_ = nullptr;
+
+  rclcpp::Logger logger_;
+};
+}  // namespace orbbec_camera

orbbec_camera_msgs/CMakeLists.txt

@@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Extrinsics.msg"
   "msg/Metadata.msg"
   "msg/IMUInfo.msg"
+  "msg/RGBD.msg"
   "srv/GetBool.srv"
   "srv/GetDeviceInfo.srv"
   "srv/GetCameraInfo.srv"

orbbec_camera_msgs/msg/RGBD.msg

@@ -0,0 +1,6 @@
+# RGBD Message
+std_msgs/Header header
+sensor_msgs/CameraInfo rgb_camera_info
+sensor_msgs/CameraInfo depth_camera_info
+sensor_msgs/Image rgb
+sensor_msgs/Image depth